diff --git a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_launch/xs_launch.py b/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_launch/xs_launch.py index 3fe4198e..1719cfdd 100644 --- a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_launch/xs_launch.py +++ b/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_launch/xs_launch.py @@ -104,6 +104,7 @@ def __init__( self, *, default_value: Optional[SomeSubstitutionsType] = Command([ + 'sh -c "', FindExecutable(name='xacro'), ' ', PathJoinSubstitution([ FindPackageShare('interbotix_xslocobot_descriptions'), @@ -122,6 +123,9 @@ def __init__( 'use_lidar:=', LaunchConfiguration('use_lidar'), ' ', 'external_urdf_loc:=', LaunchConfiguration('external_urdf_loc'), ' ', 'hardware_type:=', LaunchConfiguration('hardware_type'), ' ', + # Stripping comments from the URDF is necessary for gazebo_ros2_control to parse the + # robot_description parameter override + '| ', FindExecutable(name='perl'), ' -0777 -pe \'s///gs\'"' ]), **kwargs ) -> None: 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 c797c96c..8d44e550 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,13 +33,13 @@ Python. """ -from builtin_interfaces.msg import Duration from geometry_msgs.msg import Pose from interbotix_xs_modules.xs_robot.core import InterbotixRobotXSCore 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 rclpy.duration import Duration from std_msgs.msg import Header @@ -78,28 +78,28 @@ def __init__( nav_timeout_sec=nav_timeout_sec, use_nav=use_nav, ) - self.pub_base_sound = self.core.create_publisher( + self.pub_base_sound = self.core.get_node().create_publisher( msg_type=AudioNoteVector, topic='mobile_base/cmd_audio', qos_profile=1 ) - self.client_reset_pose = self.core.create_client( + self.client_reset_pose = self.core.get_node().create_client( srv_type=ResetPose, srv_name='mobile_base/reset_pose', ) - self.core.get_clock().sleep_for(Duration(nanosec=int(0.5*S_TO_NS))) - self.core.get_logger().info('Initialized InterbotixCreate3Interface!') + self.core.get_node().get_clock().sleep_for(Duration(nanoseconds=int(0.5*S_TO_NS))) + self.core.get_node().loginfo('Initialized InterbotixCreate3Interface!') def reset_odom(self) -> None: """Reset odometry to zero.""" future_reset_odom = self.client_reset_pose.call_async(ResetPose.Request(pose=Pose())) - self.core.robot_spin_once_until_future_complete(future_reset_odom) + self.core.get_node().wait_until_future_complete(future_reset_odom) self.play_sound( sound=AudioNoteVector( header=Header( frame_id=f'{self.robot_name}/base_link', - stamp=self.core.get_clock().now().to_msg() + stamp=self.core.get_node().get_clock().now().to_msg() ), notes=[AudioNote(frequency=6000, max_runtime=Duration(seconds=1.0))], append=False, diff --git a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/locobot.py b/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/locobot.py index e4ec91f8..5118f913 100644 --- a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/locobot.py +++ b/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/locobot.py @@ -33,15 +33,12 @@ """ from enum import Enum -from threading import Thread -import time +from interbotix_common_modules.common_robot.robot import InterbotixRobotNode from interbotix_xs_modules.xs_robot.arm import InterbotixArmXSInterface from interbotix_xs_modules.xs_robot.core import InterbotixRobotXSCore from interbotix_xs_modules.xs_robot.gripper import InterbotixGripperXSInterface from interbotix_xs_modules.xs_robot.turret import InterbotixTurretXSInterface -import rclpy -from rclpy.executors import MultiThreadedExecutor from rclpy.logging import LoggingSeverity @@ -72,7 +69,7 @@ def __init__( use_nav: bool = False, logging_level: LoggingSeverity = LoggingSeverity.INFO, node_name: str = 'robot_manipulation', - start_on_init: bool = True, + node: InterbotixRobotNode = None, args=None, ): """ @@ -105,10 +102,9 @@ def __init__( ERROR, or FATAL. defaults to INFO :param node_name: (optional) name to give to the core started by this class, defaults to 'robot_manipulation' - :param start_on_init: (optional) set to `True` to start running the spin thread after the - object is built; set to `False` if intending to sub-class this. If set to `False`, - either call the `start()` method later on, or add the core to an executor in another - thread. + :param node: (optional) `rclpy.Node` or derived class to base this robot's ROS-related + components on. If nothing is given, this robot will create its own node. defaults to + `None`. """ self.core = InterbotixRobotXSCore( robot_model=robot_model, @@ -116,6 +112,7 @@ def __init__( topic_joint_states=topic_dxl_joint_states, logging_level=logging_level, node_name=node_name, + node=node, args=args ) self.camera = InterbotixTurretXSInterface( @@ -162,23 +159,6 @@ def __init__( init_node=False ) - if start_on_init: - self.start() - - def start(self) -> None: - """Start a background thread that builds and spins an executor.""" - self._execution_thread = Thread(target=self.run) - self._execution_thread.start() - - def run(self) -> None: - """Thread target.""" - self.ex = MultiThreadedExecutor() - self.ex.add_node(self.core) - self.ex.spin() - - def shutdown(self) -> None: - """Destroy the node and shut down all threads and processes.""" - self.core.destroy_node() - rclpy.shutdown() - self._execution_thread.join() - time.sleep(0.5) + def get_node(self) -> InterbotixRobotNode: + """Return the core robot node for this robot.""" + return self.core.robot_node 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 feeddc9c..cbc335fb 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 @@ -83,20 +83,20 @@ def __init__( cb_group_mobile_base = ReentrantCallbackGroup() - self.pub_base_twist = self.core.create_publisher( + self.pub_base_twist = self.core.get_node().create_publisher( msg_type=Twist, topic=topic_cmd_vel, qos_profile=1, callback_group=cb_group_mobile_base, ) - self.sub_base_states = self.core.create_subscription( + self.sub_base_states = self.core.get_node().create_subscription( msg_type=JointState, topic=topic_base_joint_states, callback=self._base_states_cb, qos_profile=1, callback_group=cb_group_mobile_base, ) - self.sub_base_odom = self.core.create_subscription( + self.sub_base_odom = self.core.get_node().create_subscription( msg_type=Odometry, topic='/mobile_base/odom', callback=self._base_odom_cb, @@ -104,13 +104,13 @@ def __init__( callback_group=cb_group_mobile_base, ) self.client_base_nav_to_pose = ActionClient( - node=self.core, + node=self.core.get_node(), action_type=NavigateToPose, action_name='navigate_to_pose', callback_group=cb_group_mobile_base, ) - self.core.get_logger().info('Initialized InterbotixMobileBaseInterface!') + self.core.get_node().loginfo('Initialized InterbotixMobileBaseInterface!') def command_velocity_xyaw( self, @@ -200,7 +200,7 @@ def command_pose( :details: note that if 'blocking' is `False`, the function will always return `True` """ if not self.use_nav: - self.core.get_logger().error('`use_nav` set to `False`. Will not execute navigation.') + self.core.get_node().logerror('`use_nav` set to `False`. Will not execute navigation.') return False goal = NavigateToPose.Goal( pose=self._stamp_pose(pose=goal_pose, frame_id=frame_id), @@ -212,11 +212,11 @@ def command_pose( feedback_callback=self._nav_to_pose_feedback_cb, ) - self.core.robot_spin_once_until_future_complete(future_send_nav_to_pose_goal) + self.core.get_node().wait_until_future_complete(future_send_nav_to_pose_goal) self.goal_handle = future_send_nav_to_pose_goal.result() if not self.goal_handle.accepted: - self.core.get_logger().error( + self.core.get_node().logerror( f'Navigation goal [{goal_pose.position.x}, {goal_pose.position.y}] was rejected.' ) return False @@ -225,12 +225,12 @@ def command_pose( while not self.is_nav_complete(): fb = self.get_nav_to_pose_feedback() if Duration.from_msg(fb.navigation_time > Duration(seconds=self.nav_timeout_sec)): - self.core.get_logger().error( + self.core.get_node().logerror( f'Navigation time ({fb.navigation_time}) exceeds timeout ' f'({self.nav_timeout_sec}). Cancelling navigation goal.' ) future_cancel_nav_to_pose_goal = self.goal_handle.cancel_goal_async() - self.core.robot_spin_once_until_future_complete( + self.core.get_node().wait_until_future_complete( future=future_cancel_nav_to_pose_goal ) return False @@ -290,11 +290,13 @@ def is_nav_complete(self): """ if not self.future_nav: return True - self.core.robot_spin_once_until_future_complete(future=self.future_nav, timeout_sec=0.1) + self.core.get_node().wait_until_future_complete(future=self.future_nav, timeout_sec=0.1) if self.future_nav.result(): self.nav_status = self.future_nav.result().status if self.nav_status != GoalStatus.STATUS_SUCCEEDED: - self.core.get_logger().error(f"Navigation failed with status '{self.nav_status}'.") + self.core.get_node().logerror( + f"Navigation failed with status '{self.nav_status}'." + ) return True else: return False 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 79398aca..4161e091 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 @@ -104,6 +104,7 @@ def __init__( logging_level=logging_level, node_name=node_name, ) + self.turret = InterbotixTurretXSInterface( core=self.core, turret_name=turret_name, @@ -126,13 +127,13 @@ def start(self) -> None: def run(self) -> None: """Thread target.""" self.ex = MultiThreadedExecutor() - self.ex.add_node(self.core) + self.ex.add_node(self.core.get_node()) while rclpy.ok(): self.ex.spin() def shutdown(self) -> None: """Destroy the node and shut down all threads and processes.""" - self.core.destroy_node() + self.core.get_node().destroy_node() rclpy.shutdown() self._execution_thread.join() time.sleep(0.5) @@ -182,8 +183,8 @@ def __init__( ) while rclpy.ok() and not self.future_group_info.done(): - rclpy.spin_until_future_complete(self.core, self.future_group_info) - rclpy.spin_once(self.core) + rclpy.spin_until_future_complete(self.core.get_node(), self.future_group_info) + rclpy.spin_once(self.core.get_node()) group_info: RobotInfo.Response = self.future_group_info.result() self.pan_name = group_info.joint_names[0] @@ -222,7 +223,7 @@ def __init__( tilt_profile_velocity, tilt_profile_acceleration ) - self.core.get_logger().info(( + self.core.get_node().loginfo(( f'Turret Group Name: {turret_name}\n' f'Pan Name: {self.pan_name}, ' f'Profile Type: {pan_profile_type}, ' @@ -233,7 +234,7 @@ def __init__( f'Profile Velocity: {tilt_profile_velocity:.1f}, ' f'Profile Acceleration: {tilt_profile_acceleration:.1f}' )) - self.core.get_logger().info('Initialized InterbotixTurretXSInterface!') + self.core.get_node().loginfo('Initialized InterbotixTurretXSInterface!') def set_trajectory_profile( self, @@ -267,7 +268,7 @@ def set_trajectory_profile( value=profile_velocity ) ) - self.core.robot_spin_once_until_future_complete( + self.core.get_node().executor.spin_once_until_future_complete( future=future_profile_velocity, timeout_sec=0.1 ) @@ -280,7 +281,7 @@ def set_trajectory_profile( value=int(profile_velocity * 1000) ) ) - self.core.robot_spin_once_until_future_complete( + self.core.get_node().executor.spin_once_until_future_complete( future=future_profile_velocity, timeout_sec=0.1 ) @@ -298,7 +299,7 @@ def set_trajectory_profile( value=profile_acceleration ) ) - self.core.robot_spin_once_until_future_complete( + self.core.get_node().executor.spin_once_until_future_complete( future=future_profile_acceleration, timeout_sec=0.1 ) @@ -311,7 +312,7 @@ def set_trajectory_profile( value=int(profile_acceleration * 1000) ) ) - self.core.robot_spin_once_until_future_complete( + self.core.get_node().executor.spin_once_until_future_complete( future=future_profile_acceleration, timeout_sec=0.1 ) @@ -354,13 +355,16 @@ def move( self.info[joint_name]['command'] = position if (self.info[joint_name]['profile_type'] == 'time' and blocking): print(self.info[joint_name]['profile_velocity']) - self.core.get_clock().sleep_for( - Duration(nanoseconds=int(self.info[joint_name]['profile_velocity'] * S_TO_NS)) + self.core.get_node().get_clock().sleep_for( + Duration(nanoseconds=int( + self.info[joint_name]['profile_velocity'] * S_TO_NS)) ) else: - self.core.get_clock().sleep_for(Duration(nanoseconds=int(delay * S_TO_NS))) + self.core.get_node().get_clock().sleep_for( + Duration(nanoseconds=int(delay * S_TO_NS)) + ) else: - self.core.get_logger().error( + self.core.get_node().logerror( f"Goal position is outside the '{joint_name}' joint's limits. Will not execute." ) @@ -527,12 +531,12 @@ def pan_tilt_move( pan_profile_acceleration ) self.set_trajectory_profile( - self.tilt_name, - tilt_profile_velocity, - tilt_profile_acceleration + self.tilt_name, + tilt_profile_velocity, + tilt_profile_acceleration ) self.core.pub_group.publish( - JointGroupCommand(name=self.turret_name, cmd=[pan_position, tilt_position])) + JointGroupCommand(name=self.turret_name, cmd=[pan_position, tilt_position])) self.info[self.pan_name]['command'] = pan_position self.info[self.tilt_name]['command'] = tilt_position if ( @@ -540,7 +544,7 @@ def pan_tilt_move( (self.info[self.tilt_name]['profile_type'] == 'time') and (blocking) ): - self.core.get_clock().sleep_for( + self.core.get_node().get_clock().sleep_for( Duration( nanoseconds=int(max( self.info[self.pan_name]['profile_velocity'], @@ -549,9 +553,11 @@ def pan_tilt_move( ) ) else: - self.core.get_clock().sleep_for(Duration(nanoseconds=int(delay * S_TO_NS))) + self.core.get_node().get_clock().sleep_for( + Duration(nanoseconds=int(delay * S_TO_NS)) + ) else: - self.core.get_logger().error( + self.core.get_node().logerror( 'One or both goal positions are outside the limits. Will not execute' ) @@ -584,7 +590,7 @@ def change_profile( profile_acceleration=profile_acceleration ) ) - self.core.robot_spin_once_until_future_complete( + self.core.get_node().executor.spin_once_until_future_complete( future=future_operating_modes, timeout_sec=0.1 ) @@ -601,7 +607,7 @@ def change_profile( profile_acceleration=int(profile_acceleration * 1000) ) ) - self.core.robot_spin_once_until_future_complete( + self.core.get_node().executor.spin_once_until_future_complete( future=future_operating_modes, timeout_sec=0.1 )