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 
296 
318  void transformLaserScanToPointCloud(const std::string &target_frame,
319  const sensor_msgs::LaserScan &scan_in,
320  sensor_msgs::PointCloud2 &cloud_out,
321  const std::string &fixed_frame,
323  double range_cutoff = -1.0,
324  int channel_options = channel_option::Default)
325  {
326  transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, fixed_frame, tf, range_cutoff, channel_options);
327  }
328 
329  protected:
330 
332 
337  const boost::numeric::ublas::matrix<double>& getUnitVectors_(double angle_min,
338  double angle_max,
339  double angle_increment,
340  unsigned int length);
341 
342  private:
343 
345  void projectLaser_ (const sensor_msgs::LaserScan& scan_in,
346  sensor_msgs::PointCloud& cloud_out,
347  double range_cutoff,
348  bool preservative,
349  int channel_options);
350 
352  void projectLaser_ (const sensor_msgs::LaserScan& scan_in,
353  sensor_msgs::PointCloud2 &cloud_out,
354  double range_cutoff,
355  int channel_options);
356 
358  void transformLaserScanToPointCloud_ (const std::string& target_frame,
359  sensor_msgs::PointCloud& cloud_out,
360  const sensor_msgs::LaserScan& scan_in,
362  double range_cutoff,
363  int channel_options);
364 
366  void transformLaserScanToPointCloud_ (const std::string &target_frame,
367  const sensor_msgs::LaserScan &scan_in,
368  sensor_msgs::PointCloud2 &cloud_out,
369  tf::Transformer &tf,
370  double range_cutoff,
371  int channel_options);
372 
374  void transformLaserScanToPointCloud_ (const std::string &target_frame,
375  const sensor_msgs::LaserScan &scan_in,
376  sensor_msgs::PointCloud2 &cloud_out,
377  tf2::BufferCore &tf,
378  double range_cutoff,
379  int channel_options);
380 
382  void transformLaserScanToPointCloud_ (const std::string &target_frame,
383  const sensor_msgs::LaserScan &scan_in,
384  sensor_msgs::PointCloud2 &cloud_out,
385  const std::string &fixed_frame,
386  tf2::BufferCore &tf,
387  double range_cutoff,
388  int channel_options);
389 
391  void transformLaserScanToPointCloud_ (const std::string &target_frame,
392  const sensor_msgs::LaserScan &scan_in,
393  sensor_msgs::PointCloud2 &cloud_out,
394  tf2::Quaternion quat_start,
395  tf2::Vector3 origin_start,
396  tf2::Quaternion quat_end,
397  tf2::Vector3 origin_end,
398  double range_cutoff,
399  int channel_options);
400 
402  std::map<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_;
403  float angle_min_;
404  float angle_max_;
405  Eigen::ArrayXXd co_sine_map_;
406  boost::mutex guv_mutex_;
407  };
408 
409 }
410 
411 #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
void transformLaserScanToPointCloud(const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, const std::string &fixed_frame, 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.
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, William Woodall
autogenerated on Sun Aug 21 2022 02:54:21