From 913fca91bad40cbd6738fc9296a5bb96ba0a2217 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Mon, 6 Apr 2020 17:35:19 +0200 Subject: [PATCH] Added an unknown tag to the gait instruction message to set the state machine back to state UNKNOWN. Also added a button which has a callback to publish this tag. --- .../src/march_rqt_input_device/input_device_controller.py | 5 +++++ .../src/march_rqt_input_device/input_device_view.py | 5 ++++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/march_rqt_input_device/src/march_rqt_input_device/input_device_controller.py b/march_rqt_input_device/src/march_rqt_input_device/input_device_controller.py index 22128e4..098bb5c 100644 --- a/march_rqt_input_device/src/march_rqt_input_device/input_device_controller.py +++ b/march_rqt_input_device/src/march_rqt_input_device/input_device_controller.py @@ -57,3 +57,8 @@ def publish_error(self): rospy.logdebug('Mock Input Device published error') self.error_pub.publish(Error(std_msgs.msg.Header(stamp=rospy.Time.now()), 'Fake error thrown by the develop input device.', Error.FATAL)) + + def publish_sm_to_unknown(self): + rospy.logdebug('Mock Input Device published state machine to unknown') + self.instruction_gait_pub.publish(GaitInstruction(std_msgs.msg.Header(stamp=rospy.Time.now()), + GaitInstruction.UNKNOWN, '')) diff --git a/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py b/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py index 06e4bae..01f69bb 100644 --- a/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py +++ b/march_rqt_input_device/src/march_rqt_input_device/input_device_view.py @@ -225,11 +225,14 @@ def __init__(self, context): error_button = self.create_button('error', image_path='/error.png', callback=lambda: self.controller.publish_error()) + sm_to_unknown_button = self.create_button('force unknown', + callback=lambda: self.controller.publish_sm_to_unknown()) + # The button layout. # Position in the array determines position on screen. march_button_layout = [ - [home_sit, home_stand, gait_walk, gait_walk_small, gait_walk_large], + [home_sit, home_stand, gait_walk, gait_walk_small, gait_walk_large, sm_to_unknown_button], [gait_sit, gait_stand, rocker_switch_increment, rocker_switch_decrement, stop_button, error_button],