Add configuration files and launch scripts for left and right arm RM75 teleoperation

2026-05-21 17:25:59 +08:00
parent 5a48619599
commit 4dea1ae530
16 changed files with 1576 additions and 122 deletions
--- a/xr_rm_bringup/launch/arm_debug.launch.py
+++ b/xr_rm_bringup/launch/arm_debug.launch.py
@ -0,0 +1,121 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def _as_bool(value: str) -> bool:
+    return value.strip().lower() in ("1", "true", "yes", "on")
+
+
+def _config_file(name: str) -> PathJoinSubstitution:
+    return PathJoinSubstitution([
+        FindPackageShare("xr_rm_bringup"),
+        "config",
+        name,
+    ])
+
+
+def _udp_receiver_node() -> Node:
+    return Node(
+        package="xr_rm_input",
+        executable="udp_controller_receiver",
+        name="udp_controller_receiver",
+        output="screen",
+        parameters=[{
+            "udp_host": LaunchConfiguration("udp_host"),
+            "udp_port": LaunchConfiguration("udp_port"),
+            "left_topic": "/xr/left_controller",
+            "right_topic": "/xr/right_controller",
+        }],
+    )
+
+
+def _single_arm_node(arm: str, use_mock: bool, move_to_initial_pose: bool) -> Node:
+    config_name = "left_arm_rm75.yaml" if arm == "left" else "right_arm_rm75.yaml"
+    robot_ip = LaunchConfiguration("left_robot_ip" if arm == "left" else "right_robot_ip")
+    return Node(
+        package="xr_rm_teleop",
+        executable="single_arm_velocity_teleop",
+        name="single_arm_velocity_teleop",
+        output="screen",
+        parameters=[
+            _config_file(config_name),
+            {
+                "use_mock": use_mock,
+                "robot_ip": robot_ip,
+                "robot_port": LaunchConfiguration("robot_port"),
+                "move_to_initial_pose_on_connect": move_to_initial_pose,
+            },
+        ],
+    )
+
+
+def _dual_arm_nodes(use_mock: bool, move_to_initial_pose: bool) -> list[Node]:
+    config_file = _config_file("dual_arm_rm75.yaml")
+    return [
+        Node(
+            package="xr_rm_teleop",
+            executable="single_arm_velocity_teleop",
+            name="left_arm_teleop",
+            output="screen",
+            parameters=[
+                config_file,
+                {
+                    "use_mock": use_mock,
+                    "robot_ip": LaunchConfiguration("left_robot_ip"),
+                    "robot_port": LaunchConfiguration("robot_port"),
+                    "move_to_initial_pose_on_connect": move_to_initial_pose,
+                },
+            ],
+        ),
+        Node(
+            package="xr_rm_teleop",
+            executable="single_arm_velocity_teleop",
+            name="right_arm_teleop",
+            output="screen",
+            parameters=[
+                config_file,
+                {
+                    "use_mock": use_mock,
+                    "robot_ip": LaunchConfiguration("right_robot_ip"),
+                    "robot_port": LaunchConfiguration("robot_port"),
+                    "move_to_initial_pose_on_connect": move_to_initial_pose,
+                },
+            ],
+        ),
+    ]
+
+
+def _launch_setup(context, *args, **kwargs):
+    del args, kwargs
+    arm = LaunchConfiguration("arm").perform(context).strip().lower()
+    use_mock = _as_bool(LaunchConfiguration("use_mock").perform(context))
+    move_to_initial_pose = _as_bool(
+        LaunchConfiguration("move_to_initial_pose_on_connect").perform(context)
+    )
+
+    if arm not in ("left", "right", "both"):
+        raise ValueError("arm must be one of: left, right, both")
+
+    nodes = [_udp_receiver_node()]
+    if arm == "both":
+        nodes.extend(_dual_arm_nodes(use_mock, move_to_initial_pose))
+    else:
+        nodes.append(_single_arm_node(arm, use_mock, move_to_initial_pose))
+    return nodes
+
+
+def generate_launch_description() -> LaunchDescription:
+    return LaunchDescription([
+        DeclareLaunchArgument("arm", default_value="right"),
+        DeclareLaunchArgument("use_mock", default_value="true"),
+        DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
+        DeclareLaunchArgument("udp_port", default_value="15000"),
+        DeclareLaunchArgument("left_robot_ip", default_value="192.168.192.18"),
+        DeclareLaunchArgument("right_robot_ip", default_value="192.168.192.19"),
+        DeclareLaunchArgument("robot_port", default_value="8080"),
+        DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"),
+        OpaqueFunction(function=_launch_setup),
+    ])