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

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