From 86931121e732c7c214bfcc24e309584fd21fd62f Mon Sep 17 00:00:00 2001 From: bjoernellens1 <64093272+bjoernellens1@users.noreply.github.com> Date: Thu, 16 Jan 2025 13:24:13 +0100 Subject: [PATCH] Update rmp220_middleware_extended.py --- .../rmp220_middleware_extended.py | 180 ++++++++++++++++++ 1 file changed, 180 insertions(+) diff --git a/rmp220_middleware/rmp220_middleware_extended.py b/rmp220_middleware/rmp220_middleware_extended.py index 8b13789..983b78b 100644 --- a/rmp220_middleware/rmp220_middleware_extended.py +++ b/rmp220_middleware/rmp220_middleware_extended.py @@ -1 +1,181 @@ +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 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) + + # Service client initialization with a check + self.chassis_enable_client = None # Ensure it's initialized to None + try: + rospy.wait_for_service('set_chassis_enable', timeout=5) + self.chassis_enable_client = rospy.ServiceProxy('set_chassis_enable', RosSetChassisEnableCmdSrv) + 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 + + # Initialize JoystickServiceCaller + self.joystick_service_caller = JoystickServiceCaller() + + 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...')