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


laser_geometry
Author(s): Tully Foote, Radu Bogdan Rusu, William Woodall
autogenerated on Mon Oct 19 2020 03:12:03