laser_utils.cpp
Go to the documentation of this file.
1 /*
2  * laser_utils
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 
20 #include <cmath>
21 
22 namespace laser_utils
23 {
24 
26 {
27 };
28 
30 {
31 }
32 
34 {
35  laser = lsr;
36  inverted = invert;
37 };
38 
40 {
41  return inverted;
42 }
43 
45 {
46  return laser;
47 }
48 
49 void LaserMetadata::invertScan(sensor_msgs::LaserScan& scan) const
50 {
51  sensor_msgs::LaserScan temp;
52  temp.intensities.reserve(scan.intensities.size());
53  temp.ranges.reserve(scan.ranges.size());
54  const bool has_intensities = scan.intensities.size() > 0 ? true : false;
55 
56  for (int i = scan.ranges.size(); i != 0; i--)
57  {
58  temp.ranges.push_back(scan.ranges[i]);
59  if (has_intensities)
60  {
61  temp.intensities.push_back(scan.intensities[i]);
62  }
63  }
64 
65  scan.ranges = temp.ranges;
66  scan.intensities = temp.intensities;
67  return;
68 };
69 
70 
72  tf2_ros::Buffer* tf, const std::string& base_frame)
73  : nh_(nh), tf_(tf), base_frame_(base_frame)
74 {
75 };
76 
78 {
79 };
80 
81 LaserMetadata LaserAssistant::toLaserMetadata(sensor_msgs::LaserScan scan)
82 {
83  scan_ = scan;
84  frame_ = scan_.header.frame_id;
85 
86  double mountingYaw;
87  bool inverted = isInverted(mountingYaw);
88  karto::LaserRangeFinder* laser = makeLaser(mountingYaw);
89  LaserMetadata laserMeta(laser, inverted);
90  return laserMeta;
91 };
92 
94 {
95  karto::LaserRangeFinder* laser =
97  karto::LaserRangeFinder_Custom, karto::Name("Custom Described Lidar"));
98  laser->SetOffsetPose(karto::Pose2(laser_pose_.transform.translation.x,
99  laser_pose_.transform.translation.y, mountingYaw));
100  laser->SetMinimumRange(scan_.range_min);
101  laser->SetMaximumRange(scan_.range_max);
102  laser->SetMinimumAngle(scan_.angle_min);
103  laser->SetMaximumAngle(scan_.angle_max);
104  laser->SetAngularResolution(scan_.angle_increment);
105 
106  bool is_360_lidar = false;
107  const float angular_range = std::fabs(scan_.angle_max - scan_.angle_min);
108  if (std::fabs(angular_range - 2.0 * M_PI) < (scan_.angle_increment - (std::numeric_limits<float>::epsilon() * 2.0f*M_PI))) {
109  is_360_lidar = true;
110  }
111 
112  // Check if we have a 360 laser, but incorrectly setup as to produce
113  // measurements in range [0, 360] rather than appropriately as [0, 360)
114  if (angular_range > 6.10865 /*350 deg*/ && (angular_range / scan_.angle_increment) + 1 == scan_.ranges.size()) {
115  is_360_lidar = false;
116  }
117 
118  laser->SetIs360Laser(is_360_lidar);
119 
120  double max_laser_range = 25;
121  nh_.getParam("max_laser_range", max_laser_range);
122  if (max_laser_range > scan_.range_max)
123  {
124  ROS_WARN("maximum laser range setting (%.1f m) exceeds the capabilities "
125  "of the used Lidar (%.1f m)",
126  max_laser_range, scan_.range_max);
127  max_laser_range = scan_.range_max;
128  }
129  laser->SetRangeThreshold(max_laser_range);
130  return laser;
131 }
132 
133 bool LaserAssistant::isInverted(double& mountingYaw)
134 {
135  geometry_msgs::TransformStamped laser_ident;
136  laser_ident.header.stamp = scan_.header.stamp;
137  laser_ident.header.frame_id = frame_;
138  laser_ident.transform.rotation.w = 1.0;
139 
140  laser_pose_ = tf_->transform(laser_ident, base_frame_);
141  mountingYaw = tf2::getYaw(laser_pose_.transform.rotation);
142 
143  ROS_DEBUG("laser %s's pose wrt base: %.3f %.3f %.3f %.3f",
144  frame_.c_str(), laser_pose_.transform.translation.x,
145  laser_pose_.transform.translation.y,
146  laser_pose_.transform.translation.z, mountingYaw);
147 
148  geometry_msgs::Vector3Stamped laser_orient;
149  laser_orient.vector.z = laser_orient.vector.y = 0.;
150  laser_orient.vector.z = 1 + laser_pose_.transform.translation.z;
151  laser_orient.header.stamp = scan_.header.stamp;
152  laser_orient.header.frame_id = base_frame_;
153  laser_orient = tf_->transform(laser_orient, frame_);
154 
155  if (laser_orient.vector.z <= 0)
156  {
157  ROS_DEBUG("laser is mounted upside-down");
158  return true;
159  }
160 
161  return false;
162 };
163 
164 ScanHolder::ScanHolder(std::map<std::string, laser_utils::LaserMetadata>& lasers)
165 : lasers_(lasers)
166 {
167  current_scans_ = std::make_unique<std::vector<sensor_msgs::LaserScan> >();
168 };
169 
171 {
172  current_scans_.reset();
173 };
174 
175 sensor_msgs::LaserScan ScanHolder::getCorrectedScan(const int& id)
176 {
177  sensor_msgs::LaserScan scan = current_scans_->at(id);
178  const laser_utils::LaserMetadata& laser = lasers_[scan.header.frame_id];
179  if (laser.isInverted())
180  {
181  laser.invertScan(scan);
182  }
183  return scan;
184 };
185 
186 void ScanHolder::addScan(const sensor_msgs::LaserScan scan)
187 {
188  current_scans_->push_back(scan);
189 };
190 
191 } // end namespace
karto::Name
Definition: Karto.h:389
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::SetAngularResolution
void SetAngularResolution(kt_double angularResolution)
Definition: Karto.h:4058
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
tf2::getYaw
double getYaw(const A &a)
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
laser_utils::ScanHolder::getCorrectedScan
sensor_msgs::LaserScan getCorrectedScan(const int &id)
Definition: laser_utils.cpp:175
laser_utils::LaserAssistant::tf_
tf2_ros::Buffer * tf_
Definition: laser_utils.hpp:84
laser_utils::LaserMetadata
Definition: laser_utils.hpp:56
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
karto::LaserRangeFinder::SetMinimumRange
void SetMinimumRange(kt_double minimumRange)
Definition: Karto.h:3954
karto::Sensor::SetOffsetPose
void SetOffsetPose(const Pose2 &rPose)
Definition: Karto.h:3641
ROS_DEBUG
#define ROS_DEBUG(...)
laser_utils::LaserMetadata::invertScan
void invertScan(sensor_msgs::LaserScan &scan) const
Definition: laser_utils.cpp:49
karto::LaserRangeFinder::SetIs360Laser
void SetIs360Laser(bool is_360_laser)
Definition: Karto.h:4144
laser_utils::ScanHolder::lasers_
std::map< std::string, laser_utils::LaserMetadata > & lasers_
Definition: laser_utils.hpp:101
karto::LaserRangeFinder::SetRangeThreshold
void SetRangeThreshold(kt_double rangeThreshold)
Definition: Karto.h:3994
laser_utils::LaserMetadata::LaserMetadata
LaserMetadata()
Definition: laser_utils.cpp:25
laser_utils::LaserAssistant::nh_
ros::NodeHandle nh_
Definition: laser_utils.hpp:83
karto::LaserRangeFinder::SetMaximumRange
void SetMaximumRange(kt_double maximumRange)
Definition: Karto.h:3974
ROS_WARN
#define ROS_WARN(...)
tf2_ros::Buffer
laser_utils::LaserAssistant::base_frame_
std::string base_frame_
Definition: laser_utils.hpp:86
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
tf2_ros::BufferInterface::transform
B & transform(const A &in, B &out, const std::string &target_frame, const ros::Time &target_time, const std::string &fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const
laser_utils::LaserAssistant::toLaserMetadata
LaserMetadata toLaserMetadata(sensor_msgs::LaserScan scan)
Definition: laser_utils.cpp:81
karto::LaserRangeFinder::SetMaximumAngle
void SetMaximumAngle(kt_double maximumAngle)
Definition: Karto.h:4038
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
karto::LaserRangeFinder_Custom
@ LaserRangeFinder_Custom
Definition: Karto.h:3086
tf
karto::Pose2
Definition: Karto.h:2046
karto::LaserRangeFinder::CreateLaserRangeFinder
static LaserRangeFinder * CreateLaserRangeFinder(LaserRangeFinderType type, const Name &rName)
Definition: Karto.h:4187
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
karto::LaserRangeFinder::SetMinimumAngle
void SetMinimumAngle(kt_double minimumAngle)
Definition: Karto.h:4018
laser_utils.hpp
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