From 635bbe7767da16b772ab0a9f01649d69743d6043 Mon Sep 17 00:00:00 2001 From: Aaron Edsinger Date: Tue, 4 Apr 2023 13:24:14 -0700 Subject: [PATCH] moved push_command to single place --- stretch_core/nodes/joint_trajectory_server.py | 2 +- stretch_core/nodes/stretch_driver | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index 5a9a99e..50330bf 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -108,7 +108,7 @@ class JointTrajectoryAction: robot_status = self.node.robot.get_status() # uses lock held by robot for c in self.command_groups: c.init_execution(self.node.robot, robot_status) - self.node.robot.push_command() + #self.node.robot.push_command() #Moved to an asynchronous call in stretch_driver goals_reached = [c.goal_reached() for c in self.command_groups] update_rate = rospy.Rate(15.0) diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 14bdc26..bb0c6dd 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -69,11 +69,11 @@ class StretchDriverNode: time_since_last_twist = rospy.get_time() - self.last_twist_time if time_since_last_twist < self.timeout: self.robot.base.set_velocity(self.linear_velocity_mps, self.angular_velocity_radps) - self.robot.push_command() + #self.robot.push_command() #Moved to main else: # Watchdog timer stops motion if no communication within timeout self.robot.base.set_velocity(0.0, 0.0) - self.robot.push_command() + #self.robot.push_command() #Moved to main # TODO: In the future, consider using time stamps from the robot's # motor control boards and other boards. These would need to @@ -326,7 +326,7 @@ class StretchDriverNode: self.robot.base.rotate_by(0.0) self.robot.arm.move_by(0.0) self.robot.lift.move_by(0.0) - self.robot.push_command() + #self.robot.push_command() Moved to main for joint in self.robot.head.joints: self.robot.head.move_by(joint, 0.0) @@ -378,7 +378,7 @@ class StretchDriverNode: self.robot.base.rotate_by(0.0) self.robot.arm.move_by(0.0) self.robot.lift.move_by(0.0) - self.robot.push_command() + #self.robot.push_command() Moved to main for joint in self.robot.head.joints: self.robot.head.move_by(joint, 0.0) @@ -528,8 +528,8 @@ class StretchDriverNode: # odometry, and publish joint states while not rospy.is_shutdown(): self.robot.pimu.set_fan_on() - self.robot.push_command() self.command_mobile_base_velocity_and_publish_state() + self.robot.push_command() command_base_velocity_and_publish_joint_state_rate.sleep() except (rospy.ROSInterruptException, ThreadServiceExit): self.robot.stop()