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

Enhanced gripper control functionality

Browse Source
This commit is contained in:
YikaiFu-cart
2026-06-08 14:30:08 +08:00
parent 3fc8024648
commit d103439f01
11 changed files with 175 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
xr_rm_teleop/xr_rm_teleop/fun_peripheral.py
View File

@ -5,7 +5,8 @@ scissorgripper 参数约定：
1：Modbus 控制的电动夹爪
2：控制器 DO3/DO4 控制的小型剪刀夹爪
当前 XR 工程只在真机连接阶段做外设初始化；主遥操作流程不调用开合命令。
当前 XR 工程在真机连接阶段初始化外设，遥操作节点随后复用同一个
RealMan 连接执行手动 topic 或 grip-gated trigger 夹爪开合命令。
"""
from __future__ import annotations

132
xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
View File

@ -7,6 +7,8 @@
from __future__ import annotations
import math
import queue
import threading
from typing import Iterable
import rclpy
@ -76,6 +78,8 @@ class SingleArmVelocityTeleop(Node):
self.declare_parameter("canfd_trajectory_mode", 2)
self.declare_parameter("canfd_radio", 0)
self.declare_parameter("enable_tool_control", True)
self.declare_parameter("enable_trigger_gripper_control", True)
self.declare_parameter("trigger_close_threshold", 0.95)
self.declare_parameter("configure_peripheral_on_connect", True)
self.declare_parameter("peripheral_config_file", "")
self.declare_parameter("peripheral_arm", "")
@ -106,6 +110,9 @@ class SingleArmVelocityTeleop(Node):
self._xr_to_robot_matrix = self._float_list_parameter("xr_to_robot_matrix", 9)
self._current_pose_poll_hz = float(self.get_parameter("current_pose_poll_hz").value)
self._follow = self._bool_parameter("follow")
self._enable_tool_control = self._bool_parameter("enable_tool_control")
self._enable_trigger_gripper_control = self._bool_parameter("enable_trigger_gripper_control")
self._trigger_close_threshold = float(self.get_parameter("trigger_close_threshold").value)
self._debug_topic_prefix = str(self.get_parameter("debug_topic_prefix").value).rstrip("/")
if not self._debug_topic_prefix:
self._debug_topic_prefix = "/xr_rm"
@ -122,6 +129,10 @@ class SingleArmVelocityTeleop(Node):
self._last_current_pose: ArmPose | None = None
self._last_current_pose_time: Time | None = None
self._stop_sent = True
self._last_trigger_tool_open: bool | None = None
self._tool_command_queue: queue.Queue[tuple[bool, str] | None] | None = None
self._tool_worker_stop = threading.Event()
self._tool_worker_thread: threading.Thread | None = None
self._adapter = self._make_adapter()
self._adapter.connect()
@ -171,7 +182,11 @@ class SingleArmVelocityTeleop(Node):
)
def _setup_tool_control(self) -> None:
if not self._bool_parameter("enable_tool_control"):
if not self._enable_tool_control:
if self._enable_trigger_gripper_control:
self.get_logger().warn(
f"{self._arm_name} trigger 夹爪控制已请求，但 enable_tool_control=false，已禁用。"
)
return
peripheral_arm = self._peripheral_arm_name()
@ -179,21 +194,84 @@ class SingleArmVelocityTeleop(Node):
if self._bool_parameter("configure_peripheral_on_connect"):
self._adapter.configure_peripheral(config_file, peripheral_arm)
self._start_tool_worker()
topic = str(self.get_parameter("tool_command_topic").value).strip()
if not topic:
topic = f"{self._debug_topic_prefix}/{self._arm_name}/tool_enable"
self.create_subscription(Bool, topic, self._on_tool_command, 10)
self.get_logger().info(f"{self._arm_name} 工具控制已启用，监听：{topic} (true=open, false=close)")
trigger_text = (
f", trigger 需 grip=true 且 >= {self._trigger_close_threshold:.2f} 时闭合"
if self._enable_trigger_gripper_control
else ""
)
self.get_logger().info(
f"{self._arm_name} 工具控制已启用，监听：{topic} "
f"(true=open, false=close{trigger_text})"
)
def _on_tool_command(self, msg: Bool) -> None:
open_tool = bool(msg.data)
action = "open" if open_tool else "close"
try:
self._adapter.set_tool_enabled(open_tool)
except Exception as exc:
self.get_logger().error(f"{self._arm_name} tool {action} failed: {exc}")
self._last_trigger_tool_open = None
self._enqueue_tool_command(open_tool, "topic")
def _start_tool_worker(self) -> None:
if self._tool_worker_thread is not None:
return
self.get_logger().info(f"{self._arm_name} tool {action} command sent")
self._tool_command_queue = queue.Queue(maxsize=1)
self._tool_worker_stop.clear()
self._tool_worker_thread = threading.Thread(
target=self._tool_worker_loop,
name=f"{self._arm_name}_tool_worker",
daemon=True,
)
self._tool_worker_thread.start()
def _enqueue_tool_command(self, open_tool: bool, source: str) -> None:
if self._tool_command_queue is None:
self.get_logger().warn(
f"{self._arm_name} 工具控制未启用，忽略 {source} 夹爪命令。",
throttle_duration_sec=1.0,
)
return
item = (open_tool, source)
while True:
try:
self._tool_command_queue.put_nowait(item)
return
except queue.Full:
try:
self._tool_command_queue.get_nowait()
self._tool_command_queue.task_done()
except queue.Empty:
continue
def _tool_worker_loop(self) -> None:
assert self._tool_command_queue is not None
while not self._tool_worker_stop.is_set():
try:
item = self._tool_command_queue.get(timeout=0.1)
except queue.Empty:
continue
try:
if item is None:
return
open_tool, source = item
action = "open" if open_tool else "close"
try:
self._adapter.set_tool_enabled(open_tool)
except Exception as exc:
self.get_logger().error(
f"{self._arm_name} tool {action} failed from {source}: {exc}"
)
continue
self.get_logger().info(
f"{self._arm_name} tool {action} command sent from {source}"
)
finally:
self._tool_command_queue.task_done()
def _peripheral_arm_name(self) -> str:
configured = str(self.get_parameter("peripheral_arm").value).strip().lower()
@ -209,6 +287,21 @@ class SingleArmVelocityTeleop(Node):
def _on_controller(self, msg: XrController) -> None:
self._last_msg = msg
self._last_msg_time = self.get_clock().now()
self._handle_trigger_gripper(msg)
def _handle_trigger_gripper(self, msg: XrController) -> None:
if not self._enable_tool_control or not self._enable_trigger_gripper_control:
return
if not msg.grip:
return
should_close = msg.trigger >= self._trigger_close_threshold
open_tool = not should_close
if self._last_trigger_tool_open == open_tool:
return
self._last_trigger_tool_open = open_tool
self._enqueue_tool_command(open_tool, "trigger")
def _control_tick(self) -> None:
now = self.get_clock().now()
@ -599,6 +692,8 @@ class SingleArmVelocityTeleop(Node):
raise ValueError("max_linear_speed must be > 0")
if self._current_pose_poll_hz < 0.0:
raise ValueError("current_pose_poll_hz must be >= 0")
if not 0.0 <= self._trigger_close_threshold <= 1.0:
raise ValueError("trigger_close_threshold must be in [0, 1]")
for axis, (low, high) in enumerate(zip(self._workspace_min, self._workspace_max)):
if low >= high:
raise ValueError(f"workspace_min[{axis}] must be smaller than workspace_max[{axis}]")
@ -610,7 +705,28 @@ class SingleArmVelocityTeleop(Node):
if self._low_z_min_radius < 0.0:
raise ValueError("low_z_min_radius must be >= 0")
def _shutdown_tool_worker(self) -> None:
if self._tool_worker_thread is None or self._tool_command_queue is None:
return
self._tool_worker_stop.set()
while True:
try:
self._tool_command_queue.put_nowait(None)
break
except queue.Full:
try:
self._tool_command_queue.get_nowait()
self._tool_command_queue.task_done()
except queue.Empty:
continue
self._tool_worker_thread.join(timeout=3.0)
if self._tool_worker_thread.is_alive():
self.get_logger().warn(f"{self._arm_name} 工具控制后台线程未在超时时间内退出。")
self._tool_worker_thread = None
self._tool_command_queue = None
def destroy_node(self) -> bool:
self._shutdown_tool_worker()
self._adapter.close()
return super().destroy_node()