LaserPublisher.cpp
Go to the documentation of this file.
00001 
00002 #ifdef ADEPT_PKG
00003   #include <Aria.h>
00004 #else
00005   #include <Aria/Aria.h>
00006 #endif
00007 
00008 #include "LaserPublisher.h"
00009 #include "ArTimeToROSTime.h"
00010 
00011 #include <math.h>
00012 
00013 #include <boost/algorithm/string.hpp>
00014 
00015 // TODO publish pointcloud of cumulative readings in separate topic?
00016 // TODO generic pointcloud sensor publisher (seprate point cloud stuff there)
00017 // TODO make  similar sonar publisher?
00018 
00019 LaserPublisher::LaserPublisher(ArLaser *_l, ros::NodeHandle& _n, bool _broadcast_tf, const std::string& _tf_frame, const std::string& _parent_tf_frame, const std::string& _global_tf_frame) :
00020   laserReadingsCB(this, &LaserPublisher::readingsCB),
00021   node(_n),
00022   laser(_l),
00023   tfname(_tf_frame),
00024   parenttfname(_parent_tf_frame),
00025   globaltfname(_global_tf_frame),
00026   broadcast_tf(_broadcast_tf)
00027 {
00028   assert(_l);
00029   laser->lockDevice();
00030   laser->addReadingCB(&laserReadingsCB);
00031   laser->unlockDevice();
00032   std::string laserscan_name(laser->getName());
00033   boost::erase_all(laserscan_name,".");
00034   laserscan_name += "_laserscan";
00035   std::string pointcloud_name(laser->getName());
00036   boost::erase_all(pointcloud_name,".");
00037   pointcloud_name += "_pointcloud";
00038   laserscan_pub = node.advertise<sensor_msgs::LaserScan>(laserscan_name, 20);
00039   pointcloud_pub = node.advertise<sensor_msgs::PointCloud>(pointcloud_name, 50);
00040 
00041   tf::Quaternion q;
00042   if(laser->hasSensorPosition())
00043   {
00044     lasertf.setOrigin(tf::Vector3(laser->getSensorPositionX()/1000.0, laser->getSensorPositionY()/1000.0, laser->getSensorPositionZ()/1000.0));
00045     q.setRPY(0, 0, ArMath::degToRad(laser->getSensorPositionTh()));
00046   }
00047   else
00048   {
00049     lasertf.setOrigin(tf::Vector3(0, 0, 0));
00050     q.setRPY(0, 0, 0);
00051   }
00052   lasertf.setRotation(q);
00053   
00054 
00055   laserscan.header.frame_id = "laser_frame";
00056   laserscan.angle_min = ArMath::degToRad(laser->getStartDegrees());
00057   laserscan.angle_max = ArMath::degToRad(laser->getEndDegrees());
00058   //laserscan.time_increment = ?
00059   laserscan.range_min = 0; //laser->getMinRange() / 1000.0;
00060   laserscan.range_max = laser->getMaxRange() / 1000.0;
00061   pointcloud.header.frame_id = globaltfname;
00062   
00063   // Get angle_increment of the laser
00064   laserscan.angle_increment = 0;
00065   if(laser->canSetIncrement()) {
00066     laserscan.angle_increment = laser->getIncrement();
00067   }
00068   else if(laser->getIncrementChoice() != NULL) {
00069     laserscan.angle_increment = laser->getIncrementChoiceDouble();
00070   }
00071   assert(laserscan.angle_increment > 0);
00072   laserscan.angle_increment *= M_PI/180.0;
00073 
00074   //readingsCallbackTime = new ArTime;
00075 }
00076 
00077 LaserPublisher::~LaserPublisher()
00078 {
00079   laser->lockDevice();
00080   laser->remReadingCB(&laserReadingsCB);
00081   laser->unlockDevice();
00082   //delete readingsCallbackTime;
00083 }
00084 
00085 void LaserPublisher::readingsCB()
00086 {
00087   //printf("readingsCB(): %lu ms since last readingsCB() call.\n", readingsCallbackTime->mSecSince());
00088   assert(laser);
00089   laser->lockDevice();
00090   publishLaserScan();
00091   publishPointCloud();
00092   laser->unlockDevice();
00093   if(broadcast_tf)
00094     transform_broadcaster.sendTransform(tf::StampedTransform(lasertf, convertArTimeToROS(laser->getLastReadingTime()), parenttfname, tfname));
00095   //readingsCallbackTime->setToNow();
00096 }
00097 
00098 void LaserPublisher::publishLaserScan()
00099 {
00100   laserscan.header.stamp = convertArTimeToROS(laser->getLastReadingTime());
00101   const std::list<ArSensorReading*> *readings = laser->getRawReadings(); 
00102   assert(readings);
00103   //printf("laserscan: %lu readings\n", readings->size());
00104   laserscan.ranges.resize(readings->size());
00105   size_t n = 0;
00106   if (laser->getFlipped()) {
00107     // Reverse the data
00108     for(std::list<ArSensorReading*>::const_reverse_iterator r = readings->rbegin(); r != readings->rend(); ++r)
00109     {
00110       assert(*r);
00111       
00112       if ((*r)->getIgnoreThisReading()) {
00113         laserscan.ranges[n] = -1;
00114       }
00115       else {
00116         laserscan.ranges[n] = (*r)->getRange() / 1000.0;
00117       }
00118       
00119       ++n;
00120     }
00121   }
00122   else {
00123     for(std::list<ArSensorReading*>::const_iterator r = readings->begin(); r != readings->end(); ++r)
00124     {
00125       assert(*r);
00126       
00127       if ((*r)->getIgnoreThisReading()) {
00128         laserscan.ranges[n] = -1;
00129       }
00130       else {
00131         laserscan.ranges[n] = (*r)->getRange() / 1000.0;
00132       }
00133       
00134       ++n;
00135     }
00136   }
00137 
00138   laserscan_pub.publish(laserscan);
00139 }
00140 
00141 void LaserPublisher::publishPointCloud()
00142 {
00143   assert(laser);
00144   pointcloud.header.stamp = convertArTimeToROS(laser->getLastReadingTime());
00145   assert(laser->getCurrentBuffer());
00146   const std::list<ArPoseWithTime*> *p = laser->getCurrentRangeBuffer()->getBuffer();
00147   assert(p);
00148   pointcloud.points.resize(p->size());
00149   size_t n = 0;
00150   for(std::list<ArPoseWithTime*>::const_iterator i = p->begin(); i != p->end(); ++i)
00151   {
00152     assert(*i);
00153     pointcloud.points[n].x = (*i)->getX() / 1000.0;
00154     pointcloud.points[n].y = (*i)->getY() / 1000.0;
00155     pointcloud.points[n].z = (laser->hasSensorPosition() ?  laser->getSensorPositionZ() / 1000.0 : 0.0);
00156     ++n;
00157   }
00158   pointcloud_pub.publish(pointcloud);
00159 }
00160   
00161   


rosaria
Author(s): Srećko Jurić-Kavelj
autogenerated on Sat May 13 2017 02:58:12