ZhengLiu-cart/acRealman_xr_from_Yikai
1
0
Fork 0
forked from YikaiFu-cart/acRealman_xr
Code Pull Requests 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

10
xr_rm_input/launch/udp_receiver.launch.py
View File

@ -1,3 +1,10 @@
"""XR 手柄 UDP 接收节点的独立启动入口。
该 launch 文件只启动 `udp_controller_receiver`，用于低层通信调试：确认 PICO
或 sample_udp_sender 发出的 UDP JSON 能被 ROS2 接收，并发布到指定手柄话题。
完整单臂/双臂遥操作请优先使用 xr_rm_bringup 中的 launch 文件。
"""
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
@ -6,9 +13,12 @@ from launch_ros.actions import Node
def generate_launch_description() -> LaunchDescription:
return LaunchDescription([
# UDP 监听地址和端口，需要与发送端配置一致。
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
DeclareLaunchArgument("udp_port", default_value="15000"),
# 兼容旧版单手柄调试入口；双臂调试会使用 left_topic/right_topic。
DeclareLaunchArgument("topic", default_value="/xr/right_controller"),
# 仅启动 UDP 接收节点，不启动任何机械臂控制节点。
Node(
package="xr_rm_input",
executable="udp_controller_receiver",

6
xr_rm_input/setup.py
View File

@ -1,3 +1,9 @@
"""xr_rm_input 包安装配置。
该包负责把 XR/PICO 端发来的 UDP 手柄数据接入 ROS2，并提供一个
sample_udp_sender 方便在没有真实手柄时做链路调试。
"""
from glob import glob
from setuptools import setup

3
xr_rm_input/xr_rm_input/__init__.py
View File

@ -1 +1,4 @@
"""XR 输入包。
包含 UDP 手柄数据接收节点和用于现场调试的模拟 UDP 发送脚本。
"""

253
xr_rm_input/xr_rm_input/sample_udp_sender.py
View File

@ -1,3 +1,9 @@
"""XR 手柄 UDP 数据模拟发送器。
用于在没有真实 PICO/XR 手柄时向 udp_controller_receiver 发送测试数据。
默认轨迹按 +X/-X、+Y/-Y、+Z/-Z 成对扫轴，便于用肉眼检查机械臂方向映射。
"""
import argparse
import json
import math
@ -5,14 +11,188 @@ import socket
import time
BASE_POS = [0.0, 1.0, 0.0]
# 方向名称会打印到终端，方便现场观察机械臂运动时对照坐标轴。
AXIS_DIRECTIONS = (
("XR +X", (1.0, 0.0, 0.0)),
("XR -X", (-1.0, 0.0, 0.0)),
("XR +Y", (0.0, 1.0, 0.0)),
("XR -Y", (0.0, -1.0, 0.0)),
("XR +Z", (0.0, 0.0, 1.0)),
("XR -Z", (0.0, 0.0, -1.0)),
)
AXIS_SWEEP_SEGMENTS = (
("XR +X right", (1.0, 0.0, 0.0), "hold"),
("XR -X left", (-1.0, 0.0, 0.0), "hold"),
("CENTER / X done", (0.0, 0.0, 0.0), "center"),
("XR +Y up", (0.0, 1.0, 0.0), "hold"),
("XR -Y down", (0.0, -1.0, 0.0), "hold"),
("CENTER / Y done", (0.0, 0.0, 0.0), "center"),
("XR +Z back", (0.0, 0.0, 1.0), "hold"),
("XR -Z front", (0.0, 0.0, -1.0), "hold"),
("CENTER / Z done", (0.0, 0.0, 0.0), "center"),
)
def _validate_positive(name: str, value: float) -> float:
if value <= 0.0:
raise argparse.ArgumentTypeError(f"{name} must be > 0")
return value
def _positive_float(name: str):
def parser(value: str) -> float:
return _validate_positive(name, float(value))
return parser
def _position_from_direction(direction: tuple[float, float, float], amplitude: float) -> list[float]:
return [
BASE_POS[0] + amplitude * direction[0],
BASE_POS[1] + amplitude * direction[1],
BASE_POS[2] + amplitude * direction[2],
]
def _pattern_period(pattern: str, hold_seconds: float, center_seconds: float) -> float:
if pattern == "axis_sweep":
return sum(
hold_seconds if segment_type == "hold" else center_seconds
for _, _, segment_type in AXIS_SWEEP_SEGMENTS
)
if pattern == "axis_steps":
return (hold_seconds + center_seconds) * len(AXIS_DIRECTIONS)
return 5.0
def _axis_sweep_position(
t: float,
amplitude: float,
hold_seconds: float,
center_seconds: float,
initial_center_seconds: float,
) -> tuple[str, list[float]]:
if t < initial_center_seconds:
return "CENTER / lock origin", BASE_POS.copy()
# +方向和-方向连续切换，比每次都很快回中心更容易看清实际运动方向。
cycle_t = (t - initial_center_seconds) % _pattern_period(
"axis_sweep",
hold_seconds,
center_seconds,
)
for label, direction, segment_type in AXIS_SWEEP_SEGMENTS:
duration = hold_seconds if segment_type == "hold" else center_seconds
if cycle_t < duration:
return label, _position_from_direction(direction, amplitude)
cycle_t -= duration
return "CENTER", BASE_POS.copy()
def _axis_step_position(
t: float,
amplitude: float,
hold_seconds: float,
center_seconds: float,
initial_center_seconds: float,
) -> tuple[str, list[float]]:
if t < initial_center_seconds:
return "CENTER / lock origin", BASE_POS.copy()
# 每个方向由“保持目标点”和“回中心”两段组成，循环遍历六个方向。
segment_seconds = hold_seconds + center_seconds
cycle_t = (t - initial_center_seconds) % (segment_seconds * len(AXIS_DIRECTIONS))
direction_index = int(cycle_t // segment_seconds)
segment_t = cycle_t - direction_index * segment_seconds
label, direction = AXIS_DIRECTIONS[direction_index]
if segment_t >= hold_seconds:
return f"CENTER after {label}", BASE_POS.copy()
return label, _position_from_direction(direction, amplitude)
def _sine_position(t: float, amplitude: float, phase: float) -> tuple[str, list[float]]:
return "SINE X", [
amplitude * math.sin(2.0 * math.pi * 0.2 * t + phase),
BASE_POS[1],
BASE_POS[2],
]
def _pattern_position(
pattern: str,
t: float,
amplitude: float,
hold_seconds: float,
center_seconds: float,
initial_center_seconds: float,
phase: float,
) -> tuple[str, list[float]]:
if pattern == "sine":
return _sine_position(t, amplitude, phase)
if pattern == "axis_steps":
return _axis_step_position(
t,
amplitude,
hold_seconds,
center_seconds,
initial_center_seconds,
)
return _axis_sweep_position(
t,
amplitude,
hold_seconds,
center_seconds,
initial_center_seconds,
)
def main() -> None:
parser = argparse.ArgumentParser(description="通过 UDP 发送一段模拟 XR 左/右手柄数据。")
parser.add_argument("--host", default="127.0.0.1")
parser.add_argument("--port", type=int, default=15000)
parser.add_argument("--seconds", type=float, default=5.0)
parser.add_argument("--hz", type=float, default=60.0)
parser.add_argument("--amplitude", type=float, default=0.04)
parser.add_argument("--seconds", type=_positive_float("seconds"), default=30.0)
parser.add_argument("--hz", type=_positive_float("hz"), default=60.0)
parser.add_argument("--amplitude", type=_positive_float("amplitude"), default=0.50)
parser.add_argument(
"--pattern",
choices=("axis_sweep", "axis_steps", "sine"),
default="axis_sweep",
help="axis_sweep 成对扫轴最适合肉眼看方向；axis_steps 每个方向后回中心；sine 保留旧轨迹。",
)
parser.add_argument(
"--hold-seconds",
type=_positive_float("hold-seconds"),
default=4.0,
help="axis_sweep/axis_steps 模式下，每个方向保持多久。增大可让机械臂走得更明显。",
)
parser.add_argument(
"--center-seconds",
type=_positive_float("center-seconds"),
default=1.2,
help="axis_sweep/axis_steps 模式下，回中心段保持多久。",
)
parser.add_argument(
"--initial-center-seconds",
type=_positive_float("initial-center-seconds"),
default=1.0,
help="axis_sweep/axis_steps 模式下，开头保持中心点多久，用于让遥操节点锁定原点。",
)
parser.add_argument(
"--trigger",
type=float,
default=0.0,
help="发送的 trigger 值。方向调试默认不驱动夹爪。",
)
parser.add_argument("--hand", choices=("left", "right", "both"), default="right")
parser.add_argument(
"--both-mode",
choices=("synchronized", "staggered"),
default="synchronized",
help="hand=both 时 synchronized 左右臂同时动；staggered 先左后右，便于逐只观察。",
)
args = parser.parse_args()
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
@ -20,37 +200,86 @@ def main() -> None:
t0 = time.monotonic()
hands = ("left", "right") if args.hand == "both" else (args.hand,)
# 每只手柄独立发送同格式数据包，接收端根据 hand 字段分发左右话题。
packets = {
hand: {
"t": 0.0,
"hand": hand,
"grip": False,
"trigger": 0.0,
"pos": [0.0, 1.0, 0.0],
"pos": BASE_POS.copy(),
"quat": [0.0, 0.0, 0.0, 1.0],
}
for hand in hands
}
pattern_period = _pattern_period(args.pattern, args.hold_seconds, args.center_seconds)
one_hand_seconds = args.initial_center_seconds + pattern_period
full_sweep_seconds = one_hand_seconds * len(hands) if args.both_mode == "staggered" else one_hand_seconds
print(
"Sample UDP sender: "
f"hand={args.hand}, both_mode={args.both_mode}, pattern={args.pattern}, "
f"amplitude={args.amplitude:.3f}m, hold={args.hold_seconds:.2f}s, "
f"center={args.center_seconds:.2f}s, seconds={args.seconds:.1f}s",
flush=True,
)
if args.pattern != "sine" and args.seconds < full_sweep_seconds:
print(
"提示：当前 seconds 不足以跑完一轮完整方向序列；"
f"建议至少 {full_sweep_seconds:.1f}s。",
flush=True,
)
last_labels = {hand: None for hand in hands}
while time.monotonic() - t0 < args.seconds:
t = time.monotonic() - t0
# 模拟按住握持键后缓慢移动手柄，用来测试相对位移控制链路。
# 默认用清晰的成对扫轴测试相对位移链路，便于肉眼确认坐标映射。
for index, packet in enumerate(packets.values()):
phase = index * math.pi
hand = packet["hand"]
pattern_t = t
if args.hand == "both" and args.both_mode == "staggered":
hand_start_t = index * one_hand_seconds
hand_end_t = hand_start_t + one_hand_seconds
if t < hand_start_t:
label, pos = "WAIT / center", BASE_POS.copy()
elif t >= hand_end_t:
label, pos = "DONE / center", BASE_POS.copy()
else:
pattern_t = t - hand_start_t
label, pos = _pattern_position(
args.pattern,
pattern_t,
args.amplitude,
args.hold_seconds,
args.center_seconds,
args.initial_center_seconds,
index * math.pi,
)
else:
label, pos = _pattern_position(
args.pattern,
pattern_t,
args.amplitude,
args.hold_seconds,
args.center_seconds,
args.initial_center_seconds,
index * math.pi,
)
if label != last_labels[hand]:
print(f"[{t:5.2f}s] {hand:<5} {label:<22} pos={pos}", flush=True)
last_labels[hand] = label
packet["t"] = t
packet["grip"] = True
packet["trigger"] = max(0.0, math.sin(2.0 * math.pi * 0.1 * t))
packet["pos"] = [
args.amplitude * math.sin(2.0 * math.pi * 0.2 * t + phase),
1.0,
0.0,
]
packet["trigger"] = min(max(args.trigger, 0.0), 1.0)
packet["pos"] = pos
sock.sendto(json.dumps(packet).encode("utf-8"), (args.host, args.port))
time.sleep(dt)
for packet in packets.values():
packet["grip"] = False
packet["trigger"] = 0.0
packet["pos"] = BASE_POS.copy()
sock.sendto(json.dumps(packet).encode("utf-8"), (args.host, args.port))

8
xr_rm_input/xr_rm_input/udp_controller_receiver.py
View File

@ -1,3 +1,9 @@
"""XR 手柄 UDP 接收节点。
从 UDP JSON 数据包中解析左右手柄位姿、握持键和扳机值，并发布为
`xr_rm_interfaces/XrController` 消息，供遥操作和夹爪节点订阅。
"""
import json
import socket
from collections.abc import Iterable, Mapping
@ -90,6 +96,7 @@ class UdpControllerReceiver(Node):
controllers = payload.get("controllers", payload.get("controller"))
if isinstance(controllers, Mapping):
# 兼容两类格式：controllers 本身就是单个手柄，或按 left/right 分组。
if self._contains_pose(controllers):
hand = self._normalize_hand(controllers.get("hand", self._default_hand))
yield hand, controllers
@ -143,6 +150,7 @@ class UdpControllerReceiver(Node):
def _extract_pose(self, payload: Mapping[str, Any]) -> tuple[list[float], list[float]]:
pose = payload.get("pose")
# 支持多种常见字段命名，方便 Unity/OpenXR/Python 调试脚本接入同一节点。
if isinstance(pose, Mapping):
pos = pose.get("position", pose.get("pos", pose.get("p")))
quat = pose.get("orientation", pose.get("quat", pose.get("q", [0.0, 0.0, 0.0, 1.0])))