36 #include <angles/angles.h>
43 #include "pluginlib/class_list_macros.hpp"
44 #include "geometry_msgs/msg/point_stamped.hpp"
45 #include "nav2_costmap_2d/range_sensor_layer.hpp"
49 using nav2_costmap_2d::LETHAL_OBSTACLE;
50 using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
51 using nav2_costmap_2d::NO_INFORMATION;
53 using namespace std::literals::chrono_literals;
58 RangeSensorLayer::RangeSensorLayer() {}
60 void RangeSensorLayer::onInitialize()
64 buffered_readings_ = 0;
65 last_reading_time_ = clock_->now();
66 default_value_ = to_cost(0.5);
71 auto node = node_.lock();
73 throw std::runtime_error{
"Failed to lock node"};
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);
88 double temp_tf_tol = 0.0;
89 node->get_parameter(
"transform_tolerance", temp_tf_tol);
90 transform_tolerance_ = tf2::durationFromSec(temp_tf_tol);
92 std::vector<std::string> topic_names = node->declare_or_get_parameter(
93 name_ +
"." +
"topics", std::vector<std::string>{});
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"));
100 sensor_type_name.begin(), sensor_type_name.end(),
101 sensor_type_name.begin(), ::toupper);
103 logger_,
"%s: %s as input_sensor_type given",
104 name_.c_str(), sensor_type_name.c_str());
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;
114 logger_,
"%s: Invalid input sensor type: %s. Defaulting to ALL.",
115 name_.c_str(), sensor_type_name.c_str());
119 if (topic_names.empty()) {
121 logger_,
"Invalid topic names list: it must"
122 "be a non-empty list of strings");
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);
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());
148 range_subs_.push_back(
149 node->create_subscription<sensor_msgs::msg::Range>(
152 &RangeSensorLayer::bufferIncomingRangeMsg,
this,
153 std::placeholders::_1),
157 logger_,
"RangeSensorLayer: subscribed to "
158 "topic %s", range_subs_.back()->get_topic_name());
160 global_frame_ = layered_costmap_->getGlobalFrameID();
164 double RangeSensorLayer::gamma(
double theta)
166 if (fabs(theta) > max_angle_) {
169 return 1 - pow(theta / max_angle_, 2);
173 double RangeSensorLayer::delta(
double phi)
175 return 1 - (1 + tanh(2 * (phi - phi_v_))) / 2;
178 void RangeSensorLayer::get_deltas(
double angle,
double * dx,
double * dy)
180 double ta = tan(angle);
184 *dx = resolution_ / ta;
187 *dx = copysign(*dx, cos(angle));
188 *dy = copysign(resolution_, sin(angle));
191 double RangeSensorLayer::sensor_model(
double r,
double phi,
double theta)
193 double lbda = delta(phi) * gamma(theta);
195 double delta = resolution_;
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) +
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;
210 void RangeSensorLayer::bufferIncomingRangeMsg(
211 const sensor_msgs::msg::Range::ConstSharedPtr & range_message)
213 range_message_mutex_.lock();
214 range_msgs_buffer_.push_back(*range_message);
215 range_message_mutex_.unlock();
218 void RangeSensorLayer::updateCostmap()
220 std::list<sensor_msgs::msg::Range> range_msgs_buffer_copy;
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();
227 for (
auto & range_msgs_it : range_msgs_buffer_copy) {
228 processRangeMessageFunc_(range_msgs_it);
232 void RangeSensorLayer::processRangeMsg(sensor_msgs::msg::Range & range_message)
234 if (range_message.min_range == range_message.max_range) {
235 processFixedRangeMsg(range_message);
237 processVariableRangeMsg(range_message);
241 void RangeSensorLayer::processFixedRangeMsg(sensor_msgs::msg::Range & range_message)
243 if (!std::isinf(range_message.range)) {
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());
252 bool clear_sensor_cone =
false;
254 if (range_message.range > 0) {
255 if (!clear_on_max_reading_) {
258 clear_sensor_cone =
true;
261 range_message.range = range_message.min_range;
263 updateCostmap(range_message, clear_sensor_cone);
266 void RangeSensorLayer::processVariableRangeMsg(sensor_msgs::msg::Range & range_message)
268 if (range_message.range < range_message.min_range || range_message.range >
269 range_message.max_range)
274 bool clear_sensor_cone =
false;
276 if (range_message.range >= range_message.max_range && clear_on_max_reading_) {
277 clear_sensor_cone =
true;
280 updateCostmap(range_message, clear_sensor_cone);
283 void RangeSensorLayer::updateCostmap(
284 sensor_msgs::msg::Range & range_message,
285 bool clear_sensor_cone)
287 max_angle_ = range_message.field_of_view / 2;
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;
293 geometry_msgs::msg::TransformStamped transform;
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 &) {
301 logger_,
"Range sensor layer can't transform from %s to %s",
302 in.header.frame_id.c_str(), global_frame_.c_str());
306 tf2::doTransform(in, out, transform);
308 double ox = out.point.x, oy = out.point.y;
310 in.point.x = range_message.range;
312 tf2::doTransform(in, out, transform);
314 double tx = out.point.x, ty = out.point.y;
317 double dx = tx - ox, dy = ty - oy, theta = atan2(dy, dx), d = sqrt(dx * dx + dy * dy);
320 int bx0, by0, bx1, by1;
324 int Ox, Oy, Ax, Ay, Bx, By;
327 worldToMapNoBounds(ox, oy, Ox, Oy);
330 touch(ox, oy, &min_x_, &min_y_, &max_x_, &max_y_);
334 if (worldToMap(tx, ty, aa, ab)) {
335 setCost(aa, ab, 233);
336 touch(tx, ty, &min_x_, &min_y_, &max_x_, &max_y_);
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_);
352 mx = ox + cos(theta + max_angle_) * d * 1.2;
353 my = oy + sin(theta + max_angle_) * d * 1.2;
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_);
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);
369 for (
int x = bx0; x <= bx1; x++) {
370 for (
int y = by0; y <= by1; y++) {
371 bool update_xy_cell =
true;
378 if (inflate_cone_ < 1.0) {
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);
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;
390 if (update_xy_cell) {
392 mapToWorld(x, y, wx, wy);
393 update_cell(ox, oy, theta, range_message.range, wx, wy, clear_sensor_cone);
398 buffered_readings_++;
399 last_reading_time_ = clock_->now();
402 void RangeSensorLayer::update_cell(
403 double ox,
double oy,
double ot,
double r,
404 double nx,
double ny,
bool clear)
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);
414 sensor = sensor_model(r, phi, theta);
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);
423 "%f %f | %f %f = %f", dx, dy, theta, phi, sensor);
426 "%f | %f %f | %f", prior, prob_occ, prob_not, new_prob);
427 unsigned char c = to_cost(new_prob);
432 void RangeSensorLayer::resetRange()
434 min_x_ = min_y_ = std::numeric_limits<double>::max();
435 max_x_ = max_y_ = -std::numeric_limits<double>::max();
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)
443 robot_yaw = 0 + robot_yaw;
444 if (layered_costmap_->isRolling()) {
445 updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
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_);
462 if (buffered_readings_ == 0) {
463 if (no_readings_timeout_ > 0.0 &&
464 (clock_->now() - last_reading_time_).seconds() >
465 no_readings_timeout_)
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_);
477 void RangeSensorLayer::updateCosts(
479 int min_i,
int min_j,
int max_i,
int max_j)
485 unsigned char * master_array = master_grid.
getCharMap();
487 unsigned char clear = to_cost(clear_threshold_), mark = to_cost(mark_threshold_);
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) {
497 }
else if (prob > mark) {
498 current = nav2_costmap_2d::LETHAL_OBSTACLE;
499 }
else if (prob < clear) {
500 current = nav2_costmap_2d::FREE_SPACE;
506 unsigned char old_cost = master_array[it];
508 if (old_cost == NO_INFORMATION || old_cost < current) {
509 master_array[it] = current;
515 buffered_readings_ = 0;
518 if (!isCurrent() && was_reset_) {
524 void RangeSensorLayer::reset()
526 RCLCPP_DEBUG(logger_,
"Resetting range sensor layer...");
533 void RangeSensorLayer::deactivate()
535 range_msgs_buffer_.clear();
538 void RangeSensorLayer::activate()
540 range_msgs_buffer_.clear();
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".
unsigned char * getCharMap() const
Will return a pointer to the underlying unsigned char array used as the costmap.
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
Abstract class for layered costmap plugin implementations.
Takes in IR/Sonar/similar point measurement sensors and populates in costmap.