From 77a5a57c92c5af3bd006362ae1578ef9ffebdac5 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Sat, 30 Sep 2023 12:23:17 -0700 Subject: [PATCH] Fix mode switching out of runstop deadlock After Stretch has been runstopped, any Twist commands will be rejected. However, the Twist callback forgets to release the mode lock before rejecting the command. This leads to a deadlock when the driver attempts to switch out of runstop. --- stretch_core/nodes/stretch_driver | 1 + 1 file changed, 1 insertion(+) diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index f7681d5..151ceed 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -56,6 +56,7 @@ class StretchDriverNode: if self.robot_mode != 'navigation': error_string = '{0} action server must be in navigation mode to receive a twist on cmd_vel. Current mode = {1}.'.format(self.node_name, self.robot_mode) rospy.logerr(error_string) + self.robot_mode_rwlock.release_read() return self.linear_velocity_mps = twist.linear.x self.angular_velocity_radps = twist.angular.z