diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp index 0c53fd54c8..dfee124ef2 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp @@ -22,6 +22,11 @@ #include "nav2_behavior_tree/utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/twist_subscriber.hpp" class DriveOnHeadingActionServer : public TestActionServer { @@ -167,6 +172,50 @@ TEST_F(DriveOnHeadingActionTestFixture, test_tick) EXPECT_EQ(goal->speed, 0.26f); } +TEST_F(DriveOnHeadingActionTestFixture, test_velocity_constraints) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + node_->declare_parameter("DriveOnHeading.acceleration_limit", 0.1); + node_->declare_parameter("DriveOnHeading.deceleration_limit", -0.1); + node_->declare_parameter("DriveOnHeading.minimum_speed", 0.01); + + std::vector linear_vels; + auto subscription_stamped = node_->create_subscription( + "cmd_vel", + 1, + [&](geometry_msgs::msg::TwistStamped msg) { + linear_vels.push_back(msg.twist.linear.x); + }); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + rclcpp::spin_some(node_); + } + + EXPECT_GT(linear_vels.size(), 0); + for (unsigned int i = 0; i != linear_vels.size(); i++) { + if (i > 0) { + double diff = linear_vels[i] - linear_vels[i - 1]; + double accel = diff / 0.01; // bt_loop_duration is 10ms + + EXPECT_TRUE(accel <= 0.1); + EXPECT_TRUE(accel >= -0.1); + } + + EXPECT_TRUE(linear_vels[i] >= 0.01); + } + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); +} + TEST_F(DriveOnHeadingActionTestFixture, test_failure) { std::string xml_txt = 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 8a37f26a63..0bfe8f2e0a 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" @@ -133,7 +134,35 @@ 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_; + if (acceleration_limit_ <= 0.0 || deceleration_limit_ >= 0.0) { + cmd_vel->twist.linear.x = command_speed_; + } else { + double current_speed = last_vel_ == std::numeric_limits::max() ? 0.0 : last_vel_; + + double min_feasible_speed = current_speed + deceleration_limit_ / this->cycle_frequency_; + double max_feasible_speed = current_speed + acceleration_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; + bool forward = command_speed_ > 0.0; + if (forward) { + double max_vel_to_stop = std::sqrt(-2.0 * deceleration_limit_ * remaining_distance); + if (max_vel_to_stop < cmd_vel->twist.linear.x) { + cmd_vel->twist.linear.x = max_vel_to_stop; + } + } else { + double max_vel_to_stop = -std::sqrt(2.0 * acceleration_limit_ * remaining_distance); + if (max_vel_to_stop > cmd_vel->twist.linear.x) { + cmd_vel->twist.linear.x = 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; @@ -146,6 +175,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}; @@ -157,6 +187,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 @@ -214,6 +252,19 @@ 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.01)); + 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_); } typename ActionT::Feedback::SharedPtr feedback_; @@ -225,6 +276,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 a5e4a539c0..6ab61e4ec7 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -317,8 +317,14 @@ behavior_server: plugin: "nav2_behaviors::Spin" backup: plugin: "nav2_behaviors::BackUp" + acceleration_limit: 2.5 + deceleration_limit: -2.5 + minimum_speed: 0.01 drive_on_heading: plugin: "nav2_behaviors::DriveOnHeading" + acceleration_limit: 2.5 + deceleration_limit: -2.5 + minimum_speed: 0.01 wait: plugin: "nav2_behaviors::Wait" assisted_teleop: