Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
range_sensor_layer.cpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018 David V. Lu!!
5  * Copyright (c) 2020, Bytes Robotics
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the copyright holder nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
36 #include <angles/angles.h>
37 #include <algorithm>
38 #include <list>
39 #include <limits>
40 #include <string>
41 #include <vector>
42 
43 #include "pluginlib/class_list_macros.hpp"
44 #include "geometry_msgs/msg/point_stamped.hpp"
45 #include "nav2_costmap_2d/range_sensor_layer.hpp"
46 
48 
49 using nav2_costmap_2d::LETHAL_OBSTACLE;
50 using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
51 using nav2_costmap_2d::NO_INFORMATION;
52 
53 using namespace std::literals::chrono_literals;
54 
55 namespace nav2_costmap_2d
56 {
57 
58 RangeSensorLayer::RangeSensorLayer() {}
59 
60 void RangeSensorLayer::onInitialize()
61 {
62  setCurrent(true);
63  was_reset_ = false;
64  buffered_readings_ = 0;
65  last_reading_time_ = clock_->now();
66  default_value_ = to_cost(0.5);
67 
68  matchSize();
69  resetRange();
70 
71  auto node = node_.lock();
72  if (!node) {
73  throw std::runtime_error{"Failed to lock node"};
74  }
75 
76  enabled_ = node->declare_or_get_parameter(name_ + "." + "enabled", true);
77  phi_v_ = node->declare_or_get_parameter(name_ + "." + "phi", 1.2);
78  inflate_cone_ = node->declare_or_get_parameter(name_ + "." + "inflate_cone", 1.0);
79  no_readings_timeout_ = node->declare_or_get_parameter(
80  name_ + "." + "no_readings_timeout", 0.0);
81  clear_threshold_ = node->declare_or_get_parameter(
82  name_ + "." + "clear_threshold", 0.2);
83  mark_threshold_ = node->declare_or_get_parameter(
84  name_ + "." + "mark_threshold", 0.8);
85  clear_on_max_reading_ = node->declare_or_get_parameter(
86  name_ + "." + "clear_on_max_reading", false);
87 
88  double temp_tf_tol = 0.0;
89  node->get_parameter("transform_tolerance", temp_tf_tol);
90  transform_tolerance_ = tf2::durationFromSec(temp_tf_tol);
91 
92  std::vector<std::string> topic_names = node->declare_or_get_parameter(
93  name_ + "." + "topics", std::vector<std::string>{});
94 
95  InputSensorType input_sensor_type = InputSensorType::ALL;
96  std::string sensor_type_name = node->declare_or_get_parameter(
97  name_ + "." + "input_sensor_type", std::string("ALL"));
98 
99  std::transform(
100  sensor_type_name.begin(), sensor_type_name.end(),
101  sensor_type_name.begin(), ::toupper);
102  RCLCPP_INFO(
103  logger_, "%s: %s as input_sensor_type given",
104  name_.c_str(), sensor_type_name.c_str());
105 
106  if (sensor_type_name == "VARIABLE") {
107  input_sensor_type = InputSensorType::VARIABLE;
108  } else if (sensor_type_name == "FIXED") {
109  input_sensor_type = InputSensorType::FIXED;
110  } else if (sensor_type_name == "ALL") {
111  input_sensor_type = InputSensorType::ALL;
112  } else {
113  RCLCPP_ERROR(
114  logger_, "%s: Invalid input sensor type: %s. Defaulting to ALL.",
115  name_.c_str(), sensor_type_name.c_str());
116  }
117 
118  // Validate topic names list: it must be a (normally non-empty) list of strings
119  if (topic_names.empty()) {
120  RCLCPP_FATAL(
121  logger_, "Invalid topic names list: it must"
122  "be a non-empty list of strings");
123  return;
124  }
125 
126  // Traverse the topic names list subscribing to all of them with the same callback method
127  for (auto & topic_name : topic_names) {
128  topic_name = joinWithParentNamespace(topic_name);
129  if (input_sensor_type == InputSensorType::VARIABLE) {
130  processRangeMessageFunc_ = std::bind(
131  &RangeSensorLayer::processVariableRangeMsg, this,
132  std::placeholders::_1);
133  } else if (input_sensor_type == InputSensorType::FIXED) {
134  processRangeMessageFunc_ = std::bind(
135  &RangeSensorLayer::processFixedRangeMsg, this,
136  std::placeholders::_1);
137  } else if (input_sensor_type == InputSensorType::ALL) {
138  processRangeMessageFunc_ = std::bind(
139  &RangeSensorLayer::processRangeMsg, this,
140  std::placeholders::_1);
141  } else {
142  RCLCPP_ERROR(
143  logger_,
144  "%s: Invalid input sensor type: %s. Did you make a new type"
145  "and forgot to choose the subscriber for it?",
146  name_.c_str(), sensor_type_name.c_str());
147  }
148  range_subs_.push_back(
149  node->create_subscription<sensor_msgs::msg::Range>(
150  topic_name,
151  std::bind(
152  &RangeSensorLayer::bufferIncomingRangeMsg, this,
153  std::placeholders::_1),
155 
156  RCLCPP_INFO(
157  logger_, "RangeSensorLayer: subscribed to "
158  "topic %s", range_subs_.back()->get_topic_name());
159  }
160  global_frame_ = layered_costmap_->getGlobalFrameID();
161 }
162 
163 
164 double RangeSensorLayer::gamma(double theta)
165 {
166  if (fabs(theta) > max_angle_) {
167  return 0.0;
168  } else {
169  return 1 - pow(theta / max_angle_, 2);
170  }
171 }
172 
173 double RangeSensorLayer::delta(double phi)
174 {
175  return 1 - (1 + tanh(2 * (phi - phi_v_))) / 2;
176 }
177 
178 void RangeSensorLayer::get_deltas(double angle, double * dx, double * dy)
179 {
180  double ta = tan(angle);
181  if (ta == 0) {
182  *dx = 0;
183  } else {
184  *dx = resolution_ / ta;
185  }
186 
187  *dx = copysign(*dx, cos(angle));
188  *dy = copysign(resolution_, sin(angle));
189 }
190 
191 double RangeSensorLayer::sensor_model(double r, double phi, double theta)
192 {
193  double lbda = delta(phi) * gamma(theta);
194 
195  double delta = resolution_;
196 
197  if (phi >= 0.0 && phi < r - 2 * delta * r) {
198  return (1 - lbda) * (0.5);
199  } else if (phi < r - delta * r) {
200  return lbda * 0.5 * pow((phi - (r - 2 * delta * r)) / (delta * r), 2) +
201  (1 - lbda) * .5;
202  } else if (phi < r + delta * r) {
203  double J = (r - phi) / (delta * r);
204  return lbda * ((1 - (0.5) * pow(J, 2)) - 0.5) + 0.5;
205  } else {
206  return 0.5;
207  }
208 }
209 
210 void RangeSensorLayer::bufferIncomingRangeMsg(
211  const sensor_msgs::msg::Range::ConstSharedPtr & range_message)
212 {
213  range_message_mutex_.lock();
214  range_msgs_buffer_.push_back(*range_message);
215  range_message_mutex_.unlock();
216 }
217 
218 void RangeSensorLayer::updateCostmap()
219 {
220  std::list<sensor_msgs::msg::Range> range_msgs_buffer_copy;
221 
222  range_message_mutex_.lock();
223  range_msgs_buffer_copy = std::list<sensor_msgs::msg::Range>(range_msgs_buffer_);
224  range_msgs_buffer_.clear();
225  range_message_mutex_.unlock();
226 
227  for (auto & range_msgs_it : range_msgs_buffer_copy) {
228  processRangeMessageFunc_(range_msgs_it);
229  }
230 }
231 
232 void RangeSensorLayer::processRangeMsg(sensor_msgs::msg::Range & range_message)
233 {
234  if (range_message.min_range == range_message.max_range) {
235  processFixedRangeMsg(range_message);
236  } else {
237  processVariableRangeMsg(range_message);
238  }
239 }
240 
241 void RangeSensorLayer::processFixedRangeMsg(sensor_msgs::msg::Range & range_message)
242 {
243  if (!std::isinf(range_message.range)) {
244  RCLCPP_ERROR(
245  logger_,
246  "Fixed distance ranger (min_range == max_range) in frame %s sent invalid value. "
247  "Only -Inf (== object detected) and Inf (== no object detected) are valid.",
248  range_message.header.frame_id.c_str());
249  return;
250  }
251 
252  bool clear_sensor_cone = false;
253 
254  if (range_message.range > 0) { // +inf
255  if (!clear_on_max_reading_) {
256  return; // no clearing at all
257  }
258  clear_sensor_cone = true;
259  }
260 
261  range_message.range = range_message.min_range;
262 
263  updateCostmap(range_message, clear_sensor_cone);
264 }
265 
266 void RangeSensorLayer::processVariableRangeMsg(sensor_msgs::msg::Range & range_message)
267 {
268  if (range_message.range < range_message.min_range || range_message.range >
269  range_message.max_range)
270  {
271  return;
272  }
273 
274  bool clear_sensor_cone = false;
275 
276  if (range_message.range >= range_message.max_range && clear_on_max_reading_) {
277  clear_sensor_cone = true;
278  }
279 
280  updateCostmap(range_message, clear_sensor_cone);
281 }
282 
283 void RangeSensorLayer::updateCostmap(
284  sensor_msgs::msg::Range & range_message,
285  bool clear_sensor_cone)
286 {
287  max_angle_ = range_message.field_of_view / 2;
288 
289  geometry_msgs::msg::PointStamped in, out;
290  in.header.stamp = range_message.header.stamp;
291  in.header.frame_id = range_message.header.frame_id;
292 
293  geometry_msgs::msg::TransformStamped transform;
294  try {
295  transform = tf_->lookupTransform(
296  global_frame_, in.header.frame_id,
297  tf2_ros::fromMsg(in.header.stamp),
298  tf2_ros::fromRclcpp(transform_tolerance_));
299  } catch (const tf2::TransformException &) {
300  RCLCPP_INFO(
301  logger_, "Range sensor layer can't transform from %s to %s",
302  in.header.frame_id.c_str(), global_frame_.c_str());
303  return;
304  }
305 
306  tf2::doTransform(in, out, transform);
307 
308  double ox = out.point.x, oy = out.point.y;
309 
310  in.point.x = range_message.range;
311 
312  tf2::doTransform(in, out, transform);
313 
314  double tx = out.point.x, ty = out.point.y;
315 
316  // calculate target props
317  double dx = tx - ox, dy = ty - oy, theta = atan2(dy, dx), d = sqrt(dx * dx + dy * dy);
318 
319  // Integer Bounds of Update
320  int bx0, by0, bx1, by1;
321 
322  // Triangle that will be really updated; the other cells within bounds are ignored
323  // This triangle is formed by the origin and left and right sides of sonar cone
324  int Ox, Oy, Ax, Ay, Bx, By;
325 
326  // Bounds includes the origin
327  worldToMapNoBounds(ox, oy, Ox, Oy);
328  bx1 = bx0 = Ox;
329  by1 = by0 = Oy;
330  touch(ox, oy, &min_x_, &min_y_, &max_x_, &max_y_);
331 
332  // Update Map with Target Point
333  unsigned int aa, ab;
334  if (worldToMap(tx, ty, aa, ab)) {
335  setCost(aa, ab, 233);
336  touch(tx, ty, &min_x_, &min_y_, &max_x_, &max_y_);
337  }
338 
339  double mx, my;
340 
341  // Update left side of sonar cone
342  mx = ox + cos(theta - max_angle_) * d * 1.2;
343  my = oy + sin(theta - max_angle_) * d * 1.2;
344  worldToMapNoBounds(mx, my, Ax, Ay);
345  bx0 = std::min(bx0, Ax);
346  bx1 = std::max(bx1, Ax);
347  by0 = std::min(by0, Ay);
348  by1 = std::max(by1, Ay);
349  touch(mx, my, &min_x_, &min_y_, &max_x_, &max_y_);
350 
351  // Update right side of sonar cone
352  mx = ox + cos(theta + max_angle_) * d * 1.2;
353  my = oy + sin(theta + max_angle_) * d * 1.2;
354 
355  worldToMapNoBounds(mx, my, Bx, By);
356  bx0 = std::min(bx0, Bx);
357  bx1 = std::max(bx1, Bx);
358  by0 = std::min(by0, By);
359  by1 = std::max(by1, By);
360  touch(mx, my, &min_x_, &min_y_, &max_x_, &max_y_);
361 
362  // Limit Bounds to Grid
363  bx0 = std::max(0, bx0);
364  by0 = std::max(0, by0);
365  bx1 = std::min(static_cast<int>(size_x_) - 1, bx1);
366  by1 = std::min(static_cast<int>(size_y_) - 1, by1);
367 
368  // Signed indices, as the bounds are empty if the cone lies wholly off the grid
369  for (int x = bx0; x <= bx1; x++) {
370  for (int y = by0; y <= by1; y++) {
371  bool update_xy_cell = true;
372 
373  // Unless inflate_cone_ is set to 100 %, we update cells only within the
374  // (partially inflated) sensor cone, projected on the costmap as a triangle.
375  // 0 % corresponds to just the triangle, but if your sensor fov is very
376  // narrow, the covered area can become zero due to cell discretization.
377  // See wiki description for more details
378  if (inflate_cone_ < 1.0) {
379  // Determine barycentric coordinates
380  int w0 = orient2d(Ax, Ay, Bx, By, x, y);
381  int w1 = orient2d(Bx, By, Ox, Oy, x, y);
382  int w2 = orient2d(Ox, Oy, Ax, Ay, x, y);
383 
384  // Barycentric coordinates inside area threshold; this is not mathematically
385  // sound at all, but it works!
386  float bcciath = -static_cast<float>(inflate_cone_) * area(Ax, Ay, Bx, By, Ox, Oy);
387  update_xy_cell = w0 >= bcciath && w1 >= bcciath && w2 >= bcciath;
388  }
389 
390  if (update_xy_cell) {
391  double wx, wy;
392  mapToWorld(x, y, wx, wy);
393  update_cell(ox, oy, theta, range_message.range, wx, wy, clear_sensor_cone);
394  }
395  }
396  }
397 
398  buffered_readings_++;
399  last_reading_time_ = clock_->now();
400 }
401 
402 void RangeSensorLayer::update_cell(
403  double ox, double oy, double ot, double r,
404  double nx, double ny, bool clear)
405 {
406  unsigned int x, y;
407  if (worldToMap(nx, ny, x, y)) {
408  double dx = nx - ox, dy = ny - oy;
409  double theta = atan2(dy, dx) - ot;
410  theta = angles::normalize_angle(theta);
411  double phi = sqrt(dx * dx + dy * dy);
412  double sensor = 0.0;
413  if (!clear) {
414  sensor = sensor_model(r, phi, theta);
415  }
416  double prior = to_prob(getCost(x, y));
417  double prob_occ = sensor * prior;
418  double prob_not = (1 - sensor) * (1 - prior);
419  double new_prob = prob_occ / (prob_occ + prob_not);
420 
421  RCLCPP_DEBUG(
422  logger_,
423  "%f %f | %f %f = %f", dx, dy, theta, phi, sensor);
424  RCLCPP_DEBUG(
425  logger_,
426  "%f | %f %f | %f", prior, prob_occ, prob_not, new_prob);
427  unsigned char c = to_cost(new_prob);
428  setCost(x, y, c);
429  }
430 }
431 
432 void RangeSensorLayer::resetRange()
433 {
434  min_x_ = min_y_ = std::numeric_limits<double>::max();
435  max_x_ = max_y_ = -std::numeric_limits<double>::max();
436 }
437 
438 void RangeSensorLayer::updateBounds(
439  double robot_x, double robot_y,
440  double robot_yaw, double * min_x, double * min_y,
441  double * max_x, double * max_y)
442 {
443  robot_yaw = 0 + robot_yaw; // Avoid error if variable not in use
444  if (layered_costmap_->isRolling()) {
445  updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
446  }
447 
448  updateCostmap();
449 
450  *min_x = std::min(*min_x, min_x_);
451  *min_y = std::min(*min_y, min_y_);
452  *max_x = std::max(*max_x, max_x_);
453  *max_y = std::max(*max_y, max_y_);
454 
455  resetRange();
456 
457  if (!enabled_) {
458  setCurrent(true);
459  return;
460  }
461 
462  if (buffered_readings_ == 0) {
463  if (no_readings_timeout_ > 0.0 &&
464  (clock_->now() - last_reading_time_).seconds() >
465  no_readings_timeout_)
466  {
467  RCLCPP_WARN(
468  logger_,
469  "No range readings received for %.2f seconds, while expected at least every %.2f seconds.",
470  (clock_->now() - last_reading_time_).seconds(),
471  no_readings_timeout_);
472  setCurrent(false);
473  }
474  }
475 }
476 
477 void RangeSensorLayer::updateCosts(
478  nav2_costmap_2d::Costmap2D & master_grid,
479  int min_i, int min_j, int max_i, int max_j)
480 {
481  if (!enabled_) {
482  return;
483  }
484 
485  unsigned char * master_array = master_grid.getCharMap();
486  unsigned int span = master_grid.getSizeInCellsX();
487  unsigned char clear = to_cost(clear_threshold_), mark = to_cost(mark_threshold_);
488 
489  for (int j = min_j; j < max_j; j++) {
490  unsigned int it = j * span + min_i;
491  for (int i = min_i; i < max_i; i++) {
492  unsigned char prob = costmap_[it];
493  unsigned char current;
494  if (prob == nav2_costmap_2d::NO_INFORMATION) {
495  it++;
496  continue;
497  } else if (prob > mark) {
498  current = nav2_costmap_2d::LETHAL_OBSTACLE;
499  } else if (prob < clear) {
500  current = nav2_costmap_2d::FREE_SPACE;
501  } else {
502  it++;
503  continue;
504  }
505 
506  unsigned char old_cost = master_array[it];
507 
508  if (old_cost == NO_INFORMATION || old_cost < current) {
509  master_array[it] = current;
510  }
511  it++;
512  }
513  }
514 
515  buffered_readings_ = 0;
516 
517  // if not current due to reset, set current now after clearing
518  if (!isCurrent() && was_reset_) {
519  was_reset_ = false;
520  setCurrent(true);
521  }
522 }
523 
524 void RangeSensorLayer::reset()
525 {
526  RCLCPP_DEBUG(logger_, "Resetting range sensor layer...");
527  deactivate();
528  resetMaps();
529  was_reset_ = true;
530  activate();
531 }
532 
533 void RangeSensorLayer::deactivate()
534 {
535  range_msgs_buffer_.clear();
536 }
537 
538 void RangeSensorLayer::activate()
539 {
540  range_msgs_buffer_.clear();
541 }
542 
543 } // namespace nav2_costmap_2d
A QoS profile for best-effort sensor data with a history of 10 messages.
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition: costmap_2d.hpp:69
unsigned char * getCharMap() const
Will return a pointer to the underlying unsigned char array used as the costmap.
Definition: costmap_2d.cpp:260
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
Definition: costmap_2d.cpp:548
Abstract class for layered costmap plugin implementations.
Definition: layer.hpp:62
Takes in IR/Sonar/similar point measurement sensors and populates in costmap.