ZhengLiu-cart/acRealman_xr_from_Yikai
1
0
Fork 0
forked from YikaiFu-cart/acRealman_xr
Code Pull Requests Activity

feat: Enhance single arm velocity teleoperation with zero velocity handling and new parameters

Browse Source
This commit is contained in:
YikaiFu-cart
2026-05-26 13:51:15 +08:00
parent e57890cb11
commit f137e28ed7
9 changed files with 933 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

136
xr_rm_bringup/tools/launcher_ui.py
View File

@ -41,6 +41,8 @@ SAMPLE_SENDER_ARGS = (
TERMINAL_TITLE_PREFIX = "XR-RM Terminal - "
TOPIC_MONITOR_TITLE = "XR-RM Topic Monitor"
TOPIC_MONITOR_ACTION = "__xr_rm_topic_monitor__"
CMD_VEL_MONITOR_TITLE = "XR-RM Cmd Vel Monitor"
CMD_VEL_MONITOR_ACTION = "__xr_rm_cmd_vel_monitor__"
ROS_GRAPH_MONITOR_TITLE = "XR-RM ROS Graph Monitor"
ROS_GRAPH_MONITOR_ACTION = "__xr_rm_ros_graph_monitor__"
@ -49,6 +51,11 @@ TOPIC_MONITORS = [
("Right Controller", "/xr/right_controller"),
]
CMD_VEL_MONITORS = [
("Left Cmd Vel", "/xr_rm/left_rm75/cmd_vel"),
("Right Cmd Vel", "/xr_rm/right_rm75/cmd_vel"),
]
ROS_GRAPH_MONITORS = [
("ROS Topic List", "ros2 topic list"),
("ROS Node List", "ros2 node list"),
@ -153,11 +160,40 @@ def _topic_monitor_item() -> tuple[str, str]:
return ("Open Controller Topic Monitor", TOPIC_MONITOR_ACTION)
def _cmd_vel_monitor_item() -> tuple[str, str]:
return ("Open Cmd Vel Topic Monitor", CMD_VEL_MONITOR_ACTION)
def _is_topic_monitor_action(action: str) -> bool:
return action in (TOPIC_MONITOR_ACTION, CMD_VEL_MONITOR_ACTION)
def _topic_monitor_spec(action: str) -> tuple[str, list[tuple[str, str]], str, str, str]:
if action == CMD_VEL_MONITOR_ACTION:
return (
CMD_VEL_MONITOR_TITLE,
CMD_VEL_MONITORS,
"xr_rm_cmd_vel_monitor",
"xr_rm_cmd_vel_monitor_",
"cmd_vel topic",
)
return (
TOPIC_MONITOR_TITLE,
TOPIC_MONITORS,
"xr_rm_topic_monitor",
"xr_rm_topic_monitor_",
"controller topic",
)
def _finalize_items(
items: list[tuple[str, str]],
one_click: tuple[str, str] | None = None,
) -> list[tuple[str, str]]:
final_items = items + _diagnostic_commands() + [_topic_monitor_item()]
final_items = items + _diagnostic_commands() + [
_topic_monitor_item(),
_cmd_vel_monitor_item(),
]
if one_click is not None:
final_items.append(one_click)
return _with_index(final_items)
@ -344,6 +380,7 @@ class LauncherApp:
self.status = tk.Label(root, text="Ready", bd=1, relief=tk.SUNKEN, anchor=tk.W)
self.status.grid(row=1, column=0, sticky="ew")
self.root.protocol("WM_DELETE_WINDOW", self.on_close_requested)
self.refresh_command_list()
def refresh_command_list(self) -> None:
@ -371,9 +408,10 @@ class LauncherApp:
def update_preview(self) -> None:
selected = self.selected_command()
text = selected[1] if selected else ""
if text == TOPIC_MONITOR_ACTION:
topics = ", ".join(topic for _title, topic in TOPIC_MONITORS)
text = f"Open one Terminator window split into controller topic panes:\n{topics}"
if _is_topic_monitor_action(text):
_window_title, monitors, _layout_name, _config_prefix, _label = _topic_monitor_spec(text)
topics = ", ".join(topic for _title, topic in monitors)
text = f"Open one Terminator window split into topic panes:\n{topics}"
elif text == ROS_GRAPH_MONITOR_ACTION:
text = "Open one Terminator window split into 2 horizontal panes:\nros2 topic list | ros2 node list"
self.preview.configure(state=tk.NORMAL)
@ -417,8 +455,8 @@ class LauncherApp:
return
name, command = selected
if command == TOPIC_MONITOR_ACTION:
self.launch_topic_monitor()
if _is_topic_monitor_action(command):
self.launch_topic_monitor(command)
return
if command == ROS_GRAPH_MONITOR_ACTION:
self.launch_ros_graph_monitor()
@ -574,8 +612,8 @@ class LauncherApp:
except OSError:
return Path(terminal).name
def topic_pane_command(self, title: str, topic: str) -> str:
pane_title = f"{TOPIC_MONITOR_TITLE} - {title}"
def topic_pane_command(self, title: str, topic: str, window_title: str = TOPIC_MONITOR_TITLE) -> str:
pane_title = f"{window_title} - {title}"
lines = _source_lines(self.workspace_root)
lines.extend([
"export XR_RM_LAUNCHER_SESSION=1",
@ -610,16 +648,22 @@ class LauncherApp:
])
return "\n".join(lines)
def write_topic_monitor_script(self, title: str, topic: str) -> Path:
def write_topic_monitor_script(
self,
title: str,
topic: str,
window_title: str = TOPIC_MONITOR_TITLE,
config_prefix: str = "xr_rm_topic_monitor_",
) -> Path:
script_file = tempfile.NamedTemporaryFile(
mode="w",
prefix="xr_rm_topic_monitor_",
prefix=config_prefix,
suffix=".sh",
delete=False,
encoding="utf-8",
)
with script_file:
script_file.write(self.topic_pane_command(title, topic))
script_file.write(self.topic_pane_command(title, topic, window_title))
script_file.write("\n")
os.chmod(script_file.name, 0o755)
return Path(script_file.name)
@ -931,13 +975,17 @@ 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, target_rect: tuple[int, int, int, int] | None = None) -> Path:
def write_topic_monitor_config(
self,
target_rect: tuple[int, int, int, int] | None = None,
action: str = TOPIC_MONITOR_ACTION,
) -> Path:
window_title, monitors, layout_name, config_prefix, _label = _topic_monitor_spec(action)
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
self.write_topic_monitor_script(title, topic, window_title, config_prefix)
for title, topic in monitors
]
layout_name = "xr_rm_topic_monitor"
config_text = "\n".join([
"[global_config]",
" geometry_hinting = False",
@ -952,7 +1000,7 @@ class LauncherApp:
" [[[window0]]]",
" type = Window",
" parent = \"\"",
f" title = {_config_quote(TOPIC_MONITOR_TITLE)}",
f" title = {_config_quote(window_title)}",
f" position = {_config_quote(f'{left}:{top}')}",
f" size = {width}, {height}",
" maximised = False",
@ -977,7 +1025,7 @@ class LauncherApp:
])
config_file = tempfile.NamedTemporaryFile(
mode="w",
prefix="xr_rm_topic_monitor_",
prefix=config_prefix,
suffix=".conf",
delete=False,
encoding="utf-8",
@ -1041,7 +1089,8 @@ class LauncherApp:
config_file.write(config_text)
return Path(config_file.name)
def launch_topic_monitor(self) -> None:
def launch_topic_monitor(self, action: str = TOPIC_MONITOR_ACTION) -> None:
window_title, _monitors, layout_name, _config_prefix, label = _topic_monitor_spec(action)
terminal = shutil.which("x-terminal-emulator")
if terminal is None:
messagebox.showerror(
@ -1053,22 +1102,22 @@ class LauncherApp:
if self.x_terminal_target() != "terminator":
messagebox.showwarning(
"Topic Monitor",
"The controller topic monitor uses Terminator's split-layout support. "
f"The {label} monitor uses Terminator's split-layout support. "
"Please set x-terminal-emulator to Terminator.",
)
return
target_rect = self.topic_monitor_rect()
config_path = self.write_topic_monitor_config(target_rect)
config_path = self.write_topic_monitor_config(target_rect, action)
command = [
terminal,
"--no-dbus",
"--title",
TOPIC_MONITOR_TITLE,
window_title,
"--config",
str(config_path),
"--layout",
"xr_rm_topic_monitor",
layout_name,
]
try:
subprocess.Popen(
@ -1077,8 +1126,8 @@ class LauncherApp:
stdout=subprocess.DEVNULL,
stderr=subprocess.DEVNULL,
)
self.root.after(60, lambda: self.force_monitor_window_geometry(TOPIC_MONITOR_TITLE, target_rect))
self.status.config(text="Opened Terminator controller topic monitor.")
self.root.after(60, lambda: self.force_monitor_window_geometry(window_title, target_rect))
self.status.config(text=f"Opened Terminator {label} monitor.")
except Exception as exc:
messagebox.showerror("Topic Monitor Failed", f"Failed to launch topic monitor:\n{exc}")
self.status.config(text="Topic monitor launch failed")
@ -1126,7 +1175,12 @@ class LauncherApp:
self.status.config(text="ROS graph monitor launch failed")
def close_related_terminal_windows(self) -> int:
title_patterns = (TERMINAL_TITLE_PREFIX, TOPIC_MONITOR_TITLE, ROS_GRAPH_MONITOR_TITLE)
title_patterns = (
TERMINAL_TITLE_PREFIX,
TOPIC_MONITOR_TITLE,
CMD_VEL_MONITOR_TITLE,
ROS_GRAPH_MONITOR_TITLE,
)
closed = 0
if shutil.which("wmctrl"):
@ -1157,13 +1211,19 @@ class LauncherApp:
return closed
def on_close_requested(self) -> None:
if self.stop_launched_processes(confirm=True, notify=False):
self.root.destroy()
def kill_launched_processes(self) -> None:
confirm = messagebox.askyesno(
self.stop_launched_processes(confirm=True, notify=True)
def stop_launched_processes(self, *, confirm: bool, notify: bool) -> bool:
if confirm and not messagebox.askyesno(
"Confirm Stop",
"Stop XR-RM launcher terminals, topic monitors, and ROS nodes started from this workspace?",
)
if not confirm:
return
):
return False
# 只清理由启动器常用命令创建的 ROS/监控进程，并保护当前 UI 进程。
patterns = [
@ -1173,14 +1233,18 @@ class LauncherApp:
"XR_RM_LAUNCHER_SESSION=1",
TERMINAL_TITLE_PREFIX,
TOPIC_MONITOR_TITLE,
CMD_VEL_MONITOR_TITLE,
ROS_GRAPH_MONITOR_TITLE,
"xr_rm_topic_monitor_",
"xr_rm_cmd_vel_monitor_",
"xr_rm_ros_graph_monitor_",
"udp_controller_receiver",
"sample_udp_sender",
"single_arm_velocity_teleop",
"ros2 topic echo /xr/left_controller",
"ros2 topic echo /xr/right_controller",
"ros2 topic echo /xr_rm/left_rm75/cmd_vel",
"ros2 topic echo /xr_rm/right_rm75/cmd_vel",
"ros2 topic list",
"ros2 node list",
]
@ -1214,12 +1278,14 @@ class LauncherApp:
self.status.config(
text=f"Stop requested: {len(killed)} matching processes, {closed_windows} terminal windows."
)
messagebox.showinfo(
"Stop Complete",
"Stop requested for:\n"
f"- {len(killed)} matching processes, including launcher terminal wrappers\n"
f"- {closed_windows} related terminal windows",
)
if notify:
messagebox.showinfo(
"Stop Complete",
"Stop requested for:\n"
f"- {len(killed)} matching processes, including launcher terminal wrappers\n"
f"- {closed_windows} related terminal windows",
)
return True
def main() -> None: