Add acceleration limits to DriveOnHeading and BackUp behaviors

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<nav2_msgs::action::DriveOnHeading>
 {
@@ -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"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <DriveOnHeading dist_to_travel="2" speed="0.26" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(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<double> linear_vels;
+  auto subscription_stamped = node_->create_subscription<geometry_msgs::msg::TwistStamped>(
+    "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 =

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

@@ -19,6 +19,7 @@
 #include <chrono>
 #include <memory>
 #include <utility>
+#include <limits>
 
 #include "nav2_behaviors/timed_behavior.hpp"
 #include "nav2_msgs/action/drive_on_heading.hpp"
@@ -133,7 +134,35 @@ class DriveOnHeading : public TimedBehavior<ActionT>
     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<double>::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<ActionT>
       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<ActionT>
    */
   CostmapInfoType getResourceInfo() override {return CostmapInfoType::LOCAL;}
 
+  void onCleanup() override {last_vel_ = std::numeric_limits<double>::max();}
+
+  void onActionCompletion(std::shared_ptr<typename ActionT::Result>/*result*/)
+  override
+  {
+    last_vel_ = std::numeric_limits<double>::max();
+  }
+
 protected:
   /**
    * @brief Check if pose is collision free
@@ -214,6 +252,19 @@ class DriveOnHeading : public TimedBehavior<ActionT>
       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<ActionT>
   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<double>::max();
 };
 
 }  // namespace nav2_behaviors

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: