38 #include "nav2_costmap_2d/costmap_filters/speed_filter.hpp"
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"
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)
61 const std::string & filter_info_topic)
63 std::lock_guard<CostmapFilter::mutex_t> guard(*
getMutex());
65 nav2::LifecycleNode::SharedPtr node = node_.lock();
67 throw std::runtime_error{
"Failed to lock node"};
71 std::string speed_limit_topic = node->declare_or_get_parameter(name_ +
"." +
"speed_limit_topic",
72 std::string(
"speed_limit"));
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"));
90 if (enable_path_lookahead_) {
91 if (max_decel_ >= 0.0) {
94 "SpeedFilter: max_decel should be negative,"
95 "lookahead distance will be set to max_lookahead instead");
97 if (min_lookahead_ < 0.0) {
100 "SpeedFilter: min_lookahead = %f is negative,"
101 "clamping to 0.0m", min_lookahead_);
102 min_lookahead_ = 0.0;
104 if (max_lookahead_ < min_lookahead_) {
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_;
118 "SpeedFilter: Subscribing to \"%s\" topic for filter info...",
120 filter_info_sub_ = node->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(
126 global_frame_ = layered_costmap_->getGlobalFrameID();
129 speed_limit_pub_ = node->create_publisher<nav2_msgs::msg::SpeedLimit>(
131 speed_limit_pub_->on_activate();
134 if (enable_path_lookahead_) {
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>(
144 odom_smoother_ = std::make_shared<nav2_util::OdomSmoother>(
145 node, 0.3, odom_topic);
147 lookahead_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>(
148 name_ +
"/lookahead_point");
149 lookahead_pub_->on_activate();
153 base_ = BASE_DEFAULT;
154 multiplier_ = MULTIPLIER_DEFAULT;
158 held_lookahead_dist_ = 0.0;
159 lookahead_start_idx_ = 0;
163 const nav2_msgs::msg::CostmapFilterInfo::ConstSharedPtr & msg)
165 std::lock_guard<CostmapFilter::mutex_t> guard(*
getMutex());
167 nav2::LifecycleNode::SharedPtr node = node_.lock();
169 throw std::runtime_error{
"Failed to lock node"};
179 "SpeedFilter: New costmap filter info arrived from %s topic. Updating old filter info.",
187 multiplier_ = msg->multiplier;
188 if (msg->type == SPEED_FILTER_PERCENT) {
193 "SpeedFilter: Using expressed in a percent from maximum speed"
194 "speed_limit = %f + filter_mask_data * %f",
196 }
else if (msg->type == SPEED_FILTER_ABSOLUTE) {
201 "SpeedFilter: Using absolute speed_limit = %f + filter_mask_data * %f",
204 RCLCPP_ERROR(logger_,
"SpeedFilter: Mode is not supported");
213 "SpeedFilter: Subscribing to \"%s\" topic for filter mask...",
215 mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
222 const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & msg)
224 std::lock_guard<CostmapFilter::mutex_t> guard(*
getMutex());
229 "SpeedFilter: Received filter mask from %s topic.",
mask_topic_.c_str());
233 "SpeedFilter: New filter mask arrived from %s topic. Updating old filter mask.",
235 filter_mask_.reset();
242 const nav_msgs::msg::Path::ConstSharedPtr & msg)
244 std::lock_guard<CostmapFilter::mutex_t> guard(*
getMutex());
249 lookahead_start_idx_ = 0;
253 const geometry_msgs::msg::Pose & pose,
254 const geometry_msgs::msg::TransformStamped & mask_transform,
255 double & speed_limit)
257 geometry_msgs::msg::Pose mask_pose;
260 auto planar_pose = pose;
261 planar_pose.position.z = 0.0;
262 tf2::doTransform(planar_pose, mask_pose, mask_transform);
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))
275 int8_t speed_mask_data =
getMaskData(filter_mask_, mask_robot_i, mask_robot_j);
276 if (speed_mask_data == SPEED_MASK_NO_LIMIT) {
279 speed_limit = NO_SPEED_LIMIT;
280 }
else if (speed_mask_data == SPEED_MASK_UNKNOWN) {
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);
291 speed_limit = speed_mask_data * multiplier_ + base_;
293 if (speed_limit < 0.0 || speed_limit > 100.0) {
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;
302 if (speed_limit < 0.0) {
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;
316 const geometry_msgs::msg::Pose & robot_pose,
317 double lookahead_dist,
318 const geometry_msgs::msg::TransformStamped & mask_transform,
319 double & speed_limit)
321 double min_speed_limit = std::numeric_limits<double>::max();
324 held_lookahead_dist_ = 0.0;
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;
333 nav_msgs::msg::Path transformed_path;
334 if (!nav2_util::transformPathInTargetFrame(
335 *current_path_, transformed_path, *tf_, global_frame_,
338 RCLCPP_ERROR_THROTTLE(
339 logger_, *(clock_), 5000,
340 "SpeedFilter: Failed to transform path to global frame, "
341 "no speed limit will be published");
345 const auto & poses = transformed_path.poses;
346 const size_t pose_search_start =
347 (lookahead_start_idx_ < poses.size()) ? lookahead_start_idx_ : 0;
350 lookahead_start_idx_ = nav2_util::distance_from_path(
351 transformed_path, robot_pose, pose_search_start).closest_segment_index;
354 double limit_at_robot_pose = NO_SPEED_LIMIT;
357 RCLCPP_ERROR_THROTTLE(logger_, *(clock_), 5000,
358 "SpeedFilter: Failed to get speed limit at robot pose");
362 if (limit_at_robot_pose != NO_SPEED_LIMIT) {
363 min_speed_limit = limit_at_robot_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) {
374 lookahead_point_msg->point = poses[i].pose.position;
376 double sampled_speed_limit = NO_SPEED_LIMIT;
378 sampled_speed_limit != NO_SPEED_LIMIT)
380 min_speed_limit = std::min(min_speed_limit, sampled_speed_limit);
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);
390 if (lookahead_pub_ && lookahead_pub_->get_subscription_count() > 0) {
391 lookahead_pub_->publish(std::move(lookahead_point_msg));
395 if (min_speed_limit == std::numeric_limits<double>::max()) {
396 min_speed_limit = NO_SPEED_LIMIT;
400 if (limit_at_robot_pose != min_speed_limit) {
401 held_lookahead_dist_ = lookahead_dist;
404 speed_limit = min_speed_limit;
410 int ,
int ,
int ,
int ,
411 const geometry_msgs::msg::Pose & pose)
413 std::lock_guard<CostmapFilter::mutex_t> guard(*
getMutex());
417 RCLCPP_WARN_THROTTLE(
418 logger_, *(clock_), 2000,
419 "SpeedFilter: Filter mask was not received");
423 geometry_msgs::msg::TransformStamped mask_transform;
424 if (!nav2_util::lookupTransformWithStalenessCheck(
425 *tf_, filter_mask_->header.frame_id, global_frame_, clock_->now(),
433 const bool use_path_lookahead =
434 enable_path_lookahead_ &&
435 current_path_ && !current_path_->poses.empty();
437 if (use_path_lookahead) {
438 const auto twist = odom_smoother_->getTwist();
439 const double linear_vel = std::abs(twist.linear.x);
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_);
448 d_lookahead = std::max(d_lookahead, held_lookahead_dist_);
452 RCLCPP_ERROR(logger_,
"SpeedFilter: Failed to get speed limit from lookahead");
457 RCLCPP_ERROR(logger_,
"SpeedFilter: Failed to get speed limit at pose");
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_);
466 RCLCPP_DEBUG(logger_,
"SpeedFilter: Speed limit is set to its default value");
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));
478 speed_limit_prev_ = speed_limit_;
484 std::lock_guard<CostmapFilter::mutex_t> guard(*
getMutex());
486 filter_info_sub_.reset();
488 if (speed_limit_pub_) {
489 speed_limit_pub_->on_deactivate();
490 speed_limit_pub_.reset();
492 if (lookahead_pub_) {
493 lookahead_pub_->on_deactivate();
494 lookahead_pub_.reset();
500 std::lock_guard<CostmapFilter::mutex_t> guard(*
getMutex());
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".
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.
std::string joinWithParentNamespace(const std::string &topic)
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.
SpeedFilter()
A constructor.
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.