Update according to changes to Python-ROS 2 API (#82)

* strip comments from locobot urdf * Update according to changes to Python-ROS 2 API * Apply suggestions from code review * Apply suggestions from code review * Apply suggestions from code review * Update interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_launch/xs_launch.py --------- Co-authored-by: lukeschmitt-tr <[email protected]>
Interbotix · Feb 1, 2025 · 3d510be · 3d510be
1 parent c5d8db7
commit 3d510be
Show file tree

Hide file tree

Showing 5 changed files with 63 additions and 71 deletions.
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
@@ -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
@@ -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
@@ -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,17 +102,17 @@ 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,
             robot_name=robot_name,
             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
@@ -83,34 +83,34 @@ 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,
             qos_profile=1,
             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