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

20
xr_rm_bringup/config/dual_arm_rm75.yaml
View File

@ -23,7 +23,7 @@ left_arm_teleop:
kp_linear: 1.8
deadband_m: 0.002
low_pass_alpha: 0.35
max_linear_speed: 0.12
max_linear_speed: 0.2
enable_position_axes: [true, true, true]
# 来自 acDual-arm 的 bounds_p[left]：
@ -36,10 +36,11 @@ left_arm_teleop:
# PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。
# 左臂 base 坐标：+X 向下，+Z 向右，前方工作区对应 -Y。
# 映射关系：机器人位移增量 = [-手柄y, -手柄z, 手柄x]。
# 现场模拟 XR 调试确认 X/Z 轴符号与原标定相反。
# 映射关系：机器人位移增量 = [-手柄y, 手柄z, -手柄x]。
xr_to_robot_matrix: [0.0, -1.0, 0.0,
0.0, 0.0, -1.0,
1.0, 0.0, 0.0]
0.0, 0.0, 1.0,
-1.0, 0.0, 0.0]
use_mock: false
mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
@ -76,23 +77,24 @@ right_arm_teleop:
kp_linear: 1.8
deadband_m: 0.002
low_pass_alpha: 0.35
max_linear_speed: 0.12
max_linear_speed: 0.2
enable_position_axes: [true, true, true]
# 来自 acDual-arm 的 bounds_p[right]：
# x[-0.70, 0.50]，y[-0.60, 0.40]，z[0.10, 0.70]。
workspace_min: [-0.70, -0.60, 0.10]
workspace_max: [0.50, 0.40, 0.70]
workspace_max: [0.70, 0.40, 0.70]
cyl_radius_limit: [0.20, 0.60]
low_z_threshold: 0.20
low_z_min_radius: 0.21
# PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。
# 右臂 base 坐标：+X 向上，+Z 向左，前方工作区对应 -Y。
# 映射关系：机器人位移增量 = [手柄y, -手柄z, -手柄x]。
# 现场模拟 XR 调试确认 X/Z 轴符号与原标定相反。
# 映射关系：机器人位移增量 = [手柄y, 手柄z, 手柄x]。
xr_to_robot_matrix: [0.0, 1.0, 0.0,
0.0, 0.0, -1.0,
-1.0, 0.0, 0.0]
0.0, 0.0, 1.0,
1.0, 0.0, 0.0]
use_mock: false
mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]

5
xr_rm_bringup/config/left_arm_rm75.yaml
View File

@ -20,9 +20,10 @@ single_arm_velocity_teleop:
low_z_threshold: 0.20
low_z_min_radius: 0.21
# 现场模拟 XR 调试确认 X/Z 轴符号与原标定相反，因此翻转手柄 X、Z 两列。
xr_to_robot_matrix: [0.0, -1.0, 0.0,
0.0, 0.0, -1.0,
1.0, 0.0, 0.0]
0.0, 0.0, 1.0,
-1.0, 0.0, 0.0]
use_mock: false
mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]

5
xr_rm_bringup/config/right_arm_rm75.yaml
View File

@ -20,9 +20,10 @@ single_arm_velocity_teleop:
low_z_threshold: 0.20
low_z_min_radius: 0.21
# 现场模拟 XR 调试确认 X/Z 轴符号与原标定相反，因此翻转手柄 X、Z 两列。
xr_to_robot_matrix: [0.0, 1.0, 0.0,
0.0, 0.0, -1.0,
-1.0, 0.0, 0.0]
0.0, 0.0, 1.0,
1.0, 0.0, 0.0]
use_mock: false
mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]

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:

319
xr_rm_bringup/tools/realman_dual_arm_state_monitor.py
View File

