CMakeLists.txt

@@ -23,7 +23,6 @@ catkin_package(
 # Install Python scripts
 catkin_install_python(PROGRAMS
   rmp220_middleware/rmp220_middleware.py  # Replace with the actual script path
-  rmp220_middleware/rmp220_middleware_extended.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

rmp220_middleware/rmp220_middleware.py

@@ -1,42 +1,131 @@
+#!/usr/bin/env python
+
 import rospy
+from std_msgs.msg import Bool
+from geometry_msgs.msg import Twist
 from sensor_msgs.msg import Joy
-from segway_msgs.srv import ros_set_chassis_enable_cmd as RosSetChassisEnableCmdSrv  # Replace with the actual service type
+from enum import Enum
+from segway_msgs.srv import RosSetChassisEnableCmdSrv as RosSetChassisEnableCmd
+from segway_msgs.msg import ChassisModeFb
 
-class JoystickServiceCaller:
+class State(Enum):
+    DISABLED = 0  # solid yellow
+    ENABLED = 1  # solid green
+    PASSIVE = 2  # solid white (push)
+    STOPPED = 3  # solid red
+    PAUSED = 4  # no extra visual feedback, solid yellow
+
+class StateMachineNode:
     def __init__(self):
-        rospy.init_node('joystick_service_caller')
+        rospy.init_node('state_machine_node', anonymous=True)
 
-        # Map buttons for select and start
+        # Initialize state and other variables
-        self.select_button = 6  # Select button index
+        self.state = State.DISABLED
-        self.start_button = 7   # Start button index
+        self.timeout = 20.0  # Timeout in seconds
 
-        # Initialize service proxy
+        self.twist = Twist()
-        self.service_proxy = rospy.ServiceProxy('/ros_set_chassis_enable_cmd_srv', RosSetChassisEnableCmdSrv)
+        self.latest_cmd_vel = Twist()
+        self.abs_x = 0.0
+        self.abs_z = 0.0
 
-        # Subscribe to the joystick topic
+        # Publishers and subscribers
-        rospy.Subscriber('/joy', Joy, self.joy_callback)
+        self.cmd_vel_pub = rospy.Publisher('/cmd_vel_out', Twist, queue_size=10)
+        self.cmd_vel_sub = rospy.Subscriber('/cmd_vel_mux', Twist, self.cmd_vel_callback)
+        self.joy_sub = rospy.Subscriber('/joy', Joy, self.joy_callback)
+        self.chassis_mode_sub = rospy.Subscriber('/chassis_mode_fb', ChassisModeFb, self.chassis_mode_callback)
 
-        rospy.loginfo("Joystick Service Caller Node Initialized")
+        # 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.')
+
+        # 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}')
+
+    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}')
+
+    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}')
 
     def joy_callback(self, msg):
-        try:
+        start_button = msg.buttons[7]  # Joystick button 'start'
-            if msg.buttons[self.select_button] == 1:  # Select button pressed
+        select_button = msg.buttons[6]  # Joystick button 'select'
-                self.call_service(False)
-            elif msg.buttons[self.start_button] == 1:  # Start button pressed
-                self.call_service(True)
-        except rospy.ServiceException as e:
-            rospy.logerr(f"Service call failed: {e}")
-        except IndexError as e:
-            rospy.logerr(f"Invalid button index: {e}")
 
-    def call_service(self, enable):
+        if start_button == 1:
-        rospy.wait_for_service('/ros_set_chassis_enable_cmd_srv')
+            rospy.loginfo("State: ENABLED (Button 'start')")
-        response = self.service_proxy(ros_set_chassis_enable_cmd=enable)
+            self.enable_chassis()
-        rospy.loginfo(f"Service called with enable={enable}, response: {response}")
+            self.timeout = 20
+
+        if select_button == 1:
+            rospy.loginfo("State: DISABLED (Button 'select')")
+            self.pause_chassis()
+
+    def cmd_vel_callback(self, msg):
+        self.latest_cmd_vel = msg
+        self.abs_x = abs(msg.linear.x)
+        self.abs_z = abs(msg.angular.z)
+        self.timeout = 20.0
+
+    def timer_callback(self, event):
+        if self.state in [State.PAUSED, State.STOPPED, State.PASSIVE]:
+            return
+
+        if self.state == State.ENABLED:
+            if self.timeout <= 0:
+                self.state = State.DISABLED
+                rospy.loginfo("State: DISABLED (Timeout)")
+                self.disable_chassis()
+            else:
+                self.timeout -= 0.01
+                self.cmd_vel_pub.publish(self.latest_cmd_vel)
+
+        if self.state == State.DISABLED and (self.abs_x > 0.002 or self.abs_z > 0.002):
+            self.state = State.ENABLED
+            rospy.loginfo("State: ENABLED (cmd_vel)")
+            self.enable_chassis()
 
 if __name__ == '__main__':
+    node = StateMachineNode()
     try:
-        node = JoystickServiceCaller()
         rospy.spin()
     except rospy.ROSInterruptException:
-        rospy.loginfo("Joystick Service Caller Node Shutting Down")
+        pass
+    finally:
+        node.disable_chassis()
+        rospy.loginfo('Shutting down node...')

rmp220_middleware/rmp220_middleware_extended.py

