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
00016
00017
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
00059 laserscan.range_min = 0;
00060 laserscan.range_max = laser->getMaxRange() / 1000.0;
00061 pointcloud.header.frame_id = globaltfname;
00062
00063
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
00075 }
00076
00077 LaserPublisher::~LaserPublisher()
00078 {
00079 laser->lockDevice();
00080 laser->remReadingCB(&laserReadingsCB);
00081 laser->unlockDevice();
00082
00083 }
00084
00085 void LaserPublisher::readingsCB()
00086 {
00087
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
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
00104 laserscan.ranges.resize(readings->size());
00105 size_t n = 0;
00106 if (laser->getFlipped()) {
00107
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