Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
speed_filter.cpp
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2020 Samsung Research Russia
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 <ORGANIZATION> 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 OWNER 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  * Author: Alexey Merzlyakov
36  *********************************************************************/
37 
38 #include "nav2_costmap_2d/costmap_filters/speed_filter.hpp"
39 
40 #include <cmath>
41 #include <limits>
42 #include <utility>
43 #include <memory>
44 #include <string>
45 
46 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
47 #include "nav2_util/occ_grid_utils.hpp"
48 #include "nav2_util/robot_utils.hpp"
49 
50 namespace nav2_costmap_2d
51 {
52 
54 : filter_info_sub_(nullptr), mask_sub_(nullptr),
55  speed_limit_pub_(nullptr), filter_mask_(nullptr), global_frame_(""),
56  speed_limit_(NO_SPEED_LIMIT), speed_limit_prev_(NO_SPEED_LIMIT)
57 {
58 }
59 
61  const std::string & filter_info_topic)
62 {
63  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
64 
65  nav2::LifecycleNode::SharedPtr node = node_.lock();
66  if (!node) {
67  throw std::runtime_error{"Failed to lock node"};
68  }
69 
70  // Declare "speed_limit_topic" parameter specific to SpeedFilter only
71  std::string speed_limit_topic = node->declare_or_get_parameter(name_ + "." + "speed_limit_topic",
72  std::string("speed_limit"));
73  speed_limit_topic = joinWithParentNamespace(speed_limit_topic);
74 
75  // Path lookahead parameters
76  enable_path_lookahead_ = node->declare_or_get_parameter(
77  name_ + "." + "enable_path_lookahead", false);
78  max_decel_ = node->declare_or_get_parameter(
79  name_ + "." + "max_decel", -0.5);
80  min_lookahead_ = node->declare_or_get_parameter(
81  name_ + "." + "min_lookahead", 0.3);
82  max_lookahead_ = node->declare_or_get_parameter(
83  name_ + "." + "max_lookahead", 5.0);
84  std::string path_topic = node->declare_or_get_parameter(
85  name_ + "." + "path_topic", std::string("plan"));
86  std::string odom_topic = node->declare_or_get_parameter(
87  name_ + "." + "odom_topic", std::string("odom"));
88 
89  // Check params
90  if (enable_path_lookahead_) {
91  if (max_decel_ >= 0.0) {
92  RCLCPP_WARN(
93  logger_,
94  "SpeedFilter: max_decel should be negative,"
95  "lookahead distance will be set to max_lookahead instead");
96  }
97  if (min_lookahead_ < 0.0) {
98  RCLCPP_WARN(
99  logger_,
100  "SpeedFilter: min_lookahead = %f is negative,"
101  "clamping to 0.0m", min_lookahead_);
102  min_lookahead_ = 0.0;
103  }
104  if (max_lookahead_ < min_lookahead_) {
105  RCLCPP_WARN(
106  logger_,
107  "SpeedFilter: max_lookahead = %f is less than min_lookahead = %f,"
108  "clamping to min_lookahead.",
109  max_lookahead_, min_lookahead_);
110  max_lookahead_ = min_lookahead_;
111  }
112  }
113 
114  filter_info_topic_ = joinWithParentNamespace(filter_info_topic);
115  // Setting new costmap filter info subscriber
116  RCLCPP_INFO(
117  logger_,
118  "SpeedFilter: Subscribing to \"%s\" topic for filter info...",
119  filter_info_topic_.c_str());
120  filter_info_sub_ = node->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(
122  std::bind(&SpeedFilter::filterInfoCallback, this, std::placeholders::_1),
124 
125  // Get global frame required for speed limit publisher
126  global_frame_ = layered_costmap_->getGlobalFrameID();
127 
128  // Create new speed limit publisher
129  speed_limit_pub_ = node->create_publisher<nav2_msgs::msg::SpeedLimit>(
130  speed_limit_topic);
131  speed_limit_pub_->on_activate();
132 
133  // Path subscriptions and odom smoother if lookahead enabled
134  if (enable_path_lookahead_) {
135  std::string resolved_path_topic = joinWithParentNamespace(path_topic);
136  RCLCPP_INFO(
137  logger_,
138  "SpeedFilter: Path lookahead enabled. Subscribing to \"%s\" topic for path...",
139  resolved_path_topic.c_str());
140  path_sub_ = node->create_subscription<nav_msgs::msg::Path>(
141  resolved_path_topic,
142  std::bind(&SpeedFilter::pathCallback, this, std::placeholders::_1));
143 
144  odom_smoother_ = std::make_shared<nav2_util::OdomSmoother>(
145  node, 0.3, odom_topic);
146 
147  lookahead_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>(
148  name_ + "/lookahead_point");
149  lookahead_pub_->on_activate();
150  }
151 
152  // Reset speed conversion states
153  base_ = BASE_DEFAULT;
154  multiplier_ = MULTIPLIER_DEFAULT;
155  percentage_ = false;
156 
157  // Reset path lookahead states
158  held_lookahead_dist_ = 0.0;
159  lookahead_start_idx_ = 0;
160 }
161 
163  const nav2_msgs::msg::CostmapFilterInfo::ConstSharedPtr & msg)
164 {
165  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
166 
167  nav2::LifecycleNode::SharedPtr node = node_.lock();
168  if (!node) {
169  throw std::runtime_error{"Failed to lock node"};
170  }
171 
172  if (!mask_sub_) {
173  RCLCPP_INFO(
174  logger_,
175  "SpeedFilter: Received filter info from %s topic.", filter_info_topic_.c_str());
176  } else {
177  RCLCPP_WARN(
178  logger_,
179  "SpeedFilter: New costmap filter info arrived from %s topic. Updating old filter info.",
180  filter_info_topic_.c_str());
181  // Resetting previous subscriber each time when new costmap filter information arrives
182  mask_sub_.reset();
183  }
184 
185  // Set base_/multiplier_ or use speed limit in % of maximum speed
186  base_ = msg->base;
187  multiplier_ = msg->multiplier;
188  if (msg->type == SPEED_FILTER_PERCENT) {
189  // Using speed limit in % of maximum speed
190  percentage_ = true;
191  RCLCPP_INFO(
192  logger_,
193  "SpeedFilter: Using expressed in a percent from maximum speed"
194  "speed_limit = %f + filter_mask_data * %f",
195  base_, multiplier_);
196  } else if (msg->type == SPEED_FILTER_ABSOLUTE) {
197  // Using speed limit in m/s
198  percentage_ = false;
199  RCLCPP_INFO(
200  logger_,
201  "SpeedFilter: Using absolute speed_limit = %f + filter_mask_data * %f",
202  base_, multiplier_);
203  } else {
204  RCLCPP_ERROR(logger_, "SpeedFilter: Mode is not supported");
205  return;
206  }
207 
208  mask_topic_ = joinWithParentNamespace(msg->filter_mask_topic);
209 
210  // Setting new filter mask subscriber
211  RCLCPP_INFO(
212  logger_,
213  "SpeedFilter: Subscribing to \"%s\" topic for filter mask...",
214  mask_topic_.c_str());
215  mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
216  mask_topic_,
217  std::bind(&SpeedFilter::maskCallback, this, std::placeholders::_1),
219 }
220 
222  const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & msg)
223 {
224  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
225 
226  if (!filter_mask_) {
227  RCLCPP_INFO(
228  logger_,
229  "SpeedFilter: Received filter mask from %s topic.", mask_topic_.c_str());
230  } else {
231  RCLCPP_WARN(
232  logger_,
233  "SpeedFilter: New filter mask arrived from %s topic. Updating old filter mask.",
234  mask_topic_.c_str());
235  filter_mask_.reset();
236  }
237 
238  filter_mask_ = msg;
239 }
240 
242  const nav_msgs::msg::Path::ConstSharedPtr & msg)
243 {
244  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
245 
246  current_path_ = msg;
247 
248  // Reset cached start index when new path is received
249  lookahead_start_idx_ = 0;
250 }
251 
253  const geometry_msgs::msg::Pose & pose,
254  const geometry_msgs::msg::TransformStamped & mask_transform,
255  double & speed_limit)
256 {
257  geometry_msgs::msg::Pose mask_pose; // robot coordinates in mask frame
258 
259  // All samples in this process call use the same global-to-mask transform.
260  auto planar_pose = pose;
261  planar_pose.position.z = 0.0;
262  tf2::doTransform(planar_pose, mask_pose, mask_transform);
263 
264  // Converting mask_pose robot position to filter_mask_ indexes (mask_robot_i, mask_robot_j)
265  unsigned int mask_robot_i, mask_robot_j;
266  if (!nav2_util::worldToMap(
267  filter_mask_, mask_pose.position.x, mask_pose.position.y,
268  mask_robot_i, mask_robot_j))
269  {
270  return false;
271  }
272 
273  // Getting filter_mask data from cell where the robot placed and
274  // calculating speed limit value
275  int8_t speed_mask_data = getMaskData(filter_mask_, mask_robot_i, mask_robot_j);
276  if (speed_mask_data == SPEED_MASK_NO_LIMIT) {
277  // Corresponding filter mask cell is free.
278  // Setting no speed limit there.
279  speed_limit = NO_SPEED_LIMIT;
280  } else if (speed_mask_data == SPEED_MASK_UNKNOWN) {
281  // Corresponding filter mask cell is unknown.
282  // Do nothing.
283  RCLCPP_ERROR(
284  logger_,
285  "SpeedFilter: Found unknown cell in filter_mask[%i, %i], "
286  "which is invalid for this kind of filter",
287  mask_robot_i, mask_robot_j);
288  return false;
289  } else {
290  // Normal case: speed_mask_data in range of [1..100]
291  speed_limit = speed_mask_data * multiplier_ + base_;
292  if (percentage_) {
293  if (speed_limit < 0.0 || speed_limit > 100.0) {
294  RCLCPP_WARN(
295  logger_,
296  "SpeedFilter: Speed limit in filter_mask[%i, %i] is %f%%, "
297  "out of bounds of [0, 100]. Setting it to no-limit value.",
298  mask_robot_i, mask_robot_j, speed_limit);
299  speed_limit = NO_SPEED_LIMIT;
300  }
301  } else {
302  if (speed_limit < 0.0) {
303  RCLCPP_WARN(
304  logger_,
305  "SpeedFilter: Speed limit in filter_mask[%i, %i] is less than 0 m/s, "
306  "which can not be true. Setting it to no-limit value.",
307  mask_robot_i, mask_robot_j);
308  speed_limit = NO_SPEED_LIMIT;
309  }
310  }
311  }
312  return true;
313 }
314 
316  const geometry_msgs::msg::Pose & robot_pose,
317  double lookahead_dist,
318  const geometry_msgs::msg::TransformStamped & mask_transform,
319  double & speed_limit)
320 {
321  double min_speed_limit = std::numeric_limits<double>::max();
322 
323  // Release the hold by default, a stricter limit ahead re-arms it
324  held_lookahead_dist_ = 0.0;
325 
326  // Lookahead endpoint for visualization
327  auto lookahead_point_msg = std::make_unique<geometry_msgs::msg::PointStamped>();
328  lookahead_point_msg->header.frame_id = global_frame_;
329  lookahead_point_msg->header.stamp = clock_->now();
330  lookahead_point_msg->point = robot_pose.position;
331 
332  // Transform path if not in the global frame
333  nav_msgs::msg::Path transformed_path;
334  if (!nav2_util::transformPathInTargetFrame(
335  *current_path_, transformed_path, *tf_, global_frame_,
336  tf2::durationToSec(transform_tolerance_)))
337  {
338  RCLCPP_ERROR_THROTTLE(
339  logger_, *(clock_), 5000,
340  "SpeedFilter: Failed to transform path to global frame, "
341  "no speed limit will be published");
342  return false;
343  }
344 
345  const auto & poses = transformed_path.poses;
346  const size_t pose_search_start =
347  (lookahead_start_idx_ < poses.size()) ? lookahead_start_idx_ : 0;
348 
349  // Update cached start index
350  lookahead_start_idx_ = nav2_util::distance_from_path(
351  transformed_path, robot_pose, pose_search_start).closest_segment_index;
352 
353  // Check robot's current pose
354  double limit_at_robot_pose = NO_SPEED_LIMIT;
355  if (!getSpeedLimitAtPose(robot_pose, mask_transform, limit_at_robot_pose)) {
356  // Pose mapped outside mask or transform failed
357  RCLCPP_ERROR_THROTTLE(logger_, *(clock_), 5000,
358  "SpeedFilter: Failed to get speed limit at robot pose");
359  return false;
360  }
361 
362  if (limit_at_robot_pose != NO_SPEED_LIMIT) {
363  min_speed_limit = limit_at_robot_pose;
364  }
365 
366  // Walk poses from the lookahead start index forward, sampling the speed limit at each pose.
367  double dist_along_path = 0.0;
368  for (size_t i = lookahead_start_idx_; i < poses.size(); ++i) {
369  if (dist_along_path > lookahead_dist) {
370  break;
371  }
372 
373  // Update lookahead endpoint for visualization
374  lookahead_point_msg->point = poses[i].pose.position;
375 
376  double sampled_speed_limit = NO_SPEED_LIMIT;
377  if (getSpeedLimitAtPose(poses[i].pose, mask_transform, sampled_speed_limit) &&
378  sampled_speed_limit != NO_SPEED_LIMIT)
379  {
380  min_speed_limit = std::min(min_speed_limit, sampled_speed_limit);
381  }
382 
383  // Accumulate distance to the next pose for the following iteration's check
384  if (i + 1 < poses.size()) {
385  dist_along_path += nav2_util::geometry_utils::euclidean_distance(
386  poses[i].pose.position, poses[i + 1].pose.position);
387  }
388  }
389 
390  if (lookahead_pub_ && lookahead_pub_->get_subscription_count() > 0) {
391  lookahead_pub_->publish(std::move(lookahead_point_msg));
392  }
393 
394  // No limit found anywhere along the lookahead, fall back to no-limit
395  if (min_speed_limit == std::numeric_limits<double>::max()) {
396  min_speed_limit = NO_SPEED_LIMIT;
397  }
398 
399  // Hold the lookahead distance if upcoming speed limit is different from the current one
400  if (limit_at_robot_pose != min_speed_limit) {
401  held_lookahead_dist_ = lookahead_dist;
402  }
403 
404  speed_limit = min_speed_limit;
405  return true;
406 }
407 
409  nav2_costmap_2d::Costmap2D & /*master_grid*/,
410  int /*min_i*/, int /*min_j*/, int /*max_i*/, int /*max_j*/,
411  const geometry_msgs::msg::Pose & pose)
412 {
413  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
414 
415  if (!filter_mask_) {
416  // Show warning message every 2 seconds to not litter an output
417  RCLCPP_WARN_THROTTLE(
418  logger_, *(clock_), 2000,
419  "SpeedFilter: Filter mask was not received");
420  return;
421  }
422 
423  geometry_msgs::msg::TransformStamped mask_transform;
424  if (!nav2_util::lookupTransformWithStalenessCheck(
425  *tf_, filter_mask_->header.frame_id, global_frame_, clock_->now(),
426  transform_staleness_threshold_, mask_transform))
427  {
428  return;
429  }
430 
431  // Decide path lookahead vs just checking at robot pose.
432  // Path lookahead requires a non-empty path received
433  const bool use_path_lookahead =
434  enable_path_lookahead_ &&
435  current_path_ && !current_path_->poses.empty();
436 
437  if (use_path_lookahead) {
438  const auto twist = odom_smoother_->getTwist();
439  const double linear_vel = std::abs(twist.linear.x);
440 
441  // Calculate lookahead distance at current velocity
442  double d_lookahead = max_lookahead_;
443  if (max_decel_ < 0.0) {
444  d_lookahead = (linear_vel * linear_vel) / (2.0 * std::abs(max_decel_));
445  d_lookahead = std::clamp(d_lookahead, min_lookahead_, max_lookahead_);
446 
447  // If lookahead distance is being held, don't let it shrink below the held value
448  d_lookahead = std::max(d_lookahead, held_lookahead_dist_);
449  }
450 
451  if (!getSpeedLimitFromLookahead(pose, d_lookahead, mask_transform, speed_limit_)) {
452  RCLCPP_ERROR(logger_, "SpeedFilter: Failed to get speed limit from lookahead");
453  return;
454  }
455  } else {
456  if (!getSpeedLimitAtPose(pose, mask_transform, speed_limit_)) {
457  RCLCPP_ERROR(logger_, "SpeedFilter: Failed to get speed limit at pose");
458  return;
459  }
460  }
461 
462  if (speed_limit_ != speed_limit_prev_) {
463  if (speed_limit_ != NO_SPEED_LIMIT) {
464  RCLCPP_DEBUG(logger_, "SpeedFilter: Speed limit is set to %f", speed_limit_);
465  } else {
466  RCLCPP_DEBUG(logger_, "SpeedFilter: Speed limit is set to its default value");
467  }
468 
469  // Forming and publishing new SpeedLimit message
470  std::unique_ptr<nav2_msgs::msg::SpeedLimit> msg =
471  std::make_unique<nav2_msgs::msg::SpeedLimit>();
472  msg->header.frame_id = global_frame_;
473  msg->header.stamp = clock_->now();
474  msg->percentage = percentage_;
475  msg->speed_limit = speed_limit_;
476  speed_limit_pub_->publish(std::move(msg));
477 
478  speed_limit_prev_ = speed_limit_;
479  }
480 }
481 
483 {
484  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
485 
486  filter_info_sub_.reset();
487  mask_sub_.reset();
488  if (speed_limit_pub_) {
489  speed_limit_pub_->on_deactivate();
490  speed_limit_pub_.reset();
491  }
492  if (lookahead_pub_) {
493  lookahead_pub_->on_deactivate();
494  lookahead_pub_.reset();
495  }
496 }
497 
499 {
500  std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
501 
502  if (filter_mask_) {
503  return true;
504  }
505  return false;
506 }
507 
508 } // namespace nav2_costmap_2d
509 
510 #include "pluginlib/class_list_macros.hpp"
A QoS profile for latched, reliable topics 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
int8_t getMaskData(nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, const unsigned int mx, const unsigned int my) const
Get the data of a cell in the filter mask.
double transform_staleness_threshold_
: Max age for transforms. Check disabled if <= 0.
std::string filter_info_topic_
: Name of costmap filter info topic
std::string mask_topic_
: Name of filter mask topic
tf2::Duration transform_tolerance_
: mask_frame->global_frame_ transform tolerance
mutex_t * getMutex()
: returns pointer to a mutex
Abstract class for layered costmap plugin implementations.
Definition: layer.hpp:62
std::string joinWithParentNamespace(const std::string &topic)
Definition: layer.cpp:83
Reads in a speed restriction mask and enables a robot to dynamically adjust speed based on pose in ma...
void resetFilter() override
Reset the costmap filter / topic / info.
bool getSpeedLimitFromLookahead(const geometry_msgs::msg::Pose &robot_pose, double lookahead_dist, const geometry_msgs::msg::TransformStamped &mask_transform, double &speed_limit)
Get the speed limit from the path lookahead.
void filterInfoCallback(const nav2_msgs::msg::CostmapFilterInfo::ConstSharedPtr &msg)
Callback for the filter information.
void process(nav2_costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j, const geometry_msgs::msg::Pose &pose) override
Process the keepout layer at the current pose / bounds / grid.
void pathCallback(const nav_msgs::msg::Path::ConstSharedPtr &msg)
Callback for the planned path used by path lookahead.
bool getSpeedLimitAtPose(const geometry_msgs::msg::Pose &pose, const geometry_msgs::msg::TransformStamped &mask_transform, double &speed_limit)
Look up the speed limit at a single pose in the costmap's global frame.
void maskCallback(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr &msg)
Callback for the filter mask.
void initializeFilter(const std::string &filter_info_topic) override
Initialize the filter and subscribe to the info topic.
bool isActive()
If this filter is active.