From a8e3426706e2926f6bccc93bac9c7b897a5fbacc Mon Sep 17 00:00:00 2001
From: retinfai <aferetipama@gmail.com>
Date: Wed, 6 Sep 2023 12:25:37 +1200
Subject: [PATCH 1/3] feat: add twist to ackermann

---
 src/controllers/controllers/controller.py | 28 ++++++++++++++++++++++-
 1 file changed, 27 insertions(+), 1 deletion(-)

diff --git a/src/controllers/controllers/controller.py b/src/controllers/controllers/controller.py
index ea9bdad8..bcfa097a 100644
--- a/src/controllers/controllers/controller.py
+++ b/src/controllers/controllers/controller.py
@@ -1,4 +1,5 @@
 import rclpy
+import math
 from geometry_msgs.msg import Twist
 from message_filters import Subscriber, ApproximateTimeSynchronizer
 from rclpy import Future
@@ -81,7 +82,7 @@ def get_data(self):
         data = future.result()
         return data['odom'], data['lidar']
 
-    def set_velocity(self, linear, angular):
+    def set_velocity(self, linear, angular, L):
         """
         Publish Twist messages to f1tenth cmd_vel topic
         """
@@ -91,6 +92,31 @@ def set_velocity(self, linear, angular):
 
         self.cmd_vel_pub.publish(velocity_msg)
 
+    def omega_to_ackerman(omega, linear_v, L):
+        '''
+        Convert CG angular velocity to Ackerman steering angle.
+
+        Parameters:
+        - omega: CG angular velocity in rad/s
+        - v: Vehicle speed in m/s
+        - L: Wheelbase of the vehicle in m
+
+        Returns:
+        - delta: Ackerman steering angle in radians
+
+        Derivation:
+        R = v / omega 
+        R = L / tan(delta)  equation 10 from https://www.researchgate.net/publication/228464812_Electric_Vehicle_Stability_with_Rear_Electronic_Differential_Traction#pf3
+        tan(delta) = L * omega / v
+        delta = arctan(L * omega/ v)
+        '''
+
+        delta = math.atan2((L * omega), linear_v)
+
+        return delta
+
+
+
     def sleep(self):
         while not self.timer_future.done():
             rclpy.spin_once(self)

From b63748d7360b59df9dd13d6409f1e8ad4d9091fb Mon Sep 17 00:00:00 2001
From: Peiran <124853163+AmorphisPZ@users.noreply.github.com>
Date: Wed, 6 Sep 2023 14:18:01 +1200
Subject: [PATCH 2/3] Corrected math.atan()

---
 src/controllers/controllers/controller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/controllers/controllers/controller.py b/src/controllers/controllers/controller.py
index bcfa097a..30245c93 100644
--- a/src/controllers/controllers/controller.py
+++ b/src/controllers/controllers/controller.py
@@ -111,7 +111,7 @@ def omega_to_ackerman(omega, linear_v, L):
         delta = arctan(L * omega/ v)
         '''
 
-        delta = math.atan2((L * omega), linear_v)
+        delta = math.atan((L * omega) / v)
 
         return delta
 

From 12c576cb72ba307d9b95fa7a488c55f58f07e293 Mon Sep 17 00:00:00 2001
From: Peiran <124853163+AmorphisPZ@users.noreply.github.com>
Date: Wed, 6 Sep 2023 14:19:12 +1200
Subject: [PATCH 3/3] Update controller.py

---
 src/controllers/controllers/controller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/controllers/controllers/controller.py b/src/controllers/controllers/controller.py
index 30245c93..3ce78eb8 100644
--- a/src/controllers/controllers/controller.py
+++ b/src/controllers/controllers/controller.py
@@ -111,7 +111,7 @@ def omega_to_ackerman(omega, linear_v, L):
         delta = arctan(L * omega/ v)
         '''
 
-        delta = math.atan((L * omega) / v)
+        delta = math.atan((L * omega) / linear_v)
 
         return delta