Update rmp220_middleware.py

rmp220_middleware/rmp220_middleware.py

@@ -1,150 +1,42 @@
-#!/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 enum import Enum
+from your_package.srv import RosSetChassisEnableCmdSrv  # Replace with the actual service type
-from segway_msgs.srv import ros_set_chassis_enable_cmd as RosSetChassisEnableCmd
-from segway_msgs.msg import chassis_mode_fb as ChassisModeFb
 
-class State(Enum):
+class JoystickServiceCaller:
-    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('state_machine_node', anonymous=True)
+        rospy.init_node('joystick_service_caller')
 
-        # Initialize state and other variables
+        # Map buttons for select and start
-        self.state = State.DISABLED
+        self.select_button = 6  # Select button index
-        self.timeout = 20.0  # Timeout in seconds
+        self.start_button = 7   # Start button index
 
-        self.twist = Twist()
+        # Initialize service proxy
-        self.latest_cmd_vel = Twist()
+        self.service_proxy = rospy.ServiceProxy('/ros_set_chassis_enable_cmd_srv', RosSetChassisEnableCmdSrv)
-        self.abs_x = 0.0
-        self.abs_z = 0.0
 
-        # Publishers and subscribers
+        # Subscribe to the joystick topic
-        self.cmd_vel_pub = rospy.Publisher('/cmd_vel_out', Twist, queue_size=10)
+        rospy.Subscriber('/joy', Joy, self.joy_callback)
-        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)
 
-        # Service client initialization with a check
+        rospy.loginfo("Joystick Service Caller Node Initialized")
-        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):
-        """
-        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'
+        try:
-        select_button = msg.buttons[6]  # Joystick button 'select'
+            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}")
 
-        if start_button == 1:
+    def call_service(self, enable):
-            rospy.loginfo("State: ENABLED (Button 'start')")
+        rospy.wait_for_service('/ros_set_chassis_enable_cmd_srv')
-            self.enable_chassis()
+        response = self.service_proxy(ros_set_chassis_enable_cmd=enable)
-            self.timeout = 20
+        rospy.loginfo(f"Service called with enable={enable}, response: {response}")
-
-        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:
-        pass
+        rospy.loginfo("Joystick Service Caller Node Shutting Down")
-    finally:
-        node.disable_chassis()
-        rospy.loginfo('Shutting down node...')