ellensohn/rmp220_middleware
1
0
Fork 0
mirror of https://github.com/bjoernellens1/rmp220_middleware.git synced 2025-01-18 16:16:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

Update rmp220_middleware.py fix node attributes

Browse Source
This commit is contained in:
bjoernellens1 2025-01-16 13:11:21 +01:00 committed by GitHub
parent 7155eb3449
commit 90a6d1d9f9
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 35 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
rmp220_middleware/rmp220_middleware.py
View File

@ -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'