From 3d129b77c27fbbdaeaed272bac30642307617b70 Mon Sep 17 00:00:00 2001 From: RBT22 Date: Thu, 19 Dec 2024 11:37:28 +0000 Subject: [PATCH 1/5] Add acceleration limits --- .../plugins/drive_on_heading.hpp | 54 +++++++++++++++++-- 1 file changed, 51 insertions(+), 3 deletions(-) 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 8bffca8ae5..03a93206f5 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -123,10 +123,34 @@ class DriveOnHeading : public TimedBehavior return ResultStatus{Status::SUCCEEDED, ActionT::Goal::NONE}; } - auto cmd_vel = std::make_unique(); + auto cmd_vel = std::make_shared(); cmd_vel->linear.y = 0.0; cmd_vel->angular.z = 0.0; - cmd_vel->linear.x = command_speed_; + if (!last_vel_ || (acceleration_limit_ <= 0.0 && deceleration_limit_ <= 0.0)) { + cmd_vel->linear.x = command_speed_; + } else { + bool forward = command_speed_ > 0.0 ? true : false; + auto current_speed = last_vel_->linear.x; + auto remaining_distance = std::fabs(command_x_) - distance; + double min_feasible_speed = -std::numeric_limits::infinity(); + double max_feasible_speed = std::numeric_limits::infinity(); + if (deceleration_limit_ > 0.0) { + min_feasible_speed = current_speed - deceleration_limit_ / this->cycle_frequency_; + if (forward) { + max_feasible_speed = std::sqrt(2.0 * deceleration_limit_ * remaining_distance); + } + } + if (acceleration_limit_ > 0.0) { + max_feasible_speed = current_speed + acceleration_limit_ / this->cycle_frequency_; + if (!forward) { + min_feasible_speed = std::max( + -std::sqrt( + 2.0 * acceleration_limit_ * remaining_distance), min_feasible_speed); + } + } + + cmd_vel->linear.x = std::clamp(command_speed_, min_feasible_speed, max_feasible_speed); + } geometry_msgs::msg::Pose2D pose2d; pose2d.x = current_pose.pose.position.x; @@ -139,11 +163,23 @@ class DriveOnHeading : public TimedBehavior return ResultStatus{Status::FAILED, ActionT::Goal::COLLISION_AHEAD}; } - this->vel_pub_->publish(std::move(cmd_vel)); + this->vel_pub_->publish(*cmd_vel); + + last_vel_ = cmd_vel; return ResultStatus{Status::RUNNING, ActionT::Goal::NONE}; } + void onCleanup() override + { + last_vel_.reset(); + } + + void onActionCompletion() override + { + last_vel_.reset(); + } + protected: /** * @brief Check if pose is collision free @@ -198,6 +234,15 @@ class DriveOnHeading : public TimedBehavior node, sim_ahead_time, rclcpp::ParameterValue(2.0)); node->get_parameter(sim_ahead_time, simulate_ahead_time_); + + nav2_util::declare_parameter_if_not_declared( + node, this->behavior_name_ + ".acceleration_limit", + rclcpp::ParameterValue(0.0)); + nav2_util::declare_parameter_if_not_declared( + node, this->behavior_name_ + ".deceleration_limit", + rclcpp::ParameterValue(0.0)); + node->get_parameter(this->behavior_name_ + ".acceleration_limit", acceleration_limit_); + node->get_parameter(this->behavior_name_ + ".deceleration_limit", deceleration_limit_); } typename ActionT::Feedback::SharedPtr feedback_; @@ -208,6 +253,9 @@ 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_; + geometry_msgs::msg::Twist::SharedPtr last_vel_; }; } // namespace nav2_behaviors From e5d7f127bd7cc0d1e918a0e8cb151155952f5427 Mon Sep 17 00:00:00 2001 From: RBT22 Date: Thu, 19 Dec 2024 11:44:55 +0000 Subject: [PATCH 2/5] Store only the forward vel --- .../nav2_behaviors/plugins/drive_on_heading.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) 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 03a93206f5..95ae4365db 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -123,14 +123,14 @@ class DriveOnHeading : public TimedBehavior return ResultStatus{Status::SUCCEEDED, ActionT::Goal::NONE}; } - auto cmd_vel = std::make_shared(); + auto cmd_vel = std::make_unique(); cmd_vel->linear.y = 0.0; cmd_vel->angular.z = 0.0; if (!last_vel_ || (acceleration_limit_ <= 0.0 && deceleration_limit_ <= 0.0)) { cmd_vel->linear.x = command_speed_; } else { bool forward = command_speed_ > 0.0 ? true : false; - auto current_speed = last_vel_->linear.x; + auto current_speed = *last_vel_; auto remaining_distance = std::fabs(command_x_) - distance; double min_feasible_speed = -std::numeric_limits::infinity(); double max_feasible_speed = std::numeric_limits::infinity(); @@ -163,9 +163,9 @@ class DriveOnHeading : public TimedBehavior return ResultStatus{Status::FAILED, ActionT::Goal::COLLISION_AHEAD}; } - this->vel_pub_->publish(*cmd_vel); + last_vel_ = std::make_shared(cmd_vel->linear.x); - last_vel_ = cmd_vel; + this->vel_pub_->publish(std::move(cmd_vel)); return ResultStatus{Status::RUNNING, ActionT::Goal::NONE}; } @@ -255,7 +255,7 @@ class DriveOnHeading : public TimedBehavior double simulate_ahead_time_; double acceleration_limit_; double deceleration_limit_; - geometry_msgs::msg::Twist::SharedPtr last_vel_; + std::shared_ptr last_vel_; }; } // namespace nav2_behaviors From b9cc15771907dc107e37291a8d7c52668e0ebea9 Mon Sep 17 00:00:00 2001 From: RBT22 Date: Thu, 19 Dec 2024 13:56:10 +0000 Subject: [PATCH 3/5] Fix edge cases --- .../plugins/drive_on_heading.hpp | 24 +++++++++++-------- 1 file changed, 14 insertions(+), 10 deletions(-) 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 95ae4365db..a3a4a14039 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -129,27 +129,31 @@ class DriveOnHeading : public TimedBehavior if (!last_vel_ || (acceleration_limit_ <= 0.0 && deceleration_limit_ <= 0.0)) { cmd_vel->linear.x = command_speed_; } else { - bool forward = command_speed_ > 0.0 ? true : false; auto current_speed = *last_vel_; auto remaining_distance = std::fabs(command_x_) - distance; double min_feasible_speed = -std::numeric_limits::infinity(); double max_feasible_speed = std::numeric_limits::infinity(); if (deceleration_limit_ > 0.0) { min_feasible_speed = current_speed - deceleration_limit_ / this->cycle_frequency_; - if (forward) { - max_feasible_speed = std::sqrt(2.0 * deceleration_limit_ * remaining_distance); - } } if (acceleration_limit_ > 0.0) { max_feasible_speed = current_speed + acceleration_limit_ / this->cycle_frequency_; - if (!forward) { - min_feasible_speed = std::max( - -std::sqrt( - 2.0 * acceleration_limit_ * remaining_distance), min_feasible_speed); - } } - cmd_vel->linear.x = std::clamp(command_speed_, min_feasible_speed, max_feasible_speed); + + // Check if we need to slow down to avoid overshooting + bool forward = command_speed_ > 0.0 ? true : false; + if ((forward && deceleration_limit_ > 0.0)) { + double max_vel_to_stop = std::sqrt(2.0 * deceleration_limit_ * remaining_distance); + if (max_vel_to_stop < cmd_vel->linear.x) { + cmd_vel->linear.x = max_vel_to_stop; + } + } else if ((!forward && acceleration_limit_ > 0.0)) { + double max_vel_to_stop = -std::sqrt(2.0 * acceleration_limit_ * remaining_distance); + if (max_vel_to_stop > cmd_vel->linear.x) { + cmd_vel->linear.x = max_vel_to_stop; + } + } } geometry_msgs::msg::Pose2D pose2d; From d3bdb797fe453edde4bc49d64680197edf7a8dca Mon Sep 17 00:00:00 2001 From: RBT22 Date: Thu, 19 Dec 2024 14:41:06 +0000 Subject: [PATCH 4/5] Fix first value --- .../include/nav2_behaviors/plugins/drive_on_heading.hpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) 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 a3a4a14039..79ae868f98 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -126,10 +126,13 @@ class DriveOnHeading : public TimedBehavior auto cmd_vel = std::make_unique(); cmd_vel->linear.y = 0.0; cmd_vel->angular.z = 0.0; - if (!last_vel_ || (acceleration_limit_ <= 0.0 && deceleration_limit_ <= 0.0)) { + if (acceleration_limit_ <= 0.0 && deceleration_limit_ <= 0.0) { cmd_vel->linear.x = command_speed_; } else { - auto current_speed = *last_vel_; + double current_speed = 0.0; + if (last_vel_) { + current_speed = *last_vel_; + } auto remaining_distance = std::fabs(command_x_) - distance; double min_feasible_speed = -std::numeric_limits::infinity(); double max_feasible_speed = std::numeric_limits::infinity(); From 267813ed75f47b8fee8382373ecdc070e8729c0c Mon Sep 17 00:00:00 2001 From: RBT22 Date: Thu, 19 Dec 2024 14:48:43 +0000 Subject: [PATCH 5/5] Format code --- .../include/nav2_behaviors/plugins/drive_on_heading.hpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) 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 79ae868f98..cc0219fcfc 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -129,10 +129,7 @@ class DriveOnHeading : public TimedBehavior if (acceleration_limit_ <= 0.0 && deceleration_limit_ <= 0.0) { cmd_vel->linear.x = command_speed_; } else { - double current_speed = 0.0; - if (last_vel_) { - current_speed = *last_vel_; - } + double current_speed = last_vel_ ? *last_vel_ : 0.0; auto remaining_distance = std::fabs(command_x_) - distance; double min_feasible_speed = -std::numeric_limits::infinity(); double max_feasible_speed = std::numeric_limits::infinity();