From 7cbea942dac858e75af2eca91acda8619e17cdf3 Mon Sep 17 00:00:00 2001
From: bjoernellens1 <64093272+bjoernellens1@users.noreply.github.com>
Date: Tue, 15 Aug 2023 18:10:57 +0200
Subject: [PATCH] Update rmp220_middleware.py

optimize python code to only publish the correct messages at a rate of 100 Hz. Should cause less confusion to the controller.
---
 rmp220_middleware/rmp220_middleware.py | 42 ++++++++++++++------------
 1 file changed, 23 insertions(+), 19 deletions(-)

diff --git a/rmp220_middleware/rmp220_middleware.py b/rmp220_middleware/rmp220_middleware.py
index 4857bc1..9ccbb38 100644
--- a/rmp220_middleware/rmp220_middleware.py
+++ b/rmp220_middleware/rmp220_middleware.py
@@ -76,26 +76,30 @@ class StateMachineNode(Node):
             self.disable_chassis()
 
     def cmd_vel_callback(self, msg):
-        if self.state == State.ENABLED and (abs(msg.linear.x) >= 0.03 or abs(msg.angular.z) >= 0.03):
-            self.cmd_vel_pub.publish(msg)
-            self.timeout = 20.0  # Reset timeout when receiving commands
-            if msg.linear.x == 0.0 and msg.angular.z == 0.0:
-                # No data received on cmd_vel_mux, publish zeros
-                twist_msg = Twist()
-                self.cmd_vel_pub.publish(twist_msg)
-            else:
-                # Forward received data to cmd_vel_out
-                self.cmd_vel_pub.publish(msg)
-            self.timeout = 10.0  # Reset timeout when receiving commands
+        # if self.state == State.ENABLED and (abs(msg.linear.x) >= 0.03 or abs(msg.angular.z) >= 0.03):
+        #     self.cmd_vel_pub.publish(msg)
+        #     self.timeout = 20.0  # Reset timeout when receiving commands
+        #     if msg.linear.x == 0.0 and msg.angular.z == 0.0:
+        #         # No data received on cmd_vel_mux, publish zeros
+        #         twist_msg = Twist()
+        #         self.cmd_vel_pub.publish(twist_msg)
+        #     else:
+        #         # Forward received data to cmd_vel_out
+        #         self.cmd_vel_pub.publish(msg)
+        #     self.timeout = 10.0  # Reset timeout when receiving commands
 
-            # Forward received data to cmd_vel_out
-            self.cmd_vel_pub.publish(msg)
-            self.timeout = 10.0  # Reset timeout when receiving commands
+        #     # Forward received data to cmd_vel_out
+        #     self.cmd_vel_pub.publish(msg)
+        #     self.timeout = 10.0  # Reset timeout when receiving commands
 
-        if abs(msg.linear.x) > 0.03 or abs(msg.angular.z) > 0.03:
-            self.latest_cmd_vel = msg
-        else:
-             self.latest_cmd_vel = Twist()
+        # if abs(msg.linear.x) > 0.03 or abs(msg.angular.z) > 0.03:
+        #     self.latest_cmd_vel = msg
+        # else:
+        #      self.latest_cmd_vel = Twist()
+
+        # This method shall only update the latest_cmd_vel attribute so it can be republished by the timer_callback with 100 HZ. Should have a look at performance though.
+        self.latest_cmd_vel = msg
+        # TODO: Add setting chassis state to enabled automatically upon receiving commands (with filter maybe)
 
     def timer_callback(self):
         #self.cmd_vel_pub.publish(self.twist)
@@ -107,7 +111,7 @@ class StateMachineNode(Node):
                 self.disable_chassis()
             else:
                 self.timeout -= 0.1
-            self.cmd_vel_pub.publish(self.latest_cmd_vel)
+                self.cmd_vel_pub.publish(self.latest_cmd_vel)
         if self.state == State.DISABLED:
             self.cmd_vel_pub.publish(self.twist)