rossensorpublisher.h
Go to the documentation of this file.
00001 // Copyright (c) 2010 Rosen Diankov
00002 //
00003 // Licensed under the Apache License, Version 2.0 (the "License");
00004 // you may not use this file except in compliance with the License.
00005 // You may obtain a copy of the License at
00006 //     http://www.apache.org/licenses/LICENSE-2.0
00007 //
00008 // Unless required by applicable law or agreed to in writing, software
00009 // distributed under the License is distributed on an "AS IS" BASIS,
00010 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00011 // See the License for the specific language governing permissions and
00012 // limitations under the License.
00013 //
00014 // \author Rosen Diankov
00015 #ifndef OPENRAVE_SENSORPUBLISHER_H
00016 #define OPENRAVE_SENSORPUBLISHER_H
00017 
00018 #include "plugindefs.h"
00019 #include <sensor_msgs/Image.h>
00020 #include <sensor_msgs/CameraInfo.h>
00021 #include <sensor_msgs/LaserScan.h>
00022 #include <sensor_msgs/PointCloud2.h>
00023 #include <sensor_msgs/PointCloud.h>
00024 #include <sensor_msgs/PointField.h>
00025 #include <sensor_msgs/Imu.h>
00026 #include <sensor_msgs/JointState.h>
00027 #include <nav_msgs/Odometry.h>
00028 #include <geometry_msgs/WrenchStamped.h>
00029 #include <geometry_msgs/Pose.h>
00030 #include <geometry_msgs/Vector3.h>
00031 
00032 class ROSSensorPublisher : public ProblemInstance
00033 {
00034 public:
00035     ROSSensorPublisher(EnvironmentBasePtr penv) : ProblemInstance(penv), _bDestroyThread(true) {
00036         __description = ":Interface Author: Rosen Diankov\n\nPublishes openrave robot sensor messages to the ROS network";
00037         RegisterCommand("RegisterRobot",boost::bind(&ROSSensorPublisher::RegisterRobot,this,_1,_2),
00038                         "Registers a robot to publish its sensor data.");
00039         RegisterCommand("RegisterSensor",boost::bind(&ROSSensorPublisher::RegisterSensor,this,_1,_2),
00040                         "Registers a sensor directly added to the environment for ROS publishing.");
00041         RegisterCommand("UnregisterRobot",boost::bind(&ROSSensorPublisher::UnregisterRobot,this,_1,_2),
00042                         "Unregisters a robot.");
00043         RegisterCommand("UnregisterSensor",boost::bind(&ROSSensorPublisher::UnregisterSensor,this,_1,_2),
00044                         "Unregisters a sensor added with RegisterSensor.");
00045     }
00046     virtual ~ROSSensorPublisher() {
00047         Destroy();
00048     }
00049 
00050     virtual void Destroy()
00051     {
00052         _bDestroyThread = true;
00053         _ros.reset();
00054         _threadros.join();
00055         {
00056             boost::mutex::scoped_lock lock(_mutex);
00057         }
00058         ProblemInstance::Destroy();
00059     }
00060 
00061     virtual void Reset()
00062     {
00063         _robots.clear();
00064         _sensors.clear();
00065         {
00066             boost::mutex::scoped_lock lock(_mutex);
00067         }
00068         ProblemInstance::Reset();
00069     }
00070 
00071     virtual int main(const std::string& args)
00072     {
00073         stringstream ss(args);
00074         _bDestroyThread = false;
00075         int argc=0;
00076         ros::init(argc,NULL,"openrave", ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00077 
00078         if( !ros::master::check() ) {
00079             RAVELOG_WARN("failed to create ros\n");
00080             return -1;
00081         }
00082         _ros.reset(new ros::NodeHandle());
00083         _threadros = boost::thread(boost::bind(&ROSSensorPublisher::_threadrosfn, this));
00084         return 0;
00085     }
00086 
00087     virtual bool SimulationStep(dReal fElapsedTime)
00088     {
00089         return false;
00090     }
00091 
00092 protected:
00093     inline boost::shared_ptr<ROSSensorPublisher> shared_problem() {
00094         return boost::static_pointer_cast<ROSSensorPublisher>(shared_from_this());
00095     }
00096     inline boost::shared_ptr<ROSSensorPublisher const> shared_problem_const() const {
00097         return boost::static_pointer_cast<ROSSensorPublisher const>(shared_from_this());
00098     }
00099 
00100     virtual void _threadrosfn()
00101     {
00102         while(!_bDestroyThread && ros::ok()) {
00103             ros::spinOnce();
00104             usleep(1000); // query every 1ms?
00105         }
00106     }
00107 
00108     static geometry_msgs::Pose rosPose(const Transform& t)
00109     {
00110         geometry_msgs::Pose p;
00111         p.position.x = t.trans.x;
00112         p.position.y = t.trans.y;
00113         p.position.z = t.trans.z;
00114         p.orientation.x = t.rot.y;
00115         p.orientation.y = t.rot.z;
00116         p.orientation.z = t.rot.w;
00117         p.orientation.w = t.rot.x;
00118         return p;
00119     }
00120 
00121     static geometry_msgs::Vector3 rosVector3(const Vector& v)
00122     {
00123         geometry_msgs::Vector3 r;
00124         r.x = v.x;
00125         r.y = v.y;
00126         r.z = v.z;
00127         return r;
00128     }
00129 
00130     class SensorPublisher
00131     {
00132 public:
00133         SensorPublisher(SensorBasePtr sensor, const string& frame_id, boost::shared_ptr<ros::NodeHandle> node) : _sensor(sensor), _frame_id(frame_id), _node(node), _failcount(0) {
00134         }
00135 
00136         class DataSensorPublisher
00137         {
00138 public:
00139             DataSensorPublisher(boost::shared_ptr<ros::NodeHandle> node, SensorBasePtr sensor, const string& frame_id) : _node(node), _sensor(sensor), _frame_id(frame_id) {
00140             }
00141             virtual ~DataSensorPublisher() {
00142             }
00143             virtual bool Publish() = 0;
00144 
00145 protected:
00146             boost::shared_ptr<ros::NodeHandle> _node;
00147             SensorBasePtr _sensor;
00148             string _frame_id;
00149         };
00150 
00151         class CameraSensorPublisher : public DataSensorPublisher
00152         {
00153 public:
00154             CameraSensorPublisher(boost::shared_ptr<ros::NodeHandle> node, SensorBasePtr sensor, const string& frame_id) : DataSensorPublisher(node, sensor, frame_id) {
00155                 _camerainfopub = _node->advertise<sensor_msgs::CameraInfo>("camera_info",5);
00156                 _imagepub = _node->advertise<sensor_msgs::Image>("image",5);
00157                 _data = boost::dynamic_pointer_cast<SensorBase::CameraSensorData>(_sensor->CreateSensorData(SensorBase::ST_Camera));
00158                 BOOST_ASSERT(!!_data);
00159             }
00160 
00161             bool Publish()
00162             {
00163                 if( !_sensor->GetSensorData(_data) ) {
00164                     return false;
00165                 }
00166                 std_msgs::Header header;
00167                 header.stamp = ros::Time(_data->__stamp/1000000,(uint32_t)(1000*(_data->__stamp%1000000)));
00168                 header.frame_id = _frame_id;
00169                 if( _imagemsg.header.stamp == header.stamp || _camerainfomsg.header.stamp == header.stamp ) {
00170                     return true;
00171                 }
00172                 boost::shared_ptr<SensorBase::CameraGeomData> pcamerageom = boost::static_pointer_cast<SensorBase::CameraGeomData>(_sensor->GetSensorGeometry());
00173                 if( _camerainfopub.getNumSubscribers() > 0 ) {
00174                     _camerainfomsg.header = header;
00175                     _camerainfomsg.width = pcamerageom->width;
00176                     _camerainfomsg.height = pcamerageom->height;
00177 #if ROS_VERSION >= ROS_VERSION_COMBINED(1,4,0)
00178                     _camerainfomsg.distortion_model = pcamerageom->KK.distortion_model;
00179 #endif
00180                     _camerainfomsg.D.resize(pcamerageom->KK.distortion_coeffs.size());
00181                     std::copy(pcamerageom->KK.distortion_coeffs.begin(),pcamerageom->KK.distortion_coeffs.end(),_camerainfomsg.D.begin());
00182                     _camerainfomsg.K[0] = pcamerageom->KK.fx; _camerainfomsg.K[1] = 0; _camerainfomsg.K[2] = pcamerageom->KK.cx;
00183                     _camerainfomsg.K[3] = 0; _camerainfomsg.K[4] = pcamerageom->KK.fy; _camerainfomsg.K[5] = pcamerageom->KK.cy;
00184                     _camerainfomsg.K[6] = 0; _camerainfomsg.K[7] = 0; _camerainfomsg.K[8] = 1;
00185                     _camerainfomsg.R[0] = 1; _camerainfomsg.R[1] = 0; _camerainfomsg.R[2] = 0;
00186                     _camerainfomsg.R[3] = 0; _camerainfomsg.R[4] = 1; _camerainfomsg.R[5] = 0;
00187                     _camerainfomsg.R[6] = 0; _camerainfomsg.R[7] = 0; _camerainfomsg.R[8] = 1;
00188                     _camerainfomsg.P[0] = pcamerageom->KK.fx; _camerainfomsg.P[1] = 0; _camerainfomsg.P[2] = pcamerageom->KK.cx; _camerainfomsg.P[3] = 0;
00189                     _camerainfomsg.P[4] = 0; _camerainfomsg.P[5] = pcamerageom->KK.fy; _camerainfomsg.P[6] = pcamerageom->KK.cy; _camerainfomsg.P[7] = 0;
00190                     _camerainfomsg.P[8] = 0; _camerainfomsg.P[9] = 0; _camerainfomsg.P[10] = 1; _camerainfomsg.P[11] = 0;
00191                     _camerainfopub.publish(_camerainfomsg);
00192                 }
00193                 if( _imagepub.getNumSubscribers() > 0 ) {
00194                     _imagemsg.header = header;
00195                     _imagemsg.width = pcamerageom->width;
00196                     _imagemsg.height = pcamerageom->height;
00197                     _imagemsg.encoding = "rgb8";
00198                     _imagemsg.step = pcamerageom->width*3;
00199                     _imagemsg.data = _data->vimagedata;
00200                     _imagepub.publish(_imagemsg);
00201                 }
00202                 return true;
00203             }
00204 
00205 protected:
00206             sensor_msgs::Image _imagemsg;
00207             sensor_msgs::CameraInfo _camerainfomsg;
00208             ros::Publisher _imagepub, _camerainfopub;
00209             boost::shared_ptr<SensorBase::CameraSensorData> _data;
00210         };
00211 
00212         class LaserSensorPublisher : public DataSensorPublisher
00213         {
00214 public:
00215             LaserSensorPublisher(boost::shared_ptr<ros::NodeHandle> node, SensorBasePtr sensor, const string& frame_id) : DataSensorPublisher(node, sensor, frame_id) {
00216                 _laserscanpub = _node->advertise<sensor_msgs::Image>("scan",5);
00217                 _pointcloudpub = _node->advertise<sensor_msgs::PointCloud>("pointcloud_old",5);
00218                 _pointcloud2pub = _node->advertise<sensor_msgs::PointCloud2>("pointcloud",5);
00219                 _data = boost::dynamic_pointer_cast<SensorBase::LaserSensorData>(_sensor->CreateSensorData(SensorBase::ST_Laser));
00220                 BOOST_ASSERT(!!_data);
00221             }
00222 
00223             bool Publish()
00224             {
00225                 if( !_sensor->GetSensorData(_data) ) {
00226                     return false;
00227                 }
00228                 std_msgs::Header header;
00229                 header.stamp = ros::Time(_data->__stamp/1000000,(uint32_t)(1000*(_data->__stamp%1000000)));
00230                 header.frame_id = _frame_id;
00231                 if( _laserscanmsg.header.stamp == header.stamp || _pointcloudmsg.header.stamp == header.stamp || _pointcloud2msg.header.stamp == header.stamp ) {
00232                     return true;
00233                 }
00234                 boost::shared_ptr<SensorBase::LaserGeomData> plasergeom = boost::static_pointer_cast<SensorBase::LaserGeomData>(_sensor->GetSensorGeometry());
00235                 Transform tinv = _data->__trans.inverse();
00236 
00237                 if( _laserscanpub.getNumSubscribers() > 0 ) {
00238                     _laserscanmsg.header = header;
00239                     _laserscanmsg.angle_min = plasergeom->min_angle[0];
00240                     _laserscanmsg.angle_max = plasergeom->max_angle[0];
00241                     _laserscanmsg.angle_increment = plasergeom->resolution[0];
00242                     _laserscanmsg.time_increment = plasergeom->time_increment;
00243                     _laserscanmsg.scan_time = plasergeom->time_scan;
00244                     _laserscanmsg.range_min = plasergeom->min_range;
00245                     _laserscanmsg.range_max = plasergeom->max_range;
00246                     _laserscanmsg.ranges.resize(_data->ranges.size());
00247                     for(size_t i = 0; i < _data->ranges.size(); ++i) {
00248                         _laserscanmsg.ranges[i] = RaveSqrt(_data->ranges[i].lengthsqr3());
00249                     }
00250                     _laserscanmsg.intensities.resize(_data->intensity.size());
00251                     std::copy(_data->intensity.begin(),_data->intensity.end(),_laserscanmsg.intensities.begin());
00252                     _laserscanpub.publish(_laserscanmsg);
00253                 }
00254 
00255                 if( _pointcloud2pub.getNumSubscribers() > 0 ) {
00256                     _pointcloud2msg.header = header;
00257                     _pointcloud2msg.height = 1;
00258                     _pointcloud2msg.width = _data->ranges.size();
00259                     uint8_t drealtype = 0;
00260                     switch(sizeof(dReal)) {
00261                     case 4: drealtype = sensor_msgs::PointField::FLOAT32; break;
00262                     case 8: drealtype = sensor_msgs::PointField::FLOAT64; break;
00263                     default:
00264                         RAVELOG_WARN("bad type\n");
00265                         return false;
00266                     }
00267                     drealtype = sensor_msgs::PointField::FLOAT32;
00268                     _pointcloud2msg.point_step = 0;
00269                     _pointcloud2msg.fields.resize(3 + (_data->intensity.size()==_data->ranges.size()));
00270                     _pointcloud2msg.fields[0].name = "x";
00271                     _pointcloud2msg.fields[0].offset = _pointcloud2msg.point_step;
00272                     _pointcloud2msg.fields[0].datatype = drealtype;
00273                     _pointcloud2msg.fields[0].count = 1;
00274                     _pointcloud2msg.point_step += sizeof(float);
00275                     _pointcloud2msg.fields[1].name = "y";
00276                     _pointcloud2msg.fields[1].offset = _pointcloud2msg.point_step;
00277                     _pointcloud2msg.fields[1].datatype = drealtype;
00278                     _pointcloud2msg.fields[1].count = 1;
00279                     _pointcloud2msg.point_step += sizeof(float);
00280                     _pointcloud2msg.fields[2].name = "z";
00281                     _pointcloud2msg.fields[2].offset = _pointcloud2msg.point_step;
00282                     _pointcloud2msg.fields[2].datatype = drealtype;
00283                     _pointcloud2msg.fields[2].count = 1;
00284                     _pointcloud2msg.point_step += sizeof(float);
00285                     if( _data->intensity.size()==_data->ranges.size() ) {
00286                         _pointcloud2msg.fields[3].name = "intensity";
00287                         _pointcloud2msg.fields[3].offset = _pointcloud2msg.point_step;
00288                         _pointcloud2msg.fields[3].datatype = drealtype;
00289                         _pointcloud2msg.fields[3].count = 1;
00290                         _pointcloud2msg.point_step += sizeof(float);
00291                     }
00292                     _pointcloud2msg.is_bigendian = false;
00293                     _pointcloud2msg.row_step = _data->ranges.size()*_pointcloud2msg.point_step;
00294                     _pointcloud2msg.data.resize(_pointcloud2msg.row_step);
00295                     for(size_t i = 0; i < _data->ranges.size(); ++i) {
00296                         float* p = (float*)(&_pointcloud2msg.data.at(i*_pointcloud2msg.point_step));
00297                         if( i < _data->positions.size() ) {
00298                             Vector v = tinv*(_data->ranges[i] + _data->positions[i]);
00299                             p[0] = (float)v.x;
00300                             p[1] = (float)v.y;
00301                             p[2] = (float)v.z;
00302                         }
00303                         else if( _data->positions.size() > 0 ) {
00304                             Vector v = tinv*(_data->ranges[i] + _data->positions[0]);
00305                             p[0] = (float)v.x;
00306                             p[1] = (float)v.y;
00307                             p[2] = (float)v.z;
00308                         }
00309                         else {
00310                             Vector v = tinv*_data->ranges[i];
00311                             p[0] = (float)v.x;
00312                             p[1] = (float)v.y;
00313                             p[2] = (float)v.z;
00314                         }
00315                         if( _data->intensity.size()==_data->ranges.size() ) {
00316                             p[3] = _data->intensity[i];
00317                         }
00318                     }
00319                     _pointcloud2msg.is_dense = true;
00320                     _pointcloud2pub.publish(_pointcloud2msg);
00321                 }
00322 
00323                 if( _pointcloudpub.getNumSubscribers() > 0 ) {
00324                     _pointcloudmsg.header = header;
00325                     if( _data->intensity.size()==_data->ranges.size() ) {
00326                         _pointcloudmsg.channels.resize(1);
00327                         _pointcloudmsg.channels[0].name = "intensity";
00328                         _pointcloudmsg.channels[0].values.resize(_data->intensity.size());
00329                         for(size_t i = 0; i < _data->intensity.size(); ++i) {
00330                             _pointcloudmsg.channels[0].values[i] = _data->intensity[i];
00331                         }
00332                     }
00333                     _pointcloudmsg.points.resize(_data->ranges.size());
00334                     for(size_t i = 0; i < _data->ranges.size(); ++i) {
00335                         if( i < _data->positions.size() ) {
00336                             Vector v = tinv*(_data->ranges[i] + _data->positions[i]);
00337                             _pointcloudmsg.points[i].x = v.x;
00338                             _pointcloudmsg.points[i].y = v.y;
00339                             _pointcloudmsg.points[i].z = v.z;
00340                         }
00341                         else if( _data->positions.size() > 0 ) {
00342                             Vector v = tinv*(_data->ranges[i] + _data->positions[0]);
00343                             _pointcloudmsg.points[i].x = v.x;
00344                             _pointcloudmsg.points[i].y = v.y;
00345                             _pointcloudmsg.points[i].z = v.z;
00346                         }
00347                         else {
00348                             Vector v = tinv*(_data->ranges[i]);
00349                             _pointcloudmsg.points[i].x = v.x;
00350                             _pointcloudmsg.points[i].y = v.y;
00351                             _pointcloudmsg.points[i].z = v.z;
00352                         }
00353                     }
00354                     _pointcloudpub.publish(_pointcloudmsg);
00355                 }
00356                 return true;
00357             }
00358 
00359 protected:
00360             sensor_msgs::LaserScan _laserscanmsg;
00361             sensor_msgs::PointCloud _pointcloudmsg;
00362             sensor_msgs::PointCloud2 _pointcloud2msg;
00363             ros::Publisher _laserscanpub, _pointcloudpub, _pointcloud2pub;
00364             boost::shared_ptr<SensorBase::LaserSensorData> _data;
00365         };
00366 
00367         class Force6DSensorPublisher : public DataSensorPublisher
00368         {
00369 public:
00370             Force6DSensorPublisher(boost::shared_ptr<ros::NodeHandle> node, SensorBasePtr sensor, const string& frame_id) : DataSensorPublisher(node, sensor, frame_id) {
00371                 _wrenchpub = _node->advertise<geometry_msgs::WrenchStamped>("force",5);
00372                 _data = boost::dynamic_pointer_cast<SensorBase::Force6DSensorData>(_sensor->CreateSensorData(SensorBase::ST_Force6D));
00373                 BOOST_ASSERT(!!_data);
00374             }
00375 
00376             bool Publish()
00377             {
00378                 if( !_sensor->GetSensorData(_data) ) {
00379                     return false;
00380                 }
00381                 std_msgs::Header header;
00382                 header.stamp = ros::Time(_data->__stamp/1000000,(uint32_t)(1000*(_data->__stamp%1000000)));
00383                 header.frame_id = _frame_id;
00384                 if( _wrenchmsg.header.stamp == header.stamp ) {
00385                     return true;
00386                 }
00387                 boost::shared_ptr<SensorBase::Force6DGeomData> pforce6dgeom = boost::static_pointer_cast<SensorBase::Force6DGeomData>(_sensor->GetSensorGeometry());
00388                 if( _wrenchpub.getNumSubscribers() > 0 ) {
00389                     _wrenchmsg.header = header;
00390                     _wrenchmsg.wrench.force = rosVector3(_data->force);
00391                     _wrenchmsg.wrench.torque = rosVector3(_data->torque);
00392                     _wrenchpub.publish(_wrenchmsg);
00393                 }
00394                 return true;
00395             }
00396 protected:
00397             geometry_msgs::WrenchStamped _wrenchmsg;
00398             ros::Publisher _wrenchpub;
00399             boost::shared_ptr<SensorBase::Force6DSensorData> _data;
00400         };
00401 
00402         class IMUSensorPublisher : public DataSensorPublisher
00403         {
00404 public:
00405             IMUSensorPublisher(boost::shared_ptr<ros::NodeHandle> node, SensorBasePtr sensor, const string& frame_id) : DataSensorPublisher(node, sensor, frame_id) {
00406                 _imupub = _node->advertise<sensor_msgs::Imu>("imu",5);
00407                 _data = boost::dynamic_pointer_cast<SensorBase::IMUSensorData>(_sensor->CreateSensorData(SensorBase::ST_IMU));
00408                 BOOST_ASSERT(!!_data);
00409             }
00410 
00411             bool Publish()
00412             {
00413                 if( !_sensor->GetSensorData(_data) ) {
00414                     return false;
00415                 }
00416                 std_msgs::Header header;
00417                 header.stamp = ros::Time(_data->__stamp/1000000,(uint32_t)(1000*(_data->__stamp%1000000)));
00418                 header.frame_id = _frame_id;
00419                 if( _imumsg.header.stamp == header.stamp ) {
00420                     return true;
00421                 }
00422                 boost::shared_ptr<SensorBase::IMUGeomData> pimugeom = boost::static_pointer_cast<SensorBase::IMUGeomData>(_sensor->GetSensorGeometry());
00423                 if( _imupub.getNumSubscribers() > 0 ) {
00424                     _imumsg.header = header;
00425                     _imumsg.orientation.x = _data->rotation.y;
00426                     _imumsg.orientation.y = _data->rotation.z;
00427                     _imumsg.orientation.z = _data->rotation.w;
00428                     _imumsg.orientation.w = _data->rotation.x;
00429                     std::copy(_data->rotation_covariance.begin(),_data->rotation_covariance.end(),_imumsg.orientation_covariance.begin());
00430                     _imumsg.angular_velocity.x = _data->angular_velocity.x;
00431                     _imumsg.angular_velocity.y = _data->angular_velocity.y;
00432                     _imumsg.angular_velocity.z = _data->angular_velocity.z;
00433                     std::copy(_data->angular_velocity_covariance.begin(), _data->angular_velocity_covariance.end(), _imumsg.angular_velocity_covariance.begin());
00434                     _imumsg.linear_acceleration.x = _data->linear_acceleration.x;
00435                     _imumsg.linear_acceleration.y = _data->linear_acceleration.y;
00436                     _imumsg.linear_acceleration.z = _data->linear_acceleration.z;
00437                     std::copy(_data->linear_acceleration_covariance.begin(), _data->linear_acceleration_covariance.end(), _imumsg.linear_acceleration_covariance.begin());
00438                     _imupub.publish(_imumsg);
00439                 }
00440                 return true;
00441             }
00442 
00443 protected:
00444             sensor_msgs::Imu _imumsg;
00445             ros::Publisher _imupub;
00446             boost::shared_ptr<SensorBase::IMUSensorData> _data;
00447         };
00448 
00449         class OdometrySensorPublisher : public DataSensorPublisher
00450         {
00451 public:
00452             OdometrySensorPublisher(boost::shared_ptr<ros::NodeHandle> node, SensorBasePtr sensor, const string& frame_id) : DataSensorPublisher(node, sensor, frame_id) {
00453                 _odometrypub = _node->advertise<nav_msgs::Odometry>("odometry",5);
00454                 _data = boost::dynamic_pointer_cast<SensorBase::OdometrySensorData>(_sensor->CreateSensorData(SensorBase::ST_Odometry));
00455                 BOOST_ASSERT(!!_data);
00456             }
00457 
00458             bool Publish()
00459             {
00460                 if( !_sensor->GetSensorData(_data) ) {
00461                     return false;
00462                 }
00463                 std_msgs::Header header;
00464                 header.stamp = ros::Time(_data->__stamp/1000000,(uint32_t)(1000*(_data->__stamp%1000000)));
00465                 header.frame_id = _frame_id;
00466                 if( _odometrymsg.header.stamp == header.stamp ) {
00467                     return true;
00468                 }
00469                 boost::shared_ptr<SensorBase::OdometryGeomData> podometrygeom = boost::static_pointer_cast<SensorBase::OdometryGeomData>(_sensor->GetSensorGeometry());
00470                 if( _odometrypub.getNumSubscribers() > 0 ) {
00471                     _odometrymsg.header = header;
00472                     _odometrymsg.child_frame_id = podometrygeom->targetid;
00473                     _odometrymsg.pose.pose = rosPose(_data->pose);
00474                     std::copy(_data->pose_covariance.begin(),_data->pose_covariance.end(),_odometrymsg.pose.covariance.begin());
00475                     _odometrymsg.twist.twist.linear = rosVector3(_data->linear_velocity);
00476                     _odometrymsg.twist.twist.angular = rosVector3(_data->angular_velocity);
00477                     std::copy(_data->velocity_covariance.begin(),_data->velocity_covariance.end(),_odometrymsg.twist.covariance.begin());
00478                     _odometrypub.publish(_odometrymsg);
00479                 }
00480                 return true;
00481             }
00482 protected:
00483             nav_msgs::Odometry _odometrymsg;
00484             ros::Publisher _odometrypub;
00485             boost::shared_ptr<SensorBase::OdometrySensorData> _data;
00486         };
00487 
00488         void Publish()
00489         {
00490             if( !_sensor->Configure(SensorBase::CC_PowerCheck) ) {
00491                 return;
00492             }
00493 
00494             bool bCheckStamp=true;
00495             if( _sensordata.size() == 0 ) {
00496                 // a sensor can support multiple types, so iterative through all of them
00497                 for(int itype = 0; itype <= SensorBase::ST_NumberofSensorTypes; ++itype) {
00498                     SensorBase::SensorType type = (SensorBase::SensorType)itype;
00499                     if( _sensor->Supports(type) ) {
00500                         switch(type) {
00501                         case SensorBase::ST_Camera: _sensordata[type].reset(new CameraSensorPublisher(_node,_sensor, _frame_id)); break;
00502                         case SensorBase::ST_Laser: _sensordata[type].reset(new LaserSensorPublisher(_node,_sensor, _frame_id)); break;
00503                         case SensorBase::ST_Force6D: _sensordata[type].reset(new Force6DSensorPublisher(_node,_sensor, _frame_id)); break;
00504                         case SensorBase::ST_IMU: _sensordata[type].reset(new IMUSensorPublisher(_node,_sensor, _frame_id)); break;
00505                         case SensorBase::ST_Odometry: _sensordata[type].reset(new OdometrySensorPublisher(_node,_sensor, _frame_id)); break;
00506                         default:
00507                             RAVELOG_WARN(str(boost::format("do not support publishing sensor type %d\n")%type));
00508                         }
00509                     }
00510                 }
00511                 bCheckStamp = false;
00512             }
00513 
00514             FOREACH(itdata, _sensordata) {
00515                 if( !itdata->second->Publish() ) {
00516                     RAVELOG_DEBUG(str(boost::format("failed to publish sensor type %d\n")%itdata->first));
00517                 }
00518             }
00519         }
00520 private:
00521         SensorBasePtr _sensor;
00522         string _frame_id;
00523         boost::shared_ptr<ros::NodeHandle> _node;
00524         std::map<SensorBase::SensorType, boost::shared_ptr<DataSensorPublisher> > _sensordata;
00525         int _failcount;
00526     };
00527 
00528     class RobotPublisher
00529     {
00530 public:
00531         RobotPublisher(RobotBasePtr robot, const string& rosnamespace, dReal polltime, boost::shared_ptr<ros::NodeHandle> node) : _robot(robot), _rosnamespace(rosnamespace), _node(node) {
00532             if( _rosnamespace.size() > 0 && _rosnamespace[_rosnamespace.size()-1] != '/' ) {
00533                 _rosnamespace.push_back('/');
00534             }
00535             _pubjointstate = _node->advertise<sensor_msgs::JointState>(_rosnamespace+"joint_state",10);
00536             _timer = ros::WallTimer(_node->createWallTimer(ros::WallDuration(polltime), boost::bind(&RobotPublisher::Publish,this,_1)));
00537         }
00538         virtual ~RobotPublisher() {
00539         }
00540 
00541         void Publish(const ros::WallTimerEvent& event)
00542         {
00543             EnvironmentMutex::scoped_lock lock(_robot->GetEnv()->GetMutex());
00544 
00545             {
00546                 // publish the joint state
00547                 vector<dReal> vjointvalues;
00548                 _robot->GetDOFValues(vjointvalues);
00549                 _jointstate.name.resize(_robot->GetDOF());
00550                 _jointstate.position.resize(_robot->GetDOF());
00551                 for(int i = 0; i < _robot->GetDOF(); ++i) {
00552                     KinBody::JointPtr pjoint = _robot->GetJointFromDOFIndex(i);
00553                     if( pjoint->GetDOF() > 1 ) {
00554                         _jointstate.name[i] = str(boost::format("%s%d")%pjoint->GetName()%(i-pjoint->GetDOFIndex()));
00555                     }
00556                     else {
00557                         _jointstate.name[i] = pjoint->GetName();
00558                     }
00559                     _jointstate.position[i] = vjointvalues.at(i);
00560                 }
00561 
00562                 _robot->GetDOFVelocities(vjointvalues);
00563                 _jointstate.velocity.resize(vjointvalues.size());
00564                 std::copy(vjointvalues.begin(),vjointvalues.end(),_jointstate.velocity.begin());
00565                 _jointstate.effort.resize(0);
00566                 if( !!_robot->GetController() ) {
00567                     try {
00568                         _robot->GetController()->GetTorque(vjointvalues);
00569                         _jointstate.effort.resize(vjointvalues.size());
00570                         std::copy(vjointvalues.begin(),vjointvalues.end(),_jointstate.effort.begin());
00571                     }
00572                     catch(...) { //const openrave_exception& ex) {
00573                     }
00574                 }
00575                 _pubjointstate.publish(_jointstate);
00576             }
00577 
00578             // publish each sensor
00579             FOREACH(itsensor,_robot->GetAttachedSensors()) {
00580                 if( !(*itsensor)->GetSensor() ) {
00581                     continue;
00582                 }
00583                 if( _publishers.find(*itsensor) == _publishers.end() ) {
00584                     _publishers[*itsensor].reset(new SensorPublisher((*itsensor)->GetSensor(),(*itsensor)->GetName(),boost::shared_ptr<ros::NodeHandle>(new ros::NodeHandle(*_node, _rosnamespace+(*itsensor)->GetName()))));
00585                 }
00586                 _publishers[*itsensor]->Publish();
00587             }
00588         }
00589 
00590         RobotBasePtr _robot;
00591         string _rosnamespace;
00592         boost::shared_ptr<ros::NodeHandle> _node;
00593         map<RobotBase::AttachedSensorPtr, boost::shared_ptr<SensorPublisher> > _publishers;
00594         sensor_msgs::JointState _jointstate;
00595         ros::Publisher _pubjointstate;
00596         ros::WallTimer _timer;
00597     };
00598 
00599     bool RegisterRobot(ostream& sout, istream& sinput)
00600     {
00601         string cmd;
00602         dReal polltime=0.001f;
00603         RobotBasePtr robot;
00604         string rosnamespace;
00605         while(!sinput.eof()) {
00606             sinput >> cmd;
00607             if( !sinput )
00608                 break;
00609             std::transform(cmd.begin(), cmd.end(), cmd.begin(), ::tolower);
00610 
00611             if( cmd == "robot" ) {
00612                 string robotname;
00613                 sinput >> robotname;
00614                 robot = GetEnv()->GetRobot(robotname);
00615             }
00616             else if( cmd == "polltime" ) {
00617                 sinput >> polltime;
00618             }
00619             else if( cmd == "namespace" ) {
00620                 sinput >> rosnamespace;
00621             }
00622             else {
00623                 RAVELOG_WARNA(str(boost::format("unrecognized command: %s\n")%cmd));
00624                 break;
00625             }
00626 
00627             if( !sinput ) {
00628                 RAVELOG_ERRORA(str(boost::format("failed processing command %s\n")%cmd));
00629                 return false;
00630             }
00631         }
00632 
00633         if( !robot ) {
00634             return false;
00635         }
00636         _robots[robot] = boost::shared_ptr<RobotPublisher>(new RobotPublisher(robot,rosnamespace,polltime,_ros));
00637         return true;
00638     }
00639 
00640     bool UnregisterRobot(ostream& sout, istream& sinput)
00641     {
00642         string cmd;
00643         while(!sinput.eof()) {
00644             sinput >> cmd;
00645             if( !sinput )
00646                 break;
00647             std::transform(cmd.begin(), cmd.end(), cmd.begin(), ::tolower);
00648 
00649             if( cmd == "robot" ) {
00650                 string robotname;
00651                 sinput >> robotname;
00652                 RobotBasePtr probot = GetEnv()->GetRobot(robotname);
00653                 if( !!probot )
00654                     _robots.erase(probot);
00655             }
00656             else {
00657                 RAVELOG_WARNA(str(boost::format("unrecognized command: %s\n")%cmd));
00658                 break;
00659             }
00660 
00661             if( !sinput ) {
00662                 RAVELOG_ERRORA(str(boost::format("failed processing command %s\n")%cmd));
00663                 return false;
00664             }
00665         }
00666         return true;
00667     }
00668 
00669     class EnvironmentSensorPublisher
00670     {
00671 public:
00672         EnvironmentSensorPublisher(SensorBasePtr sensor, const string& rosnamespace, dReal polltime, boost::shared_ptr<ros::NodeHandle> node) : _sensor(sensor), _rosnamespace(rosnamespace), _node(node) {
00673             if( _rosnamespace.size() > 0 && _rosnamespace[_rosnamespace.size()-1] != '/' ) {
00674                 _rosnamespace.push_back('/');
00675             }
00676             _timer = ros::WallTimer(_node->createWallTimer(ros::WallDuration(polltime), boost::bind(&EnvironmentSensorPublisher::Publish,this,_1)));
00677         }
00678         virtual ~EnvironmentSensorPublisher() {
00679         }
00680 
00681         void Publish(const ros::WallTimerEvent& event)
00682         {
00683             EnvironmentMutex::scoped_lock lock(_sensor->GetEnv()->GetMutex());
00684             if( !_publisher ) {
00685                 _publisher.reset(new SensorPublisher(_sensor,_sensor->GetName(),boost::shared_ptr<ros::NodeHandle>(new ros::NodeHandle(*_node, _rosnamespace+_sensor->GetName()))));
00686             }
00687             _publisher->Publish();
00688         }
00689 
00690         SensorBasePtr _sensor;
00691         string _rosnamespace;
00692         boost::shared_ptr<ros::NodeHandle> _node;
00693         boost::shared_ptr<SensorPublisher> _publisher;
00694         ros::WallTimer _timer;
00695     };
00696 
00697     bool RegisterSensor(ostream& sout, istream& sinput)
00698     {
00699         string cmd;
00700         dReal polltime=0.001f;
00701         SensorBasePtr sensor;
00702         string rosnamespace;
00703         while(!sinput.eof()) {
00704             sinput >> cmd;
00705             if( !sinput ) {
00706                 break;
00707             }
00708             std::transform(cmd.begin(), cmd.end(), cmd.begin(), ::tolower);
00709             if( cmd == "sensor" ) {
00710                 string name;
00711                 sinput >> name;
00712                 sensor = GetEnv()->GetSensor(name);
00713             }
00714             else if( cmd == "polltime" ) {
00715                 sinput >> polltime;
00716             }
00717             else if( cmd == "namespace" ) {
00718                 sinput >> rosnamespace;
00719             }
00720             else {
00721                 RAVELOG_WARNA(str(boost::format("unrecognized command: %s\n")%cmd));
00722                 break;
00723             }
00724 
00725             if( !sinput ) {
00726                 RAVELOG_ERRORA(str(boost::format("failed processing command %s\n")%cmd));
00727                 return false;
00728             }
00729         }
00730 
00731         if( !sensor ) {
00732             return false;
00733         }
00734         _sensors[sensor] = boost::shared_ptr<EnvironmentSensorPublisher>(new EnvironmentSensorPublisher(sensor,rosnamespace,polltime,_ros));
00735         return true;
00736     }
00737 
00738     bool UnregisterSensor(ostream& sout, istream& sinput)
00739     {
00740         string cmd;
00741         while(!sinput.eof()) {
00742             sinput >> cmd;
00743             if( !sinput ) {
00744                 break;
00745             }
00746             std::transform(cmd.begin(), cmd.end(), cmd.begin(), ::tolower);
00747             if( cmd == "sensor" ) {
00748                 string name;
00749                 sinput >> name;
00750                 SensorBasePtr psensor = GetEnv()->GetSensor(name);
00751                 if( !!psensor ) {
00752                     _sensors.erase(psensor);
00753                 }
00754             }
00755             else {
00756                 RAVELOG_WARNA(str(boost::format("unrecognized command: %s\n")%cmd));
00757                 break;
00758             }
00759             if( !sinput ) {
00760                 RAVELOG_ERRORA(str(boost::format("failed processing command %s\n")%cmd));
00761                 return false;
00762             }
00763         }
00764         return true;
00765     }
00766 
00767     boost::shared_ptr<ros::NodeHandle> _ros;
00768     map<RobotBasePtr,boost::shared_ptr<RobotPublisher> > _robots;
00769     map<SensorBasePtr,boost::shared_ptr<EnvironmentSensorPublisher> > _sensors;
00770     boost::thread _threadros;
00771     boost::mutex _mutex;
00772     bool _bDestroyThread;
00773 };
00774 
00775 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


openrave_sensors
Author(s): Rosen Diankov (rosen.diankov@gmail.com)
autogenerated on Sat Mar 23 2013 18:07:40