From 5cb0be703fe11b5452392e18bfdc9cbe3dc75a71 Mon Sep 17 00:00:00 2001
From: Daniel Neurath <daniel.neurath@axelspringer.com>
Date: Tue, 20 Jun 2023 11:52:23 +0200
Subject: [PATCH] twist message smoothing (needs to be tested)

---
 src/dcaitirobot/dcaitirobot/twist.py | 86 ++++++++++++++++++++--------
 1 file changed, 63 insertions(+), 23 deletions(-)

diff --git a/src/dcaitirobot/dcaitirobot/twist.py b/src/dcaitirobot/dcaitirobot/twist.py
index 7fd9f5a..fab6d26 100644
--- a/src/dcaitirobot/dcaitirobot/twist.py
+++ b/src/dcaitirobot/dcaitirobot/twist.py
@@ -7,54 +7,94 @@ from sensor_msgs.msg import Joy
 
 import numpy as np
 
-SCALE_LINEAR_VELOCITY = 0.5
-SCALE_ANGULAR_VELOCITY = 0.4
-PUBLISH_FREQUENCY_HZ = 10
+SCALE_LINEAR_VELOCITY = 0.1
+SCALE_ANGULAR_VELOCITY = 0.1
+PUBLISH_FREQUENCY_HZ = 30
 
+SMOOTH_WEIGHTS = np.array([1, 2, 4, 10, 20])
 
-def calc_speed_from_joy(x, y):    
-    linear = -y * SCALE_LINEAR_VELOCITY
-    angular = x * SCALE_ANGULAR_VELOCITY
-
-    return linear, angular
 
 class TwistPublisher(Node):
+    def __init__(
+        self,
+        smooth_weights=np.ones(5),
+        linear_scale=1.0,
+        angular_scale=1.0,
+        publish_frequency_hz=30,
+    ):
+        super().__init__("twist_publisher")
 
-    def __init__(self):
-        super().__init__('twist_publisher')
-        self.linear = 0.0
-        self.angular = 0.0
-        self.publisher_ = self.create_publisher(Twist, '/diff_cont/cmd_vel_unstamped', 1)
+        self.linear_window = np.zeros(smooth_weights.shape[0])
+        self.angular_window = np.zeros(smooth_weights.shape[0])
+        self.linear_recent = 0.0
+        self.angular_recent = 0.0
+
+        self.linear_out = 0.0
+        self.angular_out = 0.0
+
+        self.weights = smooth_weights / smooth_weights.sum()
+        self.window_size = smooth_weights.shape[0]
+
+        self.linear_scale = linear_scale
+        self.angular_scale = angular_scale
+
+        self.publisher_ = self.create_publisher(
+            Twist, "/diff_cont/cmd_vel_unstamped", 1
+        )
         self.create_subscription(Joy, "/joy", self.joy_callback, 1)
-        self.timer = self.create_timer(1/PUBLISH_FREQUENCY_HZ, self.timer_callback)
-
+        self.timer = self.create_timer(1 / publish_frequency_hz, self.timer_callback)
 
     def joy_callback(self, joy_message):
         x, y = joy_message.axes[:2]
+        self.linear_recent = -y * SCALE_LINEAR_VELOCITY
+        self.angular_recent = x * SCALE_ANGULAR_VELOCITY
 
-        self.linear, self.angular = calc_speed_from_joy(x,y)
+    def update_smooth_window(self):
+        self.linear_window = np.append(self.linear_window, self.linear_recent)[
+            -self.window_size :
+        ]
+        self.angular_window = np.append(self.angular_window, self.angular_recent)[
+            -self.window_size :
+        ]
+
+    def calc_smooth_speed(self):
+        self.linear_out = np.average(self.linear_window, weights=self.weights)
+        self.angular_out = np.average(self.angular_window, weights=self.weights)
 
     def timer_callback(self):
         msg = Twist()
+
+        self.update_smooth_window()
+        self.calc_smooth_speed()
+
         msg.linear.x = self.linear
         msg.angular.z = self.angular
         self.publisher_.publish(msg)
-        self.get_logger().info('Publishing: "%s"' % msg)
+        self.get_logger().info(
+            "Linear_window: {}, Linear_out: {}, Angular_window: {}, Angular_out: {}".format(
+                self.linear_window,
+                self.linear_out,
+                self.angular_window,
+                self.angular_out,
+            )
+        )
 
 
 def main(args=None):
     rclpy.init(args=args)
 
-    minimal_publisher = TwistPublisher()
+    minimal_publisher = TwistPublisher(
+        SMOOTH_WEIGHTS,
+        SCALE_LINEAR_VELOCITY,
+        SCALE_ANGULAR_VELOCITY,
+        PUBLISH_FREQUENCY_HZ,
+    )
 
     rclpy.spin(minimal_publisher)
 
-    # Destroy the node explicitly
-    # (optional - otherwise it will be done automatically
-    # when the garbage collector destroys the node object)
     minimal_publisher.destroy_node()
     rclpy.shutdown()
 
 
-if __name__ == '__main__':
-    main()
\ No newline at end of file
+if __name__ == "__main__":
+    main()