From f5e08389e2431ffbd9c5e02f0cead7871eeaf803 Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+pimpest@users.noreply.github.com>
Date: Wed, 4 Mar 2026 20:35:46 +0100
Subject: [PATCH] Revized simple rotate for easier redability

---
 toid_behaviors/src/simple_rotate.cpp | 25 ++++++++++---------------
 1 file changed, 10 insertions(+), 15 deletions(-)

diff --git a/toid_behaviors/src/simple_rotate.cpp b/toid_behaviors/src/simple_rotate.cpp
index a084b4b..42a9bb2 100644
--- a/toid_behaviors/src/simple_rotate.cpp
+++ b/toid_behaviors/src/simple_rotate.cpp
@@ -53,30 +53,25 @@ ResultStatus SimpleRotateBehavior::updateVel(
   geometry_msgs::msg::Twist & out_vel)
 {
   const double current_yaw = tf2::getYaw(pose.orientation);
-  const double angle_change = angles::normalize_angle(current_yaw - last_angle_);
+  const double angle_change = angles::shortest_angular_distance(last_angle_ , current_yaw);
   last_angle_ = current_yaw;
 
+  double err = angles::shortest_angular_distance(current_yaw, target_angle_);
+  double sign = (err < 0)? -1.0 : 1.0;
+  err = std::abs(err);
+
   if (min_turn_angle_ > 0.0) {
-    min_turn_angle_ = fmax(0.0, min_turn_angle_ - initial_direction_ * angle_change);
+    min_turn_angle_ = std::max(0.0, min_turn_angle_ - initial_direction_ * angle_change);
+    err = std::max( initial_direction_ * sign * err, 0.0);
+    err = std::max(min_turn_angle_, err);
+    sign = initial_direction_;
   }
 
   const double upper_vel_ = angular_speed_ + max_angular_accel_ * control_duration_;
   const double lower_vel_ = angular_speed_ - max_angular_accel_ * control_duration_;
 
-  double anglular_distance_to_target =
-    angles::shortest_angular_distance(current_yaw, target_angle_);
-  double sign = (anglular_distance_to_target >= 0.0) ? 1.0 : -1.0;
-
-  if (min_turn_angle_ > 0.0 && initial_direction_ != sign) {
-    anglular_distance_to_target = angles::two_pi_complement(anglular_distance_to_target);
-    sign = initial_direction_;
-  }
-
-  const double angular_distance_to_heading =
-    sign * fmax(std::fabs(anglular_distance_to_target), min_turn_angle_);
-
   const double speed_until_overshoot =
-    std::sqrt(2.0 * max_angular_accel_ * std::fabs(angular_distance_to_heading));
+    std::sqrt(2.0 * max_angular_accel_ * std::fabs(err));
 
   const double requested_speed = sign * std::min(speed_until_overshoot, max_angular_speed_);
   const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_);