laser_geometry.h
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 #ifndef LASER_SCAN_UTILS_LASERSCAN_H
31 #define LASER_SCAN_UTILS_LASERSCAN_H
32 
33 #include <map>
34 #include <iostream>
35 #include <sstream>
36 
37 #include "boost/numeric/ublas/matrix.hpp"
38 #include "boost/thread/mutex.hpp"
39 
40 #include <tf/tf.h>
41 #include <tf2/buffer_core.h>
42 
43 #include "sensor_msgs/LaserScan.h"
44 #include "sensor_msgs/PointCloud.h"
45 #include "sensor_msgs/PointCloud.h"
46 
47 #include <Eigen/Core>
48 #include <sensor_msgs/PointCloud2.h>
49 
50 #include <ros/macros.h>
51 
52 // Import/export for windows dll's and visibility for gcc shared libraries.
53 
54 #ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries
55  #ifdef laser_geometry_EXPORTS // we are building a shared lib/dll
56  #define LASER_GEOMETRY_DECL ROS_HELPER_EXPORT
57  #else // we are using shared lib/dll
58  #define LASER_GEOMETRY_DECL ROS_HELPER_IMPORT
59  #endif
60 #else // ros is being built around static libraries
61  #define LASER_GEOMETRY_DECL
62 #endif
63 
64 namespace laser_geometry
65 {
66  // NOTE: invalid scan errors (will be present in LaserScan.msg in D-Turtle)
67  const float LASER_SCAN_INVALID = -1.0;
68  const float LASER_SCAN_MIN_RANGE = -2.0;
69  const float LASER_SCAN_MAX_RANGE = -3.0;
70 
71  namespace channel_option
72  {
74 
80  {
81  None = 0x00,
82  Intensity = 0x01,
83  Index = 0x02,
84  Distance = 0x04,
85  Timestamp = 0x08,
86  Viewpoint = 0x10,
88  };
89  }
90 
92 
114  {
115 
116  public:
117 
118  LaserProjection() : angle_min_(0), angle_max_(0) {}
119 
121  ~LaserProjection();
122 
124 
139  void projectLaser (const sensor_msgs::LaserScan& scan_in,
140  sensor_msgs::PointCloud& cloud_out,
141  double range_cutoff = -1.0,
142  int channel_options = channel_option::Default)
143  {
144  return projectLaser_ (scan_in, cloud_out, range_cutoff, false, channel_options);
145  }
146 
148 
163  void projectLaser (const sensor_msgs::LaserScan& scan_in,
164  sensor_msgs::PointCloud2 &cloud_out,
165  double range_cutoff = -1.0,
166  int channel_options = channel_option::Default)
167  {
168  projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
169  }
170 
171 
173 
193  void transformLaserScanToPointCloud (const std::string& target_frame,
194  const sensor_msgs::LaserScan& scan_in,
195  sensor_msgs::PointCloud& cloud_out,
197  double range_cutoff,
198  int channel_options = channel_option::Default)
199  {
200  return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, range_cutoff, channel_options);
201  }
202 
204 
222  void transformLaserScanToPointCloud (const std::string& target_frame,
223  const sensor_msgs::LaserScan& scan_in,
224  sensor_msgs::PointCloud& cloud_out,
226  int channel_options = channel_option::Default)
227  {
228  return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, -1.0, channel_options);
229  }
230 
232 
253  void transformLaserScanToPointCloud(const std::string &target_frame,
254  const sensor_msgs::LaserScan &scan_in,
255  sensor_msgs::PointCloud2 &cloud_out,
257  double range_cutoff = -1.0,
258  int channel_options = channel_option::Default)
259  {
260  transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options);
261  }
262 
264 
285  void transformLaserScanToPointCloud(const std::string &target_frame,
286  const sensor_msgs::LaserScan &scan_in,
287  sensor_msgs::PointCloud2 &cloud_out,
289  double range_cutoff = -1.0,
290  int channel_options = channel_option::Default)
291  {
292  transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options);
293  }
294 
295  protected:
296 
298 
303  const boost::numeric::ublas::matrix<double>& getUnitVectors_(double angle_min,
304  double angle_max,
305  double angle_increment,
306  unsigned int length);
307 
308  private:
309 
311  void projectLaser_ (const sensor_msgs::LaserScan& scan_in,
312  sensor_msgs::PointCloud& cloud_out,
313  double range_cutoff,
314  bool preservative,
315  int channel_options);
316 
318  void projectLaser_ (const sensor_msgs::LaserScan& scan_in,
319  sensor_msgs::PointCloud2 &cloud_out,
320  double range_cutoff,
321  int channel_options);
322 
324  void transformLaserScanToPointCloud_ (const std::string& target_frame,
325  sensor_msgs::PointCloud& cloud_out,
326  const sensor_msgs::LaserScan& scan_in,
328  double range_cutoff,
329  int channel_options);
330 
332  void transformLaserScanToPointCloud_ (const std::string &target_frame,
333  const sensor_msgs::LaserScan &scan_in,
334  sensor_msgs::PointCloud2 &cloud_out,
335  tf::Transformer &tf,
336  double range_cutoff,
337  int channel_options);
338 
340  void transformLaserScanToPointCloud_ (const std::string &target_frame,
341  const sensor_msgs::LaserScan &scan_in,
342  sensor_msgs::PointCloud2 &cloud_out,
343  tf2::BufferCore &tf,
344  double range_cutoff,
345  int channel_options);
346 
348  void transformLaserScanToPointCloud_ (const std::string &target_frame,
349  const sensor_msgs::LaserScan &scan_in,
350  sensor_msgs::PointCloud2 &cloud_out,
351  tf2::Quaternion quat_start,
352  tf2::Vector3 origin_start,
353  tf2::Quaternion quat_end,
354  tf2::Vector3 origin_end,
355  double range_cutoff,
356  int channel_options);
357 
359  std::map<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_;
360  float angle_min_;
361  float angle_max_;
362  Eigen::ArrayXXd co_sine_map_;
363  boost::mutex guv_mutex_;
364  };
365 
366 }
367 
368 #endif //LASER_SCAN_UTILS_LASERSCAN_H
void projectLaser(const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out, double range_cutoff=-1.0, int channel_options=channel_option::Default)
Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud.
void projectLaser(const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, double range_cutoff=-1.0, int channel_options=channel_option::Default)
Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2.
#define LASER_GEOMETRY_DECL
A Class to Project Laser Scan.
const float LASER_SCAN_INVALID
void transformLaserScanToPointCloud(const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, tf::Transformer &tf, double range_cutoff=-1.0, int channel_options=channel_option::Default)
Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 in target frame.
std::map< std::string, boost::numeric::ublas::matrix< double > * > unit_vector_map_
Internal map of pointers to stored values.
void transformLaserScanToPointCloud(const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, tf2::BufferCore &tf, double range_cutoff=-1.0, int channel_options=channel_option::Default)
Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 in target frame.
void transformLaserScanToPointCloud(const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out, tf::Transformer &tf, int channel_options=channel_option::Default)
Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud in target frame.
Enable "intensities" and "index" channels.
ChannelOption
Enumerated output channels options.
Enable "viewpoint" channel.
const float LASER_SCAN_MAX_RANGE
const float LASER_SCAN_MIN_RANGE
Enable "intensities" channel.
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
Enable "distances" channel.
void transformLaserScanToPointCloud(const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out, tf::Transformer &tf, double range_cutoff, int channel_options=channel_option::Default)
Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud in target frame.


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