laser_geometry.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include "laser_geometry/laser_geometry.h"
31 #include <algorithm>
32 #include <ros/assert.h>
33 #include <tf2/LinearMath/Transform.h>
34 
35 namespace laser_geometry
36 {
37 
38  void
39  LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud & cloud_out, double range_cutoff,
40  bool preservative, int mask)
41  {
42  boost::numeric::ublas::matrix<double> ranges(2, scan_in.ranges.size());
43 
44  // Fill the ranges matrix
45  for (unsigned int index = 0; index < scan_in.ranges.size(); index++)
46  {
47  ranges(0,index) = (double) scan_in.ranges[index];
48  ranges(1,index) = (double) scan_in.ranges[index];
49  }
50 
51  //Do the projection
52  // NEWMAT::Matrix output = NEWMAT::SP(ranges, getUnitVectors(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment));
53  boost::numeric::ublas::matrix<double> output = element_prod(ranges, getUnitVectors_(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment, scan_in.ranges.size()));
54 
55  //Stuff the output cloud
56  cloud_out.header = scan_in.header;
57  cloud_out.points.resize (scan_in.ranges.size());
58 
59  // Define 4 indices in the channel array for each possible value type
60  int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1;
61 
62  cloud_out.channels.resize(0);
63 
64  // Check if the intensity bit is set
65  if ((mask & channel_option::Intensity) && scan_in.intensities.size() > 0)
66  {
67  int chan_size = cloud_out.channels.size();
68  cloud_out.channels.resize (chan_size + 1);
69  cloud_out.channels[0].name = "intensities";
70  cloud_out.channels[0].values.resize (scan_in.intensities.size());
71  idx_intensity = 0;
72  }
73 
74  // Check if the index bit is set
75  if (mask & channel_option::Index)
76  {
77  int chan_size = cloud_out.channels.size();
78  cloud_out.channels.resize (chan_size +1);
79  cloud_out.channels[chan_size].name = "index";
80  cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
81  idx_index = chan_size;
82  }
83 
84  // Check if the distance bit is set
85  if (mask & channel_option::Distance)
86  {
87  int chan_size = cloud_out.channels.size();
88  cloud_out.channels.resize (chan_size + 1);
89  cloud_out.channels[chan_size].name = "distances";
90  cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
91  idx_distance = chan_size;
92  }
93 
94  if (mask & channel_option::Timestamp)
95  {
96  int chan_size = cloud_out.channels.size();
97  cloud_out.channels.resize (chan_size + 1);
98  cloud_out.channels[chan_size].name = "stamps";
99  cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
100  idx_timestamp = chan_size;
101  }
102 
103  if (range_cutoff < 0)
104  range_cutoff = scan_in.range_max;
105 
106  unsigned int count = 0;
107  for (unsigned int index = 0; index< scan_in.ranges.size(); index++)
108  {
109  const float range = ranges(0, index);
110  if (preservative || ((range < range_cutoff) && (range >= scan_in.range_min))) //if valid or preservative
111  {
112  cloud_out.points[count].x = output(0,index);
113  cloud_out.points[count].y = output(1,index);
114  cloud_out.points[count].z = 0.0;
115 
116  //double x = cloud_out.points[count].x;
117  //double y = cloud_out.points[count].y;
118  //if(x*x + y*y < scan_in.range_min * scan_in.range_min){
119  // ROS_INFO("(%.2f, %.2f)", cloud_out.points[count].x, cloud_out.points[count].y);
120  //}
121 
122  // Save the original point index
123  if (idx_index != -1)
124  cloud_out.channels[idx_index].values[count] = index;
125 
126  // Save the original point distance
127  if (idx_distance != -1)
128  cloud_out.channels[idx_distance].values[count] = range;
129 
130  // Save intensities channel
131  if (scan_in.intensities.size() >= index)
132  {
133  if (idx_intensity != -1)
134  cloud_out.channels[idx_intensity].values[count] = scan_in.intensities[index];
135  }
136 
137  // Save timestamps to seperate channel if asked for
138  if( idx_timestamp != -1)
139  cloud_out.channels[idx_timestamp].values[count] = (float)index*scan_in.time_increment;
140 
141  count++;
142  }
143  }
144 
145  //downsize if necessary
146  cloud_out.points.resize (count);
147  for (unsigned int d = 0; d < cloud_out.channels.size(); d++)
148  cloud_out.channels[d].values.resize(count);
149  };
150 
151 const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(double angle_min, double angle_max, double angle_increment, unsigned int length)
152  {
153  boost::mutex::scoped_lock guv_lock(this->guv_mutex_);
154 
155  //construct string for lookup in the map
156  std::stringstream anglestring;
157  anglestring <<angle_min<<","<<angle_max<<","<<angle_increment<<","<<length;
158  std::map<std::string, boost::numeric::ublas::matrix<double>* >::iterator it;
159  it = unit_vector_map_.find(anglestring.str());
160  //check the map for presense
161  if (it != unit_vector_map_.end())
162  return *((*it).second); //if present return
163 
164  boost::numeric::ublas::matrix<double> * tempPtr = new boost::numeric::ublas::matrix<double>(2,length);
165  for (unsigned int index = 0;index < length; index++)
166  {
167  (*tempPtr)(0,index) = cos(angle_min + (double) index * angle_increment);
168  (*tempPtr)(1,index) = sin(angle_min + (double) index * angle_increment);
169  }
170  //store
171  unit_vector_map_[anglestring.str()] = tempPtr;
172  //and return
173  return *tempPtr;
174  };
175 
176 
177  LaserProjection::~LaserProjection()
178  {
179  std::map<std::string, boost::numeric::ublas::matrix<double>*>::iterator it;
180  it = unit_vector_map_.begin();
181  while (it != unit_vector_map_.end())
182  {
183  delete (*it).second;
184  it++;
185  }
186  };
187 
188  void
189  LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, sensor_msgs::PointCloud &cloud_out, const sensor_msgs::LaserScan &scan_in,
190  tf::Transformer& tf, double range_cutoff, int mask)
191  {
192  cloud_out.header = scan_in.header;
193 
194  tf::Stamped<tf::Point> pointIn;
195  tf::Stamped<tf::Point> pointOut;
196 
197  //check if the user has requested the index field
198  bool requested_index = false;
199  if ((mask & channel_option::Index))
200  requested_index = true;
201 
202  //we need to make sure that we include the index in our mask
203  //in order to guarantee that we get our timestamps right
204  mask |= channel_option::Index;
205 
206  pointIn.frame_id_ = scan_in.header.frame_id;
207 
208  projectLaser_ (scan_in, cloud_out, range_cutoff, false, mask);
209 
210  cloud_out.header.frame_id = target_frame;
211 
212  // Extract transforms for the beginning and end of the laser scan
213  ros::Time start_time = scan_in.header.stamp ;
214  ros::Time end_time = scan_in.header.stamp ;
215  if(!scan_in.ranges.empty()) end_time += ros::Duration().fromSec( (scan_in.ranges.size()-1) * scan_in.time_increment);
216 
217  tf::StampedTransform start_transform ;
218  tf::StampedTransform end_transform ;
219  tf::StampedTransform cur_transform ;
220 
221  tf.lookupTransform(target_frame, scan_in.header.frame_id, start_time, start_transform) ;
222  tf.lookupTransform(target_frame, scan_in.header.frame_id, end_time, end_transform) ;
223 
224  //we need to find the index of the index channel
225  int index_channel_idx = -1;
226  for(unsigned int i = 0; i < cloud_out.channels.size(); ++i)
227  {
228  if(cloud_out.channels[i].name == "index")
229  {
230  index_channel_idx = i;
231  break;
232  }
233  }
234 
235  //check just in case
236  ROS_ASSERT(index_channel_idx >= 0);
237 
238  for(unsigned int i = 0; i < cloud_out.points.size(); ++i)
239  {
240  //get the index for this point
241  uint32_t pt_index = cloud_out.channels[index_channel_idx].values[i];
242 
243  // Instead, assume constant motion during the laser-scan, and use slerp to compute intermediate transforms
244  tfScalar ratio = pt_index / ( (double) scan_in.ranges.size() - 1.0) ;
245 
247 
248  //Interpolate translation
249  tf::Vector3 v (0, 0, 0);
250  v.setInterpolate3(start_transform.getOrigin(), end_transform.getOrigin(), ratio) ;
251  cur_transform.setOrigin(v) ;
252 
253  //Interpolate rotation
254  tf::Quaternion q1, q2 ;
255  start_transform.getBasis().getRotation(q1) ;
256  end_transform.getBasis().getRotation(q2) ;
257 
258  // Compute the slerp-ed rotation
259  cur_transform.setRotation( slerp( q1, q2 , ratio) ) ;
260 
261  // Apply the transform to the current point
262  tf::Vector3 pointIn(cloud_out.points[i].x, cloud_out.points[i].y, cloud_out.points[i].z) ;
263  tf::Vector3 pointOut = cur_transform * pointIn ;
264 
265  // Copy transformed point into cloud
266  cloud_out.points[i].x = pointOut.x();
267  cloud_out.points[i].y = pointOut.y();
268  cloud_out.points[i].z = pointOut.z();
269  }
270 
271  //if the user didn't request the index, we want to remove it from the channels
272  if(!requested_index)
273  cloud_out.channels.erase(cloud_out.channels.begin() + index_channel_idx);
274  }
275 
276  void LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in,
277  sensor_msgs::PointCloud2 &cloud_out,
278  double range_cutoff,
279  int channel_options)
280  {
281  size_t n_pts = scan_in.ranges.size ();
282  Eigen::ArrayXXd ranges (n_pts, 2);
283  Eigen::ArrayXXd output (n_pts, 2);
284 
285  // Get the ranges into Eigen format
286  for (size_t i = 0; i < n_pts; ++i)
287  {
288  ranges (i, 0) = (double) scan_in.ranges[i];
289  ranges (i, 1) = (double) scan_in.ranges[i];
290  }
291 
292  // Check if our existing co_sine_map is valid
293  if (co_sine_map_.rows () != (int)n_pts || angle_min_ != scan_in.angle_min || angle_max_ != scan_in.angle_max )
294  {
295  ROS_DEBUG ("[projectLaser] No precomputed map given. Computing one.");
296  co_sine_map_ = Eigen::ArrayXXd (n_pts, 2);
297  angle_min_ = scan_in.angle_min;
298  angle_max_ = scan_in.angle_max;
299  // Spherical->Cartesian projection
300  for (size_t i = 0; i < n_pts; ++i)
301  {
302  co_sine_map_ (i, 0) = cos (scan_in.angle_min + (double) i * scan_in.angle_increment);
303  co_sine_map_ (i, 1) = sin (scan_in.angle_min + (double) i * scan_in.angle_increment);
304  }
305  }
306 
307  output = ranges * co_sine_map_;
308 
309  // Set the output cloud accordingly
310  cloud_out.header = scan_in.header;
311  cloud_out.height = 1;
312  cloud_out.width = scan_in.ranges.size ();
313  cloud_out.fields.resize (3);
314  cloud_out.fields[0].name = "x";
315  cloud_out.fields[0].offset = 0;
316  cloud_out.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
317  cloud_out.fields[0].count = 1;
318  cloud_out.fields[1].name = "y";
319  cloud_out.fields[1].offset = 4;
320  cloud_out.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
321  cloud_out.fields[1].count = 1;
322  cloud_out.fields[2].name = "z";
323  cloud_out.fields[2].offset = 8;
324  cloud_out.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
325  cloud_out.fields[2].count = 1;
326 
327  // Define 4 indices in the channel array for each possible value type
328  int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1, idx_vpx = -1, idx_vpy = -1, idx_vpz = -1;
329 
330  //now, we need to check what fields we need to store
331  int offset = 12;
332  if ((channel_options & channel_option::Intensity) && scan_in.intensities.size() > 0)
333  {
334  int field_size = cloud_out.fields.size();
335  cloud_out.fields.resize(field_size + 1);
336  cloud_out.fields[field_size].name = "intensity";
337  cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
338  cloud_out.fields[field_size].offset = offset;
339  cloud_out.fields[field_size].count = 1;
340  offset += 4;
341  idx_intensity = field_size;
342  }
343 
344  if ((channel_options & channel_option::Index))
345  {
346  int field_size = cloud_out.fields.size();
347  cloud_out.fields.resize(field_size + 1);
348  cloud_out.fields[field_size].name = "index";
349  cloud_out.fields[field_size].datatype = sensor_msgs::PointField::INT32;
350  cloud_out.fields[field_size].offset = offset;
351  cloud_out.fields[field_size].count = 1;
352  offset += 4;
353  idx_index = field_size;
354  }
355 
356  if ((channel_options & channel_option::Distance))
357  {
358  int field_size = cloud_out.fields.size();
359  cloud_out.fields.resize(field_size + 1);
360  cloud_out.fields[field_size].name = "distances";
361  cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
362  cloud_out.fields[field_size].offset = offset;
363  cloud_out.fields[field_size].count = 1;
364  offset += 4;
365  idx_distance = field_size;
366  }
367 
368  if ((channel_options & channel_option::Timestamp))
369  {
370  int field_size = cloud_out.fields.size();
371  cloud_out.fields.resize(field_size + 1);
372  cloud_out.fields[field_size].name = "stamps";
373  cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
374  cloud_out.fields[field_size].offset = offset;
375  cloud_out.fields[field_size].count = 1;
376  offset += 4;
377  idx_timestamp = field_size;
378  }
379 
380  if ((channel_options & channel_option::Viewpoint))
381  {
382  int field_size = cloud_out.fields.size();
383  cloud_out.fields.resize(field_size + 3);
384 
385  cloud_out.fields[field_size].name = "vp_x";
386  cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
387  cloud_out.fields[field_size].offset = offset;
388  cloud_out.fields[field_size].count = 1;
389  offset += 4;
390 
391  cloud_out.fields[field_size + 1].name = "vp_y";
392  cloud_out.fields[field_size + 1].datatype = sensor_msgs::PointField::FLOAT32;
393  cloud_out.fields[field_size + 1].offset = offset;
394  cloud_out.fields[field_size + 1].count = 1;
395  offset += 4;
396 
397  cloud_out.fields[field_size + 2].name = "vp_z";
398  cloud_out.fields[field_size + 2].datatype = sensor_msgs::PointField::FLOAT32;
399  cloud_out.fields[field_size + 2].offset = offset;
400  cloud_out.fields[field_size + 2].count = 1;
401  offset += 4;
402 
403  idx_vpx = field_size;
404  idx_vpy = field_size + 1;
405  idx_vpz = field_size + 2;
406  }
407 
408  cloud_out.point_step = offset;
409  cloud_out.row_step = cloud_out.point_step * cloud_out.width;
410  cloud_out.data.resize (cloud_out.row_step * cloud_out.height);
411  cloud_out.is_dense = false;
412 
413  if (range_cutoff < 0)
414  range_cutoff = scan_in.range_max;
415 
416  unsigned int count = 0;
417  for (size_t i = 0; i < n_pts; ++i)
418  {
419  //check to see if we want to keep the point
420  const float range = scan_in.ranges[i];
421  if (range < range_cutoff && range >= scan_in.range_min)
422  {
423  float *pstep = (float*)&cloud_out.data[count * cloud_out.point_step];
424 
425  // Copy XYZ
426  pstep[0] = output (i, 0);
427  pstep[1] = output (i, 1);
428  pstep[2] = 0;
429 
430  // Copy intensity
431  if(idx_intensity != -1)
432  pstep[idx_intensity] = scan_in.intensities[i];
433 
434  //Copy index
435  if(idx_index != -1)
436  ((int*)(pstep))[idx_index] = i;
437 
438  // Copy distance
439  if(idx_distance != -1)
440  pstep[idx_distance] = range;
441 
442  // Copy timestamp
443  if(idx_timestamp != -1)
444  pstep[idx_timestamp] = i * scan_in.time_increment;
445 
446  // Copy viewpoint (0, 0, 0)
447  if(idx_vpx != -1 && idx_vpy != -1 && idx_vpz != -1)
448  {
449  pstep[idx_vpx] = 0;
450  pstep[idx_vpy] = 0;
451  pstep[idx_vpz] = 0;
452  }
453 
454  //make sure to increment count
455  ++count;
456  }
457 
458  /* TODO: Why was this done in this way, I don't get this at all, you end up with a ton of points with NaN values
459  * why can't you just leave them out?
460  *
461  // Invalid measurement?
462  if (scan_in.ranges[i] >= range_cutoff || scan_in.ranges[i] <= scan_in.range_min)
463  {
464  if (scan_in.ranges[i] != LASER_SCAN_MAX_RANGE)
465  {
466  for (size_t s = 0; s < cloud_out.fields.size (); ++s)
467  pstep[s] = bad_point;
468  }
469  else
470  {
471  // Kind of nasty thing:
472  // We keep the oringinal point information for max ranges but set x to NAN to mark the point as invalid.
473  // Since we still might need the x value we store it in the distance field
474  pstep[0] = bad_point; // X -> NAN to mark a bad point
475  pstep[1] = co_sine_map (i, 1); // Y
476  pstep[2] = 0; // Z
477 
478  if (store_intensity)
479  {
480  pstep[3] = bad_point; // Intensity -> NAN to mark a bad point
481  pstep[4] = co_sine_map (i, 0); // Distance -> Misused to store the originnal X
482  }
483  else
484  pstep[3] = co_sine_map (i, 0); // Distance -> Misused to store the originnal X
485  }
486  }
487  */
488  }
489 
490  //resize if necessary
491  cloud_out.width = count;
492  cloud_out.row_step = cloud_out.point_step * cloud_out.width;
493  cloud_out.data.resize (cloud_out.row_step * cloud_out.height);
494  }
495 
496  void LaserProjection::transformLaserScanToPointCloud_(const std::string &target_frame,
497  const sensor_msgs::LaserScan &scan_in,
498  sensor_msgs::PointCloud2 &cloud_out,
499  tf2::Quaternion quat_start,
500  tf2::Vector3 origin_start,
501  tf2::Quaternion quat_end,
502  tf2::Vector3 origin_end,
503  double range_cutoff,
504  int channel_options)
505  {
506  //check if the user has requested the index field
507  bool requested_index = false;
508  if ((channel_options & channel_option::Index))
509  requested_index = true;
510 
511  //we'll enforce that we get index values for the laser scan so that we
512  //ensure that we use the correct timestamps
513  channel_options |= channel_option::Index;
514 
515  projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
516 
517  //we'll assume no associated viewpoint by default
518  bool has_viewpoint = false;
519  uint32_t vp_x_offset = 0;
520 
521  //we need to find the offset of the intensity field in the point cloud
522  //we also know that the index field is guaranteed to exist since we
523  //set the channel option above. To be really safe, it might be worth
524  //putting in a check at some point, but I'm just going to put in an
525  //assert for now
526  uint32_t index_offset = 0;
527  for(unsigned int i = 0; i < cloud_out.fields.size(); ++i)
528  {
529  if(cloud_out.fields[i].name == "index")
530  {
531  index_offset = cloud_out.fields[i].offset;
532  }
533 
534  //we want to check if the cloud has a viewpoint associated with it
535  //checking vp_x should be sufficient since vp_x, vp_y, and vp_z all
536  //get put in together
537  if(cloud_out.fields[i].name == "vp_x")
538  {
539  has_viewpoint = true;
540  vp_x_offset = cloud_out.fields[i].offset;
541  }
542  }
543 
544  ROS_ASSERT(index_offset > 0);
545 
546  cloud_out.header.frame_id = target_frame;
547 
548  tf2::Transform cur_transform ;
549 
550  double ranges_norm = 1 / ((double) scan_in.ranges.size () - 1.0);
551 
552  //we want to loop through all the points in the cloud
553  for(size_t i = 0; i < cloud_out.width; ++i)
554  {
555  // Apply the transform to the current point
556  float *pstep = (float*)&cloud_out.data[i * cloud_out.point_step + 0];
557 
558  //find the index of the point
559  uint32_t pt_index;
560  memcpy(&pt_index, &cloud_out.data[i * cloud_out.point_step + index_offset], sizeof(uint32_t));
561 
562  // Assume constant motion during the laser-scan, and use slerp to compute intermediate transforms
563  tfScalar ratio = pt_index * ranges_norm;
564 
566  // Interpolate translation
567  tf2::Vector3 v (0, 0, 0);
568  v.setInterpolate3 (origin_start, origin_end, ratio);
569  cur_transform.setOrigin (v);
570 
571  // Compute the slerp-ed rotation
572  cur_transform.setRotation (slerp (quat_start, quat_end , ratio));
573 
574  tf2::Vector3 point_in (pstep[0], pstep[1], pstep[2]);
575  tf2::Vector3 point_out = cur_transform * point_in;
576 
577  // Copy transformed point into cloud
578  pstep[0] = point_out.x ();
579  pstep[1] = point_out.y ();
580  pstep[2] = point_out.z ();
581 
582  // Convert the viewpoint as well
583  if(has_viewpoint)
584  {
585  float *vpstep = (float*)&cloud_out.data[i * cloud_out.point_step + vp_x_offset];
586  point_in = tf2::Vector3 (vpstep[0], vpstep[1], vpstep[2]);
587  point_out = cur_transform * point_in;
588 
589  // Copy transformed point into cloud
590  vpstep[0] = point_out.x ();
591  vpstep[1] = point_out.y ();
592  vpstep[2] = point_out.z ();
593  }
594  }
595 
596  //if the user didn't request the index field, then we need to copy the PointCloud and drop it
597  if(!requested_index)
598  {
599  sensor_msgs::PointCloud2 cloud_without_index;
600 
601  //copy basic meta data
602  cloud_without_index.header = cloud_out.header;
603  cloud_without_index.width = cloud_out.width;
604  cloud_without_index.height = cloud_out.height;
605  cloud_without_index.is_bigendian = cloud_out.is_bigendian;
606  cloud_without_index.is_dense = cloud_out.is_dense;
607 
608  //copy the fields
609  cloud_without_index.fields.resize(cloud_out.fields.size());
610  unsigned int field_count = 0;
611  unsigned int offset_shift = 0;
612  for(unsigned int i = 0; i < cloud_out.fields.size(); ++i)
613  {
614  if(cloud_out.fields[i].name != "index")
615  {
616  cloud_without_index.fields[field_count] = cloud_out.fields[i];
617  cloud_without_index.fields[field_count].offset -= offset_shift;
618  ++field_count;
619  }
620  else
621  {
622  //once we hit the index, we'll set the shift
623  offset_shift = 4;
624  }
625  }
626 
627  //resize the fields
628  cloud_without_index.fields.resize(field_count);
629 
630  //compute the size of the new data
631  cloud_without_index.point_step = cloud_out.point_step - offset_shift;
632  cloud_without_index.row_step = cloud_without_index.point_step * cloud_without_index.width;
633  cloud_without_index.data.resize (cloud_without_index.row_step * cloud_without_index.height);
634 
635  uint32_t i = 0;
636  uint32_t j = 0;
637  //copy over the data from one cloud to the other
638  while (i < cloud_out.data.size())
639  {
640  if((i % cloud_out.point_step) < index_offset || (i % cloud_out.point_step) >= (index_offset + 4))
641  {
642  cloud_without_index.data[j++] = cloud_out.data[i];
643  }
644  i++;
645  }
646 
647  //make sure to actually set the output
648  cloud_out = cloud_without_index;
649  }
650  }
651 
652  void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame,
653  const sensor_msgs::LaserScan &scan_in,
654  sensor_msgs::PointCloud2 &cloud_out,
655  tf::Transformer &tf,
656  double range_cutoff,
657  int channel_options)
658  {
659  ros::Time start_time = scan_in.header.stamp;
660  ros::Time end_time = scan_in.header.stamp;
661  if(!scan_in.ranges.empty()) end_time += ros::Duration ().fromSec ( (scan_in.ranges.size()-1) * scan_in.time_increment);
662 
663  tf::StampedTransform start_transform, end_transform ;
664 
665  tf.lookupTransform (target_frame, scan_in.header.frame_id, start_time, start_transform);
666  tf.lookupTransform (target_frame, scan_in.header.frame_id, end_time, end_transform);
667 
668  tf::Quaternion q;
669  start_transform.getBasis().getRotation(q);
670  tf2::Quaternion quat_start(q.getX(), q.getY(), q.getZ(), q.getW());
671  end_transform.getBasis().getRotation(q);
672  tf2::Quaternion quat_end(q.getX(), q.getY(), q.getZ(), q.getW());
673 
674  tf2::Vector3 origin_start(start_transform.getOrigin().getX(),
675  start_transform.getOrigin().getY(),
676  start_transform.getOrigin().getZ());
677  tf2::Vector3 origin_end(end_transform.getOrigin().getX(),
678  end_transform.getOrigin().getY(),
679  end_transform.getOrigin().getZ());
680  transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out,
681  quat_start, origin_start,
682  quat_end, origin_end,
683  range_cutoff,
684  channel_options);
685  }
686 
687  void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame,
688  const sensor_msgs::LaserScan &scan_in,
689  sensor_msgs::PointCloud2 &cloud_out,
690  tf2::BufferCore &tf,
691  double range_cutoff,
692  int channel_options)
693  {
694  ros::Time start_time = scan_in.header.stamp;
695  ros::Time end_time = scan_in.header.stamp;
696  if(!scan_in.ranges.empty()) end_time += ros::Duration ().fromSec ( (scan_in.ranges.size()-1) * scan_in.time_increment);
697 
698  geometry_msgs::TransformStamped start_transform = tf.lookupTransform (target_frame, scan_in.header.frame_id, start_time);
699  geometry_msgs::TransformStamped end_transform = tf.lookupTransform (target_frame, scan_in.header.frame_id, end_time);
700 
701  tf2::Quaternion quat_start(start_transform.transform.rotation.x,
702  start_transform.transform.rotation.y,
703  start_transform.transform.rotation.z,
704  start_transform.transform.rotation.w);
705  tf2::Quaternion quat_end(end_transform.transform.rotation.x,
706  end_transform.transform.rotation.y,
707  end_transform.transform.rotation.z,
708  end_transform.transform.rotation.w);
709 
710  tf2::Vector3 origin_start(start_transform.transform.translation.x,
711  start_transform.transform.translation.y,
712  start_transform.transform.translation.z);
713  tf2::Vector3 origin_end(end_transform.transform.translation.x,
714  end_transform.transform.translation.y,
715  end_transform.transform.translation.z);
716  transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out,
717  quat_start, origin_start,
718  quat_end, origin_end,
719  range_cutoff,
720  channel_options);
721  }
722 
723 } //laser_geometry
laser_geometry::LaserProjection::projectLaser_
void projectLaser_(const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out, double range_cutoff, bool preservative, int channel_options)
Internal hidden representation of projectLaser.
setup.d
d
Definition: setup.py:6
tf::Transform::setRotation
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
tfScalar
double tfScalar
laser_geometry::channel_option::Index
Enable "index" channel.
Definition: laser_geometry.h:83
tf2::Transform::setRotation
TF2SIMD_FORCE_INLINE void setRotation(const Quaternion &q)
tf::Transform::getBasis
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
laser_geometry::LaserProjection::unit_vector_map_
std::map< std::string, boost::numeric::ublas::matrix< double > * > unit_vector_map_
Internal map of pointers to stored values.
Definition: laser_geometry.h:359
tf::Vector3::getY
TFSIMD_FORCE_INLINE const tfScalar & getY() const
tf::Quaternion::getW
TFSIMD_FORCE_INLINE const tfScalar & getW() const
tf::Vector3::getZ
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
laser_geometry::LaserProjection::angle_min_
float angle_min_
Definition: laser_geometry.h:360
laser_geometry
Definition: laser_geometry.h:64
slerp
TFSIMD_FORCE_INLINE Quaternion slerp(const Quaternion &q1, const Quaternion &q2, const tfScalar &t)
laser_geometry::LaserProjection::co_sine_map_
Eigen::ArrayXXd co_sine_map_
Definition: laser_geometry.h:362
tf::Matrix3x3::getRotation
void getRotation(Quaternion &q) const
tf::Vector3::x
TFSIMD_FORCE_INLINE const tfScalar & x() const
tf::Vector3::z
TFSIMD_FORCE_INLINE const tfScalar & z() const
tf::Vector3::setInterpolate3
TFSIMD_FORCE_INLINE void setInterpolate3(const Vector3 &v0, const Vector3 &v1, tfScalar rt)
laser_geometry::LaserProjection::~LaserProjection
~LaserProjection()
Destructor to deallocate stored unit vectors.
Definition: laser_geometry.cpp:177
DurationBase< Duration >::fromSec
Duration & fromSec(double t)
tf::Vector3::y
TFSIMD_FORCE_INLINE const tfScalar & y() const
laser_geometry::LaserProjection::guv_mutex_
boost::mutex guv_mutex_
Definition: laser_geometry.h:363
laser_geometry::LaserProjection::angle_max_
float angle_max_
Definition: laser_geometry.h:361
laser_geometry::channel_option::Timestamp
Enable "stamps" channel.
Definition: laser_geometry.h:85
laser_geometry::LaserProjection::getUnitVectors_
const boost::numeric::ublas::matrix< double > & getUnitVectors_(double angle_min, double angle_max, double angle_increment, unsigned int length)
Internal protected representation of getUnitVectors.
Definition: laser_geometry.cpp:151
tf2::Transform::setOrigin
TF2SIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
laser_geometry::LaserProjection::transformLaserScanToPointCloud_
void transformLaserScanToPointCloud_(const std::string &target_frame, sensor_msgs::PointCloud &cloud_out, const sensor_msgs::LaserScan &scan_in, tf::Transformer &tf, double range_cutoff, int channel_options)
Internal hidden representation of transformLaserScanToPointCloud.
tf::Transform::getOrigin
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
tf::Transformer::lookupTransform
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
laser_geometry::channel_option::Viewpoint
Enable "viewpoint" channel.
Definition: laser_geometry.h:86
tf::Stamped::frame_id_
std::string frame_id_
laser_geometry::channel_option::Intensity
Enable "intensities" channel.
Definition: laser_geometry.h:82
laser_geometry.h
tf::Vector3::getX
TFSIMD_FORCE_INLINE const tfScalar & getX() const
laser_geometry::channel_option::Distance
Enable "distances" channel.
Definition: laser_geometry.h:84
ROS_ASSERT
#define ROS_ASSERT(cond)
tf::Transform::setOrigin
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
tf2::BufferCore::lookupTransform
geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time) const
ROS_DEBUG
#define ROS_DEBUG(...)

laser_geometry
Author(s): Tully Foote, Radu Bogdan Rusu
autogenerated on Tue Jul 2 2019 19:09:14