00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
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);
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
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
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(...) {
00573 }
00574 }
00575 _pubjointstate.publish(_jointstate);
00576 }
00577
00578
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