move robot thread rates to YAML

hello-robot · Aug 2, 2022 · 0fa82e8 · 0fa82e8
1 parent 66648d9
commit 0fa82e8
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 7 deletions.
diff --git a/body/stretch_body/robot.py b/body/stretch_body/robot.py
@@ -55,10 +55,10 @@ def __init__(self, robot, target_rate_hz=25.0):
         threading.Thread.__init__(self)
         self.robot=robot
         self.robot_update_rate_hz = target_rate_hz
-        self.monitor_downrate_int = 5  # Step the monitor at every Nth iteration
-        self.collision_downrate_int = 5  # Step the monitor at every Nth iteration
-        self.sentry_downrate_int = 2  # Step the sentry at every Nth iteration
-        self.trajectory_downrate_int = 2  # Update hardware with waypoint trajectory segments at every Nth iteration
+        self.monitor_downrate_int = int(robot.params['rates']['NonDXLStatusThread_monitor_downrate_int'])  # Step the monitor at every Nth iteration
+        self.collision_downrate_int = int(robot.params['rates']['NonDXLStatusThread_collision_downrate_int']) # Step the monitor at every Nth iteration
+        self.sentry_downrate_int = int(robot.params['rates']['NonDXLStatusThread_sentry_downrate_int']) # Step the sentry at every Nth iteration
+        self.trajectory_downrate_int = int(robot.params['rates']['NonDXLStatusThread_trajectory_downrate_int'])  # Update hardware with waypoint trajectory segments at every Nth iteration
         if self.robot.params['use_monitor']:
             self.robot.monitor.startup()
         if self.robot.params['use_collision_manager']:
@@ -163,11 +163,11 @@ def startup(self):
         signal.signal(signal.SIGTERM, hello_utils.thread_service_shutdown)
         signal.signal(signal.SIGINT, hello_utils.thread_service_shutdown)
 
-        self.non_dxl_thread = NonDXLStatusThread(self)
+        self.non_dxl_thread = NonDXLStatusThread(self,target_rate_hz=self.params['rates']['NonDXLStatusThread_Hz'])
         self.non_dxl_thread.setDaemon(True)
         self.non_dxl_thread.start()
 
-        self.dxl_thread = DXLStatusThread(self)
+        self.dxl_thread = DXLStatusThread(self,target_rate_hz=self.params['rates']['DXLStatusThread_Hz'])
         self.dxl_thread.setDaemon(True)
         self.dxl_thread.start()
 
@@ -358,7 +358,7 @@ def _update_trajectory_dynamixel(self):
             self.end_of_arm.update_trajectory()
             self.head.update_trajectory()
         except SerialException:
-            self.logger.warning('Serial Exception on Robot._update_trajectory_dynamixe()')
+            self.logger.warning('Serial Exception on Robot._update_trajectory_dynamixel()')
 
     def _pull_status_non_dynamixel(self):
         self.wacc.pull_status()

diff --git a/body/stretch_body/robot_params_RE1V0.py b/body/stretch_body/robot_params_RE1V0.py
@@ -411,6 +411,13 @@
         'temp_LPF': 1.0,
         'voltage_LPF': 1.0}},
     'robot':{
+        'rates':{
+            'DXLStatusThread_Hz': 15.0,
+            'NonDXLStatusThread_Hz': 25.0,
+            'NonDXLStatusThread_monitor_downrate_int': 5,
+            'NonDXLStatusThread_collision_downrate_int': 5,
+            'NonDXLStatusThread_sentry_downrate_int': 2,
+            'NonDXLStatusThread_trajectory_downrate_int': 2},
         'tool': 'tool_stretch_gripper',
         'use_collision_manager': 1,
         'log_to_console': 0,

diff --git a/body/stretch_body/robot_params_RE1V5.py b/body/stretch_body/robot_params_RE1V5.py
@@ -407,6 +407,13 @@
         'temp_LPF': 1.0,
         'voltage_LPF': 1.0}},
     'robot':{
+        'rates':{
+            'DXLStatusThread_Hz': 15.0,
+            'NonDXLStatusThread_Hz': 25.0,
+            'NonDXLStatusThread_monitor_downrate_int': 5,
+            'NonDXLStatusThread_collision_downrate_int': 5,
+            'NonDXLStatusThread_sentry_downrate_int': 2,
+            'NonDXLStatusThread_trajectory_downrate_int': 2},
         'tool': 'tool_stretch_gripper',
         'use_collision_manager': 1,
         'log_to_console': 0,