@ -12,12 +12,42 @@ from __future__ import annotations
import argparse
import time
from dataclasses import dataclass
from typing import Optional
from numbers import Number
from pathlib import Path
from typing import Any
import numpy as np
import yaml
from realman_pkg.Robotic_Arm.rm_robot_interface import RoboticArm, rm_thread_mode_e
from realman_pkg.core.arg_cfg import host_ip, ip_l, ip_r, port_l, port_r
DEFAULT_HOST_IP = "0.0.0.0"
DEFAULT_LEFT_IP = "192.168.192.18"
DEFAULT_RIGHT_IP = "192.168.192.19"
DEFAULT_ROBOT_PORT = 8080
def default_config_path() -> Path:
"""同时兼容源码目录和 colcon install 后的配置文件路径。"""
script_path = Path(__file__).resolve()
candidates = [
script_path.parents[1] / "config" / "dual_arm_rm75.yaml",
script_path.parents[2] / "share" / "xr_rm_bringup" / "config" / "dual_arm_rm75.yaml",
]
try:
from ament_index_python.packages import get_package_share_directory
candidates.append(Path(get_package_share_directory("xr_rm_bringup")) / "config" / "dual_arm_rm75.yaml")
except Exception:
pass
for candidate in candidates:
if candidate.is_file():
return candidate
return candidates[0]
DEFAULT_CONFIG_PATH = default_config_path()
@dataclass
@ -32,6 +62,245 @@ class ArmSnapshot:
err_text: str
@dataclass
class MonitorDefaults:
"""从 YAML 配置中读取到的监控默认参数。"""
config_path: Path
left_ip: str = DEFAULT_LEFT_IP
right_ip: str = DEFAULT_RIGHT_IP
left_port: int = DEFAULT_ROBOT_PORT
right_port: int = DEFAULT_ROBOT_PORT
host_ip: str = DEFAULT_HOST_IP
def _load_yaml(path: Path) -> dict[str, Any]:
"""读取 ROS 参数 YAML，返回顶层 dict。"""
if not path.is_file():
raise FileNotFoundError(f"配置文件不存在: {path}")
with path.open("r", encoding="utf-8") as f:
data = yaml.safe_load(f) or {}
if not isinstance(data, dict):
raise ValueError(f"配置文件格式错误，顶层应为 dict: {path}")
return data
def _node_parameters(config: dict[str, Any], node_name: str) -> dict[str, Any]:
node_config = config.get(node_name, {})
if not isinstance(node_config, dict):
return {}
params = node_config.get("ros__parameters", {})
return params if isinstance(params, dict) else {}
def load_monitor_defaults(config_path: Path) -> MonitorDefaults:
"""从双臂配置读取左右臂连接参数。"""
config = _load_yaml(config_path)
left_params = _node_parameters(config, "left_arm_teleop")
right_params = _node_parameters(config, "right_arm_teleop")
return MonitorDefaults(
config_path=config_path,
left_ip=str(left_params.get("robot_ip", DEFAULT_LEFT_IP)),
right_ip=str(right_params.get("robot_ip", DEFAULT_RIGHT_IP)),
left_port=int(left_params.get("robot_port", DEFAULT_ROBOT_PORT)),
right_port=int(right_params.get("robot_port", DEFAULT_ROBOT_PORT)),
)
def load_realman_sdk() -> tuple[Any, Any]:
"""延迟导入睿尔曼 SDK，让 --help 和参数解析不依赖现场环境。"""
try:
from Robotic_Arm.rm_robot_interface import RoboticArm, rm_thread_mode_e
except ImportError as exc:
raise RuntimeError(
"未安装睿尔曼 Python API2，无法连接真机。请先安装厂商 SDK，"
"确认可以导入 Robotic_Arm.rm_robot_interface。"
) from exc
return RoboticArm, rm_thread_mode_e
def resolve_thread_mode(rm_thread_mode_e: Any, mode: int) -> Any:
"""兼容不同 SDK 版本的线程模式枚举写法。"""
try:
return rm_thread_mode_e(mode)
except Exception:
pass
mode_names = {
0: "RM_SINGLE_MODE_E",
1: "RM_DUAL_MODE_E",
2: "RM_TRIPLE_MODE_E",
}
mode_name = mode_names.get(int(mode))
if mode_name and hasattr(rm_thread_mode_e, mode_name):
return getattr(rm_thread_mode_e, mode_name)
return mode
def create_robot_arm(robot: Any, robot_ip: str, robot_port: int, level: int) -> Any:
"""兼容 rm_create_robot_arm(ip, port) 和 rm_create_robot_arm(ip, port, level)。"""
try:
return robot.rm_create_robot_arm(robot_ip, robot_port, level)
except TypeError:
return robot.rm_create_robot_arm(robot_ip, robot_port)
def robot_handle_id(handle: Any) -> int | None:
"""从 SDK handle 中取 id，兼容对象和整数返回值。"""
if isinstance(handle, int):
return handle
return getattr(handle, "id", None)
def return_code(obj: Any) -> int:
"""从不同 SDK 返回结构中解析 ret/code。"""
if isinstance(obj, tuple) and obj and isinstance(obj[0], int):
return obj[0]
if isinstance(obj, dict):
for key in ("ret", "code", "err_code", "error_code"):
value = obj.get(key)
if isinstance(value, int):
return value
return 0
def payload(obj: Any) -> Any:
"""从 (ret, data) 风格返回值中取状态主体。"""
if isinstance(obj, tuple) and len(obj) >= 2 and isinstance(obj[0], int):
return obj[1]
return obj
def _as_number_list(value: Any, expected_len: int) -> list[float] | None:
if isinstance(value, np.ndarray):
value = value.tolist()
if isinstance(value, (list, tuple)) and len(value) >= expected_len:
if all(isinstance(item, Number) for item in value[:expected_len]):
return [float(item) for item in value[:expected_len]]
return None
def _as_pose(value: Any) -> list[float] | None:
pose = _as_number_list(value, 6)
if pose is not None:
return pose
if isinstance(value, dict):
position = value.get("position")
euler = value.get("euler")
if isinstance(position, dict) and isinstance(euler, dict):
xyz_keys = ("x", "y", "z")
rpy_keys = ("rx", "ry", "rz")
if all(key in position for key in xyz_keys) and all(key in euler for key in rpy_keys):
return [
float(position["x"]),
float(position["y"]),
float(position["z"]),
float(euler["rx"]),
float(euler["ry"]),
float(euler["rz"]),
]
if all(hasattr(value, attr) for attr in ("position", "euler")):
position = getattr(value, "position")
euler = getattr(value, "euler")
if all(hasattr(position, key) for key in ("x", "y", "z")) and all(
hasattr(euler, key) for key in ("rx", "ry", "rz")
):
return [
float(position.x),
float(position.y),
float(position.z),
float(euler.rx),
float(euler.ry),
float(euler.rz),
]
return None
def find_pose(obj: Any) -> list[float] | None:
"""递归查找常见 TCP 位姿字段。"""
pose = _as_pose(obj)
if pose is not None:
return pose
if isinstance(obj, dict):
for key in ("pose", "tool_pose", "tcp_pose", "current_pose"):
pose = _as_pose(obj.get(key))
if pose is not None:
return pose
for value in obj.values():
pose = find_pose(value)
if pose is not None:
return pose
elif isinstance(obj, (list, tuple)):
for value in obj:
pose = find_pose(value)
if pose is not None:
return pose
elif hasattr(obj, "to_dictionary"):
try:
return find_pose(obj.to_dictionary(7))
except TypeError:
return find_pose(obj.to_dictionary())
elif hasattr(obj, "to_dict"):
return find_pose(obj.to_dict())
else:
for key in ("pose", "tool_pose", "tcp_pose", "current_pose"):
if hasattr(obj, key):
pose = _as_pose(getattr(obj, key))
if pose is not None:
return pose
return None
def find_joint(obj: Any) -> list[float] | None:
"""递归查找常见关节角字段。"""
joint = _as_number_list(obj, 7)
if joint is not None:
return joint
if isinstance(obj, dict):
for key in ("joint", "joints", "joint_deg", "joint_position", "joint_positions"):
joint = _as_number_list(obj.get(key), 7)
if joint is not None:
return joint
for value in obj.values():
joint = find_joint(value)
if joint is not None:
return joint
elif isinstance(obj, (list, tuple)):
for value in obj:
joint = find_joint(value)
if joint is not None:
return joint
elif hasattr(obj, "to_dictionary"):
try:
return find_joint(obj.to_dictionary(7))
except TypeError:
return find_joint(obj.to_dictionary())
elif hasattr(obj, "to_dict"):
return find_joint(obj.to_dict())
else:
for key in ("joint", "joints", "joint_deg", "joint_position", "joint_positions"):
if hasattr(obj, key):
joint = _as_number_list(getattr(obj, key), 7)
if joint is not None:
return joint
return None
def find_error_text(obj: Any) -> str:
if isinstance(obj, dict):
for key in ("err", "error", "error_msg", "err_msg"):
if key in obj:
return str(obj[key])
return "ok"
class ArmStatePoller:
"""单臂状态轮询器。"""
@ -49,7 +318,7 @@ class ArmStatePoller:
self.level = int(level)
self.mode = int(mode)
self.robot: Optional[RoboticArm] = None
self.robot: Any | None = None
self.robot_handle = None
self.connected = False
@ -67,9 +336,11 @@ class ArmStatePoller:
def _connect(self):
"""连接机械臂。"""
try:
self.robot = RoboticArm(rm_thread_mode_e(self.mode))
self.robot_handle = self.robot.rm_create_robot_arm(self.robot_ip, self.robot_port, self.level)
if self.robot_handle.id == -1:
RoboticArm, rm_thread_mode_e = load_realman_sdk()
self.robot = RoboticArm(resolve_thread_mode(rm_thread_mode_e, self.mode))
self.robot_handle = create_robot_arm(self.robot, self.robot_ip, self.robot_port, self.level)
handle_id = robot_handle_id(self.robot_handle)
if self.robot_handle is None or handle_id == -1:
self.connected = False
self.last_snapshot.connected = False
self.last_snapshot.err_text = f"connect failed ({self.robot_ip}:{self.robot_port})"
@ -79,7 +350,7 @@ class ArmStatePoller:
self.connected = True
self.last_snapshot.connected = True
self.last_snapshot.err_text = "ok"
print(f"[{self.name}] 已连接, handle_id={self.robot_handle.id}, ip={self.robot_ip}, port={self.robot_port}")
print(f"[{self.name}] 已连接, handle_id={handle_id}, ip={self.robot_ip}, port={self.robot_port}")
except Exception as exc:
self.connected = False
self.last_snapshot.connected = False
@ -92,7 +363,9 @@ class ArmStatePoller:
return self.last_snapshot
try:
ret, state = self.robot.rm_get_current_arm_state()
raw_state = self.robot.rm_get_current_arm_state()
ret = return_code(raw_state)
state = payload(raw_state)
now_t = time.time()
if ret != 0:
@ -100,9 +373,9 @@ class ArmStatePoller:
self.last_snapshot.err_text = f"rm_get_current_arm_state ret={ret}"
return self.last_snapshot
joint = list(state.get("joint", [0.0] * 7))
pose = list(state.get("pose", [0.0] * 6))
err = state.get("err", {})
joint = find_joint(state) or [0.0] * 7
pose = find_pose(state) or [0.0] * 6
err = find_error_text(state)
if len(joint) < 7:
joint.extend([0.0] * (7 - len(joint)))
@ -156,22 +429,32 @@ def format_arm_block(name: str, snap: ArmSnapshot, now_t: float) -> str:
def build_arg_parser() -> argparse.ArgumentParser:
bootstrap = argparse.ArgumentParser(add_help=False)
bootstrap.add_argument("--config", type=Path, default=DEFAULT_CONFIG_PATH)
known_args, _ = bootstrap.parse_known_args()
defaults = load_monitor_defaults(known_args.config)
parser = argparse.ArgumentParser(description="Realman 双臂状态监控脚本（轮询版）")
parser.add_argument("--left-ip", default=ip_l, help="左臂 IP")
parser.add_argument("--right-ip", default=ip_r, help="右臂 IP")
parser.add_argument("--left-port", type=int, default=port_l, help="左臂 TCP 端口")
parser.add_argument("--right-port", type=int, default=port_r, help="右臂 TCP 端口")
parser.add_argument("--config", type=Path, default=defaults.config_path, help="双臂 YAML 配置文件")
parser.add_argument("--left-ip", default=defaults.left_ip, help="左臂 IP")
parser.add_argument("--right-ip", default=defaults.right_ip, help="右臂 IP")
parser.add_argument("--left-port", type=int, default=defaults.left_port, help="左臂 TCP 端口")
parser.add_argument("--right-port", type=int, default=defaults.right_port, help="右臂 TCP 端口")
parser.add_argument("--refresh", type=float, default=0.5, help="刷新周期（秒）")
parser.add_argument("--level", type=int, default=3, help="连接级别")
parser.add_argument("--mode", type=int, default=2, help="线程模式 0/1/2")
parser.add_argument("--host-ip", default=host_ip, help="仅展示当前配置用，轮询版不使用")
parser.add_argument("--host-ip", default=defaults.host_ip, help="仅展示当前配置用，轮询版不使用")
return parser
def main():
args = build_arg_parser().parse_args()
print(f"配置: left={args.left_ip}:{args.left_port}, right={args.right_ip}:{args.right_port}, host_ip={args.host_ip}")
print(
f"配置: config={args.config}, "
f"left={args.left_ip}:{args.left_port}, "
f"right={args.right_ip}:{args.right_port}, host_ip={args.host_ip}"
)
left = ArmStatePoller(
name="LEFT",