From 72ed627e06e3747371c30a5219db349684bd195b Mon Sep 17 00:00:00 2001 From: Balint Rozgonyi <43723477+RBT22@users.noreply.github.com> Date: Tue, 28 Jan 2025 21:21:37 +0100 Subject: [PATCH] Add acceleration limits to DriveOnHeading and BackUp behaviors (#4810) * Add acceleration constraints Signed-off-by: RBT22 * Cleanup code Signed-off-by: RBT22 * Format code Signed-off-by: RBT22 * Add header to drive_on_heading.hpp Signed-off-by: RBT22 * Remove vel pointer Signed-off-by: RBT22 * Use the limits only if both of them is set Signed-off-by: RBT22 * Fix onActionCompletion params Signed-off-by: RBT22 * Add default acc params and change decel sign Signed-off-by: RBT22 * Add minimum speed parameter Signed-off-by: RBT22 * Update minimum speed parameter to 0.10 Signed-off-by: RBT22 * Log warning when acceleration or deceleration limits are not set Signed-off-by: RBT22 * Add param sign assert Signed-off-by: RBT22 * Remove unnecessary param checking Signed-off-by: RBT22 * Refactor acceleration limits to handle forward and backward movement separately Signed-off-by: RBT22 * Fix sign checking condition Signed-off-by: RBT22 * Replace throwing with silent sign correction Signed-off-by: RBT22 --------- Signed-off-by: RBT22 (cherry picked from commit 2ee3cef85524c30e5b77ea027a238573e76829a2) --- .../plugins/drive_on_heading.hpp | 59 ++++++++++++++++++- nav2_bringup/params/nav2_params.yaml | 6 ++ 2 files changed, 64 insertions(+), 1 deletion(-) diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index 0b74a044629..c379eb343c3 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "nav2_behaviors/timed_behavior.hpp" #include "nav2_msgs/action/drive_on_heading.hpp" @@ -131,7 +132,30 @@ class DriveOnHeading : public TimedBehavior cmd_vel->header.frame_id = this->robot_base_frame_; cmd_vel->twist.linear.y = 0.0; cmd_vel->twist.angular.z = 0.0; - cmd_vel->twist.linear.x = command_speed_; + + double current_speed = last_vel_ == std::numeric_limits::max() ? 0.0 : last_vel_; + bool forward = command_speed_ > 0.0; + double min_feasible_speed, max_feasible_speed; + if (forward) { + min_feasible_speed = current_speed + deceleration_limit_ / this->cycle_frequency_; + max_feasible_speed = current_speed + acceleration_limit_ / this->cycle_frequency_; + } else { + min_feasible_speed = current_speed - acceleration_limit_ / this->cycle_frequency_; + max_feasible_speed = current_speed - deceleration_limit_ / this->cycle_frequency_; + } + cmd_vel->twist.linear.x = std::clamp(command_speed_, min_feasible_speed, max_feasible_speed); + + // Check if we need to slow down to avoid overshooting + auto remaining_distance = std::fabs(command_x_) - distance; + double max_vel_to_stop = std::sqrt(-2.0 * deceleration_limit_ * remaining_distance); + if (max_vel_to_stop < std::abs(cmd_vel->twist.linear.x)) { + cmd_vel->twist.linear.x = forward ? max_vel_to_stop : -max_vel_to_stop; + } + + // Ensure we don't go below minimum speed + if (std::fabs(cmd_vel->twist.linear.x) < minimum_speed_) { + cmd_vel->twist.linear.x = forward ? minimum_speed_ : -minimum_speed_; + } geometry_msgs::msg::Pose2D pose2d; pose2d.x = current_pose.pose.position.x; @@ -144,6 +168,7 @@ class DriveOnHeading : public TimedBehavior return ResultStatus{Status::FAILED, ActionT::Result::COLLISION_AHEAD}; } + last_vel_ = cmd_vel->twist.linear.x; this->vel_pub_->publish(std::move(cmd_vel)); return ResultStatus{Status::RUNNING, ActionT::Result::NONE}; @@ -155,6 +180,14 @@ class DriveOnHeading : public TimedBehavior */ CostmapInfoType getResourceInfo() override {return CostmapInfoType::LOCAL;} + void onCleanup() override {last_vel_ = std::numeric_limits::max();} + + void onActionCompletion(std::shared_ptr/*result*/) + override + { + last_vel_ = std::numeric_limits::max(); + } + protected: /** * @brief Check if pose is collision free @@ -208,6 +241,26 @@ class DriveOnHeading : public TimedBehavior node, "simulate_ahead_time", rclcpp::ParameterValue(2.0)); node->get_parameter("simulate_ahead_time", simulate_ahead_time_); + + nav2_util::declare_parameter_if_not_declared( + node, this->behavior_name_ + ".acceleration_limit", + rclcpp::ParameterValue(2.5)); + nav2_util::declare_parameter_if_not_declared( + node, this->behavior_name_ + ".deceleration_limit", + rclcpp::ParameterValue(-2.5)); + nav2_util::declare_parameter_if_not_declared( + node, this->behavior_name_ + ".minimum_speed", + rclcpp::ParameterValue(0.10)); + node->get_parameter(this->behavior_name_ + ".acceleration_limit", acceleration_limit_); + node->get_parameter(this->behavior_name_ + ".deceleration_limit", deceleration_limit_); + node->get_parameter(this->behavior_name_ + ".minimum_speed", minimum_speed_); + if (acceleration_limit_ <= 0.0 || deceleration_limit_ >= 0.0) { + RCLCPP_ERROR(this->logger_, + "DriveOnHeading: acceleration_limit and deceleration_limit must be " + "positive and negative respectively"); + acceleration_limit_ = std::abs(acceleration_limit_); + deceleration_limit_ = -std::abs(deceleration_limit_); + } } typename ActionT::Feedback::SharedPtr feedback_; @@ -218,6 +271,10 @@ class DriveOnHeading : public TimedBehavior rclcpp::Duration command_time_allowance_{0, 0}; rclcpp::Time end_time_; double simulate_ahead_time_; + double acceleration_limit_; + double deceleration_limit_; + double minimum_speed_; + double last_vel_ = std::numeric_limits::max(); }; } // namespace nav2_behaviors diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 69fe42195eb..065d9e88d1d 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -305,8 +305,14 @@ behavior_server: plugin: "nav2_behaviors::Spin" backup: plugin: "nav2_behaviors::BackUp" + acceleration_limit: 2.5 + deceleration_limit: -2.5 + minimum_speed: 0.10 drive_on_heading: plugin: "nav2_behaviors::DriveOnHeading" + acceleration_limit: 2.5 + deceleration_limit: -2.5 + minimum_speed: 0.10 wait: plugin: "nav2_behaviors::Wait" assisted_teleop: