From 678844a2aad82246f66602dc0912c2e44fb85294 Mon Sep 17 00:00:00 2001 From: Asa Oines Date: Fri, 8 Nov 2024 21:04:07 -0500 Subject: [PATCH 1/6] strip comments from locobot urdf --- .../interbotix_xs_modules/xs_launch/xs_launch.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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..01dda75f 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 @@ -99,11 +99,11 @@ def __init__( class DeclareInterbotixXSLoCoBotRobotDescriptionLaunchArgument(DeclareLaunchArgument): """Generate a URDF of a LoCoBot through a modified DeclareLaunchArgument object.""" - def __init__( self, *, default_value: Optional[SomeSubstitutionsType] = Command([ + 'sh -c "', FindExecutable(name='xacro'), ' ', PathJoinSubstitution([ FindPackageShare('interbotix_xslocobot_descriptions'), @@ -122,6 +122,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: From ce5f469a29227d9bc8ac650c4c52335aa4cd0692 Mon Sep 17 00:00:00 2001 From: Asa Oines Date: Sat, 9 Nov 2024 15:43:46 -0500 Subject: [PATCH 2/6] Update according to changes to Python-ROS 2 API --- .../interbotix_xs_modules/xs_robot/create3.py | 14 ++--- .../interbotix_xs_modules/xs_robot/locobot.py | 39 +++--------- .../xs_robot/mobile_base.py | 24 +++---- .../interbotix_xs_modules/xs_robot/turret.py | 63 ++++++++++--------- 4 files changed, 64 insertions(+), 76 deletions(-) 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..508e5205 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,13 @@ """ 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 @@ -70,9 +68,9 @@ def __init__( topic_dxl_joint_states: str = 'dynamixel/joint_states', topic_base_joint_states: str = 'mobile_base/joint_states', use_nav: bool = False, - logging_level: LoggingSeverity = LoggingSeverity.INFO, + logging_level: LoggingSeverity = LoggingSeverity.DEBUG, node_name: str = 'robot_manipulation', - start_on_init: bool = True, + node: InterbotixRobotNode = None, args=None, ): """ @@ -105,10 +103,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 +113,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 +160,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..4cf2ea6f 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,11 @@ 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..679c87d4 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,14 +183,16 @@ 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] self.tilt_name = group_info.joint_names[1] - pan_limits = [group_info.joint_lower_limits[0], group_info.joint_upper_limits[0]] - tilt_limits = [group_info.joint_lower_limits[1], group_info.joint_upper_limits[1]] + pan_limits = [group_info.joint_lower_limits[0], + group_info.joint_upper_limits[0]] + tilt_limits = [group_info.joint_lower_limits[1], + group_info.joint_upper_limits[1]] pan_position = self.core.joint_states.position[self.core.js_index_map[self.pan_name]] tilt_position = self.core.joint_states.position[self.core.js_index_map[self.tilt_name]] self.info = { @@ -222,7 +225,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 +236,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 +270,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,14 +283,15 @@ 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 ) self.info[joint_name]['profile_velocity'] = profile_velocity if ( (profile_acceleration is not None) and - (profile_acceleration != self.info[joint_name]['profile_acceleration']) + (profile_acceleration != + self.info[joint_name]['profile_acceleration']) ): if (self.info[joint_name]['profile_type'] == 'velocity'): future_profile_acceleration = self.core.srv_set_reg.call_async( @@ -298,7 +302,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 +315,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 ) @@ -349,18 +353,21 @@ def move( (self.info[joint_name]['lower_limit'] <= position) and (position <= self.info[joint_name]['upper_limit']) ): - self.set_trajectory_profile(joint_name, profile_velocity, profile_acceleration) - self.core.pub_single.publish(JointSingleCommand(name=joint_name, cmd=position)) + self.set_trajectory_profile( + joint_name, profile_velocity, profile_acceleration) + 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): 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 +534,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 +547,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 +556,9 @@ 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 +591,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 +608,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 ) From 33d30345f6c98f108634263c3dec5b4067cbd99b Mon Sep 17 00:00:00 2001 From: lukeschmitt-tr <85308904+lukeschmitt-tr@users.noreply.github.com> Date: Fri, 31 Jan 2025 17:49:39 -0600 Subject: [PATCH 3/6] Apply suggestions from code review --- .../interbotix_xs_modules/xs_robot/locobot.py | 2 +- .../interbotix_xs_modules/xs_robot/turret.py | 15 +++++---------- 2 files changed, 6 insertions(+), 11 deletions(-) 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 508e5205..eed83e78 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 @@ -68,7 +68,7 @@ def __init__( topic_dxl_joint_states: str = 'dynamixel/joint_states', topic_base_joint_states: str = 'mobile_base/joint_states', use_nav: bool = False, - logging_level: LoggingSeverity = LoggingSeverity.DEBUG, + logging_level: LoggingSeverity = LoggingSeverity.INFO, node_name: str = 'robot_manipulation', node: InterbotixRobotNode = None, args=None, 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 679c87d4..f7247570 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 @@ -189,10 +189,8 @@ def __init__( group_info: RobotInfo.Response = self.future_group_info.result() self.pan_name = group_info.joint_names[0] self.tilt_name = group_info.joint_names[1] - pan_limits = [group_info.joint_lower_limits[0], - group_info.joint_upper_limits[0]] - tilt_limits = [group_info.joint_lower_limits[1], - group_info.joint_upper_limits[1]] + pan_limits = [group_info.joint_lower_limits[0], group_info.joint_upper_limits[0]] + tilt_limits = [group_info.joint_lower_limits[1], group_info.joint_upper_limits[1]] pan_position = self.core.joint_states.position[self.core.js_index_map[self.pan_name]] tilt_position = self.core.joint_states.position[self.core.js_index_map[self.tilt_name]] self.info = { @@ -290,8 +288,7 @@ def set_trajectory_profile( self.info[joint_name]['profile_velocity'] = profile_velocity if ( (profile_acceleration is not None) and - (profile_acceleration != - self.info[joint_name]['profile_acceleration']) + (profile_acceleration != self.info[joint_name]['profile_acceleration']) ): if (self.info[joint_name]['profile_type'] == 'velocity'): future_profile_acceleration = self.core.srv_set_reg.call_async( @@ -353,10 +350,8 @@ def move( (self.info[joint_name]['lower_limit'] <= position) and (position <= self.info[joint_name]['upper_limit']) ): - self.set_trajectory_profile( - joint_name, profile_velocity, profile_acceleration) - self.core.pub_single.publish( - JointSingleCommand(name=joint_name, cmd=position)) + self.set_trajectory_profile(joint_name, profile_velocity, profile_acceleration) + 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): print(self.info[joint_name]['profile_velocity']) From 645c81a4a99e7fc6e4c2a7238db5142536cdb0ba Mon Sep 17 00:00:00 2001 From: lukeschmitt-tr <85308904+lukeschmitt-tr@users.noreply.github.com> Date: Fri, 31 Jan 2025 17:50:37 -0600 Subject: [PATCH 4/6] Apply suggestions from code review --- .../interbotix_xs_modules/xs_robot/turret.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 f7247570..5bbdf7ca 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 @@ -189,8 +189,8 @@ def __init__( group_info: RobotInfo.Response = self.future_group_info.result() self.pan_name = group_info.joint_names[0] self.tilt_name = group_info.joint_names[1] - pan_limits = [group_info.joint_lower_limits[0], group_info.joint_upper_limits[0]] - tilt_limits = [group_info.joint_lower_limits[1], group_info.joint_upper_limits[1]] + pan_limits = [group_info.joint_lower_limits[0], group_info.joint_upper_limits[0]] + tilt_limits = [group_info.joint_lower_limits[1], group_info.joint_upper_limits[1]] pan_position = self.core.joint_states.position[self.core.js_index_map[self.pan_name]] tilt_position = self.core.joint_states.position[self.core.js_index_map[self.tilt_name]] self.info = { From ae59c40924995583992db5cf0ecf8d67a5443dd7 Mon Sep 17 00:00:00 2001 From: lukeschmitt-tr <85308904+lukeschmitt-tr@users.noreply.github.com> Date: Fri, 31 Jan 2025 18:21:28 -0600 Subject: [PATCH 5/6] Apply suggestions from code review --- .../interbotix_xs_modules/xs_robot/locobot.py | 1 - .../interbotix_xs_modules/xs_robot/mobile_base.py | 4 +++- .../interbotix_xs_modules/xs_robot/turret.py | 8 ++++++-- 3 files changed, 9 insertions(+), 4 deletions(-) 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 eed83e78..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 @@ -39,7 +39,6 @@ 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.logging import LoggingSeverity 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 4cf2ea6f..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 @@ -294,7 +294,9 @@ def is_nav_complete(self): if self.future_nav.result(): self.nav_status = self.future_nav.result().status if self.nav_status != GoalStatus.STATUS_SUCCEEDED: - self.core.get_node().logerror(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 5bbdf7ca..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 @@ -360,7 +360,9 @@ def move( self.info[joint_name]['profile_velocity'] * S_TO_NS)) ) else: - self.core.get_node().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_node().logerror( f"Goal position is outside the '{joint_name}' joint's limits. Will not execute." @@ -551,7 +553,9 @@ def pan_tilt_move( ) ) else: - self.core.get_node().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_node().logerror( 'One or both goal positions are outside the limits. Will not execute' From bf014be73bba21883545574b6c12fa5091cbaa0b Mon Sep 17 00:00:00 2001 From: lukeschmitt-tr <85308904+lukeschmitt-tr@users.noreply.github.com> Date: Fri, 31 Jan 2025 18:32:24 -0600 Subject: [PATCH 6/6] Update interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_launch/xs_launch.py --- .../interbotix_xs_modules/xs_launch/xs_launch.py | 1 + 1 file changed, 1 insertion(+) 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 01dda75f..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 @@ -99,6 +99,7 @@ def __init__( class DeclareInterbotixXSLoCoBotRobotDescriptionLaunchArgument(DeclareLaunchArgument): """Generate a URDF of a LoCoBot through a modified DeclareLaunchArgument object.""" + def __init__( self, *,