From 13bd5e363913aed3e402ddee5b2b1cef001f35b7 Mon Sep 17 00:00:00 2001
From: Dominik Moss <dominik.moss@ipa.fraunhofer.de>
Date: Mon, 26 Jan 2026 09:57:52 +0100
Subject: [PATCH] Added velocity dependent omega accel limits

---
 .../velocity_smoother.hpp                     | 10 +++++++
 .../src/velocity_smoother.cpp                 | 27 +++++++++++++++++--
 2 files changed, 35 insertions(+), 2 deletions(-)

diff --git a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp
index f69a827a7db..fd83faad0cb 100644
--- a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp
+++ b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp
@@ -125,6 +125,13 @@ class VelocitySmoother : public nav2_util::LifecycleNode
    */
   void smootherTimer();
 
+  /**
+  * @brief Get omega acceleration limit based on current linear velocity
+  * @param lin_vel current linear velocity
+  * @return omega acceleration limit
+  */
+  double getCurrentOmegaAccelLimit(const double lin_vel);
+
   /**
    * @brief Dynamic reconfigure callback
    * @param parameters Parameter list to change
@@ -157,6 +164,9 @@ class VelocitySmoother : public nav2_util::LifecycleNode
   rclcpp::Duration velocity_timeout_{0, 0};
   rclcpp::Time last_command_time_;
 
+  double d_theta_max_;
+  double wheel_base_;
+
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
 };
 
diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp
index 7631c66f176..50c9611b3e5 100644
--- a/nav2_velocity_smoother/src/velocity_smoother.cpp
+++ b/nav2_velocity_smoother/src/velocity_smoother.cpp
@@ -120,6 +120,12 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state)
   node->get_parameter("velocity_timeout", velocity_timeout_dbl);
   velocity_timeout_ = rclcpp::Duration::from_seconds(velocity_timeout_dbl);
 
+  // Extra parameter for variable omega acceleration limits
+  declare_parameter_if_not_declared(node, "d_theta_max", rclcpp::ParameterValue(0.2));
+  declare_parameter_if_not_declared(node, "wheel_base", rclcpp::ParameterValue(1.87));
+  node->get_parameter("d_theta_max", d_theta_max_);
+  node->get_parameter("wheel_base", wheel_base_);
+  
   if (max_velocities_.size() != 3 || min_velocities_.size() != 3 ||
     max_accels_.size() != 3 || max_decels_.size() != 3 || deadband_velocities_.size() != 3)
   {
@@ -248,6 +254,23 @@ void VelocitySmoother::inputCommandCallback(
   inputCommandStampedCallback(twist_stamped);
 }
 
+double VelocitySmoother::getCurrentOmegaAccelLimit(const double lin_vel)
+{
+  // Impact of the current theta is not that big in our range [-23°, 23°] (cos(theta) in range [1, 0.92])
+  // Since we do not have that info currently we use a medium value of 16° (cos(theta)=0.96)
+  double current_theta = 0.28;
+  // The dependency on the velocity is more significant
+  
+  // Einspurmodell
+  return d_theta_max_ * lin_vel / ( std::pow(std::cos(current_theta), 2) * wheel_base_ );
+  
+  // Zweispurmodell?
+  // d_omega_max_outside = theta_max_ * lin_vel * wheel_base_ / ( std::pow(std::cos(current_theta),2) * std::pow((wheel_base_ + track_width_/2 * std::tan(current_theta)), 2) );
+  // d_omega_max_inside = theta_max_ * lin_vel * wheel_base_ / ( std::pow(std::cos(current_theta),2) * (wheel_base_ - track_width_/2  * std::tan(current_theta)), 2) );
+  // return std::min(d_omega_max_outside, d_omega_max_inside);
+}
+
+
 double VelocitySmoother::findEtaConstraint(
   const double v_curr, const double v_cmd, const double accel, const double decel)
 {
@@ -383,7 +406,7 @@ void VelocitySmoother::smootherTimer()
     }
 
     curr_eta = findEtaConstraint(
-      current_.twist.angular.z, command_->twist.angular.z, max_accels_[2], max_decels_[2]);
+      current_.twist.angular.z, command_->twist.angular.z, getCurrentOmegaAccelLimit(current_.twist.linear.x), -getCurrentOmegaAccelLimit(current_.twist.linear.x));
     if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) {
       eta = curr_eta;
     }
@@ -394,7 +417,7 @@ void VelocitySmoother::smootherTimer()
   cmd_vel->twist.linear.y = applyConstraints(
     current_.twist.linear.y, command_->twist.linear.y, max_accels_[1], max_decels_[1], eta);
   cmd_vel->twist.angular.z = applyConstraints(
-    current_.twist.angular.z, command_->twist.angular.z, max_accels_[2], max_decels_[2], eta);
+    current_.twist.angular.z, command_->twist.angular.z, getCurrentOmegaAccelLimit(current_.twist.linear.x), -getCurrentOmegaAccelLimit(current_.twist.linear.x), eta);
   last_cmd_ = *cmd_vel;
 
   // Apply deadband restrictions & publish