LaserPublisher.cpp
Go to the documentation of this file.
1 
2 #ifdef ADEPT_PKG
3  #include <Aria.h>
4 #else
5  #include <Aria/Aria.h>
6 #endif
7 
8 #include "LaserPublisher.h"
9 #include "ArTimeToROSTime.h"
10 
11 #include <math.h>
12 
13 #include <boost/algorithm/string.hpp>
14 
15 // TODO publish pointcloud of cumulative readings in separate topic?
16 // TODO generic pointcloud sensor publisher (seprate point cloud stuff there)
17 // TODO make similar sonar publisher?
18 
19 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) :
20  laserReadingsCB(this, &LaserPublisher::readingsCB),
21  node(_n),
22  laser(_l),
23  tfname(_tf_frame),
24  parenttfname(_parent_tf_frame),
25  globaltfname(_global_tf_frame),
26  broadcast_tf(_broadcast_tf)
27 {
28  assert(_l);
29  laser->lockDevice();
30  laser->addReadingCB(&laserReadingsCB);
31  laser->unlockDevice();
32  std::string laserscan_name(laser->getName());
33  boost::erase_all(laserscan_name,".");
34  laserscan_name += "_laserscan";
35  std::string pointcloud_name(laser->getName());
36  boost::erase_all(pointcloud_name,".");
37  pointcloud_name += "_pointcloud";
38  laserscan_pub = node.advertise<sensor_msgs::LaserScan>(laserscan_name, 20);
39  pointcloud_pub = node.advertise<sensor_msgs::PointCloud>(pointcloud_name, 50);
40 
42  if(laser->hasSensorPosition())
43  {
44  lasertf.setOrigin(tf::Vector3(laser->getSensorPositionX()/1000.0, laser->getSensorPositionY()/1000.0, laser->getSensorPositionZ()/1000.0));
45  q.setRPY(0, 0, ArMath::degToRad(laser->getSensorPositionTh()));
46  }
47  else
48  {
49  lasertf.setOrigin(tf::Vector3(0, 0, 0));
50  q.setRPY(0, 0, 0);
51  }
53 
54 
55  laserscan.header.frame_id = "laser_frame";
56  laserscan.angle_min = ArMath::degToRad(laser->getStartDegrees());
57  laserscan.angle_max = ArMath::degToRad(laser->getEndDegrees());
58  //laserscan.time_increment = ?
59  laserscan.range_min = 0; //laser->getMinRange() / 1000.0;
60  laserscan.range_max = laser->getMaxRange() / 1000.0;
61  pointcloud.header.frame_id = globaltfname;
62 
63  // Get angle_increment of the laser
64  laserscan.angle_increment = 0;
65  if(laser->canSetIncrement()) {
66  laserscan.angle_increment = laser->getIncrement();
67  }
68  else if(laser->getIncrementChoice() != NULL) {
69  laserscan.angle_increment = laser->getIncrementChoiceDouble();
70  }
71  assert(laserscan.angle_increment > 0);
72  laserscan.angle_increment *= M_PI/180.0;
73 
74  //readingsCallbackTime = new ArTime;
75 }
76 
78 {
79  laser->lockDevice();
80  laser->remReadingCB(&laserReadingsCB);
81  laser->unlockDevice();
82  //delete readingsCallbackTime;
83 }
84 
86 {
87  //printf("readingsCB(): %lu ms since last readingsCB() call.\n", readingsCallbackTime->mSecSince());
88  assert(laser);
89  laser->lockDevice();
92  laser->unlockDevice();
93  if(broadcast_tf)
95  //readingsCallbackTime->setToNow();
96 }
97 
99 {
100  laserscan.header.stamp = convertArTimeToROS(laser->getLastReadingTime());
101  const std::list<ArSensorReading*> *readings = laser->getRawReadings();
102  assert(readings);
103  //printf("laserscan: %lu readings\n", readings->size());
104  laserscan.ranges.resize(readings->size());
105  size_t n = 0;
106  if (laser->getFlipped()) {
107  // Reverse the data
108  for(std::list<ArSensorReading*>::const_reverse_iterator r = readings->rbegin(); r != readings->rend(); ++r)
109  {
110  assert(*r);
111 
112  if ((*r)->getIgnoreThisReading()) {
113  laserscan.ranges[n] = -1;
114  }
115  else {
116  laserscan.ranges[n] = (*r)->getRange() / 1000.0;
117  }
118 
119  ++n;
120  }
121  }
122  else {
123  for(std::list<ArSensorReading*>::const_iterator r = readings->begin(); r != readings->end(); ++r)
124  {
125  assert(*r);
126 
127  if ((*r)->getIgnoreThisReading()) {
128  laserscan.ranges[n] = -1;
129  }
130  else {
131  laserscan.ranges[n] = (*r)->getRange() / 1000.0;
132  }
133 
134  ++n;
135  }
136  }
137 
139 }
140 
142 {
143  assert(laser);
144  pointcloud.header.stamp = convertArTimeToROS(laser->getLastReadingTime());
145  assert(laser->getCurrentBuffer());
146  const std::list<ArPoseWithTime*> *p = laser->getCurrentRangeBuffer()->getBuffer();
147  assert(p);
148  pointcloud.points.resize(p->size());
149  size_t n = 0;
150  for(std::list<ArPoseWithTime*>::const_iterator i = p->begin(); i != p->end(); ++i)
151  {
152  assert(*i);
153  pointcloud.points[n].x = (*i)->getX() / 1000.0;
154  pointcloud.points[n].y = (*i)->getY() / 1000.0;
155  pointcloud.points[n].z = (laser->hasSensorPosition() ? laser->getSensorPositionZ() / 1000.0 : 0.0);
156  ++n;
157  }
159 }
160 
161 
ArFunctorC< LaserPublisher > laserReadingsCB
LaserPublisher(ArLaser *_l, ros::NodeHandle &_n, bool _broadcast_transform=true, const std::string &_tf_frame="laser", const std::string &_parent_tf_frame="base_link", const std::string &_global_tf_frame="odom")
std::string tfname
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
sensor_msgs::LaserScan laserscan
ros::NodeHandle & node
void publish(const boost::shared_ptr< M > &message) const
tf::TransformBroadcaster transform_broadcaster
sensor_msgs::PointCloud pointcloud
ros::Time convertArTimeToROS(const ArTime &t)
void sendTransform(const StampedTransform &transform)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher laserscan_pub
ArLaser * laser
std::string parenttfname
ros::Publisher pointcloud_pub
tf::Transform lasertf
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
std::string globaltfname


rosaria
Author(s): Srećko Jurić-Kavelj
autogenerated on Fri Jun 7 2019 22:02:02