forked from YikaiFu-cart/acRealman_xr
202 lines
8.0 KiB
Python
202 lines
8.0 KiB
Python
"""单臂/双臂通用调试入口。
|
||
|
||
该 launch 文件用于现场调试阶段按需启动左臂、右臂或双臂，并可通过
|
||
`use_mock` 在 mock 模式和 RM75 真机模式之间切换。它会固定启动 UDP
|
||
手柄接收节点，再根据 `arm:=left|right|both` 选择对应的遥操作节点。
|
||
"""
|
||
|
||
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:
|
||
"""把 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 _udp_receiver_node() -> Node:
|
||
"""接收 PICO/XR UDP 数据，并发布左右手柄 ROS2 话题。"""
|
||
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,
|
||
avoid_singularity: int,
|
||
frame_type: int,
|
||
configure_safety_limits: bool,
|
||
) -> Node:
|
||
"""创建单臂调试节点；左/右臂分别使用独立 YAML，节点名保持单臂默认名。"""
|
||
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"),
|
||
"avoid_singularity": avoid_singularity,
|
||
"frame_type": frame_type,
|
||
"configure_safety_limits": configure_safety_limits,
|
||
"move_to_initial_pose_on_connect": move_to_initial_pose,
|
||
},
|
||
],
|
||
)
|
||
|
||
|
||
def _dual_arm_nodes(
|
||
use_mock: bool,
|
||
move_to_initial_pose: bool,
|
||
left_avoid_singularity: int,
|
||
right_avoid_singularity: int,
|
||
frame_type: int,
|
||
configure_safety_limits: 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",
|
||
parameters=[
|
||
config_file,
|
||
{
|
||
"use_mock": use_mock,
|
||
"robot_ip": LaunchConfiguration("left_robot_ip"),
|
||
"robot_port": LaunchConfiguration("robot_port"),
|
||
"avoid_singularity": left_avoid_singularity,
|
||
"frame_type": frame_type,
|
||
"configure_safety_limits": configure_safety_limits,
|
||
"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"),
|
||
"avoid_singularity": right_avoid_singularity,
|
||
"frame_type": frame_type,
|
||
"configure_safety_limits": configure_safety_limits,
|
||
"move_to_initial_pose_on_connect": move_to_initial_pose,
|
||
},
|
||
],
|
||
),
|
||
]
|
||
|
||
|
||
def _launch_setup(context, *args, **kwargs):
|
||
"""运行时读取 launch 参数，决定启动单臂还是双臂。"""
|
||
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)
|
||
)
|
||
avoid_override = LaunchConfiguration("avoid_singularity").perform(context).strip()
|
||
left_avoid_singularity = int(
|
||
avoid_override or LaunchConfiguration("left_avoid_singularity").perform(context)
|
||
)
|
||
right_avoid_singularity = int(
|
||
avoid_override or LaunchConfiguration("right_avoid_singularity").perform(context)
|
||
)
|
||
frame_type = int(LaunchConfiguration("frame_type").perform(context))
|
||
configure_safety_limits = _as_bool(
|
||
LaunchConfiguration("configure_safety_limits").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,
|
||
left_avoid_singularity,
|
||
right_avoid_singularity,
|
||
frame_type,
|
||
configure_safety_limits,
|
||
)
|
||
)
|
||
else:
|
||
avoid_singularity = left_avoid_singularity if arm == "left" else right_avoid_singularity
|
||
nodes.append(
|
||
_single_arm_node(
|
||
arm,
|
||
use_mock,
|
||
move_to_initial_pose,
|
||
avoid_singularity,
|
||
frame_type,
|
||
configure_safety_limits,
|
||
)
|
||
)
|
||
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"),
|
||
# UDP 监听参数，需要与 PICO 端或 sample_udp_sender 保持一致。
|
||
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
|
||
DeclareLaunchArgument("udp_port", default_value="15000"),
|
||
# 左右 RM75 默认 IP，可在命令行中按现场网络覆盖。
|
||
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("left_avoid_singularity", default_value="0"),
|
||
DeclareLaunchArgument("right_avoid_singularity", default_value="1"),
|
||
# 非空时作为左右臂全局覆盖，例如 avoid_singularity:=0。
|
||
DeclareLaunchArgument("avoid_singularity", default_value=""),
|
||
DeclareLaunchArgument("frame_type", default_value="1"),
|
||
DeclareLaunchArgument("configure_safety_limits", default_value="true"),
|
||
# 默认不自动移动到初始点，确认安全区后再显式打开。
|
||
DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"),
|
||
# OpaqueFunction 允许根据 arm/use_mock 等运行时参数动态生成节点。
|
||
OpaqueFunction(function=_launch_setup),
|
||
])
|