Parameterise PandaROS2RobotInterface for multi-arm setups

PandaROS2RobotInterface previously hard-coded the controller name (panda_arm_controller), the gripper action namespace (franka_gripper) and the limb attribute (self.robot.rarm). That made it unusable for setups where the same skrobot model drives two Pandas (e.g. JSK dual-Panda with arm_id rarm and larm) or where the controller name is non-default.

Add four new keyword arguments to __init__:

- arm_id (default panda) drives the default controller name when controller_name is not given (f"{arm_id}_arm_controller").
- controller_name overrides the action / state topic prefix directly when needed (e.g. a unified dual_panda_joint_trajectory_controller spanning 14 joints).
- gripper_action_prefix replaces the franka_gripper hard-coded namespace.
- limb_attr (default rarm) selects which attribute on the robot model contributes the joint_list. For dual-Panda setups instantiate the class twice with limb_attr=rarm and larm.
- load_gripper (default True) skips creating the gripper ActionClients entirely. Useful for sim setups (mock_components) where franka_gripper is not running, and for unit tests where ActionClient.wait_for_server would otherwise block.

The legacy rarm_controller property is preserved as an alias of the new arm_controller property for backward compatibility.

Six unit tests cover the default behaviour, arm_id-driven controller name, explicit controller_name override, the rarm_controller alias, and the load_gripper=False path.

The ros2-tests CI job now runs the whole tests/skrobot_tests/interfaces_tests/ros2/ directory so test_panda.py is exercised alongside test_base.py.

Author: iory

.github/workflows/test.yml

@@ -310,5 +310,5 @@ jobs:
       run: |
         source /opt/ros/${{ matrix.ros-distribution }}/setup.bash
         pytest -v -p pytest_timeout --timeout=60 \
-          tests/skrobot_tests/interfaces_tests/ros2/test_base.py
+          tests/skrobot_tests/interfaces_tests/ros2/
 

skrobot/interfaces/ros2/panda.py

@@ -16,37 +16,96 @@
 
 
 class PandaROS2RobotInterface(ROS2RobotInterfaceBase):
+    """ROS 2 interface for a single Franka Panda arm.
+
+    Parameters
+    ----------
+    robot : skrobot.model.RobotModel
+        Robot model. Must expose a limb attribute (see ``limb_attr``)
+        with a 7-joint arm.
+    arm_id : str, default 'panda'
+        Arm namespace from the URDF. Used to derive ``controller_name``
+        when that argument is not given. Two arms on the same machine —
+        e.g. a dual-Panda with arm_id 'right_arm' and 'left_arm' — can be
+        controlled by instantiating this class twice with different
+        arm_ids.
+    controller_name : str or None
+        Joint trajectory controller name. Defaults to
+        ``f'{arm_id}_arm_controller'``. The action namespace is then
+        ``/{controller_name}/follow_joint_trajectory``.
+    gripper_action_prefix : str or None
+        Prefix for the franka_gripper action namespaces. Defaults to
+        ``'franka_gripper'``. For multi-arm setups pass e.g.
+        ``'right_arm/franka_gripper'`` so move / stop go to per-arm
+        action servers.
+    limb_attr : str, default 'rarm'
+        Attribute on ``robot`` whose ``joint_list`` defines the controlled
+        joints. The default matches skrobot's ``Panda`` model, which
+        exposes its 7-joint chain as ``robot.rarm``. For a multi-arm
+        custom model whose limbs are named ``right_arm`` / ``left_arm``,
+        instantiate the interface twice with the matching ``limb_attr``.
+    load_gripper : bool, default True
+        If False, skip creating gripper ActionClients entirely. Useful
+        for sim setups (mock_components) where ``franka_gripper`` is not
+        running, and for unit tests where blocking on
+        ``wait_for_server`` is not desired.
+    """
+
+    def __init__(self, *args,
+                 arm_id='panda',
+                 controller_name=None,
+                 gripper_action_prefix=None,
+                 limb_attr='rarm',
+                 load_gripper=True,
+                 **kwargs):
+        self._arm_id = arm_id
+        self._controller_name = controller_name or '{}_arm_controller'.format(arm_id)
+        self._gripper_action_prefix = gripper_action_prefix or 'franka_gripper'
+        self._limb_attr = limb_attr
 
