laser_utils.hpp
Go to the documentation of this file.
1 /*
2  * toolbox_types
3  * Copyright (c) 2019, Samsung Research America
4  *
5  * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
6  * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
7  * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
8  * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
9  *
10  * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
11  * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
12  * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
13  * CONDITIONS.
14  *
15  */
16 
17 /* Author: Steven Macenski */
18 
19 #ifndef SLAM_TOOLBOX_LASER_UTILS_H_
20 #define SLAM_TOOLBOX_LASER_UTILS_H_
21 
22 #include <string>
23 
24 #include "ros/ros.h"
26 #include "tf2/utils.h"
27 #include "karto_sdk/Mapper.h"
28 
29 namespace laser_utils
30 {
31 
32 // Convert a laser scan to a vector of readings
33 inline std::vector<double> scanToReadings(const sensor_msgs::LaserScan& scan, const bool& inverted)
34 {
35  std::vector<double> readings;
36 
37  if (inverted)
38  {
39  for(std::vector<float>::const_reverse_iterator it = scan.ranges.rbegin(); it != scan.ranges.rend(); ++it)
40  {
41  readings.push_back(*it);
42  }
43  }
44  else
45  {
46  for(std::vector<float>::const_iterator it = scan.ranges.begin(); it != scan.ranges.end(); ++it)
47  {
48  readings.push_back(*it);
49  }
50  }
51 
52  return readings;
53 };
54 
55 // Store laser scanner information
57 {
58 public:
59  LaserMetadata();
61  LaserMetadata(karto::LaserRangeFinder* lsr, bool invert);
62  bool isInverted() const;
64  void invertScan(sensor_msgs::LaserScan& scan) const;
65 
66 private:
68  bool inverted;
69 };
70 
71 // Help take a scan from a laser and create a laser object
73 {
74 public:
75  LaserAssistant(ros::NodeHandle& nh, tf2_ros::Buffer* tf, const std::string& base_frame);
77  LaserMetadata toLaserMetadata(sensor_msgs::LaserScan scan);
78 
79 private:
80  karto::LaserRangeFinder* makeLaser(const double& mountingYaw);
81  bool isInverted(double& mountingYaw);
82 
85  sensor_msgs::LaserScan scan_;
86  std::string frame_, base_frame_;
87  geometry_msgs::TransformStamped laser_pose_;
88 };
89 
90 // Hold some scans and utilities around them
92 {
93 public:
94  ScanHolder(std::map<std::string, laser_utils::LaserMetadata>& lasers);
95  ~ScanHolder();
96  sensor_msgs::LaserScan getCorrectedScan(const int& id);
97  void addScan(const sensor_msgs::LaserScan scan);
98 
99 private:
100  std::unique_ptr<std::vector<sensor_msgs::LaserScan> > current_scans_;
101  std::map<std::string, laser_utils::LaserMetadata>& lasers_;
102 };
103 
104 } // end namespace
105 
106 #endif //SLAM_TOOLBOX_LASER_UTILS_H_
laser_utils::LaserAssistant::~LaserAssistant
~LaserAssistant()
Definition: laser_utils.cpp:77
laser_utils::LaserAssistant::laser_pose_
geometry_msgs::TransformStamped laser_pose_
Definition: laser_utils.hpp:87
karto::LaserRangeFinder
Definition: Karto.h:3922
laser_utils::LaserMetadata::inverted
bool inverted
Definition: laser_utils.hpp:68
laser_utils::LaserAssistant::LaserAssistant
LaserAssistant(ros::NodeHandle &nh, tf2_ros::Buffer *tf, const std::string &base_frame)
Definition: laser_utils.cpp:71
utils.h
ros.h
laser_utils::scanToReadings
std::vector< double > scanToReadings(const sensor_msgs::LaserScan &scan, const bool &inverted)
Definition: laser_utils.hpp:33
laser_utils::ScanHolder::getCorrectedScan
sensor_msgs::LaserScan getCorrectedScan(const int &id)
Definition: laser_utils.cpp:175
laser_utils::LaserAssistant
Definition: laser_utils.hpp:72
laser_utils::LaserAssistant::tf_
tf2_ros::Buffer * tf_
Definition: laser_utils.hpp:84
laser_utils::LaserMetadata
Definition: laser_utils.hpp:56
laser_utils::ScanHolder
Definition: laser_utils.hpp:91
laser_utils::ScanHolder::~ScanHolder
~ScanHolder()
Definition: laser_utils.cpp:170
laser_utils::ScanHolder::addScan
void addScan(const sensor_msgs::LaserScan scan)
Definition: laser_utils.cpp:186
laser_utils::LaserMetadata::invertScan
void invertScan(sensor_msgs::LaserScan &scan) const
Definition: laser_utils.cpp:49
laser_utils::ScanHolder::lasers_
std::map< std::string, laser_utils::LaserMetadata > & lasers_
Definition: laser_utils.hpp:101
laser_utils::LaserMetadata::LaserMetadata
LaserMetadata()
Definition: laser_utils.cpp:25
laser_utils::LaserAssistant::nh_
ros::NodeHandle nh_
Definition: laser_utils.hpp:83
tf2_ros::Buffer
laser_utils::LaserAssistant::base_frame_
std::string base_frame_
Definition: laser_utils.hpp:86
toolbox_types.hpp
laser_utils::ScanHolder::ScanHolder
ScanHolder(std::map< std::string, laser_utils::LaserMetadata > &lasers)
Definition: laser_utils.cpp:164
laser_utils::LaserMetadata::getLaser
karto::LaserRangeFinder * getLaser()
Definition: laser_utils.cpp:44
laser_utils::LaserAssistant::isInverted
bool isInverted(double &mountingYaw)
Definition: laser_utils.cpp:133
laser_utils::LaserAssistant::toLaserMetadata
LaserMetadata toLaserMetadata(sensor_msgs::LaserScan scan)
Definition: laser_utils.cpp:81
laser_utils::LaserAssistant::makeLaser
karto::LaserRangeFinder * makeLaser(const double &mountingYaw)
Definition: laser_utils.cpp:93
laser_utils::LaserMetadata::isInverted
bool isInverted() const
Definition: laser_utils.cpp:39
tf
Mapper.h
laser_utils::LaserAssistant::frame_
std::string frame_
Definition: laser_utils.hpp:86
laser_utils::ScanHolder::current_scans_
std::unique_ptr< std::vector< sensor_msgs::LaserScan > > current_scans_
Definition: laser_utils.hpp:100
laser_utils::LaserMetadata::laser
karto::LaserRangeFinder * laser
Definition: laser_utils.hpp:67
laser_utils
Definition: laser_utils.hpp:29
laser_utils::LaserMetadata::~LaserMetadata
~LaserMetadata()
Definition: laser_utils.cpp:29
ros::NodeHandle
laser_utils::LaserAssistant::scan_
sensor_msgs::LaserScan scan_
Definition: laser_utils.hpp:85


slam_toolbox
Author(s): Steve Macenski
autogenerated on Thu Jan 11 2024 03:37:55