YikaiFu-cart/acRealman_xr
Template
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update trigger control logic

Browse Source
This commit is contained in:
YikaiFu-cart
2026-06-08 15:48:51 +08:00
parent d103439f01
commit 7f8ebefadc
5 changed files with 30 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
xr_rm_teleop/xr_rm_teleop/fun_peripheral.py
View File

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

22
xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
View File

@ -129,7 +129,8 @@ 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._trigger_tool_open = True
self._last_trigger_pressed: 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
@ -201,7 +202,7 @@ class SingleArmVelocityTeleop(Node):
topic = f"{self._debug_topic_prefix}/{self._arm_name}/tool_enable"
self.create_subscription(Bool, topic, self._on_tool_command, 10)
trigger_text = (
f", trigger 需 grip=true 且 >= {self._trigger_close_threshold:.2f} 时闭合"
f", trigger >= {self._trigger_close_threshold:.2f} 上升沿切换开合"
if self._enable_trigger_gripper_control
else ""
)
@ -212,7 +213,7 @@ class SingleArmVelocityTeleop(Node):
def _on_tool_command(self, msg: Bool) -> None:
open_tool = bool(msg.data)
self._last_trigger_tool_open = None
self._trigger_tool_open = open_tool
self._enqueue_tool_command(open_tool, "topic")
def _start_tool_worker(self) -> None:
@ -292,16 +293,19 @@ class SingleArmVelocityTeleop(Node):
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:
trigger_pressed = msg.trigger >= self._trigger_close_threshold
if self._last_trigger_pressed is None:
self._last_trigger_pressed = trigger_pressed
return
should_close = msg.trigger >= self._trigger_close_threshold
open_tool = not should_close
if self._last_trigger_tool_open == open_tool:
rising_edge = trigger_pressed and not self._last_trigger_pressed
self._last_trigger_pressed = trigger_pressed
if not rising_edge:
return
self._last_trigger_tool_open = open_tool
self._enqueue_tool_command(open_tool, "trigger")
self._trigger_tool_open = not self._trigger_tool_open
self._enqueue_tool_command(self._trigger_tool_open, "trigger")
def _control_tick(self) -> None:
now = self.get_clock().now()