-    def __init__(self, *args, **kwargs):
         super(PandaROS2RobotInterface, self).__init__(*args, **kwargs)
 
-        if FRANKA_GRIPPER_AVAILABLE:
-            self.gripper_move = ActionClient(
-                self,
-                franka_gripper.action.Move,
-                'franka_gripper/move')
-            self.gripper_move.wait_for_server()
-
-            self.gripper_stop = ActionClient(
-                self,
-                franka_gripper.action.Stop,
-                'franka_gripper/stop')
-            self.gripper_stop.wait_for_server()
-        else:
-            self.get_logger().warn("franka_gripper package not available. Gripper functions disabled.")
+        self.gripper_move = None
+        self.gripper_stop = None
+        if load_gripper:
+            if FRANKA_GRIPPER_AVAILABLE:
+                self.gripper_move = ActionClient(
+                    self,
+                    franka_gripper.action.Move,
+                    '{}/move'.format(self._gripper_action_prefix))
+                self.gripper_move.wait_for_server()
+
+                self.gripper_stop = ActionClient(
+                    self,
+                    franka_gripper.action.Stop,
+                    '{}/stop'.format(self._gripper_action_prefix))
+                self.gripper_stop.wait_for_server()
+            else:
+                self.get_logger().warn(
+                    "franka_gripper package not available. "
+                    "Gripper functions disabled.")
 
     @property
-    def rarm_controller(self):
+    def arm_controller(self):
+        limb = getattr(self.robot, self._limb_attr)
         return dict(
-            controller_type='rarm_controller',
-            controller_action='/panda_arm_controller/follow_joint_trajectory',
-            controller_state='/panda_arm_controller/state',
+            controller_type='{}_controller'.format(self._limb_attr),
+            controller_action='/{}/follow_joint_trajectory'.format(
+                self._controller_name),
+            controller_state='/{}/state'.format(self._controller_name),
             action_type=control_msgs.action.FollowJointTrajectory,
-            joint_names=[j.name for j in self.robot.rarm.joint_list],
+            joint_names=[j.name for j in limb.joint_list],
         )
 
+    @property
+    def rarm_controller(self):
+        # Backwards-compatible alias for the pre-parameterised API where
+        # the only controller was always called rarm_controller. Prefer
+        # `arm_controller` in new code.
+        return self.arm_controller
+
     def default_controller(self):
-        return [self.rarm_controller]
+        return [self.arm_controller]
 
     def grasp(self, width=0, **kwargs):
         self.move_gripper(width=width, **kwargs)
@@ -55,8 +114,11 @@ def ungrasp(self, **kwargs):
         self.move_gripper(width=WIDTH_MAX, **kwargs)
 
     def move_gripper(self, width, speed=WIDTH_MAX, wait=True):
-        if not FRANKA_GRIPPER_AVAILABLE:
-            self.get_logger().warn("franka_gripper package not available. Cannot move gripper.")
+        if self.gripper_move is None:
+            self.get_logger().warn(
+                "Gripper ActionClient was not initialised "
+                "(load_gripper=False or franka_gripper not available). "
+                "Cannot move gripper.")
             return
 
         goal = franka_gripper.action.Move.Goal()
@@ -75,8 +137,11 @@ def move_gripper(self, width, speed=WIDTH_MAX, wait=True):
             self.gripper_move.send_goal_async(goal)
 
     def stop_gripper(self, wait=True):
-        if not FRANKA_GRIPPER_AVAILABLE:
-            self.get_logger().warn("franka_gripper package not available. Cannot stop gripper.")
+        if self.gripper_stop is None:
+            self.get_logger().warn(
+                "Gripper ActionClient was not initialised "
+                "(load_gripper=False or franka_gripper not available). "
+                "Cannot stop gripper.")
             return
 
         goal = franka_gripper.action.Stop.Goal()

