[xs_modules] Use Durations in sleep_for calls

interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/arm.py

@@ -37,7 +37,6 @@
 import time
 from typing import Any, List, Tuple, Union
 
-from builtin_interfaces.msg import Duration
 import interbotix_common_modules.angle_manipulation as ang
 from interbotix_xs_modules.xs_robot import mr_descriptions as mrd
 from interbotix_xs_modules.xs_robot.core import InterbotixRobotXSCore
@@ -52,6 +51,7 @@
 import numpy as np
 import rclpy
 from rclpy.constants import S_TO_NS
+from rclpy.duration import Duration
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.logging import LoggingSeverity
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
@@ -269,7 +269,7 @@ def _publish_commands(
         )
         self.core.pub_group.publish(joint_commands)
         if blocking:
-            self.core.get_clock().sleep_for(self.moving_time)
+            self.core.get_clock().sleep_for(Duration(nanoseconds=int(self.moving_time*S_TO_NS)))
         self._update_Tsb()
 
     def set_trajectory_time(
@@ -463,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:
-            self.core.get_clock().sleep_for(self.moving_time)
+            self.core.get_clock().sleep_for(Duration(nanoseconds=int(self.moving_time*S_TO_NS)))
         self._update_Tsb()
         return True
 
@@ -655,8 +655,8 @@ def set_ee_cartesian_trajectory(
             joint_traj_point = JointTrajectoryPoint()
             joint_traj_point.positions = tuple(joint_positions)
             joint_traj_point.time_from_start = Duration(
-                nanosec=int(i * wp_period * S_TO_NS)
-            )
+                nanoseconds=int(i * wp_period * S_TO_NS)
+            ).to_msg()
             joint_traj.points.append(joint_traj_point)
             if i == N:
                 break
@@ -696,7 +696,9 @@ def set_ee_cartesian_trajectory(
                     cmd_type='group', name=self.group_name, traj=joint_traj
                 )
             )
-            self.core.get_clock().sleep_for(moving_time + wp_moving_time)
+            self.core.get_clock().sleep_for(
+                Duration(nanoseconds=int((moving_time + wp_moving_time)*S_TO_NS))
+            )
             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

@@ -39,6 +39,7 @@
 from interbotix_xs_modules.xs_robot.mobile_base import InterbotixMobileBaseInterface
 from irobot_create_msgs.msg import AudioNote, AudioNoteVector
 from irobot_create_msgs.srv import ResetPose
+from rclpy.constants import S_TO_NS
 from std_msgs.msg import Header
 
 
@@ -87,7 +88,7 @@ def __init__(
             srv_name='mobile_base/reset_pose',
         )
 
-        self.core.get_clock().sleep_for(0.5)
+        self.core.get_clock().sleep_for(Duration(nanosec=int(0.5*S_TO_NS)))
         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,11 +34,14 @@
 
 import sys
 from threading import Thread
+import time
 
 from interbotix_xs_modules.xs_robot.core import InterbotixRobotXSCore
 from interbotix_xs_msgs.msg import JointSingleCommand
 from interbotix_xs_msgs.srv import RobotInfo
 import rclpy
+from rclpy.constants import S_TO_NS
+from rclpy.duration import Duration
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.logging import LoggingSeverity
 
@@ -123,7 +126,7 @@ def shutdown(self) -> None:
         self.core.destroy_node()
         rclpy.shutdown()
         self._execution_thread.join()
-        self.core.get_clock().sleep_for(0.5)
+        time.sleep(0.5)
 
 
 class InterbotixGripperXSInterface:
@@ -188,7 +191,7 @@ def __init__(
             )
             sys.exit(1)
 
-        self.core.get_clock().sleep_for(0.5)
+        self.core.get_clock().sleep_for(Duration(nanoseconds=int(0.5*S_TO_NS)))
         self.core.get_logger().info(
             (
                 '\n'
@@ -235,7 +238,7 @@ def gripper_controller(self, effort: float, delay: float) -> None:
         ):
             self.core.pub_single.publish(self.gripper_command)
             self.gripper_moving = True
-            self.core.get_clock().sleep_for(delay)
+            self.core.get_clock().sleep_for(Duration(nanoseconds=int(delay*S_TO_NS)))
 
     def set_pressure(self, pressure: float) -> None:
         """