229 lines
7.9 KiB
Python
229 lines
7.9 KiB
Python
"""单臂/双臂通用调试入口。
|
||
|
||
该 launch 文件用于现场调试阶段按需启动左臂、右臂或双臂，并可通过
|
||
`use_mock` 在 mock 模式和 RM75 真机模式之间切换。它会固定启动 UDP
|
||
手柄接收节点，再根据 `arm:=left|right|both` 选择对应的遥操作节点。
|
||
"""
|
||
|
||
from pathlib import Path
|
||
|
||
from launch import LaunchDescription
|
||
from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
|
||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||
from launch_ros.actions import Node
|
||
from launch_ros.substitutions import FindPackageShare
|
||
|
||
|
||
XR_PYTHON = "/home/robot/miniconda3/envs/xr/bin/python"
|
||
|
||
|
||
def _as_bool(value: str) -> bool:
|
||
"""把 launch 字符串参数转换成 Python bool，便于在 OpaqueFunction 中分支。"""
|
||
return value.strip().lower() in ("1", "true", "yes", "on")
|
||
|
||
|
||
def _config_file(name: str) -> PathJoinSubstitution:
|
||
"""生成 xr_rm_bringup/config 下配置文件的可安装路径。"""
|
||
return PathJoinSubstitution([
|
||
FindPackageShare("xr_rm_bringup"),
|
||
"config",
|
||
name,
|
||
])
|
||
|
||
|
||
def _dual_rm75_urdf() -> PathJoinSubstitution:
|
||
return PathJoinSubstitution([
|
||
FindPackageShare("xr_rm_teleop"),
|
||
"models",
|
||
"dual_rm75",
|
||
"Dual_arm.urdf",
|
||
])
|
||
|
||
|
||
def _udp_receiver_node() -> Node:
|
||
"""接收 PICO/XR UDP 数据，并发布左右手柄 ROS2 话题。"""
|
||
return Node(
|
||
package="xr_rm_input",
|
||
executable="udp_controller_receiver",
|
||
name="udp_controller_receiver",
|
||
output="screen",
|
||
on_exit=Shutdown(
|
||
reason="XR UDP receiver exited; stopping arm_debug launch"
|
||
),
|
||
parameters=[{
|
||
"udp_host": LaunchConfiguration("udp_host"),
|
||
"udp_port": LaunchConfiguration("udp_port"),
|
||
"timer_hz": LaunchConfiguration("udp_timer_hz"),
|
||
"left_topic": "/xr/left_controller",
|
||
"right_topic": "/xr/right_controller",
|
||
}],
|
||
)
|
||
|
||
|
||
def _mujoco_node() -> Node:
|
||
"""启动只读双臂 MuJoCo 运动学显示节点。"""
|
||
return Node(
|
||
package="xr_rm_mujoco",
|
||
executable="dual_arm_simulator",
|
||
name="dual_arm_simulator",
|
||
output="screen",
|
||
prefix=[XR_PYTHON],
|
||
parameters=[
|
||
_config_file("dual_arm_mujoco.yaml"),
|
||
{"robot_urdf_path": _dual_rm75_urdf()},
|
||
],
|
||
)
|
||
|
||
|
||
def _validate_mujoco_mode(arm: str, use_mujoco: bool) -> None:
|
||
if use_mujoco and arm != "both":
|
||
raise ValueError("use_mujoco:=true requires arm:=both")
|
||
|
||
|
||
def _validate_act_mode(
|
||
arm: str,
|
||
use_mock: bool,
|
||
record_act: bool,
|
||
) -> None:
|
||
if record_act and (arm != "right" or use_mock):
|
||
raise ValueError(
|
||
"record_act:=true requires arm:=right use_mock:=false"
|
||
)
|
||
|
||
|
||
def _act_recorder_node() -> Node:
|
||
"""创建独立 ACT 数据采集节点；退出时不终止遥操作。"""
|
||
return Node(
|
||
package="xr_rm_teleop",
|
||
executable="act_episode_recorder",
|
||
name="act_episode_recorder",
|
||
output="screen",
|
||
prefix=[XR_PYTHON],
|
||
parameters=[_config_file("act_tomato_pick.yaml")],
|
||
)
|
||
|
||
|
||
def _single_arm_node(
|
||
arm: str,
|
||
use_mock: bool,
|
||
) -> Node:
|
||
"""创建单臂调试节点；左/右臂分别使用独立 YAML，节点名保持单臂默认名。"""
|
||
config_name = "left_arm_rm75.yaml" if arm == "left" else "right_arm_rm75.yaml"
|
||
arm_name = _arm_name(arm)
|
||
return Node(
|
||
package="xr_rm_teleop",
|
||
executable="single_arm_velocity_teleop",
|
||
name="single_arm_velocity_teleop",
|
||
output="screen",
|
||
prefix=[XR_PYTHON],
|
||
parameters=[
|
||
_config_file(config_name),
|
||
{
|
||
"use_mock": use_mock,
|
||
"robot_urdf_path": _dual_rm75_urdf(),
|
||
"peripheral_config_file": _config_file("peripherals_rm75.yaml"),
|
||
"peripheral_arm": arm,
|
||
"tool_command_topic": f"/xr_rm/{arm_name}/tool_enable",
|
||
},
|
||
],
|
||
)
|
||
|
||
|
||
def _arm_name(arm: str) -> str:
|
||
return "left_rm75" if arm == "left" else "right_rm75"
|
||
|
||
|
||
def _dual_arm_nodes(use_mock: bool) -> list[Node]:
|
||
"""创建双臂节点；两个节点共用双臂 YAML，但节点名区分左右臂参数命名空间。"""
|
||
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",
|
||
prefix=[XR_PYTHON],
|
||
parameters=[
|
||
config_file,
|
||
{
|
||
"use_mock": use_mock,
|
||
"robot_urdf_path": _dual_rm75_urdf(),
|
||
"peripheral_config_file": _config_file("peripherals_rm75.yaml"),
|
||
"peripheral_arm": "left",
|
||
"tool_command_topic": "/xr_rm/left_rm75/tool_enable",
|
||
},
|
||
],
|
||
),
|
||
Node(
|
||
package="xr_rm_teleop",
|
||
executable="single_arm_velocity_teleop",
|
||
name="right_arm_teleop",
|
||
output="screen",
|
||
prefix=[XR_PYTHON],
|
||
parameters=[
|
||
config_file,
|
||
{
|
||
"use_mock": use_mock,
|
||
"robot_urdf_path": _dual_rm75_urdf(),
|
||
"peripheral_config_file": _config_file("peripherals_rm75.yaml"),
|
||
"peripheral_arm": "right",
|
||
"tool_command_topic": "/xr_rm/right_rm75/tool_enable",
|
||
},
|
||
],
|
||
),
|
||
]
|
||
|
||
|
||
def _launch_setup(context, *args, **kwargs):
|
||
"""运行时读取 launch 参数，决定启动单臂还是双臂。"""
|
||
del args, kwargs
|
||
if not Path(XR_PYTHON).is_file():
|
||
raise RuntimeError(
|
||
f"XR Python not found: {XR_PYTHON}; "
|
||
"Placo 0.9.4 must not be installed globally"
|
||
)
|
||
arm = LaunchConfiguration("arm").perform(context).strip().lower()
|
||
use_mock = _as_bool(LaunchConfiguration("use_mock").perform(context))
|
||
use_mujoco = _as_bool(
|
||
LaunchConfiguration("use_mujoco").perform(context)
|
||
)
|
||
record_act = _as_bool(
|
||
LaunchConfiguration("record_act").perform(context)
|
||
)
|
||
|
||
if arm not in ("left", "right", "both"):
|
||
raise ValueError("arm must be one of: left, right, both")
|
||
_validate_mujoco_mode(arm, use_mujoco)
|
||
_validate_act_mode(arm, use_mock, record_act)
|
||
|
||
nodes = [_udp_receiver_node()]
|
||
if arm == "both":
|
||
nodes.extend(_dual_arm_nodes(use_mock))
|
||
else:
|
||
nodes.append(_single_arm_node(arm, use_mock))
|
||
if use_mujoco:
|
||
nodes.append(_mujoco_node())
|
||
if record_act:
|
||
nodes.append(_act_recorder_node())
|
||
return nodes
|
||
|
||
|
||
def generate_launch_description() -> LaunchDescription:
|
||
return LaunchDescription([
|
||
# 调试目标：left/right/both；默认右臂方便单臂逐步上真机。
|
||
DeclareLaunchArgument("arm", default_value="right"),
|
||
# true 时只跑 mock，不连接 RM75；false 时通过 RealMan SDK 连接真机。
|
||
DeclareLaunchArgument("use_mock", default_value="true"),
|
||
# true 时额外启动只读 MuJoCo 双臂显示，默认不改变现有启动行为。
|
||
DeclareLaunchArgument("use_mujoco", default_value="false"),
|
||
# true 时只允许右臂真机，并启动独立 ACT 数据采集节点。
|
||
DeclareLaunchArgument("record_act", default_value="false"),
|
||
# UDP 监听参数，需要与 PICO 端或 sample_udp_sender 保持一致。
|
||
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
|
||
DeclareLaunchArgument("udp_port", default_value="15000"),
|
||
# UDP receiver 轮询频率高于 PICO 发送频率，减少 socket 中等待时间。
|
||
DeclareLaunchArgument("udp_timer_hz", default_value="200.0"),
|
||
# OpaqueFunction 允许根据 arm/use_mock 等运行时参数动态生成节点。
|
||
OpaqueFunction(function=_launch_setup),
|
||
])
|