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

feat: Implement UDP feedback for RM75 robot arms

Browse Source
This commit is contained in:
YikaiFu-cart
2026-07-29 15:26:59 +08:00
parent 687a0b401a
commit 08996434e5
16 changed files with 1600 additions and 329 deletions
Download Patch File Download Diff File Expand all files Collapse all files
xr_rm_bringup/config/dual_arm_rm75.yaml
+14 -2
View File
@@ -39,14 +39,20 @@ left_arm_teleop:
0.0, 0.0, 1.0,
-1.0, 0.0, 0.0]
use_mock: false
robot_ip: 192.168.192.18
robot_port: 8080
realtime_push_host_ip: 192.168.192.148
realtime_push_port: 8089
realtime_push_cycle_ms: 5
avoid_singularity: 0
follow: false
canfd_trajectory_mode: 2
canfd_radio: 0
configure_safety_limits: true
enable_tool_control: true
enable_trigger_gripper_control: true
trigger_close_threshold: 0.95
configure_peripheral_on_connect: true
max_line_speed: 1.0
max_angular_speed: 1.5
max_line_acc: 1.0
@@ -89,14 +95,20 @@ right_arm_teleop:
0.0, 0.0, 1.0,
1.0, 0.0, 0.0]
use_mock: false
robot_ip: 192.168.192.19
robot_port: 8080
realtime_push_host_ip: 192.168.192.148
realtime_push_port: 8090
realtime_push_cycle_ms: 5
avoid_singularity: 1
follow: false
canfd_trajectory_mode: 2
canfd_radio: 0
configure_safety_limits: true
enable_tool_control: true
enable_trigger_gripper_control: true
trigger_close_threshold: 0.95
configure_peripheral_on_connect: true
max_line_speed: 1.0
max_angular_speed: 1.5
max_line_acc: 1.0
xr_rm_bringup/config/left_arm_rm75.yaml
+7 -1
View File
@@ -32,14 +32,20 @@ single_arm_velocity_teleop:
0.0, 0.0, 1.0,
-1.0, 0.0, 0.0]
use_mock: false
robot_ip: 192.168.192.18
robot_port: 8080
realtime_push_host_ip: 192.168.192.148
realtime_push_port: 8089
realtime_push_cycle_ms: 5
avoid_singularity: 0
follow: false
canfd_trajectory_mode: 2
canfd_radio: 0
configure_safety_limits: true
enable_tool_control: true
enable_trigger_gripper_control: true
trigger_close_threshold: 0.95
configure_peripheral_on_connect: true
max_line_speed: 1.0
max_angular_speed: 1.5
max_line_acc: 1.0
xr_rm_bringup/config/right_arm_rm75.yaml
+9 -2
View File
@@ -31,14 +31,21 @@ single_arm_velocity_teleop:
0.0, 0.0, 1.0,
1.0, 0.0, 0.0]
use_mock: false
robot_ip: 192.168.192.19
robot_port: 8080
realtime_push_host_ip: 192.168.192.148
realtime_push_port: 8090
realtime_push_cycle_ms: 5
avoid_singularity: 1
follow: False
# 厂商 MovejCANFD 示例默认低跟随；高跟随仅在验证规划轨迹后单独开启。
follow: false
canfd_trajectory_mode: 2
canfd_radio: 0
configure_safety_limits: true
enable_tool_control: true
enable_trigger_gripper_control: true
trigger_close_threshold: 0.95
configure_peripheral_on_connect: true
max_line_speed: 0.25
max_angular_speed: 0.6
max_line_acc: 1.3
xr_rm_bringup/launch/arm_debug.launch.py
+3 -141
View File
@@ -41,10 +41,6 @@ def _rm75_urdf() -> PathJoinSubstitution:
])
def _initial_pose_override(value: str) -> dict[str, bool]:
return {} if value == "auto" else {"move_to_initial_pose_on_connect": _as_bool(value)}
def _udp_receiver_node() -> Node:
"""接收 PICO/XR UDP 数据，并发布左右手柄 ROS2 话题。"""
return Node(
@@ -65,19 +61,9 @@ def _udp_receiver_node() -> Node:
def _single_arm_node(
arm: str,
use_mock: bool,
move_to_initial_pose: str,
avoid_singularity: int,
control_rate_hz: float,
follow: bool,
configure_safety_limits: bool,
enable_tool_control: bool,
enable_trigger_gripper_control: bool,
trigger_close_threshold: float,
configure_peripheral_on_connect: 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")
arm_name = _arm_name(arm)
return Node(
package="xr_rm_teleop",
@@ -89,18 +75,7 @@ def _single_arm_node(
_config_file(config_name),
{
"use_mock": use_mock,
"robot_ip": robot_ip,
"robot_port": LaunchConfiguration("robot_port"),
"avoid_singularity": avoid_singularity,
"robot_urdf_path": _rm75_urdf(),
"control_rate_hz": control_rate_hz,
"follow": follow,
"configure_safety_limits": configure_safety_limits,
**_initial_pose_override(move_to_initial_pose),
"enable_tool_control": enable_tool_control,
"enable_trigger_gripper_control": enable_trigger_gripper_control,
"trigger_close_threshold": trigger_close_threshold,
"configure_peripheral_on_connect": configure_peripheral_on_connect,
"peripheral_config_file": _config_file("peripherals_rm75.yaml"),
"peripheral_arm": arm,
"tool_command_topic": f"/xr_rm/{arm_name}/tool_enable",
@@ -113,19 +88,7 @@ def _arm_name(arm: str) -> str:
return "left_rm75" if arm == "left" else "right_rm75"
def _dual_arm_nodes(
use_mock: bool,
move_to_initial_pose: str,
left_avoid_singularity: int,
right_avoid_singularity: int,
control_rate_hz: float,
follow: bool,
configure_safety_limits: bool,
enable_tool_control: bool,
enable_trigger_gripper_control: bool,
trigger_close_threshold: float,
configure_peripheral_on_connect: bool,
) -> list[Node]:
def _dual_arm_nodes(use_mock: bool) -> list[Node]:
"""创建双臂节点；两个节点共用双臂 YAML，但节点名区分左右臂参数命名空间。"""
config_file = _config_file("dual_arm_rm75.yaml")
return [
@@ -139,18 +102,7 @@ def _dual_arm_nodes(
config_file,
{
"use_mock": use_mock,
"robot_ip": LaunchConfiguration("left_robot_ip"),
"robot_port": LaunchConfiguration("robot_port"),
"avoid_singularity": left_avoid_singularity,
"robot_urdf_path": _rm75_urdf(),
"control_rate_hz": control_rate_hz,
"follow": follow,
"configure_safety_limits": configure_safety_limits,
**_initial_pose_override(move_to_initial_pose),
"enable_tool_control": enable_tool_control,
"enable_trigger_gripper_control": enable_trigger_gripper_control,
"trigger_close_threshold": trigger_close_threshold,
"configure_peripheral_on_connect": configure_peripheral_on_connect,
"peripheral_config_file": _config_file("peripherals_rm75.yaml"),
"peripheral_arm": "left",
"tool_command_topic": "/xr_rm/left_rm75/tool_enable",
@@ -167,18 +119,7 @@ def _dual_arm_nodes(
config_file,
{
"use_mock": use_mock,
"robot_ip": LaunchConfiguration("right_robot_ip"),
"robot_port": LaunchConfiguration("robot_port"),
"avoid_singularity": right_avoid_singularity,
"robot_urdf_path": _rm75_urdf(),
"control_rate_hz": control_rate_hz,
"follow": follow,
"configure_safety_limits": configure_safety_limits,
**_initial_pose_override(move_to_initial_pose),
"enable_tool_control": enable_tool_control,
"enable_trigger_gripper_control": enable_trigger_gripper_control,
"trigger_close_threshold": trigger_close_threshold,
"configure_peripheral_on_connect": configure_peripheral_on_connect,
"peripheral_config_file": _config_file("peripherals_rm75.yaml"),
"peripheral_arm": "right",
"tool_command_topic": "/xr_rm/right_rm75/tool_enable",
@@ -198,71 +139,15 @@ def _launch_setup(context, *args, **kwargs):
)
arm = LaunchConfiguration("arm").perform(context).strip().lower()
use_mock = _as_bool(LaunchConfiguration("use_mock").perform(context))
move_to_initial_pose = LaunchConfiguration(
"move_to_initial_pose_on_connect"
).perform(context).strip().lower()
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)
)
control_rate_hz = float(LaunchConfiguration("control_rate_hz").perform(context))
follow = _as_bool(LaunchConfiguration("follow").perform(context))
configure_safety_limits = _as_bool(
LaunchConfiguration("configure_safety_limits").perform(context)
)
configure_peripheral_on_connect = _as_bool(
LaunchConfiguration("configure_peripheral_on_connect").perform(context)
)
enable_tool_control = _as_bool(
LaunchConfiguration("enable_tool_control").perform(context)
)
enable_trigger_gripper_control = _as_bool(
LaunchConfiguration("enable_trigger_gripper_control").perform(context)
)
trigger_close_threshold = float(
LaunchConfiguration("trigger_close_threshold").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,
control_rate_hz,
follow,
configure_safety_limits,
enable_tool_control,
enable_trigger_gripper_control,
trigger_close_threshold,
configure_peripheral_on_connect,
)
)
nodes.extend(_dual_arm_nodes(use_mock))
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,
control_rate_hz,
follow,
configure_safety_limits,
enable_tool_control,
enable_trigger_gripper_control,
trigger_close_threshold,
configure_peripheral_on_connect,
)
)
nodes.append(_single_arm_node(arm, use_mock))
return nodes
@@ -277,29 +162,6 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument("udp_port", default_value="15000"),
# UDP receiver 轮询频率高于 PICO 发送频率，减少 socket 中等待时间。
DeclareLaunchArgument("udp_timer_hz", default_value="200.0"),
# 左右 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=""),
# 每周期同步一次实际关节反馈、执行一次 Placo QP，再发送 rm_movej_canfd。
DeclareLaunchArgument("control_rate_hz", default_value="125.0"),
# 默认低跟随；高跟随请确认控制器和网络能稳定满足厂商周期要求后再打开。
DeclareLaunchArgument("follow", default_value="false"),
DeclareLaunchArgument("configure_safety_limits", default_value="true"),
# 工具控制通过遥操作节点复用同一个 RealMan 连接，避免两个进程抢同一机械臂连接。
DeclareLaunchArgument("enable_tool_control", default_value="true"),
# trigger 上升沿切换夹爪开/关；grip 仍只控制机械臂运动。
DeclareLaunchArgument("enable_trigger_gripper_control", default_value="true"),
DeclareLaunchArgument("trigger_close_threshold", default_value="0.95"),
# 连接成功后是否配置外设；关闭后仅订阅开合话题，但开合前需要另行完成外设配置。
DeclareLaunchArgument("configure_peripheral_on_connect", default_value="true"),
# auto 时由单/双臂 YAML 决定；也可显式传 true/false 覆盖。
DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="auto"),
# OpaqueFunction 允许根据 arm/use_mock 等运行时参数动态生成节点。
OpaqueFunction(function=_launch_setup),
])
xr_rm_bringup/test/test_launcher_ui_cleanup.py
+94
View File
@@ -0,0 +1,94 @@
import importlib.util
import signal
import subprocess
import unittest
from pathlib import Path
from unittest import mock
MODULE_PATH = Path(__file__).parents[1] / "tools" / "launcher_ui.py"
SPEC = importlib.util.spec_from_file_location("launcher_ui", MODULE_PATH)
launcher_ui = importlib.util.module_from_spec(SPEC)
assert SPEC.loader is not None
SPEC.loader.exec_module(launcher_ui)
class LauncherCleanupTest(unittest.TestCase):
def test_xrobotoolkit_cleanup_policy_only_stops_service_on_window_close(self) -> None:
stop_all_patterns = set(
launcher_ui._xrobotoolkit_cleanup_patterns(stop_pc_service=False)
)
window_close_patterns = set(
launcher_ui._xrobotoolkit_cleanup_patterns(stop_pc_service=True)
)
self.assertLessEqual(
{"RobotLinuxDemo.x86_64", "PXREAClientUnity"},
stop_all_patterns,
)
self.assertNotIn("RoboticsServiceProcess", stop_all_patterns)
self.assertIn("RoboticsServiceProcess", window_close_patterns)
def test_close_and_stop_all_select_different_pc_service_policies(self) -> None:
app = object.__new__(launcher_ui.LauncherApp)
calls = []
class Root:
destroyed = False
def destroy(self) -> None:
self.destroyed = True
app.root = Root()
app.stop_launched_processes = lambda **kwargs: calls.append(kwargs) or True
app.kill_launched_processes()
app.on_close_requested()
self.assertEqual(
calls,
[
{"confirm": True, "notify": True, "stop_pc_service": False},
{"confirm": True, "notify": False, "stop_pc_service": True},
],
)
self.assertTrue(app.root.destroyed)
def test_stop_all_keeps_oldest_pc_service_and_stops_duplicates(self) -> None:
app = object.__new__(launcher_ui.LauncherApp)
app.status = mock.Mock()
app.close_related_terminal_windows = lambda: 0
def fake_check_output(command, **_kwargs):
if command == ["pgrep", "-o", "-f", "RoboticsServiceProcess"]:
return "101\n"
if command == ["pgrep", "-f", "RoboticsServiceProcess"]:
return "101\n202\n303\n"
raise subprocess.CalledProcessError(1, command)
with (
mock.patch.object(
launcher_ui.subprocess,
"check_output",
side_effect=fake_check_output,
),
mock.patch.object(launcher_ui.os, "kill") as kill,
mock.patch.object(launcher_ui.time, "sleep"),
):
app.stop_launched_processes(
confirm=False,
notify=False,
stop_pc_service=False,
)
self.assertEqual(
kill.call_args_list,
[
mock.call(202, signal.SIGTERM),
mock.call(303, signal.SIGTERM),
],
)
if __name__ == "__main__":
unittest.main()
xr_rm_bringup/tools/launcher_ui.py
+48 -11
View File
@@ -121,6 +121,17 @@ def _xrobotoolkit_bridge_command() -> str:
)
def _xrobotoolkit_cleanup_patterns(*, stop_pc_service: bool) -> tuple[str, ...]:
patterns = (
XROBOTOOLKIT_BRIDGE_EXECUTABLE,
"RobotLinuxDemo.x86_64",
"PXREAClientUnity",
)
if stop_pc_service:
return (*patterns, "RoboticsServiceProcess")
return patterns
def _tool_command(arm: str, open_tool: bool) -> str:
arm_name = "left_rm75" if arm == "left" else "right_rm75"
value = "true" if open_tool else "false"
@@ -289,8 +300,7 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
("Ping Left RM75", f"ping -c 4 {DEFAULT_LEFT_IP}"),
(
"Left Arm RealMan Launch",
"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false "
f"left_robot_ip:={DEFAULT_LEFT_IP}",
"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false",
),
("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
("Left Tool Open", _tool_command("left", True)),
@@ -306,8 +316,7 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
("Ping Right RM75", f"ping -c 4 {DEFAULT_RIGHT_IP}"),
(
"Right Arm RealMan Launch",
"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false "
f"right_robot_ip:={DEFAULT_RIGHT_IP}",
"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false",
),
("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
("Right Tool Open", _tool_command("right", True)),
@@ -324,9 +333,7 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
("Ping Right RM75", f"ping -c 4 {DEFAULT_RIGHT_IP}"),
(
"Dual Arm RealMan Launch",
"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false "
f"left_robot_ip:={DEFAULT_LEFT_IP} right_robot_ip:={DEFAULT_RIGHT_IP} "
"move_to_initial_pose_on_connect:=false",
"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false",
),
("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
(
@@ -1283,13 +1290,27 @@ class LauncherApp:
return closed
def on_close_requested(self) -> None:
if self.stop_launched_processes(confirm=True, notify=False):
if self.stop_launched_processes(
confirm=True,
notify=False,
stop_pc_service=True,
):
self.root.destroy()
def kill_launched_processes(self) -> None:
self.stop_launched_processes(confirm=True, notify=True)
self.stop_launched_processes(
confirm=True,
notify=True,
stop_pc_service=False,
)
def stop_launched_processes(self, *, confirm: bool, notify: bool) -> bool:
def stop_launched_processes(
self,
*,
confirm: bool,
notify: bool,
stop_pc_service: bool,
) -> bool:
if confirm and not messagebox.askyesno(
"Confirm Stop",
"Stop XR-RM launcher terminals, topic monitors, ROS nodes, and bridge processes started from this workspace?",
@@ -1302,7 +1323,7 @@ class LauncherApp:
"ros2 run xr_rm_input",
"ros2 run xr_rm_teleop",
"XR_RM_LAUNCHER_SESSION=1",
XROBOTOOLKIT_BRIDGE_EXECUTABLE,
*_xrobotoolkit_cleanup_patterns(stop_pc_service=stop_pc_service),
TERMINAL_TITLE_PREFIX,
TOPIC_MONITOR_TITLE,
CMD_VEL_MONITOR_TITLE,
@@ -1326,6 +1347,20 @@ class LauncherApp:
"ros2 topic list",
"ros2 node list",
]
pc_service_keep_pid: int | None = None
if not stop_pc_service:
try:
output = subprocess.check_output(
["pgrep", "-o", "-f", "RoboticsServiceProcess"],
text=True,
)
pc_service_keep_pid = int(output.strip())
patterns.append("RoboticsServiceProcess")
except (subprocess.CalledProcessError, ValueError):
pass
except Exception as exc:
print(f"Failed to find the oldest RoboticsServiceProcess: {exc}")
protected = {os.getpid(), os.getppid()}
killed: set[int] = set()
@@ -1345,6 +1380,8 @@ class LauncherApp:
continue
if pid in protected or pid in killed:
continue
if pattern == "RoboticsServiceProcess" and pid == pc_service_keep_pid:
continue
try:
os.kill(pid, signal.SIGTERM)
killed.add(pid)