@@ -1,182 +0,0 @@
-import rospy
-from sensor_msgs.msg import Joy
-from geometry_msgs.msg import Twist
-from segway_msgs.srv import ros_set_chassis_enable_cmd as RosSetChassisEnableCmdSrv
-from segway_msgs.msg import chassis_mode_fb as ChassisModeFb  # Ensure this is the correct message type
-from enum import Enum
-
-# Define State enum for states management
-class State(Enum):
-    DISABLED = 0
-    ENABLED = 1
-    PAUSED = 2
-    STOPPED = 3
-    PASSIVE = 4
-
-class JoystickServiceCaller:
-    def __init__(self):
-        #rospy.init_node('joystick_service_caller')
-
-        # Map buttons for select and start
-        self.select_button = 6  # Select button index
-        self.start_button = 7   # Start button index
-
-        # Initialize service proxy
-        self.service_proxy = rospy.ServiceProxy('/ros_set_chassis_enable_cmd_srv', RosSetChassisEnableCmdSrv)
-
-        # Subscribe to the joystick topic
-        rospy.Subscriber('/joy', Joy, self.joy_callback)
-
-        rospy.loginfo("Joystick Service Caller Node Initialized")
-
-    def joy_callback(self, msg):
-        try:
-            if msg.buttons[self.select_button] == 1:  # Select button pressed
-                self.call_service(False)
-            elif msg.buttons[self.start_button] == 1:  # Start button pressed
-                self.call_service(True)
-        except rospy.ServiceException as e:
-            rospy.logerr(f"Service call failed: {e}")
-        except IndexError as e:
-            rospy.logerr(f"Invalid button index: {e}")
-
-    def call_service(self, enable):
-        rospy.wait_for_service('/ros_set_chassis_enable_cmd_srv')
-        response = self.service_proxy(ros_set_chassis_enable_cmd=enable)
-        rospy.loginfo(f"Service called with enable={enable}, response: {response}")
-
-class StateMachineNode:
-    def __init__(self):
-        rospy.init_node('state_machine_node', anonymous=True)
-
-        # Initialize state and other variables
-        self.state = State.DISABLED
-        self.timeout = 20.0  # Timeout in seconds
-
-        self.twist = Twist()
-        self.latest_cmd_vel = Twist()
-        self.abs_x = 0.0
-        self.abs_z = 0.0
-
-        # Publishers and subscribers
-        self.cmd_vel_pub = rospy.Publisher('/cmd_vel_out', Twist, queue_size=10)
-        self.cmd_vel_sub = rospy.Subscriber('/cmd_vel_mux', Twist, self.cmd_vel_callback)
-        self.joy_sub = rospy.Subscriber('/joy', Joy, self.joy_callback)
-        self.chassis_mode_sub = rospy.Subscriber('/chassis_mode_fb', ChassisModeFb, self.chassis_mode_callback)
-
-        # Initialize JoystickServiceCaller
-        self.joystick_service_caller = JoystickServiceCaller()
-
-         #None  # Ensure it's initialized to None
-
-        try:
-            #rospy.wait_for_service('set_chassis_enable', timeout=5)
-            # Service client initialization with a check
-            self.chassis_enable_client = self.joystick_service_caller.service_proxy
-            rospy.loginfo('Chassis enable service available.')
-        except rospy.ROSException as e:
-            rospy.logerr(f"Failed to wait for 'set_chassis_enable' service: {e}")
-
-        # Timer for periodic updates
-        self.timer = rospy.Timer(rospy.Duration(0.01), self.timer_callback)  # 100 Hz
-
-    def chassis_mode_callback(self, msg):
-        """
-        Callback function for handling the chassis mode feedback.
-        Sets the state of the robot based on the chassis mode.
-        """
-        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):
-        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):
-        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):
-        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'
-        select_button = msg.buttons[6]  # Joystick button 'select'
-
-        if start_button == 1:
-            rospy.loginfo("State: ENABLED (Button 'start')")
-            self.enable_chassis()
-            self.timeout = 20
-
-        if select_button == 1:
-            rospy.loginfo("State: DISABLED (Button 'select')")
-            self.pause_chassis()
-
-    def cmd_vel_callback(self, msg):
-        self.latest_cmd_vel = msg
-        self.abs_x = abs(msg.linear.x)
-        self.abs_z = abs(msg.angular.z)
-        self.timeout = 20.0
-
-    def timer_callback(self, event):
-        if self.state in [State.PAUSED, State.STOPPED, State.PASSIVE]:
-            return
-
-        if self.state == State.ENABLED:
-            if self.timeout <= 0:
-                self.state = State.DISABLED
-                rospy.loginfo("State: DISABLED (Timeout)")
-                self.disable_chassis()
-            else:
-                self.timeout -= 0.01
-                self.cmd_vel_pub.publish(self.latest_cmd_vel)
-
-        if self.state == State.DISABLED and (self.abs_x > 0.002 or self.abs_z > 0.002):
-            self.state = State.ENABLED
-            rospy.loginfo("State: ENABLED (cmd_vel)")
-            self.enable_chassis()
-
-if __name__ == '__main__':
-    node = StateMachineNode()
-    try:
-        rospy.spin()
-    except rospy.ROSInterruptException:
-        pass
-    finally:
-        node.disable_chassis()
-        rospy.loginfo('Shutting down node...')