From 9150116ec7ecc08cc7a01f19f1897d72396b0a3c Mon Sep 17 00:00:00 2001 From: Chintan Desai Date: Tue, 11 Jul 2023 16:05:22 -0700 Subject: [PATCH] Remove pimu.push_command() out of event loop --- stretch_core/nodes/stretch_driver | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 249dc6a..ccaf9d2 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -373,6 +373,7 @@ class StretchDriverNode: self.change_mode(self.prerunstop_mode, lambda: None) if not just_change_mode: self.robot.pimu.runstop_event_reset() + self.robot.pimu.push_command() ######## SERVICE CALLBACKS ####### @@ -576,12 +577,13 @@ class StretchDriverNode: SetBool, self.runstop_service_callback) + self.robot.pimu.set_fan_on() + self.robot.pimu.push_command() + try: # start loop to command the mobile base velocity, publish # odometry, and publish joint states while not rospy.is_shutdown(): - self.robot.pimu.set_fan_on() - self.robot.pimu.push_command() self.command_mobile_base_velocity_and_publish_state() command_base_velocity_and_publish_joint_state_rate.sleep() except (rospy.ROSInterruptException, ThreadServiceExit):