[xs_modules] Use sleep_for instead of time.sleep

interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/arm.py

@@ -269,9 +269,7 @@ def _publish_commands(
         )
         self.core.pub_group.publish(joint_commands)
         if blocking:
-            time.sleep(
-                self.moving_time
-            )  # TODO: once released, use rclpy.clock().sleep_for()
+            self.core.get_clock().sleep_for(self.moving_time)
         self._update_Tsb()
 
     def set_trajectory_time(
@@ -465,7 +463,7 @@ def set_single_joint_position(
         single_command = JointSingleCommand(name=joint_name, cmd=position)
         self.core.pub_single.publish(single_command)
         if blocking:
-            time.sleep(self.moving_time)
+            self.core.get_clock().sleep_for(self.moving_time)
         self._update_Tsb()
         return True
 
@@ -698,7 +696,7 @@ def set_ee_cartesian_trajectory(
                     cmd_type='group', name=self.group_name, traj=joint_traj
                 )
             )
-            time.sleep(moving_time + wp_moving_time)
+            self.core.get_clock().sleep_for(moving_time + wp_moving_time)
             self.T_sb = T_sd
             self.joint_commands = joint_positions
             self.set_trajectory_time(moving_time, accel_time)

interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/create3.py

@@ -33,8 +33,6 @@
 Python.
 """
 
-import time
-
 from builtin_interfaces.msg import Duration
 from geometry_msgs.msg import Pose
 from interbotix_xs_modules.xs_robot.core import InterbotixRobotXSCore
@@ -89,7 +87,7 @@ def __init__(
             srv_name='mobile_base/reset_pose',
         )
 
-        time.sleep(0.5)
+        self.core.get_clock().sleep_for(0.5)
         self.core.get_logger().info('Initialized InterbotixCreate3Interface!')
 
     def reset_odom(self) -> None:

interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/gripper.py

@@ -34,7 +34,6 @@
 
 import sys
 from threading import Thread
-import time
 
 from interbotix_xs_modules.xs_robot.core import InterbotixRobotXSCore
 from interbotix_xs_msgs.msg import JointSingleCommand
@@ -124,7 +123,7 @@ def shutdown(self) -> None:
         self.core.destroy_node()
         rclpy.shutdown()
         self._execution_thread.join()
-        time.sleep(0.5)
+        self.core.get_clock().sleep_for(0.5)
 
 
 class InterbotixGripperXSInterface:
@@ -189,7 +188,7 @@ def __init__(
             )
             sys.exit(1)
 
-        time.sleep(0.5)
+        self.core.get_clock().sleep_for(0.5)
         self.core.get_logger().info(
             (
                 '\n'
@@ -236,7 +235,7 @@ def gripper_controller(self, effort: float, delay: float) -> None:
         ):
             self.core.pub_single.publish(self.gripper_command)
             self.gripper_moving = True
-            time.sleep(delay)
+            self.core.get_clock().sleep_for(delay)
 
     def set_pressure(self, pressure: float) -> None:
         """

interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/kobuki.py

@@ -33,8 +33,6 @@
 Python.
 """
 
-from time import time
-
 from interbotix_xs_modules.xs_robot.core import InterbotixRobotXSCore
 from interbotix_xs_modules.xs_robot.mobile_base import InterbotixMobileBaseInterface
 from kobuki_ros_interfaces.msg import Sound
@@ -87,7 +85,7 @@ def __init__(
             qos_profile=1,
         )
 
-        time.sleep(0.5)
+        self.core.get_clock().sleep_for(0.5)
         self.core.get_logger().info('Initialized InterbotixKobukiInterface!')
 
     def reset_odom(self) -> None:

interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/mobile_base.py

@@ -33,7 +33,6 @@
 """
 
 from abc import ABC, abstractmethod
-import time
 from typing import List
 
 from action_msgs.msg import GoalStatus
@@ -104,7 +103,7 @@ def __init__(
             action_name='navigate_to_pose'
         )
 
-        time.sleep(0.5)
+        self.core.get_clock().sleep_for(0.5)
         self.core.get_logger().info('Initialized InterbotixMobileBaseInterface!')
 
     def command_velocity_xyaw(

interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/turret.py

@@ -351,9 +351,9 @@ def move(
             self.core.pub_single.publish(JointSingleCommand(name=joint_name, cmd=position))
             self.info[joint_name]['command'] = position
             if (self.info[joint_name]['profile_type'] == 'time' and blocking):
-                time.sleep(self.info[joint_name]['profile_velocity'])
+                self.core.get_clock().sleep_for(self.info[joint_name]['profile_velocity'])
             else:
-                time.sleep(delay)
+                self.core.get_clock().sleep_for(delay)
         else:
             self.core.get_logger().error(
                 f"Goal position is outside the '{joint_name}' joint's limits. Will not execute."
@@ -535,14 +535,14 @@ def pan_tilt_move(
                 (self.info[self.tilt_name]['profile_type'] == 'time') and
                 (blocking)
             ):
-                time.sleep(
+                self.core.get_clock().sleep_for(
                     max(
                         self.info[self.pan_name]['profile_velocity'],
                         self.info[self.tilt_name]['profile_velocity']
                     )
                 )
             else:
-                time.sleep(delay)
+                self.core.get_clock().sleep_for(delay)
         else:
             self.core.get_logger().error(
                 'One or both goal positions are outside the limits. Will not execute'