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
T & transform(const T &in, T &out, const std::string &target_frame, ros::Duration timeout=ros::Duration(0.0)) const
sensor_msgs::LaserScan getCorrectedScan(const int &id)
void SetRangeThreshold(kt_double rangeThreshold)
Definition: Karto.h:3992
ScanHolder(std::map< std::string, laser_utils::LaserMetadata > &lasers)
void addScan(const sensor_msgs::LaserScan scan)
LaserAssistant(ros::NodeHandle &nh, tf2_ros::Buffer *tf, const std::string &base_frame)
Definition: laser_utils.cpp:71
#define ROS_WARN(...)
karto::LaserRangeFinder * laser
Definition: laser_utils.hpp:67
void SetMinimumAngle(kt_double minimumAngle)
Definition: Karto.h:4016
void SetMaximumRange(kt_double maximumRange)
Definition: Karto.h:3972
void SetOffsetPose(const Pose2 &rPose)
Definition: Karto.h:3639
karto::LaserRangeFinder * getLaser()
Definition: laser_utils.cpp:44
void SetAngularResolution(kt_double angularResolution)
Definition: Karto.h:4056
void SetMaximumAngle(kt_double maximumAngle)
Definition: Karto.h:4036
geometry_msgs::TransformStamped laser_pose_
Definition: laser_utils.hpp:87
std::unique_ptr< std::vector< sensor_msgs::LaserScan > > current_scans_
void SetIs360Laser(bool is_360_laser)
Definition: Karto.h:4142
bool getParam(const std::string &key, std::string &s) const
std::map< std::string, laser_utils::LaserMetadata > & lasers_
double getYaw(const A &a)
karto::LaserRangeFinder * makeLaser(const double &mountingYaw)
Definition: laser_utils.cpp:93
static LaserRangeFinder * CreateLaserRangeFinder(LaserRangeFinderType type, const Name &rName)
Definition: Karto.h:4185
void SetMinimumRange(kt_double minimumRange)
Definition: Karto.h:3952
LaserMetadata toLaserMetadata(sensor_msgs::LaserScan scan)
Definition: laser_utils.cpp:81
void invertScan(sensor_msgs::LaserScan &scan) const
Definition: laser_utils.cpp:49
bool isInverted(double &mountingYaw)
sensor_msgs::LaserScan scan_
Definition: laser_utils.hpp:85
#define ROS_DEBUG(...)


slam_toolbox
Author(s): Steve Macenski
autogenerated on Wed Mar 3 2021 03:49:20