diff --git a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/arm.py b/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/arm.py index e273e35f..e8376989 100644 --- a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/arm.py +++ b/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) diff --git a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/create3.py b/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/create3.py index e79ba8f7..4bdb6ee0 100644 --- a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/create3.py +++ b/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: diff --git a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/gripper.py b/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/gripper.py index 477a6a94..44f2a3d9 100644 --- a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/gripper.py +++ b/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: """ diff --git a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/kobuki.py b/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/kobuki.py index a17045b2..82b07537 100644 --- a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/kobuki.py +++ b/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: diff --git a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/mobile_base.py b/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/mobile_base.py index 63bd142d..045751aa 100644 --- a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/mobile_base.py +++ b/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( diff --git a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/turret.py b/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/turret.py index f86fe89a..bafaf9c0 100644 --- a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/turret.py +++ b/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'