From 7438041390031569288e9c27ced641979d052659 Mon Sep 17 00:00:00 2001
From: aedsinger <aedsinger@hello-robot.com>
Date: Fri, 19 May 2023 19:17:45 -0400
Subject: [PATCH] move to 25hz fast rates

---
 stretch_core/launch/stretch_driver.launch     |  6 +++---
 stretch_core/nodes/joint_trajectory_server.py |  2 +-
 stretch_core/nodes/stretch_driver             | 13 +++++++++----
 3 files changed, 13 insertions(+), 8 deletions(-)

diff --git a/stretch_core/launch/stretch_driver.launch b/stretch_core/launch/stretch_driver.launch
index 8f86751..a0ef9b0 100644
--- a/stretch_core/launch/stretch_driver.launch
+++ b/stretch_core/launch/stretch_driver.launch
@@ -7,7 +7,7 @@
       name="joint_state_publisher"
       pkg="joint_state_publisher"
       type="joint_state_publisher" >
-    <param name="rate" value="15.0"/>
+    <param name="rate" value="30.0"/>
     <rosparam param="source_list">
       [/stretch/joint_states]
     </rosparam>
@@ -30,11 +30,11 @@
       name="robot_state_publisher"
       pkg="robot_state_publisher"
       type="robot_state_publisher" >
-    <param name="publish_frequency" value="15.0"/>
+    <param name="publish_frequency" value="30.0"/>
   </node>
 
   <node name="stretch_driver" pkg="stretch_core" type="stretch_driver" output="screen">
-    <param name="rate" type="double" value="15.0"/>
+    <param name="rate" type="double" value="30.0"/>
     <param name="timeout" type="double" value="0.5"/>
     <remap from="cmd_vel" to="/stretch/cmd_vel" />
     <remap from="joint_states" to="/stretch/joint_states" />
diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py
index 5a9a99e..9624e10 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() Now done by main thread
 
             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 fb7376e..aee4fcf 100755
--- a/stretch_core/nodes/stretch_driver
+++ b/stretch_core/nodes/stretch_driver
@@ -69,11 +69,10 @@ 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()
             else:
                 # Watchdog timer stops motion if no communication within timeout
                 self.robot.base.set_velocity(0.0, 0.0)
-                self.robot.push_command()
+
 
         # TODO: In the future, consider using time stamps from the robot's
         # motor control boards and other boards. These would need to
@@ -410,7 +409,7 @@ class StretchDriverNode:
             rospy.signal_shutdown('Found old stretch_body version.')
 
         self.robot = rb.Robot()
-        self.robot.startup()
+        self.robot.startup(start_non_dxl_thread=False,start_dxl_thread=True,start_sys_mon_thread=True) #Handle the non_dxl status in local loop, not thread
 
         mode = rospy.get_param('~mode', "position")
         rospy.loginfo('mode = ' + str(mode))
@@ -527,9 +526,15 @@ class StretchDriverNode:
             # start loop to command the mobile base velocity, publish
             # odometry, and publish joint states
             while not rospy.is_shutdown():
+                self.robot.non_dxl_thread.step()
                 self.robot.pimu.set_fan_on()
-                self.robot.pimu.push_command()
                 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'])
                 command_base_velocity_and_publish_joint_state_rate.sleep()
         except (rospy.ROSInterruptException, ThreadServiceExit):
             self.robot.stop()