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

235
xr_rm_bringup/tools/launcher_ui.py
View File

@ -1,4 +1,10 @@
#!/usr/bin/env python3
"""XR-RM 桌面调试启动器。
提供 Tkinter 图形界面，按“仿真/左臂/右臂/双臂/诊断”组织常用 ROS2
launch、sample_udp_sender、topic 监控和环境检查命令，降低现场调试时的命令输入成本。
"""
from __future__ import annotations
import ctypes
@ -25,6 +31,13 @@ except ImportError as exc:
DEFAULT_LEFT_IP = "192.168.192.18"
DEFAULT_RIGHT_IP = "192.168.192.19"
DEFAULT_PORT = "15000"
SAMPLE_SENDER_SECONDS = "30"
SAMPLE_SENDER_STAGGERED_SECONDS = "60"
SAMPLE_SENDER_ARGS = (
f"--host 127.0.0.1 --port {DEFAULT_PORT} "
"--pattern axis_sweep --amplitude 0.5 "
"--hold-seconds 4.0 --center-seconds 1.2 --initial-center-seconds 1.0"
)
TERMINAL_TITLE_PREFIX = "XR-RM Terminal - "
TOPIC_MONITOR_TITLE = "XR-RM Topic Monitor"
TOPIC_MONITOR_ACTION = "__xr_rm_topic_monitor__"
@ -62,7 +75,22 @@ def _with_index(items: list[tuple[str, str]]) -> list[tuple[str, str]]:
return [(f"{idx}. {title}", cmd) for idx, (title, cmd) in enumerate(items, start=1)]
def _sample_udp_sender_command(
hand: str,
seconds: str = SAMPLE_SENDER_SECONDS,
both_mode: str = "synchronized",
) -> str:
command = (
f"ros2 run xr_rm_input sample_udp_sender --hand {hand} "
f"{SAMPLE_SENDER_ARGS} --seconds {seconds}"
)
if hand == "both":
command += f" --both-mode {both_mode}"
return command
def _find_workspace_root() -> Path:
# 优先使用显式环境变量，便于从安装目录、源码目录或桌面快捷方式启动。
env_workspace = os.environ.get("XR_RM_WS")
if env_workspace:
return Path(env_workspace).expanduser().resolve()
@ -77,6 +105,7 @@ def _find_workspace_root() -> Path:
def _source_lines(workspace_root: Path) -> list[str]:
# 每个新终端都重新 source ROS 和工作空间，避免依赖启动器自身的 shell 环境。
ros_setup = Path("/opt/ros/humble/setup.bash")
install_setup = workspace_root / "install" / "setup.bash"
lines = [
@ -100,11 +129,13 @@ def _source_lines(workspace_root: Path) -> list[str]:
def _one_click_mock(arm: str, hand: str) -> str:
sender_seconds = SAMPLE_SENDER_STAGGERED_SECONDS if hand == "both" else SAMPLE_SENDER_SECONDS
both_mode = "staggered" if hand == "both" else "synchronized"
return "\n".join([
f"ros2 launch xr_rm_bringup arm_debug.launch.py arm:={arm} use_mock:=true &",
"launch_pid=$!",
"sleep 2",
f"ros2 run xr_rm_input sample_udp_sender --hand {hand} --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 20",
_sample_udp_sender_command(hand, sender_seconds, both_mode),
"echo",
"echo 'Sample sender finished. The launch process is still running in this terminal.'",
"echo 'Press Ctrl-C here, or use the cleanup button in the launcher, to stop it.'",
@ -133,22 +164,23 @@ def _finalize_items(
def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
# UI 列表只维护命令模板；真正执行时统一套上工作空间 source 和终端包装。
if mode == "Simulation":
items = [
("Left Arm Mock Launch", "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true"),
("Right Arm Mock Launch", "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true"),
("Dual Arm Mock Launch", "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true"),
(
"Sample UDP Sender (Left, 10s)",
f"ros2 run xr_rm_input sample_udp_sender --hand left --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10",
"Sample UDP Sender (Left, 30s)",
_sample_udp_sender_command("left"),
),
(
"Sample UDP Sender (Right, 10s)",
f"ros2 run xr_rm_input sample_udp_sender --hand right --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10",
"Sample UDP Sender (Right, 30s)",
_sample_udp_sender_command("right"),
),
(
"Sample UDP Sender (Both, 10s)",
f"ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10",
"Sample UDP Sender (Both Staggered, 60s)",
_sample_udp_sender_command("both", SAMPLE_SENDER_STAGGERED_SECONDS, "staggered"),
),
("One-Click Left Mock Demo", _one_click_mock("left", "left")),
("One-Click Right Mock Demo", _one_click_mock("right", "right")),
@ -165,8 +197,8 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
"move_to_initial_pose_on_connect:=false",
),
(
"Sample UDP Sender (Left, 10s)",
f"ros2 run xr_rm_input sample_udp_sender --hand left --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10",
"Sample UDP Sender (Left, 30s)",
_sample_udp_sender_command("left"),
),
]
one_click = None
@ -180,8 +212,8 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
"move_to_initial_pose_on_connect:=false",
),
(
"Sample UDP Sender (Right, 10s)",
f"ros2 run xr_rm_input sample_udp_sender --hand right --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10",
"Sample UDP Sender (Right, 30s)",
_sample_udp_sender_command("right"),
),
]
one_click = None
@ -196,8 +228,8 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
"move_to_initial_pose_on_connect:=false",
),
(
"Sample UDP Sender (Both, 10s)",
f"ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10",
"Sample UDP Sender (Both Staggered, 60s)",
_sample_udp_sender_command("both", SAMPLE_SENDER_STAGGERED_SECONDS, "staggered"),
),
]
one_click = None
@ -213,6 +245,8 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
class LauncherApp:
"""XR-RM 调试启动器主窗口，负责命令列表、终端启动和监控窗口管理。"""
def __init__(self, root: tk.Tk) -> None:
self.root = root
self.workspace_root = _find_workspace_root()
@ -421,6 +455,7 @@ class LauncherApp:
return result.returncode == 0
def check_prerequisites(self) -> None:
# 环境检查只做轻量探测，避免在 UI 线程里执行耗时的网络或硬件连接动作。
ok: list[str] = []
warnings: list[str] = []
errors: list[str] = []
@ -604,16 +639,72 @@ class LauncherApp:
return Path(script_file.name)
def topic_monitor_rect(self) -> tuple[int, int, int, int]:
screen_width = max(self.root.winfo_screenwidth(), 1)
screen_height = max(self.root.winfo_screenheight(), 1)
width = min(screen_width - 160, max(960, int(screen_width * 0.75)))
height = min(screen_height - 160, max(560, int(screen_height * 0.66)))
width = max(width, min(screen_width, 900))
height = max(height, min(screen_height, 520))
left = max((screen_width - width) // 2, 0)
top = max((screen_height - height) // 2, 0)
# 监控窗口尽量放在启动器所在显示器，双屏现场调试时更容易找回窗口。
monitor_left, monitor_top, monitor_width, monitor_height = self.active_monitor_rect()
width = min(monitor_width - 120, max(960, int(monitor_width * 0.75)))
height = min(monitor_height - 120, max(560, int(monitor_height * 0.66)))
width = max(width, min(monitor_width, 900))
height = max(height, min(monitor_height, 520))
left = monitor_left + max((monitor_width - width) // 2, 0)
top = monitor_top + max((monitor_height - height) // 2, 0)
return width, height, left, top
def active_monitor_rect(self) -> tuple[int, int, int, int]:
monitors = self.xrandr_monitor_rects()
if not monitors:
return (0, 0, max(self.root.winfo_screenwidth(), 1), max(self.root.winfo_screenheight(), 1))
self.root.update_idletasks()
root_center = (
self.root.winfo_rootx() + max(self.root.winfo_width(), 1) // 2,
self.root.winfo_rooty() + max(self.root.winfo_height(), 1) // 2,
)
pointer = (self.root.winfo_pointerx(), self.root.winfo_pointery())
for point_x, point_y in (root_center, pointer):
for left, top, width, height, _primary in monitors:
if left <= point_x < left + width and top <= point_y < top + height:
return (left, top, width, height)
for left, top, width, height, primary in monitors:
if primary:
return (left, top, width, height)
left, top, width, height, _primary = monitors[0]
return (left, top, width, height)
@staticmethod
def xrandr_monitor_rects() -> list[tuple[int, int, int, int, bool]]:
if not shutil.which("xrandr"):
return []
try:
output = subprocess.check_output(
["xrandr", "--query"],
text=True,
stderr=subprocess.DEVNULL,
)
except Exception:
return []
monitors: list[tuple[int, int, int, int, bool]] = []
pattern = re.compile(
r"^\S+ connected(?P<primary> primary)? "
r"(?P<w>\d+)x(?P<h>\d+)\+(?P<x>-?\d+)\+(?P<y>-?\d+)"
)
for line in output.splitlines():
match = pattern.match(line)
if not match:
continue
monitors.append((
int(match.group("x")),
int(match.group("y")),
int(match.group("w")),
int(match.group("h")),
bool(match.group("primary")),
))
return monitors
def window_ids_matching_title(self, title: str) -> list[int]:
if not shutil.which("xprop"):
return []
@ -641,16 +732,87 @@ class LauncherApp:
window_ids.append(int(xid_text, 16))
return window_ids
def force_monitor_window_geometry(self, title: str, attempts_left: int = 40) -> None:
def force_monitor_window_geometry(
self,
title: str,
target_rect: tuple[int, int, int, int],
attempts_left: int = 30,
) -> None:
if attempts_left <= 0 or not self.root.winfo_exists():
return
window_ids = self.window_ids_matching_title(title)
if window_ids:
width, height, left, top = self.topic_monitor_rect()
if not window_ids:
self.root.after(
80,
lambda: self.force_monitor_window_geometry(title, target_rect, attempts_left - 1),
)
return
if any(self.monitor_window_needs_geometry(window_id, target_rect) for window_id in window_ids):
width, height, left, top = target_rect
self.apply_x11_window_geometry(window_ids, left, top, width, height)
self.root.after(250, lambda: self.force_monitor_window_geometry(title, attempts_left - 1))
def monitor_window_needs_geometry(self, window_id: int, target_rect: tuple[int, int, int, int]) -> bool:
props = self.x11_window_props(window_id)
if "_NET_WM_STATE_FULLSCREEN" in props or "_NET_WM_STATE_MAXIMIZED_HORZ" in props:
return True
if "_NET_WM_STATE_MAXIMIZED_VERT" in props:
return True
geometry = self.x11_window_geometry(window_id)
if geometry is None:
return True
actual_left, actual_top, actual_width, actual_height = geometry
target_width, target_height, target_left, target_top = target_rect
if abs(actual_width - target_width) > 180 or abs(actual_height - target_height) > 180:
return True
if abs(actual_left - target_left) > 180 or abs(actual_top - target_top) > 180:
return True
return False
@staticmethod
def x11_window_props(window_id: int) -> str:
if not shutil.which("xprop"):
return ""
try:
return subprocess.check_output(
["xprop", "-id", hex(window_id), "_NET_WM_STATE"],
text=True,
stderr=subprocess.DEVNULL,
)
except Exception:
return ""
@staticmethod
def x11_window_geometry(window_id: int) -> tuple[int, int, int, int] | None:
if not shutil.which("xwininfo"):
return None
try:
output = subprocess.check_output(
["xwininfo", "-id", hex(window_id)],
text=True,
stderr=subprocess.DEVNULL,
)
except Exception:
return None
values: dict[str, int] = {}
patterns = {
"left": r"Absolute upper-left X:\s+(-?\d+)",
"top": r"Absolute upper-left Y:\s+(-?\d+)",
"width": r"Width:\s+(\d+)",
"height": r"Height:\s+(\d+)",
}
for key, pattern in patterns.items():
match = re.search(pattern, output)
if match:
values[key] = int(match.group(1))
if len(values) != 4:
return None
return values["left"], values["top"], values["width"], values["height"]
def apply_x11_window_geometry(self, window_ids: list[int], left: int, top: int, width: int, height: int) -> None:
lib_path = ctypes.util.find_library("X11")
@ -769,8 +931,8 @@ class LauncherApp:
event_mask = (1 << 19) | (1 << 20) # SubstructureNotifyMask | SubstructureRedirectMask
xlib.XSendEvent(display, root, False, event_mask, ctypes.byref(event))
def write_topic_monitor_config(self) -> Path:
width, height, left, top = self.topic_monitor_rect()
def write_topic_monitor_config(self, target_rect: tuple[int, int, int, int] | None = None) -> Path:
width, height, left, top = target_rect or self.topic_monitor_rect()
left_script, right_script = [
self.write_topic_monitor_script(title, topic)
for title, topic in TOPIC_MONITORS
@ -778,6 +940,8 @@ class LauncherApp:
layout_name = "xr_rm_topic_monitor"
config_text = "\n".join([
"[global_config]",
" geometry_hinting = False",
" window_state = normal",
" suppress_multiple_term_dialog = True",
"[keybindings]",
"[profiles]",
@ -822,8 +986,8 @@ class LauncherApp:
config_file.write(config_text)
return Path(config_file.name)
def write_ros_graph_monitor_config(self) -> Path:
width, height, left, top = self.topic_monitor_rect()
def write_ros_graph_monitor_config(self, target_rect: tuple[int, int, int, int] | None = None) -> Path:
width, height, left, top = target_rect or self.topic_monitor_rect()
topic_script, node_script = [
self.write_ros_graph_monitor_script(title, command)
for title, command in ROS_GRAPH_MONITORS
@ -831,6 +995,8 @@ class LauncherApp:
layout_name = "xr_rm_ros_graph_monitor"
config_text = "\n".join([
"[global_config]",
" geometry_hinting = False",
" window_state = normal",
" suppress_multiple_term_dialog = True",
"[keybindings]",
"[profiles]",
@ -892,7 +1058,8 @@ class LauncherApp:
)
return
config_path = self.write_topic_monitor_config()
target_rect = self.topic_monitor_rect()
config_path = self.write_topic_monitor_config(target_rect)
command = [
terminal,
"--no-dbus",
@ -910,7 +1077,7 @@ class LauncherApp:
stdout=subprocess.DEVNULL,
stderr=subprocess.DEVNULL,
)
self.root.after(150, lambda: self.force_monitor_window_geometry(TOPIC_MONITOR_TITLE))
self.root.after(60, lambda: self.force_monitor_window_geometry(TOPIC_MONITOR_TITLE, target_rect))
self.status.config(text="Opened Terminator controller topic monitor.")
except Exception as exc:
messagebox.showerror("Topic Monitor Failed", f"Failed to launch topic monitor:\n{exc}")
@ -933,7 +1100,8 @@ class LauncherApp:
)
return
config_path = self.write_ros_graph_monitor_config()
target_rect = self.topic_monitor_rect()
config_path = self.write_ros_graph_monitor_config(target_rect)
command = [
terminal,
"--no-dbus",
@ -951,7 +1119,7 @@ class LauncherApp:
stdout=subprocess.DEVNULL,
stderr=subprocess.DEVNULL,
)
self.root.after(150, lambda: self.force_monitor_window_geometry(ROS_GRAPH_MONITOR_TITLE))
self.root.after(60, lambda: self.force_monitor_window_geometry(ROS_GRAPH_MONITOR_TITLE, target_rect))
self.status.config(text="Opened Terminator ROS graph monitor.")
except Exception as exc:
messagebox.showerror("ROS Graph Monitor Failed", f"Failed to launch ROS graph monitor:\n{exc}")
@ -997,6 +1165,7 @@ class LauncherApp:
if not confirm:
return
# 只清理由启动器常用命令创建的 ROS/监控进程，并保护当前 UI 进程。
patterns = [
"ros2 launch xr_rm_bringup",
"ros2 run xr_rm_input",