tests/skrobot_tests/interfaces_tests/ros2/test_panda.py

@@ -0,0 +1,117 @@
+"""Unit tests for the parameterised PandaROS2RobotInterface."""
+import unittest
+
+import pytest
+
+
+try:
+    import rclpy
+
+    from skrobot.interfaces.ros2.panda import PandaROS2RobotInterface
+    from skrobot.models import Panda
+    ROS2_AVAILABLE = True
+except ImportError:
+    ROS2_AVAILABLE = False
+
+
+pytestmark = pytest.mark.skipif(
+    not ROS2_AVAILABLE, reason="ROS 2 / rclpy not importable")
+
+
+def _make(node_name, load_gripper=False, **kwargs):
+    """Build an interface with controllers / gripper disabled for unit tests.
+
+    Defaults: ``controller_timeout=0.1`` so the no-action-server controller
+    spawn fails fast instead of blocking 3s; ``load_gripper=False`` so the
+    gripper ``ActionClient.wait_for_server`` does not hang waiting for a
+    franka_gripper node that is not running. Tests can override either.
+    """
+    return PandaROS2RobotInterface(
+        robot=Panda(),
+        node_name=node_name,
+        controller_timeout=0.1,
+        load_gripper=load_gripper,
+        joint_states_topic='_test_panda_' + node_name,
+        **kwargs,
+    )
+
+
+class TestPandaInterfaceDefaults(unittest.TestCase):
+
+    @classmethod
+    def setUpClass(cls):
+        if not rclpy.ok():
+            rclpy.init()
+
+    def test_default_controller_action_uses_panda_arm_controller(self):
+        ri = _make('test_default_controller')
+        try:
+            spec = ri.arm_controller
+            assert spec['controller_action'] == \
+                '/panda_arm_controller/follow_joint_trajectory'
+            assert spec['controller_state'] == '/panda_arm_controller/state'
+            # Default limb_attr='rarm' on the Panda model gives 7 joints.
+            assert len(spec['joint_names']) == 7
+        finally:
+            ri.destroy_node()
+
+    def test_rarm_controller_alias_matches_arm_controller(self):
+        ri = _make('test_rarm_alias')
+        try:
+            assert ri.rarm_controller == ri.arm_controller
+        finally:
+            ri.destroy_node()
+
+
+class TestPandaInterfaceParameterisation(unittest.TestCase):
+
+    @classmethod
+    def setUpClass(cls):
+        if not rclpy.ok():
+            rclpy.init()
+
+    def test_arm_id_drives_default_controller_name(self):
+        ri = _make('test_arm_id_drives_name', arm_id='right_arm')
+        try:
+            spec = ri.arm_controller
+            assert spec['controller_action'] == \
+                '/right_arm_arm_controller/follow_joint_trajectory'
+        finally:
+            ri.destroy_node()
+
+    def test_explicit_controller_name_wins_over_arm_id(self):
+        ri = _make('test_explicit_controller_name',
+                   arm_id='right_arm',
+                   controller_name='dual_panda_joint_trajectory_controller')
+        try:
+            spec = ri.arm_controller
+            assert spec['controller_action'] == \
+                '/dual_panda_joint_trajectory_controller/follow_joint_trajectory'
+        finally:
+            ri.destroy_node()
+
+
+class TestPandaInterfaceGripperDisable(unittest.TestCase):
+
+    @classmethod
+    def setUpClass(cls):
+        if not rclpy.ok():
+            rclpy.init()
+
+    def test_load_gripper_false_skips_action_clients(self):
+        ri = _make('test_load_gripper_false')
+        try:
+            assert ri.gripper_move is None
+            assert ri.gripper_stop is None
+        finally:
+            ri.destroy_node()
+
+    def test_grasp_warns_when_gripper_disabled(self):
+        ri = _make('test_grasp_warn')
+        try:
+            # Should be a no-op (no exception, no real ActionClient call).
+            ri.grasp()
+            ri.ungrasp()
+            ri.stop_gripper()
+        finally:
+            ri.destroy_node()