YikaiFu-cart/acRealman_xr
Template
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

dual arm control from sample_udp_sender.py

Browse Source
This commit is contained in:
YikaiFu-cart
2026-05-25 17:02:37 +08:00
parent 340bd9138d
commit 7551f1e8ea
18 changed files with 976 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

88
xr_rm_bringup/launch/arm_debug.launch.py
View File

@ -1,3 +1,10 @@
"""单臂/双臂通用调试入口。
该 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
@ -6,10 +13,12 @@ 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",
@ -18,6 +27,7 @@ def _config_file(name: str) -> PathJoinSubstitution:
def _udp_receiver_node() -> Node:
"""接收 PICO/XR UDP 数据，并发布左右手柄 ROS2 话题。"""
return Node(
package="xr_rm_input",
executable="udp_controller_receiver",
@ -32,7 +42,15 @@ def _udp_receiver_node() -> Node:
)
def _single_arm_node(arm: str, use_mock: bool, move_to_initial_pose: bool) -> Node:
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(
@ -46,13 +64,24 @@ def _single_arm_node(arm: str, use_mock: bool, move_to_initial_pose: bool) -> No
"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) -> list[Node]:
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(
@ -66,6 +95,9 @@ def _dual_arm_nodes(use_mock: bool, move_to_initial_pose: bool) -> list[Node]:
"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,
},
],
@ -81,6 +113,9 @@ def _dual_arm_nodes(use_mock: bool, move_to_initial_pose: bool) -> list[Node]:
"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,
},
],
@ -89,33 +124,78 @@ def _dual_arm_nodes(use_mock: bool, move_to_initial_pose: bool) -> list[Node]:
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))
nodes.extend(
_dual_arm_nodes(
use_mock,
move_to_initial_pose,
left_avoid_singularity,
right_avoid_singularity,
frame_type,
configure_safety_limits,
)
)
else:
nodes.append(_single_arm_node(arm, use_mock, move_to_initial_pose))
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),
])

10
xr_rm_bringup/launch/dual_arm_mock.launch.py
View File

@ -1,3 +1,10 @@
"""双臂 mock 遥操作启动入口。
该 launch 文件用于不连接真实 RM75 的离线闭环验证：启动 UDP 手柄接收节点，
并分别启动左、右两个 `single_arm_velocity_teleop` 节点。两个遥操作节点使用
同一份双臂配置文件，但通过节点名读取各自的参数命名空间。
"""
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
@ -7,6 +14,7 @@ from launch_ros.substitutions import FindPackageShare
def generate_launch_description() -> LaunchDescription:
# 通过 robot_config 参数选择 xr_rm_bringup/config 下的双臂配置文件。
config_file = PathJoinSubstitution([
FindPackageShare("xr_rm_bringup"),
"config",
@ -14,9 +22,11 @@ def generate_launch_description() -> LaunchDescription:
])
return LaunchDescription([
# UDP 监听参数，需要和 PICO 端或 sample_udp_sender 的目标地址保持一致。
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
DeclareLaunchArgument("udp_port", default_value="15000"),
DeclareLaunchArgument("robot_config", default_value="dual_arm_rm75.yaml"),
# 这些开关便于单独验证 UDP、左臂或右臂链路。
DeclareLaunchArgument("run_udp", default_value="true"),
DeclareLaunchArgument("run_left_arm", default_value="true"),
DeclareLaunchArgument("run_right_arm", default_value="true"),

15
xr_rm_bringup/launch/dual_arm_realman.launch.py
View File

@ -1,3 +1,10 @@
"""双 RM75 真机遥操作启动入口。
该 launch 文件用于连接左右两台真实 RM75：启动 UDP 手柄接收节点，并分别启动
左、右两个真机遥操作节点。默认不会自动移动到初始点，只有显式设置
`move_to_initial_pose_on_connect:=true` 后才会执行初始化点位移动。
"""
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
@ -8,6 +15,7 @@ from launch_ros.substitutions import FindPackageShare
def generate_launch_description() -> LaunchDescription:
# 双臂真机默认读取 xr_rm_bringup/config/dual_arm_rm75.yaml。
config_file = PathJoinSubstitution([
FindPackageShare("xr_rm_bringup"),
"config",
@ -15,16 +23,21 @@ def generate_launch_description() -> LaunchDescription:
])
return LaunchDescription([
# UDP 监听参数，需要与 PICO 端发送地址和端口匹配。
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
DeclareLaunchArgument("udp_port", default_value="15000"),
# 左右 RM75 的默认现场 IP，可在 launch 命令中按实际网络覆盖。
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("robot_config", default_value="dual_arm_rm75.yaml"),
# 调试时可关闭部分节点，只验证 UDP 或单侧机械臂。
DeclareLaunchArgument("run_udp", default_value="true"),
DeclareLaunchArgument("run_left_arm", default_value="true"),
DeclareLaunchArgument("run_right_arm", default_value="true"),
# 真机默认不自动动臂，确认安全区清空后再手动打开。
DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"),
# 接收 PICO/XR 侧 UDP 数据，并按左右手字段分发到对应手柄话题。
Node(
package="xr_rm_input",
executable="udp_controller_receiver",
@ -38,6 +51,7 @@ def generate_launch_description() -> LaunchDescription:
"right_topic": "/xr/right_controller",
}],
),
# 左臂真机节点：连接 left_robot_ip，并读取 left_arm_teleop 参数命名空间。
Node(
package="xr_rm_teleop",
executable="single_arm_velocity_teleop",
@ -57,6 +71,7 @@ def generate_launch_description() -> LaunchDescription:
},
],
),
# 右臂真机节点：连接 right_robot_ip，并读取 right_arm_teleop 参数命名空间。
Node(
package="xr_rm_teleop",
executable="single_arm_velocity_teleop",