From 90a6d1d9f947a6a2eeb289de802d04613c63c06a Mon Sep 17 00:00:00 2001
From: bjoernellens1 <64093272+bjoernellens1@users.noreply.github.com>
Date: Thu, 16 Jan 2025 13:11:21 +0100
Subject: [PATCH] Update rmp220_middleware.py fix node attributes

---
 rmp220_middleware/rmp220_middleware.py | 74 ++++++++++++--------------
 1 file changed, 35 insertions(+), 39 deletions(-)

diff --git a/rmp220_middleware/rmp220_middleware.py b/rmp220_middleware/rmp220_middleware.py
index e0dfc49..cbb08bb 100644
--- a/rmp220_middleware/rmp220_middleware.py
+++ b/rmp220_middleware/rmp220_middleware.py
@@ -34,54 +34,50 @@ class StateMachineNode:
         self.joy_sub = rospy.Subscriber('/joy', Joy, self.joy_callback)
         self.chassis_mode_sub = rospy.Subscriber('/chassis_mode_fb', ChassisModeFb, self.chassis_mode_callback)
 
-        # Service clients
-        rospy.wait_for_service('set_chassis_enable')
-        self.chassis_enable_client = rospy.ServiceProxy('set_chassis_enable', RosSetChassisEnableCmd)
-        rospy.loginfo('Chassis enable service available.')
+        # Service client initialization with a check
+        try:
+            rospy.wait_for_service('set_chassis_enable', timeout=5)
+            self.chassis_enable_client = rospy.ServiceProxy('set_chassis_enable', RosSetChassisEnableCmd)
+            rospy.loginfo('Chassis enable service available.')
+        except rospy.ROSException as e:
+            rospy.logerr(f"Failed to wait for 'set_chassis_enable' service: {e}")
+            self.chassis_enable_client = None
 
         # Timer for periodic updates
         self.timer = rospy.Timer(rospy.Duration(0.01), self.timer_callback)  # 100 Hz
 
-    def chassis_mode_callback(self, msg):
-        if self.state == State.PAUSED:
-            return
-
-        if msg.chassis_mode == 0:
-            self.state = State.DISABLED
-            rospy.loginfo('Set chassis_mode to DISABLED')
-        elif msg.chassis_mode == 1:
-            self.state = State.ENABLED
-            rospy.loginfo('Set chassis_mode to ENABLED')
-        elif msg.chassis_mode == 2:
-            self.state = State.PASSIVE
-            rospy.loginfo('Set chassis_mode to PASSIVE')
-        elif msg.chassis_mode == 3:
-            self.state = State.STOPPED
-            rospy.loginfo('Set chassis_mode to STOPPED')
-
     def enable_chassis(self):
-        try:
-            self.chassis_enable_client(True)
-            self.state = State.ENABLED
-            rospy.loginfo('Enabling chassis...')
-        except rospy.ServiceException as e:
-            rospy.logerr(f'Failed to enable chassis: {e}')
+        if self.chassis_enable_client:
+            try:
+                self.chassis_enable_client(True)
+                self.state = State.ENABLED
+                rospy.loginfo('Enabling chassis...')
+            except rospy.ServiceException as e:
+                rospy.logerr(f'Failed to enable chassis: {e}')
+        else:
+            rospy.logerr('Chassis enable client not initialized!')
 
     def pause_chassis(self):
-        try:
-            self.chassis_enable_client(False)
-            self.state = State.PAUSED
-            rospy.loginfo('Pausing chassis...')
-        except rospy.ServiceException as e:
-            rospy.logerr(f'Failed to pause chassis: {e}')
+        if self.chassis_enable_client:
+            try:
+                self.chassis_enable_client(False)
+                self.state = State.PAUSED
+                rospy.loginfo('Pausing chassis...')
+            except rospy.ServiceException as e:
+                rospy.logerr(f'Failed to pause chassis: {e}')
+        else:
+            rospy.logerr('Chassis enable client not initialized!')
 
     def disable_chassis(self):
-        try:
-            self.chassis_enable_client(False)
-            self.state = State.DISABLED
-            rospy.loginfo('Disabling chassis...')
-        except rospy.ServiceException as e:
-            rospy.logerr(f'Failed to disable chassis: {e}')
+        if self.chassis_enable_client:
+            try:
+                self.chassis_enable_client(False)
+                self.state = State.DISABLED
+                rospy.loginfo('Disabling chassis...')
+            except rospy.ServiceException as e:
+                rospy.logerr(f'Failed to disable chassis: {e}')
+        else:
+            rospy.logerr('Chassis enable client not initialized!')
 
     def joy_callback(self, msg):
         start_button = msg.buttons[7]  # Joystick button 'start'