From 635bbe7767da16b772ab0a9f01649d69743d6043 Mon Sep 17 00:00:00 2001 From: Aaron Edsinger Date: Tue, 4 Apr 2023 13:24:14 -0700 Subject: [PATCH 1/3] 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() From ab204eee3bfdac58a5420ed07ad1175ff4a2d14c Mon Sep 17 00:00:00 2001 From: Aaron Edsinger Date: Tue, 4 Apr 2023 14:14:30 -0700 Subject: [PATCH 2/3] add error for sync --- stretch_core/nodes/joint_trajectory_server.py | 12 ++++++++++++ stretch_core/nodes/stretch_driver | 2 ++ 2 files changed, 14 insertions(+) diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index 50330bf..7613c7a 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -48,6 +48,7 @@ class JointTrajectoryAction: endofarm_cg = getattr(importlib.import_module(module_name), class_name)(node=self.node) self.command_groups.append(endofarm_cg) + def execute_cb(self, goal): with self.node.robot_stop_lock: # Escape stopped mode to execute trajectory @@ -106,9 +107,20 @@ class JointTrajectoryAction: return robot_status = self.node.robot.get_status() # uses lock held by robot + + #Relying on the stretch_driver node to do the push_command + #If it doesn't due to thread conflict, then do it here + #We rely on stretch_driver to avoid two push_commands() from two threads that are close together in tie + #As the pimu.trigger_motor_sync does not support this. + if self.node.dirty_command: + rospy.logerr("{0} joint_traj action: {1}".format(self.node.node_name, "JointTrajectoryAction failed to get a push_command")) + self.node.robot.push_command() + for c in self.command_groups: c.init_execution(self.node.robot, robot_status) + #self.node.robot.push_command() #Moved to an asynchronous call in stretch_driver + self.node.dirty_command=True 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 bb0c6dd..c2a6e3c 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -411,6 +411,7 @@ class StretchDriverNode: self.robot = rb.Robot() self.robot.startup() + self.dirty_command=True mode = rospy.get_param('~mode', "position") rospy.loginfo('mode = ' + str(mode)) @@ -530,6 +531,7 @@ class StretchDriverNode: self.robot.pimu.set_fan_on() self.command_mobile_base_velocity_and_publish_state() self.robot.push_command() + self.dirty_command=False command_base_velocity_and_publish_joint_state_rate.sleep() except (rospy.ROSInterruptException, ThreadServiceExit): self.robot.stop() From a6bcb9a9646449e9ea77d036068aae279118fc6d Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Wed, 5 Apr 2023 17:47:50 -0700 Subject: [PATCH 3/3] Remove warn in joint_trajectory_server.py --- stretch_core/nodes/joint_trajectory_server.py | 1 - 1 file changed, 1 deletion(-) diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index 7613c7a..cc03c01 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -113,7 +113,6 @@ class JointTrajectoryAction: #We rely on stretch_driver to avoid two push_commands() from two threads that are close together in tie #As the pimu.trigger_motor_sync does not support this. if self.node.dirty_command: - rospy.logerr("{0} joint_traj action: {1}".format(self.node.node_name, "JointTrajectoryAction failed to get a push_command")) self.node.robot.push_command() for c in self.command_groups: