Add acceleration limits to DriveOnHeading and BackUp behaviors (backp…

…ort #4810) (#4877)

* Add acceleration limits to DriveOnHeading and BackUp behaviors (#4810)

* Add acceleration constraints

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Cleanup code

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Format code

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Add <limits> header to drive_on_heading.hpp

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Remove vel pointer

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Use the limits only if both of them is set

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Fix onActionCompletion params

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Add default acc params and change decel sign

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Add minimum speed parameter

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Update minimum speed parameter to 0.10

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Log warning when acceleration or deceleration limits are not set

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Add param sign assert

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Remove unnecessary param checking

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Refactor acceleration limits to handle forward and backward movement separately

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Fix sign checking condition

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Replace throwing with silent sign correction

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

---------

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Update parameter defaults to zero

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Add off condition

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

* Move forward outside

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

---------

Signed-off-by: RBT22 <rozgonyibalint@gmail.com>

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"
@@ -126,7 +127,35 @@ class DriveOnHeading : public TimedBehavior<ActionT>
     auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
     cmd_vel->linear.y = 0.0;
     cmd_vel->angular.z = 0.0;
-    cmd_vel->linear.x = command_speed_;
+
+    bool forward = command_speed_ > 0.0;
+    if (acceleration_limit_ == 0.0 || deceleration_limit_ == 0.0) {
+      RCLCPP_INFO_ONCE(this->logger_, "DriveOnHeading: no acceleration or deceleration limits set");
+      cmd_vel->linear.x = command_speed_;
+    } else {
+      double current_speed = last_vel_ == std::numeric_limits<double>::max() ? 0.0 : last_vel_;
+      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->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->linear.x)) {
+        cmd_vel->linear.x = forward ? max_vel_to_stop : -max_vel_to_stop;
+      }
+    }
+
+    // Ensure we don't go below minimum speed
+    if (std::fabs(cmd_vel->linear.x) < minimum_speed_) {
+      cmd_vel->linear.x = forward ? minimum_speed_ : -minimum_speed_;
+    }
 
     geometry_msgs::msg::Pose2D pose2d;
     pose2d.x = current_pose.pose.position.x;
@@ -139,11 +168,19 @@ class DriveOnHeading : public TimedBehavior<ActionT>
       return Status::FAILED;
     }
 
+    last_vel_ = cmd_vel->linear.x;
     this->vel_pub_->publish(std::move(cmd_vel));
 
     return Status::RUNNING;
   }
 
+  void onCleanup() override {last_vel_ = std::numeric_limits<double>::max();}
+
+  void onActionCompletion() override
+  {
+    last_vel_ = std::numeric_limits<double>::max();
+  }
+
 protected:
   /**
    * @brief Check if pose is collision free
@@ -197,6 +234,26 @@ 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(0.0));
+    nav2_util::declare_parameter_if_not_declared(
+      node, this->behavior_name_ + ".deceleration_limit",
+      rclcpp::ParameterValue(0.0));
+    nav2_util::declare_parameter_if_not_declared(
+      node, this->behavior_name_ + ".minimum_speed",
+      rclcpp::ParameterValue(0.0));
+    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_;
@@ -207,6 +264,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