laser.cpp
Go to the documentation of this file.
1 
34 #include <multisense_ros/laser.h>
35 #include <multisense_ros/RawLidarData.h>
36 #include <multisense_ros/RawLidarCal.h>
37 #include <angles/angles.h>
38 
39 #include <arpa/inet.h>
40 
41 using namespace crl::multisense;
42 
43 namespace { // anonymous
44 
45 const uint32_t laser_cloud_step = 16;
46 
47 tf::Transform makeTransform(float T[4][4])
48 {
49  //
50  // Manually create the rotation matrix
51  tf::Matrix3x3 rot = tf::Matrix3x3(T[0][0],
52  T[0][1],
53  T[0][2],
54  T[1][0],
55  T[1][1],
56  T[1][2],
57  T[2][0],
58  T[2][1],
59  T[2][2]);
60 
61  //
62  // Maually create the translation vector
63  tf::Vector3 trans = tf::Vector3(T[0][3], T[1][3], T[2][3]);
64 
65  return tf::Transform(rot, trans);
66 }
67 
68 
69 
70 }; // anonymous
71 
72 namespace multisense_ros {
73 
74 const float Laser::EXPECTED_RATE = 40.0;
75 
76 namespace { // anonymous
77 
78 //
79 // Shims for c-style driver callbacks
80 
81 void lCB(const lidar::Header& header,
82  void* userDataP)
83 {
84  reinterpret_cast<Laser*>(userDataP)->scanCallback(header);
85 }
86 
87 void pCB(const lidar::Header& header,
88  void* userDataP)
89 {
90  reinterpret_cast<Laser*>(userDataP)->pointCloudCallback(header);
91 }
92 
93 }; // anonymous
94 
95 Laser::Laser(Channel* driver,
96  const std::string& tf_prefix):
97  driver_(driver),
98  subscribers_(0),
99  spindle_angle_(0.0),
100  previous_scan_time_(0.0)
101 
102 {
103 
104  //
105  // Get device info
106 
107  system::DeviceInfo deviceInfo;
108 
109  Status status = driver_->getDeviceInfo(deviceInfo);
110  if (Status_Ok != status) {
111  ROS_ERROR("Laser: failed to query device info: %s",
112  Channel::statusString(status));
113  return;
114  }
115 
116  switch(deviceInfo.hardwareRevision) {
117  case system::DeviceInfo::HARDWARE_REV_MULTISENSE_SL:
118 
119  ; // ok, this one has a laser
120 
121  break;
122  default:
123 
124  ROS_INFO("hardware does not support a laser");
125  return;
126  }
127 
128  ros::NodeHandle nh("");
129 
130  //
131  // Set frame ID
132 
133  frame_id_ = tf_prefix + "/head_hokuyo_frame";
134 
135  left_camera_optical_ = tf_prefix + "/" + "left_camera_optical_frame";
136  motor_ = tf_prefix + "/" + "motor";
137  spindle_ = tf_prefix + "/" + "spindle";
138  hokuyo_ = tf_prefix + "/" + "hokuyo_link";
139 
140  ROS_INFO("laser frame id: %s", frame_id_.c_str());
141 
142  //
143  // Stop lidar stream
144 
145  stop();
146 
147  //
148  // Query calibration from sensor
149 
151  if (Status_Ok != status)
152  ROS_WARN("could not query lidar calibration (%s), using URDF defaults",
153  Channel::statusString(status));
154  else {
155 
156  //
157  // Create two static transforms representing sensor calibration
160 
161  }
162 
163  //
164  // Create scan publisher
165 
166  scan_pub_ = nh.advertise<sensor_msgs::LaserScan>("lidar_scan", 20,
167  boost::bind(&Laser::subscribe, this),
168  boost::bind(&Laser::unsubscribe, this));
169 
170  //
171  // Initialize point cloud structure
172 
173  point_cloud_.is_bigendian = (htonl(1) == 1);
174  point_cloud_.is_dense = true;
175  point_cloud_.point_step = laser_cloud_step;
176  point_cloud_.height = 1;
177  point_cloud_.header.frame_id = tf_prefix + "/left_camera_optical_frame";
178 
179  point_cloud_.fields.resize(4);
180  point_cloud_.fields[0].name = "x";
181  point_cloud_.fields[0].offset = 0;
182  point_cloud_.fields[0].count = 1;
183  point_cloud_.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
184  point_cloud_.fields[1].name = "y";
185  point_cloud_.fields[1].offset = 4;
186  point_cloud_.fields[1].count = 1;
187  point_cloud_.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
188  point_cloud_.fields[2].name = "z";
189  point_cloud_.fields[2].offset = 8;
190  point_cloud_.fields[2].count = 1;
191  point_cloud_.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
192  point_cloud_.fields[3].name = "intensity";
193  point_cloud_.fields[3].offset = 12;
194  point_cloud_.fields[3].count = 1;
195  point_cloud_.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
196 
197  //
198  // Create point cloud publisher
199 
200  point_cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("lidar_points2", 5,
201  boost::bind(&Laser::subscribe, this),
202  boost::bind(&Laser::unsubscribe, this));
203 
204  //
205  // Create calibration publishers
206 
207  ros::NodeHandle calibration_nh(nh, "calibration");
208  raw_lidar_cal_pub_ = calibration_nh.advertise<multisense_ros::RawLidarCal>("raw_lidar_cal", 1, true);
209  raw_lidar_data_pub_ = calibration_nh.advertise<multisense_ros::RawLidarData>("raw_lidar_data", 20,
210  boost::bind(&Laser::subscribe, this),
211  boost::bind(&Laser::unsubscribe, this));
212 
213  //
214  // Publish calibration
215 
216  multisense_ros::RawLidarCal ros_msg;
217 
218  const float *calP = reinterpret_cast<const float*>(&(lidar_cal_.laserToSpindle[0][0]));
219  for(uint32_t i=0; i<16; ++i)
220  ros_msg.laserToSpindle[i] = calP[i];
221 
222  calP = reinterpret_cast<const float*>(&(lidar_cal_.cameraToSpindleFixed[0][0]));
223  for(uint32_t i=0; i<16; ++i)
224  ros_msg.cameraToSpindleFixed[i] = calP[i];
225 
226  raw_lidar_cal_pub_.publish(ros_msg);
227 
228 
229  //
230  // Populate the jointstates message for publishing the laser spindle
231  // angle
232 
233  joint_states_.name.resize(1);
234  joint_states_.position.resize(1);
235  joint_states_.velocity.resize(1);
236  joint_states_.effort.resize(1);
237  joint_states_.name[0] = tf_prefix + "/motor_joint";
238  joint_states_.position[0] = 0.0;
239  joint_states_.velocity[0] = 0.0;
240  joint_states_.effort[0] = 0.0;
241 
242  //
243  // Create a joint state publisher
244 
245  joint_states_pub_ = nh.advertise<sensor_msgs::JointState>("joint_states", 1, true);
246 
247  //
248  // Create a timer routine to publish the laser transform even when nothing
249  // is subscribed to the laser topics. Publishing occurs at 1Hz
250 
252 
253  //
254  // Register callbacks, driver creates dedicated background thread for each
255 
256  driver_->addIsolatedCallback(lCB, this);
257  driver_->addIsolatedCallback(pCB, this);
258 
259 
260 }
261 
263 {
264  boost::mutex::scoped_lock lock(sub_lock_);
265  stop();
268 }
269 
270 
272 {
273  //
274  // Get out if we have no work to do
275 
277  return;
278 
279  point_cloud_.data.resize(laser_cloud_step * header.pointCount);
280  point_cloud_.row_step = header.pointCount * laser_cloud_step;
281  point_cloud_.width = header.pointCount;
282  point_cloud_.header.stamp = ros::Time(header.timeStartSeconds,
283  1000 * header.timeStartMicroSeconds);
284  //
285  // For convenience below
286 
287  uint8_t *cloudP = reinterpret_cast<uint8_t*>(&point_cloud_.data[0]);
288  const uint32_t pointSize = 3 * sizeof(float); // x, y, z
289  const double arcRadians = 1e-6 * static_cast<double>(header.scanArc);
290  const double mirrorThetaStart = -arcRadians / 2.0;
291  const double spindleAngleStart = angles::normalize_angle(1e-6 * static_cast<double>(header.spindleAngleStart));
292  const double spindleAngleEnd = angles::normalize_angle(1e-6 * static_cast<double>(header.spindleAngleEnd));
293  const double spindleAngleRange = angles::normalize_angle(spindleAngleEnd - spindleAngleStart);
294 
295  for(uint32_t i=0; i<header.pointCount; ++i, cloudP += laser_cloud_step) {
296 
297  //
298  // Percent through the scan arc
299 
300  const double percent = static_cast<double>(i) / static_cast<double>(header.pointCount - 1);
301 
302  //
303  // The mirror angle for this point
304 
305  const double mirrorTheta = mirrorThetaStart + percent * arcRadians;
306 
307  //
308  // The rotation about the spindle
309 
310  const double spindleTheta = spindleAngleStart + percent * spindleAngleRange;
311 
312  tf::Transform spindle_to_motor = getSpindleTransform(spindleTheta);
313 
314  //
315  // The coordinate in left optical frame
316 
317  const double rangeMeters = 1e-3 * static_cast<double>(header.rangesP[i]); // from millimeters
318  const tf::Vector3 pointMotor = (laser_to_spindle_ *
319  tf::Vector3(rangeMeters * std::sin(mirrorTheta), 0.0,
320  rangeMeters * std::cos(mirrorTheta)));
321  const tf::Vector3 pointCamera = motor_to_camera_ * (spindle_to_motor * pointMotor);
322 
323  //
324  // Copy data to point cloud structure
325 
326  const float xyz[3] = {static_cast<float>(pointCamera.getX()),
327  static_cast<float>(pointCamera.getY()),
328  static_cast<float>(pointCamera.getZ())};
329 
330  memcpy(cloudP, &(xyz[0]), pointSize);
331  float* intensityChannel = reinterpret_cast<float*>(cloudP + pointSize);
332  *intensityChannel = static_cast<float>(header.intensitiesP[i]); // in device units
333  }
334 
336 }
337 
339 {
340 
341  const ros::Time start_absolute_time = ros::Time(header.timeStartSeconds,
342  1000 * header.timeStartMicroSeconds);
343  const ros::Time end_absolute_time = ros::Time(header.timeEndSeconds,
344  1000 * header.timeEndMicroSeconds);
345  const ros::Time scan_time((end_absolute_time - start_absolute_time).toSec());
346 
347  const float angle_start = 1e-6 * static_cast<float>(header.spindleAngleStart);
348  const float angle_end = 1e-6 * static_cast<float>(header.spindleAngleEnd);
349 
350  publishStaticTransforms(start_absolute_time);
351 
352  //
353  // Initialize the previous scan time to the start time if it has not
354  // been previously set
355 
357  {
358  previous_scan_time_ = start_absolute_time;
359  }
360 
361 
362  //
363  // Compute the velocity between our last scan and the start of our current
364  // scan
365 
366  float velocity = angles::normalize_angle((angle_start - spindle_angle_)) /
367  (start_absolute_time - previous_scan_time_).toSec();
368 
369  publishSpindleTransform(angle_start, velocity, start_absolute_time);
370  spindle_angle_ = angle_start;
371 
372  //
373  // Compute the velocity for the spindle during the duration of our
374  // laser scan
375 
376  velocity = angles::normalize_angle((angle_end - angle_start)) / scan_time.toSec();
377 
378  publishSpindleTransform(angle_end, velocity, end_absolute_time);
379  spindle_angle_ = angle_end;
380  previous_scan_time_ = end_absolute_time;
381 
382  if (scan_pub_.getNumSubscribers() > 0) {
383 
384  const double arcRadians = 1e-6 * static_cast<double>(header.scanArc);
385 
386  laser_msg_.header.frame_id = frame_id_;
387  laser_msg_.header.stamp = start_absolute_time;
388  laser_msg_.scan_time = scan_time.toSec();
389  laser_msg_.time_increment = laser_msg_.scan_time / header.pointCount;
390  laser_msg_.angle_min = -arcRadians / 2.0;
391  laser_msg_.angle_max = arcRadians / 2.0;
392  laser_msg_.angle_increment = arcRadians / (header.pointCount - 1);
393  laser_msg_.range_min = 0.0;
394  laser_msg_.range_max = static_cast<double>(header.maxRange) / 1000.0;
395 
396  laser_msg_.ranges.resize(header.pointCount);
397  laser_msg_.intensities.resize(header.pointCount);
398 
399  for (size_t i=0; i<header.pointCount; i++) {
400  laser_msg_.ranges[i] = 1e-3 * static_cast<float>(header.rangesP[i]); // from millimeters
401  laser_msg_.intensities[i] = static_cast<float>(header.intensitiesP[i]); // in device units
402  }
403 
405  }
406 
408 
409  RawLidarData::Ptr ros_msg(new RawLidarData);
410 
411  ros_msg->scan_count = header.scanId;
412  ros_msg->time_start = start_absolute_time;
413  ros_msg->time_end = end_absolute_time;
414  ros_msg->angle_start = header.spindleAngleStart;
415  ros_msg->angle_end = header.spindleAngleEnd;
416 
417  ros_msg->distance.resize(header.pointCount);
418  memcpy(&(ros_msg->distance[0]),
419  header.rangesP,
420  header.pointCount * sizeof(uint32_t));
421 
422  ros_msg->intensity.resize(header.pointCount);
423  memcpy(&(ros_msg->intensity[0]),
424  header.intensitiesP,
425  header.pointCount * sizeof(uint32_t));
426 
427  raw_lidar_data_pub_.publish(ros_msg);
428  }
429 }
430 
432 
433  //
434  // Publish the static transforms from our calibration.
437  motor_));
438 
439 
440 
442  time, spindle_, hokuyo_));
443 
444 
445 }
446 
447 void Laser::publishSpindleTransform(const float spindle_angle, const float velocity, const ros::Time& time) {
448  joint_states_.header.stamp = time;
449  joint_states_.position[0] = spindle_angle;
450  joint_states_.velocity[0] = velocity;
452 }
453 
455 
456  //
457  // Spindle angle turns about the z-axis to create a transform where it adjusts
458  // yaw
459  tf::Matrix3x3 spindle_rot = tf::Matrix3x3();
460  spindle_rot.setRPY(0.0, 0.0, spindle_angle);
461  tf::Transform spindle_to_motor = tf::Transform(spindle_rot);
462 
463  return spindle_to_motor;
464 }
465 
467  //
468  // If our message time is 0 or our message time is over 1 second old
469  // we are not subscribed to a laser topic anymore. Publish the default
470  // transform
471  if ( (laser_msg_.header.stamp.is_zero() ||
472  (ros::Time::now() - laser_msg_.header.stamp >= ros::Duration(1))) &&
473  (point_cloud_.header.stamp.is_zero() ||
474  (ros::Time::now() - point_cloud_.header.stamp >= ros::Duration(1))) )
475 
476  {
479  }
480 }
481 
483 {
484  subscribers_ = 0;
485 
486  Status status = driver_->stopStreams(Source_Lidar_Scan);
487  if (Status_Ok != status)
488  ROS_ERROR("Laser: failed to stop laser stream: %s",
489  Channel::statusString(status));
490 }
491 
493 {
494  boost::mutex::scoped_lock lock(sub_lock_);
495 
496  if (--subscribers_ > 0)
497  return;
498 
499  stop();
500 }
501 
503 {
504  boost::mutex::scoped_lock lock(sub_lock_);
505 
506  if (0 == subscribers_++) {
507 
508  Status status = driver_->startStreams(Source_Lidar_Scan);
509  if (Status_Ok != status)
510  ROS_ERROR("Laser: failed to start laser stream: %s",
511  Channel::statusString(status));
512  }
513 }
514 }
virtual Status startStreams(DataSource mask)=0
tf::Transform getSpindleTransform(float spindle_angle)
Definition: laser.cpp:454
sensor_msgs::JointState joint_states_
Definition: laser.h:119
void publish(const boost::shared_ptr< M > &message) const
void defaultTfPublisher(const ros::TimerEvent &event)
Definition: laser.cpp:466
crl::multisense::lidar::Calibration lidar_cal_
Definition: laser.h:87
sensor_msgs::LaserScan laser_msg_
Definition: laser.h:117
virtual Status stopStreams(DataSource mask)=0
std::string hokuyo_
Definition: laser.h:97
TFSIMD_FORCE_INLINE const tfScalar & getY() const
virtual Status getLidarCalibration(lidar::Calibration &c)=0
std::string frame_id_
Definition: laser.h:104
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
ros::Publisher raw_lidar_cal_pub_
Definition: laser.h:111
#define ROS_WARN(...)
float spindle_angle_
Definition: laser.h:135
ros::Publisher raw_lidar_data_pub_
Definition: laser.h:109
virtual Status removeIsolatedCallback(image::Callback callback)=0
virtual Status addIsolatedCallback(image::Callback callback, DataSource imageSourceMask, void *userDataP=NULL)=0
tf::Transform laser_to_spindle_
Definition: laser.h:90
const IntensityType * intensitiesP
void publishSpindleTransform(const float spindle_angle, const float velocity, const ros::Time &time)
Definition: laser.cpp:447
crl::multisense::Channel * driver_
Definition: laser.h:102
#define ROS_INFO(...)
void setRPY(tfScalar roll, tfScalar pitch, tfScalar yaw)
std::string motor_
Definition: laser.h:95
ros::Publisher joint_states_pub_
Definition: laser.h:112
sensor_msgs::PointCloud2 point_cloud_
Definition: laser.h:118
boost::mutex sub_lock_
Definition: laser.h:124
void sendTransform(const StampedTransform &transform)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
std::string spindle_
Definition: laser.h:96
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
def normalize_angle(angle)
ros::Publisher scan_pub_
Definition: laser.h:103
tf::Transform motor_to_camera_
Definition: laser.h:89
uint32_t getNumSubscribers() const
ros::Time previous_scan_time_
Definition: laser.h:141
int32_t subscribers_
Definition: laser.h:125
TFSIMD_FORCE_INLINE const tfScalar & getX() const
static Time now()
virtual Status getDeviceInfo(system::DeviceInfo &info)=0
ros::Timer timer_
Definition: laser.h:130
void scanCallback(const crl::multisense::lidar::Header &header)
Definition: laser.cpp:338
ros::Publisher point_cloud_pub_
Definition: laser.h:110
void pointCloudCallback(const crl::multisense::lidar::Header &header)
Definition: laser.cpp:271
#define ROS_ERROR(...)
tf::TransformBroadcaster static_tf_broadcaster_
Definition: laser.h:75
std::string left_camera_optical_
Definition: laser.h:94
void publishStaticTransforms(const ros::Time &time)
Definition: laser.cpp:431


multisense_ros
Author(s):
autogenerated on Wed Aug 5 2020 03:32:50