Merge pull request #96 from hello-robot/bugfix/push_command_sync

Bugfix/push command sync

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,19 @@ 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:
+                self.node.robot.push_command()
+
             for c in self.command_groups:
                 c.init_execution(self.node.robot, robot_status)
-            #self.node.robot.push_command() Now done by main thread
+
+            #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)

stretch_core/nodes/stretch_driver

@@ -70,9 +70,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() #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() #Moved to main
 
 
         # TODO: In the future, consider using time stamps from the robot's
@@ -383,7 +385,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)
@@ -583,11 +585,7 @@ class StretchDriverNode:
                 self.robot.pimu.set_fan_on()
                 self.command_mobile_base_velocity_and_publish_state()
                 self.robot.push_command()
-                # print('-------------')
-                # print('RATE Non Dxl',self.robot.non_dxl_thread.stats.status['avg_rate_hz'])
-                # print('RATE dxl_head_thread Dxl', self.robot.dxl_head_thread.stats.status['avg_rate_hz'])
-                # print('RATE dxl_end_of_arm_thread Dxl', self.robot.dxl_end_of_arm_thread.stats.status['avg_rate_hz'])
-                # print('RATE sys_thread Dxl', self.robot.sys_thread.stats.status['avg_rate_hz'])
+                self.dirty_command=False
                 command_base_velocity_and_publish_joint_state_rate.sleep()
         except (rospy.ROSInterruptException, ThreadServiceExit):
             self.robot.stop()