别人克隆后没有 install，source 会直接失败。把展开后的 install 放进仓库，符号链接和指向本机 build 的 egg-link 收成真实文件，Python 入口改走当前环境的解释器，这样不用先 colcon build 也能启动。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
+13
@@ -0,0 +1,13 @@
|
||||
Metadata-Version: 2.4
|
||||
Name: arm_application
|
||||
Version: 0.1.0
|
||||
Summary: High-level arm application nodes.
|
||||
Maintainer: arm_dev
|
||||
Maintainer-email: dev@example.com
|
||||
License: Apache-2.0
|
||||
Requires-Dist: setuptools
|
||||
Dynamic: license
|
||||
Dynamic: maintainer
|
||||
Dynamic: maintainer-email
|
||||
Dynamic: requires-dist
|
||||
Dynamic: summary
|
||||
+88
@@ -0,0 +1,88 @@
|
||||
package.xml
|
||||
setup.cfg
|
||||
setup.py
|
||||
../../build/arm_application/arm_application.egg-info/PKG-INFO
|
||||
../../build/arm_application/arm_application.egg-info/SOURCES.txt
|
||||
../../build/arm_application/arm_application.egg-info/dependency_links.txt
|
||||
../../build/arm_application/arm_application.egg-info/entry_points.txt
|
||||
../../build/arm_application/arm_application.egg-info/requires.txt
|
||||
../../build/arm_application/arm_application.egg-info/top_level.txt
|
||||
../../build/arm_application/arm_application.egg-info/zip-safe
|
||||
arm_application/__init__.py
|
||||
arm_application/box_tracker.py
|
||||
arm_application/demo_named_poses.py
|
||||
arm_application/dh_official.py
|
||||
arm_application/dh_pgea.py
|
||||
arm_application/dh_pgea_node.py
|
||||
arm_application/dual_gripper.py
|
||||
arm_application/dual_gripper_cli.py
|
||||
arm_application/dual_gripper_node.py
|
||||
arm_application/feature_guard.py
|
||||
arm_application/feature_jump_guard.py
|
||||
arm_application/gauge_assembly_node.py
|
||||
arm_application/gauge_boot.py
|
||||
arm_application/gauge_config.py
|
||||
arm_application/gauge_fsm.py
|
||||
arm_application/gauge_gripper.py
|
||||
arm_application/gauge_kin.py
|
||||
arm_application/geom_ik.py
|
||||
arm_application/goto_ee_plane.py
|
||||
arm_application/handeye_calibrate.py
|
||||
arm_application/handeye_lib.py
|
||||
arm_application/handeye_realsense.py
|
||||
arm_application/handeye_teach.py
|
||||
arm_application/handeye_waypoints.py
|
||||
arm_application/hole_detect_hough.py
|
||||
arm_application/hole_detect_node.py
|
||||
arm_application/hole_detector.py
|
||||
arm_application/image_utils.py
|
||||
arm_application/kalman_filter.py
|
||||
arm_application/model_labels.py
|
||||
arm_application/move_to_pose_client.py
|
||||
arm_application/onnx_workpiece.py
|
||||
arm_application/orisys_tactile.py
|
||||
arm_application/perception_node.py
|
||||
arm_application/pid_controller.py
|
||||
arm_application/pipeline_node.py
|
||||
arm_application/plane_feature.py
|
||||
arm_application/pose_detector.py
|
||||
arm_application/realsense_color_pub.py
|
||||
arm_application/realsense_sdk_view.py
|
||||
arm_application/realsense_view.py
|
||||
arm_application/ros_node.py
|
||||
arm_application/run_joint_trajectory.py
|
||||
arm_application/servo_node.py
|
||||
arm_application/site_io.py
|
||||
arm_application/stage2_locate_node.py
|
||||
arm_application/stage3_search_node.py
|
||||
arm_application/tactile_cli.py
|
||||
arm_application/tactile_interface.py
|
||||
arm_application/tactile_node.py
|
||||
arm_application/weights_layout.py
|
||||
arm_application/yolo_detector.py
|
||||
arm_application.egg-info/PKG-INFO
|
||||
arm_application.egg-info/SOURCES.txt
|
||||
arm_application.egg-info/dependency_links.txt
|
||||
arm_application.egg-info/entry_points.txt
|
||||
arm_application.egg-info/requires.txt
|
||||
arm_application.egg-info/top_level.txt
|
||||
arm_application.egg-info/zip-safe
|
||||
config/demo.yaml
|
||||
config/dual_grippers.yaml
|
||||
config/hole_detect.yaml
|
||||
config/hole_insert.yaml
|
||||
config/perception.yaml
|
||||
config/servo.yaml
|
||||
config/xunhuan_trajectory.yaml
|
||||
launch/demo.launch.py
|
||||
launch/dh_pgea.launch.py
|
||||
launch/dual_gripper.launch.py
|
||||
launch/gauge_assembly.launch.py
|
||||
launch/hole_detect.launch.py
|
||||
launch/pipeline.launch.py
|
||||
launch/stage2_locate.launch.py
|
||||
launch/stage3_search.launch.py
|
||||
launch/tactile.launch.py
|
||||
launch/visual_servo.launch.py
|
||||
resource/arm_application
|
||||
trajectories/xunhuan.traj
|
||||
+1
@@ -0,0 +1 @@
|
||||
|
||||
+23
@@ -0,0 +1,23 @@
|
||||
[console_scripts]
|
||||
assembly_pipeline = arm_application.pipeline_node:main
|
||||
demo_named_poses = arm_application.demo_named_poses:main
|
||||
dh_pgea = arm_application.dh_pgea_node:main
|
||||
dh_pgea_cli = arm_application.dh_pgea:main
|
||||
dual_gripper = arm_application.dual_gripper_node:main
|
||||
dual_gripper_cli = arm_application.dual_gripper_cli:main
|
||||
gauge_assembly = arm_application.gauge_assembly_node:main
|
||||
goto_ee_plane = arm_application.goto_ee_plane:main
|
||||
handeye_calibrate = arm_application.handeye_calibrate:main
|
||||
handeye_teach = arm_application.handeye_teach:main
|
||||
handeye_waypoints = arm_application.handeye_waypoints:main
|
||||
hole_detect = arm_application.hole_detect_node:main
|
||||
move_to_pose_client = arm_application.move_to_pose_client:main
|
||||
perception_node = arm_application.perception_node:main
|
||||
realsense_color_pub = arm_application.realsense_color_pub:main
|
||||
realsense_view = arm_application.realsense_view:main
|
||||
run_joint_trajectory = arm_application.run_joint_trajectory:main
|
||||
servo_node = arm_application.servo_node:main
|
||||
stage2_locate = arm_application.stage2_locate_node:main
|
||||
stage3_search = arm_application.stage3_search_node:main
|
||||
tactile = arm_application.tactile_node:main
|
||||
tactile_cli = arm_application.tactile_cli:main
|
||||
+1
@@ -0,0 +1 @@
|
||||
setuptools
|
||||
+1
@@ -0,0 +1 @@
|
||||
arm_application
|
||||
@@ -0,0 +1 @@
|
||||
|
||||
@@ -0,0 +1,32 @@
|
||||
"""arm_application —— Flexiv Rizon + D405 + DH 夹爪的现场节点包。
|
||||
|
||||
对外可直接运行的 ROS 程序（见 ``setup.py`` 的 ``console_scripts``）
|
||||
------------------------------------------------------------------
|
||||
相机： realsense_color_pub
|
||||
感知： perception_node
|
||||
伺服： servo_node
|
||||
手眼： handeye_teach、handeye_waypoints、handeye_calibrate
|
||||
夹爪： dh_pgea、dh_pgea_cli、dual_gripper、dual_gripper_cli
|
||||
入孔检测： hole_detect
|
||||
触觉： tactile、tactile_cli
|
||||
辅助： goto_ee_plane、demo_named_poses、run_joint_trajectory
|
||||
|
||||
可复用的 Python 模块（可以在你自己的节点里 import）
|
||||
------------------------------------------------------------------
|
||||
``yolo_detector`` YOLO / TensorRT 推理 + ``pick_best`` 挑最优框
|
||||
``box_tracker`` 像素平面内的匀速模型框预测（丢帧时"滑行"用）
|
||||
``plane_feature`` 在 YOLO 框内取立体平面的质心
|
||||
``dh_pgea`` 大寰 PGEA 的官方 Modbus RTU 客户端
|
||||
``dh_official`` ``dual_gripper`` 用的 DH Modbus 客户端
|
||||
``dual_gripper`` 左 PGEA + 右 RGI
|
||||
``hole_detector`` yolo / hough / two_stage 三种孔心检测
|
||||
``orisys_tactile`` Orisys 视触觉适配器
|
||||
``geom_ik`` Rizon 几何雅可比 / DLS / 手眼加载
|
||||
``handeye_lib`` 棋盘格 + OpenCV ``calibrateHandEye``
|
||||
``handeye_realsense`` D405 的话题 / 序列号辅助
|
||||
``pid_controller`` 带积分分离的 PID
|
||||
``kalman_filter`` 三维匀速模型卡尔曼滤波
|
||||
|
||||
现场操作请走 ``scripts/*.sh`` 和 ``config/site.yaml``，不要直接 import 这些模块。
|
||||
接口契约见 ``docs/INTERFACES.md``。
|
||||
"""
|
||||
@@ -0,0 +1,85 @@
|
||||
#!/usr/bin/env python3
|
||||
"""针对 YOLO ``xyxy`` 框（像素坐标系）的匀速跟踪器。
|
||||
|
||||
用于避免工件快速运动时丢失锁定：下一帧的闸门使用*预测*框，
|
||||
短暂的 YOLO 漏检则由惯性滑行（coast）维持叠加显示。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def _center(xyxy: np.ndarray) -> np.ndarray:
|
||||
return np.array(
|
||||
[0.5 * (float(xyxy[0]) + float(xyxy[2])), 0.5 * (float(xyxy[1]) + float(xyxy[3]))],
|
||||
dtype=float,
|
||||
)
|
||||
|
||||
|
||||
def _wh(xyxy: np.ndarray) -> Tuple[float, float]:
|
||||
return (
|
||||
max(8.0, float(xyxy[2]) - float(xyxy[0])),
|
||||
max(8.0, float(xyxy[3]) - float(xyxy[1])),
|
||||
)
|
||||
|
||||
|
||||
class BoxTracker:
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
vmax_px_s: float = 1800.0,
|
||||
v_lpf: float = 0.70,
|
||||
max_dt: float = 0.25,
|
||||
) -> None:
|
||||
self.vmax_px_s = float(vmax_px_s)
|
||||
self.v_lpf = float(np.clip(v_lpf, 0.05, 1.0))
|
||||
self.max_dt = float(max_dt)
|
||||
self.xyxy: Optional[np.ndarray] = None
|
||||
self.v = np.zeros(2, dtype=float) # 中心点 px/s
|
||||
self.t: Optional[float] = None
|
||||
|
||||
def reset(self) -> None:
|
||||
self.xyxy = None
|
||||
self.v[:] = 0.0
|
||||
self.t = None
|
||||
|
||||
def predict(self, t: float, img_wh: Optional[Tuple[int, int]] = None) -> Optional[np.ndarray]:
|
||||
if self.xyxy is None:
|
||||
return None
|
||||
out = self.xyxy.copy()
|
||||
if self.t is None:
|
||||
return out
|
||||
dt = float(np.clip(t - self.t, 0.0, self.max_dt))
|
||||
if dt <= 1e-4:
|
||||
return out
|
||||
c = _center(out) + self.v * dt
|
||||
w, h = _wh(out)
|
||||
x1, y1 = c[0] - 0.5 * w, c[1] - 0.5 * h
|
||||
x2, y2 = c[0] + 0.5 * w, c[1] + 0.5 * h
|
||||
if img_wh is not None:
|
||||
iw, ih = float(img_wh[0]), float(img_wh[1])
|
||||
x1 = float(np.clip(x1, 0.0, iw - 2.0))
|
||||
y1 = float(np.clip(y1, 0.0, ih - 2.0))
|
||||
x2 = float(np.clip(x2, x1 + 4.0, iw))
|
||||
y2 = float(np.clip(y2, y1 + 4.0, ih))
|
||||
return np.array([x1, y1, x2, y2], dtype=float)
|
||||
|
||||
def update(self, xyxy: np.ndarray, t: float) -> None:
|
||||
xyxy = np.asarray(xyxy, dtype=float).reshape(4)
|
||||
c = _center(xyxy)
|
||||
if self.xyxy is not None and self.t is not None:
|
||||
dt = float(t - self.t)
|
||||
if dt > 0.28:
|
||||
self.v[:] = 0.0
|
||||
elif dt > 1e-3:
|
||||
inst = (c - _center(self.xyxy)) / dt
|
||||
n = float(np.hypot(inst[0], inst[1]))
|
||||
if n > self.vmax_px_s > 0.0:
|
||||
inst = inst * (self.vmax_px_s / n)
|
||||
a = self.v_lpf
|
||||
self.v = a * inst + (1.0 - a) * self.v
|
||||
self.xyxy = xyxy.copy()
|
||||
self.t = float(t)
|
||||
+80
@@ -0,0 +1,80 @@
|
||||
#!/usr/bin/env python3
|
||||
"""演示：通过 ``/set_named_pose`` 循环切换命名位姿（对任何后端都适用）。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
|
||||
from arm_msgs.srv import SetNamedPose
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
|
||||
class DemoNamedPoses(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('demo_named_poses')
|
||||
self.declare_parameter(
|
||||
'poses', ['home', 'ready', 'inspect', 'pos_left', 'home']
|
||||
)
|
||||
self.declare_parameter('velocity_scale', 0.5)
|
||||
self.declare_parameter('dwell_sec', 0.3)
|
||||
self.declare_parameter('loops', 1)
|
||||
self.poses = list(
|
||||
self.get_parameter('poses').get_parameter_value().string_array_value
|
||||
or ['home', 'ready', 'inspect', 'home']
|
||||
)
|
||||
self.velocity_scale = float(self.get_parameter('velocity_scale').value)
|
||||
self.dwell_sec = float(self.get_parameter('dwell_sec').value)
|
||||
self.loops = int(self.get_parameter('loops').value)
|
||||
self._cli = self.create_client(SetNamedPose, 'set_named_pose')
|
||||
|
||||
def run(self) -> int:
|
||||
if not self._cli.wait_for_service(timeout_sec=15.0):
|
||||
self.get_logger().error('``/set_named_pose`` 服务不可用')
|
||||
return 1
|
||||
for loop in range(max(self.loops, 1)):
|
||||
self.get_logger().info(f'第 {loop + 1}/{self.loops} 轮')
|
||||
for pose in self.poses:
|
||||
req = SetNamedPose.Request()
|
||||
req.pose_name = pose
|
||||
req.velocity_scale = self.velocity_scale
|
||||
req.acceleration_scale = self.velocity_scale
|
||||
fut = self._cli.call_async(req)
|
||||
deadline = time.monotonic() + 60.0
|
||||
while rclpy.ok() and not fut.done():
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
if time.monotonic() > deadline:
|
||||
self.get_logger().error(f'{pose} 调用超时')
|
||||
return 2
|
||||
res = fut.result()
|
||||
if res is None or not res.success:
|
||||
self.get_logger().error(
|
||||
f'{pose} 执行失败：{res.message if res else "无响应"}'
|
||||
)
|
||||
return 3
|
||||
self.get_logger().info(f'{pose}：{res.message}')
|
||||
time.sleep(self.dwell_sec)
|
||||
self.get_logger().info('演示结束')
|
||||
return 0
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = DemoNamedPoses()
|
||||
code = 1
|
||||
try:
|
||||
code = node.run()
|
||||
except KeyboardInterrupt:
|
||||
code = 0
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
raise SystemExit(code)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,286 @@
|
||||
"""DH-Robotics 官方 Modbus RTU 控制（非 Flexiv RDK）。
|
||||
|
||||
覆盖两类设备：
|
||||
- PGE/PGEA 平行夹爪：初始化 0x0100，力 0x0101，位置 0x0103（0~1000‰），速度 0x0104。
|
||||
- RGI 旋转夹爪：在平行夹爪寄存器基础上，增加旋转角度 0x0105、旋转速度 0x0107、
|
||||
旋转力 0x0108；反馈增加旋转角度 0x0208、旋转初始化状态 0x020A、旋转状态 0x020B。
|
||||
|
||||
说明：
|
||||
- RGI 手册示例中负角度使用“反码/one's complement”：+360 -> 0x0168，-360 -> 0xFE97。
|
||||
因此这里默认按 one's complement 编解码；若后续厂家确认为 two's complement，只改
|
||||
`_encode_signed` / `_decode_signed` 两处即可。
|
||||
- 写位置/角度寄存器后会立即运动，没有 ChangingTek SDK 的 0x0108 触发寄存器。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
import time
|
||||
from typing import Iterable
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# DH-Robotics Modbus 寄存器映射
|
||||
# =============================================================================
|
||||
|
||||
# 基础控制寄存器（保持寄存器，功能码 03/06/10）
|
||||
REG_INIT = 0x0100 # 0x01 初始化，0xA5 完全初始化
|
||||
REG_FORCE = 0x0101 # 夹爪闭合力，20~100 %
|
||||
REG_POSITION = 0x0103 # 手指参考位置，0~1000 千分比
|
||||
REG_SPEED = 0x0104 # 手指开合速度，1~100 %
|
||||
|
||||
# RGI 旋转控制寄存器
|
||||
REG_ROTATION_ANGLE = 0x0105 # 目标旋转角度，有符号角度值
|
||||
REG_ROTATION_SPEED = 0x0107 # 旋转速度，1~100 %
|
||||
REG_ROTATION_FORCE = 0x0108 # 旋转力/力矩，20~100 %
|
||||
|
||||
# 反馈寄存器
|
||||
REG_INIT_STATE = 0x0200 # 0 未初始化，1 已初始化，2 初始化中
|
||||
REG_GRIPPER_STATE = 0x0201 # 0 运动中，1 已到位，2 已夹到物体，3 物体掉落
|
||||
REG_CURRENT_POSITION = 0x0202 # 当前手指位置，千分比
|
||||
REG_ROTATION_ANGLE_FB = 0x0208 # 当前旋转角度，有符号角度值
|
||||
REG_ROTATION_INIT_STATE = 0x020A # 0 未初始化，1 已初始化，2 初始化中
|
||||
REG_ROTATION_STATE = 0x020B # 0 运动中，1 已到位，2 受阻，3 曾经受阻
|
||||
|
||||
# 配置寄存器
|
||||
REG_SAVE_PARAMS = 0x0300 # 写入 1 可把所有参数保存到 flash
|
||||
|
||||
|
||||
class DHModbusError(RuntimeError):
|
||||
"""DH Modbus 通信或协议错误。"""
|
||||
|
||||
|
||||
def _clamp_int(value: float | int, lo: int, hi: int) -> int:
|
||||
return int(max(lo, min(hi, int(round(float(value))))))
|
||||
|
||||
|
||||
def _encode_signed(value: float | int) -> int:
|
||||
"""把有符号角度值编码为 DH 的反码（one's complement）寄存器值。
|
||||
|
||||
RGI 手册示例：+360 -> 0x0168，-360 -> 0xFE97。
|
||||
"""
|
||||
value = _clamp_int(value, -32767, 32767)
|
||||
if value >= 0:
|
||||
return value & 0xFFFF
|
||||
return (~abs(value)) & 0xFFFF
|
||||
|
||||
|
||||
def _decode_signed(raw: int) -> int:
|
||||
"""把 DH 的反码（one's complement）寄存器值解码为有符号整数。"""
|
||||
raw = int(raw) & 0xFFFF
|
||||
if raw & 0x8000:
|
||||
mag = (~raw) & 0x7FFF
|
||||
return -mag
|
||||
return raw & 0x7FFF
|
||||
|
||||
|
||||
class DHModbusClient:
|
||||
"""面向 DH-Robotics 夹爪的最小线程安全 Modbus RTU 客户端。"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
port: str,
|
||||
slave_id: int = 1,
|
||||
baudrate: int = 115200,
|
||||
timeout: float = 0.2,
|
||||
*,
|
||||
bytesize: int = 8,
|
||||
parity: str = "N",
|
||||
stopbits: int = 1,
|
||||
) -> None:
|
||||
try:
|
||||
import minimalmodbus
|
||||
import serial
|
||||
except ImportError as exc: # pragma: no cover - 取决于运行时环境
|
||||
raise DHModbusError(
|
||||
"缺少 minimalmodbus/pyserial。请先安装：pip install minimalmodbus pyserial"
|
||||
) from exc
|
||||
|
||||
parity_map = {
|
||||
"N": serial.PARITY_NONE,
|
||||
"E": serial.PARITY_EVEN,
|
||||
"O": serial.PARITY_ODD,
|
||||
}
|
||||
self.instrument = minimalmodbus.Instrument(port, int(slave_id))
|
||||
self.instrument.serial.baudrate = int(baudrate)
|
||||
self.instrument.serial.bytesize = int(bytesize)
|
||||
self.instrument.serial.parity = parity_map.get(str(parity).upper(), serial.PARITY_NONE)
|
||||
self.instrument.serial.stopbits = int(stopbits)
|
||||
self.instrument.serial.timeout = float(timeout)
|
||||
self.instrument.mode = minimalmodbus.MODE_RTU
|
||||
self.instrument.clear_buffers_before_each_transaction = True
|
||||
try:
|
||||
self.instrument.serial.exclusive = True
|
||||
except Exception:
|
||||
pass
|
||||
self._lock = threading.Lock()
|
||||
self.port = port
|
||||
|
||||
def close(self) -> None:
|
||||
with self._lock:
|
||||
try:
|
||||
self.instrument.serial.close()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# 底层寄存器读写
|
||||
# ------------------------------------------------------------------
|
||||
def write_register(self, address: int, value: int) -> None:
|
||||
value = _clamp_int(value, 0, 0xFFFF)
|
||||
last_exc: Exception | None = None
|
||||
for attempt in range(3):
|
||||
try:
|
||||
with self._lock:
|
||||
self.instrument.write_register(address, value, functioncode=6)
|
||||
return
|
||||
except Exception as exc:
|
||||
last_exc = exc
|
||||
time.sleep(0.05 * (attempt + 1))
|
||||
raise DHModbusError(f"写寄存器 0x{address:04X}={value} 失败: {last_exc}") from last_exc
|
||||
|
||||
def read_register(self, address: int) -> int:
|
||||
last_exc: Exception | None = None
|
||||
for attempt in range(3):
|
||||
try:
|
||||
with self._lock:
|
||||
return int(self.instrument.read_register(address, functioncode=3))
|
||||
except Exception as exc:
|
||||
last_exc = exc
|
||||
time.sleep(0.05 * (attempt + 1))
|
||||
hint = ""
|
||||
text = str(last_exc).lower()
|
||||
if "multiple access" in text or "no data" in text:
|
||||
hint = f"。串口 {getattr(self, 'port', '')} 可能被其它进程占用，先执行: fuser -k {getattr(self, 'port', '/dev/ttyUSB*')}"
|
||||
raise DHModbusError(f"读寄存器 0x{address:04X} 失败: {last_exc}{hint}") from last_exc
|
||||
|
||||
def write_registers(self, address: int, values: Iterable[int]) -> None:
|
||||
vals = [_clamp_int(v, 0, 0xFFFF) for v in values]
|
||||
with self._lock:
|
||||
self.instrument.write_registers(address, vals)
|
||||
|
||||
def read_registers(self, address: int, count: int) -> list[int]:
|
||||
with self._lock:
|
||||
return [int(v) for v in self.instrument.read_registers(address, count, functioncode=3)]
|
||||
|
||||
def write_signed_register(self, address: int, value: float | int) -> None:
|
||||
self.write_register(address, _encode_signed(value))
|
||||
|
||||
def read_signed_register(self, address: int) -> int:
|
||||
return _decode_signed(self.read_register(address))
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# PGE/PGEA/RGI 通用控制
|
||||
# ------------------------------------------------------------------
|
||||
def initialize(self, *, full: bool = False, rotation: bool = False, timeout: float = 10.0) -> None:
|
||||
"""初始化/回零夹爪，并等待其就绪。"""
|
||||
self.write_register(REG_INIT, 0xA5 if full else 0x01)
|
||||
start = time.monotonic()
|
||||
while time.monotonic() - start < timeout:
|
||||
init_state = self.read_register(REG_INIT_STATE)
|
||||
rot_ok = True
|
||||
if rotation:
|
||||
rot_ok = self.read_register(REG_ROTATION_INIT_STATE) == 1
|
||||
if init_state == 1 and rot_ok:
|
||||
return
|
||||
time.sleep(0.1)
|
||||
raise DHModbusError(f"夹爪初始化超时（>{timeout:.1f}s）")
|
||||
|
||||
def save_parameters(self) -> None:
|
||||
self.write_register(REG_SAVE_PARAMS, 1)
|
||||
|
||||
def set_force_pct(self, force_pct: float | int) -> int:
|
||||
value = _clamp_int(force_pct, 20, 100)
|
||||
self.write_register(REG_FORCE, value)
|
||||
return value
|
||||
|
||||
def set_speed_pct(self, speed_pct: float | int) -> int:
|
||||
value = _clamp_int(speed_pct, 1, 100)
|
||||
self.write_register(REG_SPEED, value)
|
||||
return value
|
||||
|
||||
def set_position_per_mille(self, position: float | int) -> int:
|
||||
value = _clamp_int(position, 0, 1000)
|
||||
# 与 dh_pgea.latched_position_break 同一份：命令已是目标时先写当前位置，
|
||||
# 读回之后再写最终值。60ms 后写回旧命令，固件采样不到，手指不动。
|
||||
from arm_application.dh_pgea import latched_position_break
|
||||
|
||||
try:
|
||||
cmd = _clamp_int(self.read_register(REG_POSITION), 0, 1000)
|
||||
except Exception:
|
||||
cmd = value
|
||||
try:
|
||||
cur = self.get_position_per_mille()
|
||||
except Exception:
|
||||
cur = value
|
||||
bridge = latched_position_break(cmd, cur, value)
|
||||
if bridge is not None:
|
||||
self.write_register(REG_POSITION, bridge)
|
||||
deadline = time.monotonic() + 0.6
|
||||
echoed = False
|
||||
while time.monotonic() < deadline:
|
||||
try:
|
||||
if _clamp_int(self.read_register(REG_POSITION), 0, 1000) == bridge:
|
||||
echoed = True
|
||||
break
|
||||
except Exception:
|
||||
pass
|
||||
time.sleep(0.05)
|
||||
if not echoed:
|
||||
raise DHModbusError(
|
||||
f"位置命令中间值 {bridge} 没有读回，不再写入最终目标 {value}"
|
||||
)
|
||||
time.sleep(0.15)
|
||||
self.write_register(REG_POSITION, value)
|
||||
return value
|
||||
|
||||
def get_position_per_mille(self) -> int:
|
||||
return _clamp_int(self.read_register(REG_CURRENT_POSITION), 0, 1000)
|
||||
|
||||
def get_gripper_state(self) -> int:
|
||||
return self.read_register(REG_GRIPPER_STATE)
|
||||
|
||||
def get_init_state(self) -> int:
|
||||
return self.read_register(REG_INIT_STATE)
|
||||
|
||||
def hold_current_position(self) -> None:
|
||||
"""在没有专用停止寄存器的 DH 夹爪上尽力停止/保持。"""
|
||||
self.set_position_per_mille(self.get_position_per_mille())
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# RGI 旋转控制
|
||||
# ------------------------------------------------------------------
|
||||
def set_rotation_speed_pct(self, speed_pct: float | int) -> int:
|
||||
value = _clamp_int(speed_pct, 1, 100)
|
||||
self.write_register(REG_ROTATION_SPEED, value)
|
||||
return value
|
||||
|
||||
def set_rotation_force_pct(self, force_pct: float | int) -> int:
|
||||
value = _clamp_int(force_pct, 20, 100)
|
||||
self.write_register(REG_ROTATION_FORCE, value)
|
||||
return value
|
||||
|
||||
def set_rotation_angle_deg(self, angle_deg: float | int) -> int:
|
||||
angle = _clamp_int(angle_deg, -32767, 32767)
|
||||
self.write_signed_register(REG_ROTATION_ANGLE, angle)
|
||||
return angle
|
||||
|
||||
def get_rotation_angle_raw(self) -> int:
|
||||
return self.read_register(REG_ROTATION_ANGLE_FB) & 0xFFFF
|
||||
|
||||
def get_rotation_angle_deg(self) -> int:
|
||||
return self.read_signed_register(REG_ROTATION_ANGLE_FB)
|
||||
|
||||
def get_rotation_init_state(self) -> int:
|
||||
return self.read_register(REG_ROTATION_INIT_STATE)
|
||||
|
||||
def get_rotation_state(self) -> int:
|
||||
return self.read_register(REG_ROTATION_STATE)
|
||||
|
||||
def hold_current_rotation(self) -> None:
|
||||
"""重新下发当前反馈角度来停止旋转。"""
|
||||
self.write_register(REG_ROTATION_ANGLE, self.get_rotation_angle_raw())
|
||||
|
||||
def hold_current_all(self) -> None:
|
||||
self.hold_current_position()
|
||||
self.hold_current_rotation()
|
||||
@@ -0,0 +1,670 @@
|
||||
#!/usr/bin/env python3
|
||||
"""DH Robotics PGE / PGEA Modbus RTU 客户端。
|
||||
|
||||
移植自官方 C++ 驱动：
|
||||
DH-Robotics/dh_gripper_ros → DH_Modbus_Gripper
|
||||
（工厂会按型号名里是否包含 ``PGE`` / ``PGEA`` 映射到本类）
|
||||
|
||||
Flexiv RDK 的 ``Gripper`` 驱动不了这个夹爪：它不是 Studio Device。
|
||||
改用 RS485（USB–UART）通信，寄存器与 DH 的 ROS 驱动完全一致。
|
||||
|
||||
寄存器（保持寄存器，FC03 读 / FC06 写，CRC-16/Modbus 小端）：
|
||||
0x0100 初始化，写 0xA5
|
||||
0x0101 力 20–100 %
|
||||
0x0103 位置 0–1000 ‰
|
||||
0x0104 速度 1–100 %
|
||||
0x0200 初始化状态 0=需要 1=完成 2=进行中
|
||||
0x0201 夹持状态 0=运动中 1=已到位 2=已夹住 3=已掉落
|
||||
0x0202 当前位置
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional
|
||||
|
||||
try:
|
||||
import serial
|
||||
except ImportError as exc: # pragma: no cover
|
||||
raise SystemExit(
|
||||
"缺少 pyserial。在 conda 环境 ros_flexiv 里执行：pip install pyserial"
|
||||
) from exc
|
||||
|
||||
|
||||
# 官方 CRC-16/Modbus（与 dh_modbus_gripper.cpp 使用同一张表）。
|
||||
_CRC_TABLE = (
|
||||
0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241,
|
||||
0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440,
|
||||
0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40,
|
||||
0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841,
|
||||
0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40,
|
||||
0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41,
|
||||
0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641,
|
||||
0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040,
|
||||
0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240,
|
||||
0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441,
|
||||
0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41,
|
||||
0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840,
|
||||
0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41,
|
||||
0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40,
|
||||
0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640,
|
||||
0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041,
|
||||
0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240,
|
||||
0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441,
|
||||
0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41,
|
||||
0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840,
|
||||
0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41,
|
||||
0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40,
|
||||
0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640,
|
||||
0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041,
|
||||
0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241,
|
||||
0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440,
|
||||
0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40,
|
||||
0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841,
|
||||
0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40,
|
||||
0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41,
|
||||
0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641,
|
||||
0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040,
|
||||
)
|
||||
|
||||
REG_INIT = 0x0100
|
||||
REG_FORCE = 0x0101
|
||||
REG_POSITION = 0x0103
|
||||
REG_SPEED = 0x0104
|
||||
REG_INIT_STATE = 0x0200
|
||||
REG_GRIP_STATE = 0x0201
|
||||
REG_CUR_POSITION = 0x0202
|
||||
|
||||
INIT_CMD = 0xA5
|
||||
|
||||
S_INIT_NOT = 0
|
||||
S_INIT_FINISHED = 1
|
||||
S_INIT_DOING = 2
|
||||
|
||||
S_GRIP_MOVING = 0
|
||||
S_GRIP_ARRIVED = 1
|
||||
S_GRIP_CAUGHT = 2
|
||||
S_GRIP_DROPPED = 3
|
||||
|
||||
GRIP_STATE_NAME = {
|
||||
S_GRIP_MOVING: "moving",
|
||||
S_GRIP_ARRIVED: "arrived",
|
||||
S_GRIP_CAUGHT: "caught",
|
||||
S_GRIP_DROPPED: "dropped",
|
||||
}
|
||||
|
||||
INIT_STATE_NAME = {
|
||||
S_INIT_NOT: "need_init",
|
||||
S_INIT_FINISHED: "initialized",
|
||||
S_INIT_DOING: "initializing",
|
||||
}
|
||||
|
||||
|
||||
def latched_position_break(command: int, current: int, target: int) -> Optional[int]:
|
||||
"""命令寄存器已经等于目标、手指却不在目标上时，返回要先写入的中间位置。
|
||||
|
||||
大寰固件：再写一遍与 0x0103 当前值相同的数，不会启动新运动。
|
||||
开爪卡在 caught 时就是这样——命令已是 1000，手指停在夹持位。
|
||||
先把命令改成当前位置（手指本来就在这，不会再夹紧），等读回确认
|
||||
固件已经采样，再写最终目标。只 sleep 几十毫秒就写回旧值，控制周期
|
||||
来不及看到中间值，最终仍是旧命令，手指不动。
|
||||
"""
|
||||
cmd = int(max(0, min(1000, command)))
|
||||
cur = int(max(0, min(1000, current)))
|
||||
pos = int(max(0, min(1000, target)))
|
||||
if cmd != pos or abs(cur - pos) <= 25:
|
||||
return None
|
||||
if cur != cmd:
|
||||
return cur
|
||||
return 0 if cmd != 0 else 40
|
||||
|
||||
|
||||
def grip_motion_settled(
|
||||
grip_state: int, position: int, target: int, seen_motion: bool
|
||||
) -> bool:
|
||||
"""这一拍能不能算「这句位置命令已经结束」。
|
||||
|
||||
张开前本来就是 caught、位置还停在夹持位：不能当成张开完成。
|
||||
闭合夹住物体：动过之后停在比目标更开的位置，算完成。
|
||||
"""
|
||||
near = abs(int(position) - int(target)) <= 25
|
||||
if grip_state in (S_GRIP_ARRIVED, S_GRIP_DROPPED) and near:
|
||||
return True
|
||||
if grip_state == S_GRIP_CAUGHT and seen_motion:
|
||||
if int(target) + 25 < int(position):
|
||||
return True
|
||||
if near:
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
def crc16_modbus(data: bytes) -> int:
|
||||
"""官方 DH CRC16（Modbus，线路上按小端发送）。"""
|
||||
crc = 0xFFFF
|
||||
for byte in data:
|
||||
crc = (crc >> 8) ^ _CRC_TABLE[(crc ^ byte) & 0xFF]
|
||||
return crc
|
||||
|
||||
|
||||
def _frame(slave: int, func: int, index: int, value: int) -> bytes:
|
||||
body = bytes(
|
||||
[
|
||||
slave & 0xFF,
|
||||
func & 0xFF,
|
||||
(index >> 8) & 0xFF,
|
||||
index & 0xFF,
|
||||
(value >> 8) & 0xFF,
|
||||
value & 0xFF,
|
||||
]
|
||||
)
|
||||
crc = crc16_modbus(body)
|
||||
return body + bytes([crc & 0xFF, (crc >> 8) & 0xFF])
|
||||
|
||||
|
||||
@dataclass
|
||||
class GripperState:
|
||||
init_state: int = S_INIT_NOT
|
||||
grip_state: int = S_GRIP_MOVING
|
||||
position: int = 0
|
||||
target_position: int = 0
|
||||
target_force: int = 0
|
||||
stroke_mm: float = 20.0
|
||||
|
||||
@property
|
||||
def initialized(self) -> bool:
|
||||
return self.init_state == S_INIT_FINISHED
|
||||
|
||||
@property
|
||||
def width_mm(self) -> float:
|
||||
return float(self.position) / 1000.0 * float(self.stroke_mm)
|
||||
|
||||
def as_tuple(self) -> tuple[float, float, float, float, float]:
|
||||
return (
|
||||
1.0 if self.initialized else 0.0,
|
||||
float(self.grip_state),
|
||||
float(self.position),
|
||||
float(self.target_position),
|
||||
float(self.target_force),
|
||||
)
|
||||
|
||||
def summary(self) -> str:
|
||||
return (
|
||||
f"init={INIT_STATE_NAME.get(self.init_state, self.init_state)} "
|
||||
f"grip={GRIP_STATE_NAME.get(self.grip_state, self.grip_state)} "
|
||||
f"pos={self.position}/1000 ({self.width_mm:.2f} mm) "
|
||||
f"target={self.target_position} force={self.target_force}%"
|
||||
)
|
||||
|
||||
|
||||
class DhPgeaError(RuntimeError):
|
||||
pass
|
||||
|
||||
|
||||
WIRING_HINT = (
|
||||
"PC 已发出 Modbus，夹爪 0 字节返回。不是力/速度参数问题。按顺序查：\n"
|
||||
" 1) 夹爪 24 V DC 已上电（指示灯亮）\n"
|
||||
" 2) USB 转接头必须是 RS485（A/B），不能是 TTL 的 TX/RX\n"
|
||||
" 3) A↔A、B↔B；不通就对调 A/B\n"
|
||||
" 4) 先 Ctrl+C 停节点，再：bash ~/ros2/scripts/run_gripper.sh --cli scan"
|
||||
)
|
||||
|
||||
|
||||
class DhPgeaGripper:
|
||||
"""面向 PGE / PGEA 的线程安全 Modbus RTU 客户端（官方寄存器映射）。"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
port: str = "/dev/ttyUSB0",
|
||||
baudrate: int = 115200,
|
||||
slave_id: int = 1,
|
||||
timeout_s: float = 0.25,
|
||||
stroke_mm: float = 20.0,
|
||||
rs485_rts: str = "",
|
||||
) -> None:
|
||||
self.port = port
|
||||
self.baudrate = int(baudrate)
|
||||
self.slave_id = int(slave_id)
|
||||
self.timeout_s = float(timeout_s)
|
||||
self.stroke_mm = float(stroke_mm)
|
||||
mode = str(rs485_rts or "").strip().lower()
|
||||
if mode in ("", "off", "none", "false", "0"):
|
||||
mode = ""
|
||||
if mode not in ("", "tx", "tx_inv"):
|
||||
raise DhPgeaError(f"rs485_rts 只能是 ''、tx 或 tx_inv，实际为 {rs485_rts!r}")
|
||||
self.rs485_rts = mode
|
||||
self._ser: Optional[serial.Serial] = None
|
||||
self._lock = threading.Lock()
|
||||
|
||||
def open(self) -> None:
|
||||
if self._ser is not None and self._ser.is_open:
|
||||
return
|
||||
try:
|
||||
self._ser = serial.Serial(
|
||||
port=self.port,
|
||||
baudrate=self.baudrate,
|
||||
bytesize=serial.EIGHTBITS,
|
||||
parity=serial.PARITY_NONE,
|
||||
stopbits=serial.STOPBITS_ONE,
|
||||
timeout=self.timeout_s,
|
||||
write_timeout=self.timeout_s,
|
||||
xonxoff=False,
|
||||
rtscts=False,
|
||||
dsrdtr=False,
|
||||
exclusive=True,
|
||||
)
|
||||
except Exception as exc:
|
||||
raise DhPgeaError(
|
||||
f"无法打开 {self.port}：{exc}。"
|
||||
"需要 24 V DC 供电 + RS485 接线，且当前用户在 dialout 组内。"
|
||||
) from exc
|
||||
try:
|
||||
self._ser.dtr = True
|
||||
except Exception:
|
||||
pass
|
||||
self._set_tx(False)
|
||||
self._ser.reset_input_buffer()
|
||||
self._ser.reset_output_buffer()
|
||||
time.sleep(0.05)
|
||||
|
||||
def close(self) -> None:
|
||||
if self._ser is not None:
|
||||
try:
|
||||
self._ser.close()
|
||||
except Exception:
|
||||
pass
|
||||
self._ser = None
|
||||
|
||||
def __enter__(self) -> "DhPgeaGripper":
|
||||
self.open()
|
||||
return self
|
||||
|
||||
def __exit__(self, *args) -> None:
|
||||
self.close()
|
||||
|
||||
def write_register(self, index: int, value: int, retries: int = 3) -> bool:
|
||||
"""FC06 写，等待回显（对应官方 WriteRegisterFunc）。"""
|
||||
frame = _frame(self.slave_id, 0x06, index, int(value) & 0xFFFF)
|
||||
with self._lock:
|
||||
for _ in range(max(1, retries)):
|
||||
if self._transact(frame, 8) == frame:
|
||||
return True
|
||||
time.sleep(0.02)
|
||||
return False
|
||||
|
||||
def read_register(self, index: int, retries: int = 8) -> int:
|
||||
"""FC03 读一个保持寄存器（对应官方 ReadRegisterFunc）。"""
|
||||
req = _frame(self.slave_id, 0x03, index, 1)
|
||||
last: Optional[bytes] = None
|
||||
with self._lock:
|
||||
for _ in range(max(1, retries)):
|
||||
rsp = self._transact(req, 7)
|
||||
last = rsp
|
||||
if rsp is None or len(rsp) != 7:
|
||||
time.sleep(0.02)
|
||||
continue
|
||||
if crc16_modbus(rsp[:-2]) != (rsp[5] | (rsp[6] << 8)):
|
||||
continue
|
||||
if rsp[0] != (self.slave_id & 0xFF) or rsp[1] != 0x03:
|
||||
continue
|
||||
return (rsp[3] << 8) | (rsp[4] & 0xFF)
|
||||
hint = f" last={last.hex() if last else '(empty)'}"
|
||||
raise DhPgeaError(
|
||||
f"在 {self.port} 上 Modbus 读 0x{index:04X} 失败 "
|
||||
f"（无有效应答{hint}）。baud={self.baudrate} slave={self.slave_id} "
|
||||
f"rts={self.rs485_rts or 'off'}"
|
||||
)
|
||||
|
||||
def probe(self) -> int:
|
||||
"""短读一次初始化状态。夹爪无应答时会抛异常。"""
|
||||
return self.read_register(REG_INIT_STATE, retries=2)
|
||||
|
||||
def _set_tx(self, tx: bool) -> None:
|
||||
if not self.rs485_rts or self._ser is None:
|
||||
return
|
||||
try:
|
||||
if self.rs485_rts == "tx":
|
||||
self._ser.rts = bool(tx)
|
||||
else:
|
||||
self._ser.rts = not bool(tx)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def _transact(self, frame: bytes, expect: int) -> Optional[bytes]:
|
||||
if self._ser is None or not self._ser.is_open:
|
||||
raise DhPgeaError("串口未打开")
|
||||
bit_s = 10.0 / max(1, self.baudrate)
|
||||
self._set_tx(True)
|
||||
if self.rs485_rts:
|
||||
time.sleep(2.0 * bit_s)
|
||||
self._ser.reset_input_buffer()
|
||||
self._ser.write(frame)
|
||||
self._ser.flush()
|
||||
if self.rs485_rts:
|
||||
time.sleep(len(frame) * bit_s + 0.0008)
|
||||
self._set_tx(False)
|
||||
deadline = time.monotonic() + self.timeout_s
|
||||
buf = bytearray()
|
||||
while time.monotonic() < deadline and len(buf) < expect:
|
||||
chunk = self._ser.read(expect - len(buf))
|
||||
if chunk:
|
||||
buf.extend(chunk)
|
||||
return bytes(buf) if buf else None
|
||||
|
||||
def initialization(self) -> bool:
|
||||
return self.write_register(REG_INIT, INIT_CMD)
|
||||
|
||||
def initialize(
|
||||
self, timeout_s: float = 20.0, poll_s: float = 0.2, *, force: bool = False
|
||||
) -> bool:
|
||||
"""写 0xA5，然后等到初始化状态 == 完成（对应官方上电流程）。
|
||||
|
||||
``force=False`` 时，已经完成就不再写（节点启动不要每次都让手指找零）。
|
||||
命令行 ``init`` 必须 ``force=True``：状态字已经是完成时，不写 0xA5
|
||||
手指根本不会回零，看起来像初始化失效。
|
||||
"""
|
||||
state = self.get_init_state()
|
||||
if state == S_INIT_FINISHED and not force:
|
||||
return True
|
||||
if not self.initialization():
|
||||
raise DhPgeaError("初始化写 0x0100=0xA5 失败（无 Modbus 回显）")
|
||||
deadline = time.monotonic() + float(timeout_s)
|
||||
# force 时必须先看到状态离开「完成」，否则读到的还是写之前的旧值。
|
||||
saw_busy = not force
|
||||
while time.monotonic() < deadline:
|
||||
state = self.get_init_state()
|
||||
if state != S_INIT_FINISHED:
|
||||
saw_busy = True
|
||||
elif saw_busy:
|
||||
return True
|
||||
time.sleep(poll_s)
|
||||
raise DhPgeaError(
|
||||
f"初始化在 {timeout_s:.0f}s 后超时（state={state}）。"
|
||||
"首次上电时手指本应先回零一次。"
|
||||
)
|
||||
|
||||
def set_target_force(self, force_pct: int) -> bool:
|
||||
force_pct = int(max(20, min(100, force_pct)))
|
||||
return self.write_register(REG_FORCE, force_pct)
|
||||
|
||||
def set_target_speed(self, speed_pct: int) -> bool:
|
||||
speed_pct = int(max(1, min(100, speed_pct)))
|
||||
return self.write_register(REG_SPEED, speed_pct)
|
||||
|
||||
def _wait_command_echo(self, value: int, timeout_s: float = 0.6) -> bool:
|
||||
"""等到 0x0103 读回就是刚写进去的值。读不回就不要再写下一笔。"""
|
||||
deadline = time.monotonic() + float(timeout_s)
|
||||
while time.monotonic() < deadline:
|
||||
try:
|
||||
if self.get_target_position(retries=1) == int(value):
|
||||
return True
|
||||
except Exception:
|
||||
pass
|
||||
time.sleep(0.05)
|
||||
return False
|
||||
|
||||
def set_target_position(self, pos: int) -> bool:
|
||||
pos = int(max(0, min(1000, pos)))
|
||||
try:
|
||||
cmd = self.get_target_position(retries=2)
|
||||
except Exception:
|
||||
cmd = pos
|
||||
try:
|
||||
cur = self.get_current_position(retries=2)
|
||||
except Exception:
|
||||
cur = pos
|
||||
bridge = latched_position_break(cmd, cur, pos)
|
||||
if bridge is not None:
|
||||
if not self.write_register(REG_POSITION, bridge):
|
||||
return False
|
||||
if not self._wait_command_echo(bridge):
|
||||
return False
|
||||
# 读回只说明寄存器改了。再留一个控制周期，避免下一笔把中间值盖掉。
|
||||
time.sleep(0.15)
|
||||
return self.write_register(REG_POSITION, pos)
|
||||
|
||||
def wait_settle(self, target: int, timeout_s: float = 6.0) -> GripperState:
|
||||
"""等到到位或夹住，或超时返回最后一帧。
|
||||
|
||||
张开前状态字已经是 caught 时不能立刻返回：那是上一句命令的残留，
|
||||
手指还停在夹持位。
|
||||
"""
|
||||
t0 = time.monotonic()
|
||||
last = None
|
||||
seen_motion = False
|
||||
st = self.get_run_states()
|
||||
while time.monotonic() - t0 < timeout_s:
|
||||
st = self.get_run_states()
|
||||
if last is not None and st.position != last:
|
||||
seen_motion = True
|
||||
last = st.position
|
||||
if st.grip_state == S_GRIP_MOVING:
|
||||
seen_motion = True
|
||||
time.sleep(0.08)
|
||||
continue
|
||||
if grip_motion_settled(st.grip_state, st.position, target, seen_motion):
|
||||
return st
|
||||
time.sleep(0.08)
|
||||
return st
|
||||
|
||||
def get_init_state(self, retries: int = 8) -> int:
|
||||
return self.read_register(REG_INIT_STATE, retries=retries)
|
||||
|
||||
def get_grip_state(self, retries: int = 8) -> int:
|
||||
return self.read_register(REG_GRIP_STATE, retries=retries)
|
||||
|
||||
def get_current_position(self, retries: int = 8) -> int:
|
||||
return self.read_register(REG_CUR_POSITION, retries=retries)
|
||||
|
||||
def get_target_position(self, retries: int = 8) -> int:
|
||||
return self.read_register(REG_POSITION, retries=retries)
|
||||
|
||||
def get_target_force(self, retries: int = 8) -> int:
|
||||
return self.read_register(REG_FORCE, retries=retries)
|
||||
|
||||
def get_run_states(self, retries: int = 8) -> GripperState:
|
||||
return GripperState(
|
||||
init_state=self.get_init_state(retries=retries),
|
||||
grip_state=self.get_grip_state(retries=retries),
|
||||
position=self.get_current_position(retries=retries),
|
||||
target_position=self.get_target_position(retries=retries),
|
||||
target_force=self.get_target_force(retries=retries),
|
||||
stroke_mm=self.stroke_mm,
|
||||
)
|
||||
|
||||
def open_jaw(self, force: int = 50, speed: int = 50) -> bool:
|
||||
ok = self.set_target_force(force)
|
||||
ok = self.set_target_speed(speed) and ok
|
||||
return self.set_target_position(1000) and ok
|
||||
|
||||
def close_jaw(self, force: int = 50, speed: int = 50) -> bool:
|
||||
ok = self.set_target_force(force)
|
||||
ok = self.set_target_speed(speed) and ok
|
||||
return self.set_target_position(0) and ok
|
||||
|
||||
def move(self, position: int, force: int = 50, speed: int = 50) -> bool:
|
||||
ok = self.set_target_force(force)
|
||||
ok = self.set_target_speed(speed) and ok
|
||||
return self.set_target_position(position) and ok
|
||||
|
||||
|
||||
def _selfcheck_crc() -> None:
|
||||
"""官方简短手册示例：设置位置 500。"""
|
||||
frame = _frame(1, 0x06, 0x0103, 500)
|
||||
expect = bytes([0x01, 0x06, 0x01, 0x03, 0x01, 0xF4, 0x78, 0x21])
|
||||
if frame != expect:
|
||||
raise AssertionError(f"CRC 不匹配：{frame.hex()} != {expect.hex()}")
|
||||
|
||||
|
||||
def _site_defaults() -> dict:
|
||||
try:
|
||||
from pathlib import Path
|
||||
|
||||
root = Path(__file__).resolve().parents[3]
|
||||
sys.path.insert(0, str(root / "scripts" / "lib"))
|
||||
from export_site_env import load_site # type: ignore
|
||||
|
||||
g = (load_site().get("gripper") or {})
|
||||
return {
|
||||
"port": str(g.get("port") or "/dev/ttyUSB0"),
|
||||
"baudrate": int(g.get("baud") or 115200),
|
||||
"slave_id": int(g.get("slave_id") or 1),
|
||||
"stroke_mm": float(g.get("stroke_mm") or 20.0),
|
||||
"force": int(g.get("force_pct") or 50),
|
||||
"speed": int(g.get("speed_pct") or 50),
|
||||
"rs485_rts": str(g.get("rs485_rts") or ""),
|
||||
}
|
||||
except Exception:
|
||||
return {
|
||||
"port": "/dev/ttyUSB0",
|
||||
"baudrate": 115200,
|
||||
"slave_id": 1,
|
||||
"stroke_mm": 20.0,
|
||||
"force": 50,
|
||||
"speed": 50,
|
||||
"rs485_rts": "",
|
||||
}
|
||||
|
||||
|
||||
def _scan_bus(port: str) -> int:
|
||||
"""遍历波特率 × 从站号 × RTS，直到某个组合有应答。"""
|
||||
bauds = (115200, 57600, 38400, 19200, 9600)
|
||||
slaves = (1, 2, 3)
|
||||
rts_modes = ("", "tx", "tx_inv")
|
||||
print(f"扫描 {port}（请先停掉 ROS 节点 —— 该串口是独占的）")
|
||||
for baud in bauds:
|
||||
for slave in slaves:
|
||||
for rts in rts_modes:
|
||||
label = f"baud={baud} slave={slave} rts={rts or 'off'}"
|
||||
g = DhPgeaGripper(
|
||||
port=port,
|
||||
baudrate=baud,
|
||||
slave_id=slave,
|
||||
timeout_s=0.08,
|
||||
rs485_rts=rts,
|
||||
)
|
||||
try:
|
||||
g.open()
|
||||
state = g.probe()
|
||||
print(f"OK {label} init_state={state}")
|
||||
print(f" → site.yaml: baud: {baud} slave_id: {slave} rs485_rts: \"{rts}\"")
|
||||
g.close()
|
||||
return 0
|
||||
except DhPgeaError:
|
||||
print(f" miss {label}")
|
||||
finally:
|
||||
g.close()
|
||||
print("所有组合都无应答。", file=sys.stderr)
|
||||
print(WIRING_HINT, file=sys.stderr)
|
||||
return 1
|
||||
|
||||
|
||||
def _cli(argv: Optional[list[str]] = None) -> int:
|
||||
site = _site_defaults()
|
||||
ap = argparse.ArgumentParser(
|
||||
description="DH PGEA Modbus 命令行工具（官方寄存器映射，不用 Flexiv RDK）"
|
||||
)
|
||||
ap.add_argument("--port", default=site["port"])
|
||||
ap.add_argument("--baud", type=int, default=site["baudrate"])
|
||||
ap.add_argument("--slave", type=int, default=site["slave_id"])
|
||||
ap.add_argument("--force", type=int, default=site["force"])
|
||||
ap.add_argument("--speed", type=int, default=site["speed"])
|
||||
ap.add_argument("--rts", default=site.get("rs485_rts") or "", help="RS485 DE：空 | tx | tx_inv")
|
||||
ap.add_argument(
|
||||
"cmd",
|
||||
choices=["probe", "init", "status", "open", "close", "pos", "selfcheck", "scan"],
|
||||
)
|
||||
ap.add_argument("value", nargs="?", default="", help="'pos' 命令的位置，0–1000")
|
||||
args = ap.parse_args(argv)
|
||||
|
||||
if args.cmd == "selfcheck":
|
||||
_selfcheck_crc()
|
||||
print("CRC 自检通过（01 06 01 03 01 F4 78 21）")
|
||||
return 0
|
||||
if args.cmd == "scan":
|
||||
return _scan_bus(args.port)
|
||||
|
||||
grip = DhPgeaGripper(
|
||||
port=args.port,
|
||||
baudrate=args.baud,
|
||||
slave_id=args.slave,
|
||||
stroke_mm=float(site["stroke_mm"]),
|
||||
rs485_rts=args.rts,
|
||||
)
|
||||
try:
|
||||
grip.open()
|
||||
except DhPgeaError as exc:
|
||||
print(f"错误：{exc}", file=sys.stderr)
|
||||
return 1
|
||||
|
||||
try:
|
||||
if args.cmd == "probe":
|
||||
st = grip.get_run_states(retries=2)
|
||||
print(st.summary())
|
||||
return 0
|
||||
if args.cmd == "init":
|
||||
print("正在初始化（手指回零，已初始化也会再做一次）…")
|
||||
grip.initialize(force=True)
|
||||
print(grip.get_run_states().summary())
|
||||
return 0
|
||||
if args.cmd == "status":
|
||||
print(grip.get_run_states().summary())
|
||||
return 0
|
||||
if args.cmd == "open":
|
||||
if not grip.open_jaw(args.force, args.speed):
|
||||
raise DhPgeaError("张开写入失败")
|
||||
st = grip.wait_settle(1000)
|
||||
print(st.summary())
|
||||
if abs(st.position - 1000) > 25:
|
||||
print(
|
||||
"张开失败：手指没有离开夹持位。"
|
||||
"若仍是 grip=caught，先取下工件再执行 init。",
|
||||
file=sys.stderr,
|
||||
)
|
||||
return 1
|
||||
return 0
|
||||
if args.cmd == "close":
|
||||
if not grip.close_jaw(args.force, args.speed):
|
||||
raise DhPgeaError("闭合写入失败")
|
||||
st = grip.wait_settle(0)
|
||||
print(st.summary())
|
||||
# 夹住物体时位置停在厚度上，grip=caught 算成功。停在张开位则失败。
|
||||
if st.grip_state != S_GRIP_CAUGHT and abs(st.position) > 25:
|
||||
print("闭合失败：手指没有朝合拢方向离开原位。", file=sys.stderr)
|
||||
return 1
|
||||
return 0
|
||||
if args.cmd == "pos":
|
||||
if args.value == "":
|
||||
print("用法：pos <0-1000>", file=sys.stderr)
|
||||
return 2
|
||||
pos = int(float(args.value))
|
||||
if not grip.move(pos, args.force, args.speed):
|
||||
raise DhPgeaError("位置写入失败")
|
||||
st = grip.wait_settle(pos)
|
||||
print(st.summary())
|
||||
grasped_short = st.grip_state == S_GRIP_CAUGHT and st.position > pos
|
||||
if abs(st.position - pos) > 25 and not grasped_short:
|
||||
print(
|
||||
f"位置 {pos} 没有到位（仍在 {st.position}）。",
|
||||
file=sys.stderr,
|
||||
)
|
||||
return 1
|
||||
return 0
|
||||
except DhPgeaError as exc:
|
||||
print(f"错误：{exc}", file=sys.stderr)
|
||||
print(WIRING_HINT, file=sys.stderr)
|
||||
return 1
|
||||
finally:
|
||||
grip.close()
|
||||
return 0
|
||||
|
||||
|
||||
def main(argv: Optional[list[str]] = None) -> int:
|
||||
return _cli(argv)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,246 @@
|
||||
#!/usr/bin/env python3
|
||||
"""DH PGEA 的 ROS 2 节点（官方 Modbus RTU，不用 Flexiv 夹爪）。
|
||||
|
||||
话题（布局与 DH-Robotics/dh_gripper_ros 一致）：
|
||||
/gripper/ctrl std_msgs/Float32MultiArray
|
||||
data: [initialize, position, force, speed]
|
||||
initialize>0.5 → 写 0xA5；否则设置力/速度/位置
|
||||
/gripper/states std_msgs/Float32MultiArray
|
||||
data: [initialized, grip_state, position, target_pos, target_force]
|
||||
/gripper/joint_states sensor_msgs/JointState
|
||||
|
||||
服务（std_srvs/Trigger）：
|
||||
/gripper/initialize /gripper/open /gripper/close
|
||||
|
||||
不要设置 Flexiv bringup 的 load_gripper:=true —— 那只是 Flexiv-GN01 用的。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
from typing import Optional
|
||||
|
||||
import rclpy
|
||||
from rcl_interfaces.msg import ParameterDescriptor
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import Float32, Float32MultiArray, MultiArrayDimension, MultiArrayLayout
|
||||
from std_srvs.srv import Trigger
|
||||
|
||||
from arm_application.dh_pgea import (
|
||||
DhPgeaError,
|
||||
DhPgeaGripper,
|
||||
GripperState,
|
||||
WIRING_HINT,
|
||||
)
|
||||
from arm_application.ros_node import TolerantNode, param_as_str
|
||||
|
||||
|
||||
def _as_ctrl_array(msg: Float32MultiArray) -> tuple[float, float, float, float]:
|
||||
data = list(msg.data)
|
||||
while len(data) < 4:
|
||||
data.append(0.0)
|
||||
return float(data[0]), float(data[1]), float(data[2]), float(data[3])
|
||||
|
||||
|
||||
class DhPgeaNode(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("dh_pgea")
|
||||
self.declare_parameter("port", "/dev/ttyUSB0")
|
||||
self.declare_parameter("baud", 115200)
|
||||
self.declare_parameter("slave_id", 1)
|
||||
self.declare_parameter("stroke_mm", 20.0)
|
||||
self.declare_parameter("force_pct", 50)
|
||||
self.declare_parameter("speed_pct", 50)
|
||||
self.declare_parameter("auto_init", True)
|
||||
self.declare_parameter("state_hz", 20.0)
|
||||
self.declare_parameter("open_position", 1000)
|
||||
self.declare_parameter("close_position", 0)
|
||||
self._model = param_as_str(self, "model", "PGEA-100-20-W")
|
||||
self._sn = param_as_str(self, "serial_number", "")
|
||||
self._rts = param_as_str(self, "rs485_rts", "")
|
||||
|
||||
self._port = str(self.get_parameter("port").value)
|
||||
self._baud = int(self.get_parameter("baud").value)
|
||||
self._slave = int(self.get_parameter("slave_id").value)
|
||||
self._stroke = float(self.get_parameter("stroke_mm").value)
|
||||
self._force = int(self.get_parameter("force_pct").value)
|
||||
self._speed = int(self.get_parameter("speed_pct").value)
|
||||
_auto = self.get_parameter("auto_init").value
|
||||
if isinstance(_auto, str):
|
||||
self._auto_init = _auto.strip().lower() in ("1", "true", "yes")
|
||||
else:
|
||||
self._auto_init = bool(_auto)
|
||||
self._open_pos = int(self.get_parameter("open_position").value)
|
||||
self._close_pos = int(self.get_parameter("close_position").value)
|
||||
hz = float(self.get_parameter("state_hz").value)
|
||||
|
||||
self._grip = DhPgeaGripper(
|
||||
port=self._port,
|
||||
baudrate=self._baud,
|
||||
slave_id=self._slave,
|
||||
stroke_mm=self._stroke,
|
||||
rs485_rts=self._rts,
|
||||
)
|
||||
self._state = GripperState(stroke_mm=self._stroke)
|
||||
self._cmd_lock = threading.Lock()
|
||||
|
||||
self._pub_state = self.create_publisher(Float32MultiArray, "/gripper/states", 10)
|
||||
self._pub_js = self.create_publisher(JointState, "/gripper/joint_states", 10)
|
||||
self.create_subscription(Float32MultiArray, "/gripper/ctrl", self._on_ctrl, 10)
|
||||
self.create_subscription(Float32, "/gripper/set_position", self._on_set_pos, 10)
|
||||
|
||||
self.create_service(Trigger, "/gripper/initialize", self._srv_init)
|
||||
self.create_service(Trigger, "/gripper/open", self._srv_open)
|
||||
self.create_service(Trigger, "/gripper/close", self._srv_close)
|
||||
|
||||
self.get_logger().info(
|
||||
f"DH {self._model} SN={self._sn or '?'} "
|
||||
f"{self._port} @{self._baud} slave={self._slave} rts={self._rts or 'off'} "
|
||||
f"（官方 Modbus RTU；未使用 Flexiv RDK 夹爪 API）"
|
||||
)
|
||||
try:
|
||||
self._grip.open()
|
||||
except DhPgeaError as exc:
|
||||
self.get_logger().error(str(exc))
|
||||
raise
|
||||
|
||||
self._online = False
|
||||
try:
|
||||
self._grip.probe()
|
||||
self._online = True
|
||||
except DhPgeaError as exc:
|
||||
self.get_logger().error(str(exc))
|
||||
for line in WIRING_HINT.splitlines():
|
||||
self.get_logger().error(line)
|
||||
self.get_logger().error("会持续重试；在总线恢复应答前 open/close 都会失败")
|
||||
|
||||
if self._auto_init and self._online:
|
||||
try:
|
||||
self._grip.initialize()
|
||||
self.get_logger().info("夹爪已初始化")
|
||||
except DhPgeaError as exc:
|
||||
self.get_logger().error(f"auto_init 失败：{exc}")
|
||||
self._online = False
|
||||
|
||||
period = 1.0 / max(1.0, hz)
|
||||
self._timer = self.create_timer(period, self._on_timer)
|
||||
|
||||
def destroy_node(self) -> bool:
|
||||
try:
|
||||
self._grip.close()
|
||||
except Exception:
|
||||
pass
|
||||
return super().destroy_node()
|
||||
|
||||
def _apply_move(self, position: int, force: Optional[int] = None, speed: Optional[int] = None) -> str:
|
||||
with self._cmd_lock:
|
||||
f = self._force if force is None else int(force)
|
||||
s = self._speed if speed is None else int(speed)
|
||||
if force is not None:
|
||||
self._force = f
|
||||
if speed is not None:
|
||||
self._speed = s
|
||||
if not self._grip.move(int(position), f, s):
|
||||
raise DhPgeaError("移动写入失败")
|
||||
return f"pos={int(position)} force={f}% speed={s}%"
|
||||
|
||||
def _on_ctrl(self, msg: Float32MultiArray) -> None:
|
||||
initialize, position, force, speed = _as_ctrl_array(msg)
|
||||
try:
|
||||
if initialize > 0.5:
|
||||
self._grip.initialize()
|
||||
self.get_logger().info("通过 /gripper/ctrl 触发初始化")
|
||||
return
|
||||
f = int(force) if force > 0 else self._force
|
||||
s = int(speed) if speed > 0 else self._speed
|
||||
info = self._apply_move(int(position), f, s)
|
||||
self.get_logger().info(f"ctrl {info}")
|
||||
except DhPgeaError as exc:
|
||||
self.get_logger().error(str(exc))
|
||||
|
||||
def _on_set_pos(self, msg: Float32) -> None:
|
||||
try:
|
||||
info = self._apply_move(int(msg.data))
|
||||
self.get_logger().info(f"set_position {info}")
|
||||
except DhPgeaError as exc:
|
||||
self.get_logger().error(str(exc))
|
||||
|
||||
def _srv_init(self, _req, resp):
|
||||
try:
|
||||
self._grip.initialize()
|
||||
resp.success = True
|
||||
resp.message = self._grip.get_run_states().summary()
|
||||
except DhPgeaError as exc:
|
||||
resp.success = False
|
||||
resp.message = str(exc)
|
||||
return resp
|
||||
|
||||
def _srv_open(self, _req, resp):
|
||||
try:
|
||||
info = self._apply_move(self._open_pos)
|
||||
resp.success = True
|
||||
resp.message = info
|
||||
except DhPgeaError as exc:
|
||||
resp.success = False
|
||||
resp.message = str(exc)
|
||||
return resp
|
||||
|
||||
def _srv_close(self, _req, resp):
|
||||
try:
|
||||
info = self._apply_move(self._close_pos)
|
||||
resp.success = True
|
||||
resp.message = info
|
||||
except DhPgeaError as exc:
|
||||
resp.success = False
|
||||
resp.message = str(exc)
|
||||
return resp
|
||||
|
||||
def _on_timer(self) -> None:
|
||||
try:
|
||||
st = self._grip.get_run_states(retries=2 if self._online else 1)
|
||||
self._state = st
|
||||
if not self._online:
|
||||
self._online = True
|
||||
self.get_logger().info("夹爪总线已恢复")
|
||||
except DhPgeaError as exc:
|
||||
self._online = False
|
||||
self.get_logger().warn(str(exc), throttle_duration_sec=5.0)
|
||||
return
|
||||
|
||||
arr = Float32MultiArray()
|
||||
arr.layout = MultiArrayLayout(
|
||||
dim=[MultiArrayDimension(label="dh_pgea", size=5, stride=5)],
|
||||
data_offset=0,
|
||||
)
|
||||
arr.data = list(st.as_tuple())
|
||||
self._pub_state.publish(arr)
|
||||
|
||||
js = JointState()
|
||||
js.header.stamp = self.get_clock().now().to_msg()
|
||||
js.name = ["gripper_finger1_joint"]
|
||||
# 官方 ROS 驱动把 0–1000 映射到约 0–0.637 rad；我们这里也发布 mm。
|
||||
js.position = [float(st.width_mm) * 0.001]
|
||||
js.effort = [float(st.target_force)]
|
||||
self._pub_js.publish(js)
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = None
|
||||
try:
|
||||
node = DhPgeaNode()
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
except DhPgeaError:
|
||||
pass
|
||||
finally:
|
||||
if node is not None:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,803 @@
|
||||
"""双臂夹爪统一控制架构（DH-Robotics 官方 Modbus RTU，非 Flexiv RDK）。
|
||||
|
||||
设计目标：
|
||||
- 左臂（Rizon4-063989）：大寰 PGEA 平行夹爪，控制行程/夹持力/速度。
|
||||
- 右臂（Rizon4-063991）：大寰 RGI-100-30-O-S-M1 旋转夹爪，控制手指夹持与旋转角度/圈数/速度。
|
||||
- 统一接口，便于后续扩展到 ROS2 节点和新项目脚本。
|
||||
|
||||
注意：DH 官方协议是“写位置/角度寄存器后立即运动”，没有 ChangingTek SDK 的 0x0108 触发寄存器。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
from abc import ABC, abstractmethod
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum, auto
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from arm_application.dh_official import DHModbusClient, DHModbusError
|
||||
|
||||
|
||||
class GripperType(Enum):
|
||||
PARALLEL = auto() # 平行夹爪
|
||||
ROTARY = auto() # 旋转夹爪
|
||||
VACUUM = auto() # 真空吸盘（预留）
|
||||
|
||||
|
||||
class GripperState(Enum):
|
||||
IDLE = auto()
|
||||
MOVING = auto()
|
||||
GRIPPING = auto()
|
||||
RELEASING = auto()
|
||||
ERROR = auto()
|
||||
|
||||
|
||||
@dataclass
|
||||
class GripperStatus:
|
||||
"""夹爪状态反馈。"""
|
||||
|
||||
state: GripperState = GripperState.IDLE
|
||||
position: float = 0.0 # 平行：mm；旋转：角度 deg
|
||||
force: float = 0.0 # 最近一次命令力/力矩 %
|
||||
velocity: float = 0.0 # 最近一次命令速度 %
|
||||
is_moving: bool = False
|
||||
is_gripped: bool = False # 是否夹持到物体
|
||||
connected: bool = False
|
||||
initialized: bool = False
|
||||
finger_position_per_mille: float = 0.0
|
||||
rotation_deg: float = 0.0
|
||||
gripper_state: int = -1 # DH 0x0201：0 moving,1 reached,2 caught,3 dropped
|
||||
rotation_state: int = -1 # RGI 0x020B：0 moving,1 reached,2 blocking,3 blocked
|
||||
error_msg: str = ""
|
||||
timestamp: float = field(default_factory=time.time)
|
||||
|
||||
|
||||
@dataclass
|
||||
class GripperCommand:
|
||||
"""夹爪控制指令。"""
|
||||
|
||||
target_position: float | None = None # 平行：mm；旋转：deg
|
||||
target_force: float | None = None # 目标力/力矩 %
|
||||
target_velocity: float | None = None # 目标速度 %
|
||||
wait_complete: bool = True # 是否等待完成
|
||||
timeout: float = 10.0 # 超时时间
|
||||
|
||||
|
||||
class BaseGripper(ABC):
|
||||
"""夹爪基类，定义统一接口。"""
|
||||
|
||||
name: str = "base_gripper"
|
||||
gripper_type: GripperType = GripperType.PARALLEL
|
||||
|
||||
def __init__(self, config: dict[str, Any] | None = None) -> None:
|
||||
self.cfg = config or {}
|
||||
self._connected = False
|
||||
self._status = GripperStatus()
|
||||
|
||||
@abstractmethod
|
||||
def connect(self) -> None: ...
|
||||
|
||||
@abstractmethod
|
||||
def disconnect(self) -> None: ...
|
||||
|
||||
@abstractmethod
|
||||
def execute(self, cmd: GripperCommand) -> GripperStatus: ...
|
||||
|
||||
@abstractmethod
|
||||
def get_status(self) -> GripperStatus: ...
|
||||
|
||||
def is_connected(self) -> bool:
|
||||
return self._connected
|
||||
|
||||
def open(self, **kwargs) -> GripperStatus:
|
||||
raise NotImplementedError
|
||||
|
||||
def close(self, **kwargs) -> GripperStatus:
|
||||
raise NotImplementedError
|
||||
|
||||
def stop(self) -> None:
|
||||
raise NotImplementedError
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 大寰平行夹爪（PGE/PGEA 系列）
|
||||
# =============================================================================
|
||||
|
||||
@dataclass
|
||||
class ParallelGripperConfig:
|
||||
"""大寰平行夹爪配置。"""
|
||||
|
||||
port: str = "/dev/ttyUSB0"
|
||||
slave_id: int = 1
|
||||
baudrate: int = 115200
|
||||
timeout: float = 0.2
|
||||
|
||||
# PGEA-100-40 行程为 40mm；若实际是 PGEA-100-26，请把 stroke_max 改成 26.0。
|
||||
stroke_min: float = 0.0
|
||||
stroke_max: float = 40.0
|
||||
|
||||
# 默认参数（DH 百分比）
|
||||
default_speed: int = 50
|
||||
default_force: int = 50
|
||||
force_min: int = 20
|
||||
force_max: int = 100
|
||||
|
||||
# 控制行为
|
||||
auto_initialize: bool = True
|
||||
init_timeout: float = 10.0
|
||||
position_tolerance_per_mille: int = 15
|
||||
status_poll_interval: float = 0.05
|
||||
|
||||
|
||||
class ParallelGripper(BaseGripper):
|
||||
"""大寰 PGE/PGEA 平行夹爪。"""
|
||||
|
||||
name = "dh_pgea_parallel"
|
||||
gripper_type = GripperType.PARALLEL
|
||||
|
||||
def __init__(self, config: ParallelGripperConfig | None = None) -> None:
|
||||
super().__init__()
|
||||
self.cfg = config or ParallelGripperConfig()
|
||||
self._client: DHModbusClient | None = None
|
||||
self._last_position_mm = float(self.cfg.stroke_min)
|
||||
self._last_force = int(self.cfg.default_force)
|
||||
self._last_speed = int(self.cfg.default_speed)
|
||||
|
||||
def connect(self) -> None:
|
||||
if self._client is not None:
|
||||
self.disconnect()
|
||||
self._client = DHModbusClient(
|
||||
self.cfg.port,
|
||||
slave_id=self.cfg.slave_id,
|
||||
baudrate=self.cfg.baudrate,
|
||||
timeout=self.cfg.timeout,
|
||||
)
|
||||
self._connected = True
|
||||
self._status.connected = True
|
||||
self._status.state = GripperState.IDLE
|
||||
self._status.error_msg = ""
|
||||
if self.cfg.auto_initialize:
|
||||
self.initialize()
|
||||
|
||||
def disconnect(self) -> None:
|
||||
if self._client is not None:
|
||||
self._client.close()
|
||||
self._client = None
|
||||
self._connected = False
|
||||
self._status.connected = False
|
||||
self._status.state = GripperState.IDLE
|
||||
|
||||
def initialize(self, full: bool = False) -> None:
|
||||
self._require_client()
|
||||
assert self._client is not None
|
||||
self._client.initialize(full=full, rotation=False, timeout=self.cfg.init_timeout)
|
||||
self._status.initialized = True
|
||||
|
||||
def _require_client(self) -> None:
|
||||
if not self._connected or self._client is None:
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = "平行夹爪未连接"
|
||||
raise DHModbusError(self._status.error_msg)
|
||||
|
||||
def _mm_to_per_mille(self, position_mm: float) -> int:
|
||||
lo = float(self.cfg.stroke_min)
|
||||
hi = float(self.cfg.stroke_max)
|
||||
if hi <= lo:
|
||||
hi = lo + 1.0
|
||||
ratio = (float(position_mm) - lo) / (hi - lo)
|
||||
return int(np.clip(round(ratio * 1000.0), 0, 1000))
|
||||
|
||||
def _per_mille_to_mm(self, position: float | int) -> float:
|
||||
lo = float(self.cfg.stroke_min)
|
||||
hi = float(self.cfg.stroke_max)
|
||||
return lo + (float(position) / 1000.0) * (hi - lo)
|
||||
|
||||
def execute(self, cmd: GripperCommand) -> GripperStatus:
|
||||
try:
|
||||
self._require_client()
|
||||
except DHModbusError:
|
||||
return self._status
|
||||
assert self._client is not None
|
||||
|
||||
position_mm = self._last_position_mm if cmd.target_position is None else float(cmd.target_position)
|
||||
position_mm = float(np.clip(position_mm, self.cfg.stroke_min, self.cfg.stroke_max))
|
||||
speed = int(np.clip(cmd.target_velocity or self.cfg.default_speed, 1, 100))
|
||||
force = int(np.clip(cmd.target_force or self.cfg.default_force, self.cfg.force_min, self.cfg.force_max))
|
||||
|
||||
self._client.set_speed_pct(speed)
|
||||
self._client.set_force_pct(force)
|
||||
target_per_mille = self._client.set_position_per_mille(self._mm_to_per_mille(position_mm))
|
||||
|
||||
self._last_position_mm = position_mm
|
||||
self._last_speed = speed
|
||||
self._last_force = force
|
||||
self._status.state = GripperState.MOVING
|
||||
self._status.error_msg = ""
|
||||
|
||||
if cmd.wait_complete:
|
||||
self._wait_complete(target_per_mille, cmd.timeout)
|
||||
return self.get_status()
|
||||
|
||||
def _wait_complete(self, target_per_mille: int, timeout: float) -> None:
|
||||
assert self._client is not None
|
||||
start = time.monotonic()
|
||||
time.sleep(0.05)
|
||||
while time.monotonic() - start < timeout:
|
||||
state = self._client.get_gripper_state()
|
||||
pos = self._client.get_position_per_mille()
|
||||
self._status.gripper_state = state
|
||||
self._status.finger_position_per_mille = float(pos)
|
||||
self._status.position = self._per_mille_to_mm(pos)
|
||||
self._status.is_moving = state == 0
|
||||
self._status.is_gripped = state == 2
|
||||
if state in (1, 2):
|
||||
self._status.state = GripperState.GRIPPING if state == 2 else GripperState.IDLE
|
||||
return
|
||||
if state == 3:
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = "夹持后物体掉落"
|
||||
return
|
||||
if abs(pos - target_per_mille) <= self.cfg.position_tolerance_per_mille and state != 0:
|
||||
self._status.state = GripperState.IDLE
|
||||
return
|
||||
time.sleep(self.cfg.status_poll_interval)
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = f"平行夹爪运动超时（>{timeout:.1f}s）"
|
||||
|
||||
def get_status(self) -> GripperStatus:
|
||||
if not self._connected or self._client is None:
|
||||
self._status.connected = False
|
||||
return self._status
|
||||
try:
|
||||
pos = self._client.get_position_per_mille()
|
||||
state = self._client.get_gripper_state()
|
||||
init_state = self._client.get_init_state()
|
||||
self._status.connected = True
|
||||
self._status.initialized = init_state == 1
|
||||
self._status.finger_position_per_mille = float(pos)
|
||||
self._status.position = self._per_mille_to_mm(pos)
|
||||
self._status.velocity = float(self._last_speed)
|
||||
self._status.force = float(self._last_force)
|
||||
self._status.gripper_state = state
|
||||
self._status.is_moving = state == 0
|
||||
self._status.is_gripped = state == 2
|
||||
if self._status.state != GripperState.ERROR:
|
||||
self._status.state = GripperState.MOVING if state == 0 else (
|
||||
GripperState.GRIPPING if state == 2 else GripperState.IDLE
|
||||
)
|
||||
self._status.timestamp = time.time()
|
||||
except Exception as exc:
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = str(exc)
|
||||
self._status.connected = False
|
||||
return self._status
|
||||
|
||||
def open(self, force: int | None = None, speed: int | None = None, **kwargs) -> GripperStatus:
|
||||
# DH 千分比：0‰=闭合，1000‰=张开，对应行程 0mm -> stroke_max。
|
||||
return self.execute(GripperCommand(
|
||||
target_position=float(self.cfg.stroke_max),
|
||||
target_force=force or self.cfg.default_force,
|
||||
target_velocity=speed or self.cfg.default_speed,
|
||||
**kwargs,
|
||||
))
|
||||
|
||||
def close(self, force: int | None = None, speed: int | None = None, **kwargs) -> GripperStatus:
|
||||
return self.execute(GripperCommand(
|
||||
target_position=float(self.cfg.stroke_min),
|
||||
target_force=force or self.cfg.default_force,
|
||||
target_velocity=speed or self.cfg.default_speed,
|
||||
**kwargs,
|
||||
))
|
||||
|
||||
def move_to(self, position_mm: float, force: int | None = None, speed: int | None = None,
|
||||
**kwargs) -> GripperStatus:
|
||||
return self.execute(GripperCommand(
|
||||
target_position=position_mm,
|
||||
target_force=force or self.cfg.default_force,
|
||||
target_velocity=speed or self.cfg.default_speed,
|
||||
**kwargs,
|
||||
))
|
||||
|
||||
def set_force(self, force_pct: int) -> None:
|
||||
self._require_client()
|
||||
assert self._client is not None
|
||||
self._last_force = self._client.set_force_pct(force_pct)
|
||||
|
||||
def set_speed(self, speed_pct: int) -> None:
|
||||
self._require_client()
|
||||
assert self._client is not None
|
||||
self._last_speed = self._client.set_speed_pct(speed_pct)
|
||||
|
||||
def stop(self) -> None:
|
||||
if self._client is not None:
|
||||
self._client.hold_current_position()
|
||||
self._status.state = GripperState.IDLE
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 大寰旋转夹爪（RGI-100-30-O-S-M1）
|
||||
# =============================================================================
|
||||
|
||||
@dataclass
|
||||
class RotaryGripperConfig:
|
||||
"""大寰 RGI 旋转夹爪配置。"""
|
||||
|
||||
port: str = "/dev/ttyUSB1"
|
||||
slave_id: int = 1
|
||||
baudrate: int = 115200
|
||||
timeout: float = 0.2
|
||||
|
||||
# RGI 旋转角度寄存器是 signed 16bit；为避免 one's-complement 负零歧义，默认限制在 ±32767。
|
||||
angle_min: float = -32767.0
|
||||
angle_max: float = 32767.0
|
||||
max_step_deg: float = 30000.0 # rotate_revolutions 分段旋转的最大单步角度
|
||||
|
||||
# DH 手册：手指位置 0~1000‰。0=闭合，1000=张开。RGI-100 行程约 70mm。
|
||||
finger_open_per_mille: int = 1000
|
||||
finger_close_per_mille: int = 0
|
||||
|
||||
# 默认参数（DH 百分比）
|
||||
default_speed: int = 50 # 旋转速度 %
|
||||
default_torque: int = 50 # 旋转力/力矩 %
|
||||
default_finger_speed: int = 50 # 手指速度 %
|
||||
default_finger_force: int = 50 # 手指夹持力 %
|
||||
torque_min: int = 20
|
||||
torque_max: int = 100
|
||||
|
||||
# 控制行为
|
||||
auto_initialize: bool = True
|
||||
init_timeout: float = 15.0 # RGI 初始化包含旋转找零，时间更长
|
||||
angle_tolerance_deg: float = 1.0
|
||||
status_poll_interval: float = 0.05
|
||||
|
||||
|
||||
class RotaryGripper(BaseGripper):
|
||||
"""大寰 RGI-100-30-O-S-M1 旋转夹爪。"""
|
||||
|
||||
name = "dh_rgi_rotary"
|
||||
gripper_type = GripperType.ROTARY
|
||||
|
||||
def __init__(self, config: RotaryGripperConfig | None = None) -> None:
|
||||
super().__init__()
|
||||
self.cfg = config or RotaryGripperConfig()
|
||||
self._client: DHModbusClient | None = None
|
||||
self._last_angle = 0.0
|
||||
self._last_torque = int(self.cfg.default_torque)
|
||||
self._last_speed = int(self.cfg.default_speed)
|
||||
self._last_finger_force = int(self.cfg.default_finger_force)
|
||||
self._last_finger_speed = int(self.cfg.default_finger_speed)
|
||||
self._last_finger_per_mille = int(self.cfg.finger_open_per_mille)
|
||||
|
||||
def connect(self) -> None:
|
||||
if self._client is not None:
|
||||
self.disconnect()
|
||||
self._client = DHModbusClient(
|
||||
self.cfg.port,
|
||||
slave_id=self.cfg.slave_id,
|
||||
baudrate=self.cfg.baudrate,
|
||||
timeout=self.cfg.timeout,
|
||||
)
|
||||
self._connected = True
|
||||
self._status.connected = True
|
||||
self._status.state = GripperState.IDLE
|
||||
self._status.error_msg = ""
|
||||
if self.cfg.auto_initialize:
|
||||
self.initialize()
|
||||
|
||||
def disconnect(self) -> None:
|
||||
if self._client is not None:
|
||||
self._client.close()
|
||||
self._client = None
|
||||
self._connected = False
|
||||
self._status.connected = False
|
||||
self._status.state = GripperState.IDLE
|
||||
|
||||
def initialize(self, full: bool = False) -> None:
|
||||
self._require_client()
|
||||
assert self._client is not None
|
||||
self._client.initialize(full=full, rotation=True, timeout=self.cfg.init_timeout)
|
||||
self._status.initialized = True
|
||||
|
||||
def _require_client(self) -> None:
|
||||
if not self._connected or self._client is None:
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = "旋转夹爪未连接"
|
||||
raise DHModbusError(self._status.error_msg)
|
||||
|
||||
def execute(self, cmd: GripperCommand) -> GripperStatus:
|
||||
"""执行旋转指令；cmd.target_position 单位为度。"""
|
||||
return self.rotate_to(
|
||||
self._last_angle if cmd.target_position is None else float(cmd.target_position),
|
||||
speed=cmd.target_velocity,
|
||||
torque=cmd.target_force,
|
||||
wait_complete=cmd.wait_complete,
|
||||
timeout=cmd.timeout,
|
||||
)
|
||||
|
||||
def _wait_complete(self, target_angle: float, timeout: float) -> None:
|
||||
assert self._client is not None
|
||||
start = time.monotonic()
|
||||
start_angle = float(self._client.get_rotation_angle_deg())
|
||||
expected = abs(float(target_angle) - start_angle) + 30.0
|
||||
settled = 0
|
||||
last_angle = start_angle
|
||||
time.sleep(0.05)
|
||||
while time.monotonic() - start < timeout:
|
||||
rot_state = self._client.get_rotation_state()
|
||||
angle = float(self._client.get_rotation_angle_deg())
|
||||
self._status.rotation_state = rot_state
|
||||
self._status.rotation_deg = angle
|
||||
self._status.position = angle
|
||||
self._status.is_moving = rot_state == 0
|
||||
if abs(angle - start_angle) > expected:
|
||||
self._client.hold_current_rotation()
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = (
|
||||
f"旋转超程保护：目标 {target_angle:.1f}°，从 {start_angle:.1f}° 跑到 {angle:.1f}°，已发停止"
|
||||
)
|
||||
return
|
||||
if rot_state in (1, 2):
|
||||
self._status.state = GripperState.IDLE if rot_state == 1 else GripperState.GRIPPING
|
||||
if rot_state == 2:
|
||||
self._status.error_msg = "旋转受阻（blocking）"
|
||||
return
|
||||
if rot_state == 3:
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = "旋转曾受阻且未到目标位"
|
||||
return
|
||||
if abs(angle - float(target_angle)) <= self.cfg.angle_tolerance_deg:
|
||||
settled += 1
|
||||
if settled >= 3:
|
||||
self._status.state = GripperState.IDLE
|
||||
self._status.is_moving = False
|
||||
return
|
||||
elif abs(angle - last_angle) <= self.cfg.angle_tolerance_deg:
|
||||
settled += 1
|
||||
if settled >= 8 and abs(angle - float(target_angle)) <= 15.0:
|
||||
self._status.state = GripperState.IDLE
|
||||
self._status.is_moving = False
|
||||
return
|
||||
else:
|
||||
settled = 0
|
||||
last_angle = angle
|
||||
time.sleep(self.cfg.status_poll_interval)
|
||||
self._client.hold_current_rotation()
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = f"旋转运动超时（>{timeout:.1f}s），已发停止"
|
||||
|
||||
def get_status(self) -> GripperStatus:
|
||||
if not self._connected or self._client is None:
|
||||
self._status.connected = False
|
||||
return self._status
|
||||
try:
|
||||
finger_pos = self._client.get_position_per_mille()
|
||||
finger_state = self._client.get_gripper_state()
|
||||
init_state = self._client.get_init_state()
|
||||
rot_angle = self._client.get_rotation_angle_deg()
|
||||
rot_state = self._client.get_rotation_state()
|
||||
rot_init = self._client.get_rotation_init_state()
|
||||
|
||||
self._status.connected = True
|
||||
self._status.initialized = init_state == 1 and rot_init == 1
|
||||
self._status.finger_position_per_mille = float(finger_pos)
|
||||
self._status.rotation_deg = float(rot_angle)
|
||||
self._status.position = float(rot_angle)
|
||||
self._status.velocity = float(self._last_speed)
|
||||
self._status.force = float(self._last_torque)
|
||||
self._status.gripper_state = finger_state
|
||||
self._status.rotation_state = rot_state
|
||||
self._status.is_moving = finger_state == 0 or rot_state == 0
|
||||
self._status.is_gripped = finger_state == 2
|
||||
if self._status.state != GripperState.ERROR:
|
||||
if finger_state == 2 or rot_state == 2:
|
||||
self._status.state = GripperState.GRIPPING
|
||||
elif finger_state == 0 or rot_state == 0:
|
||||
self._status.state = GripperState.MOVING
|
||||
else:
|
||||
self._status.state = GripperState.IDLE
|
||||
self._status.timestamp = time.time()
|
||||
except Exception as exc:
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = str(exc)
|
||||
self._status.connected = False
|
||||
return self._status
|
||||
|
||||
# -----------------------------
|
||||
# 手指夹持
|
||||
# -----------------------------
|
||||
def move_fingers(self, position_per_mille: int, force: int | None = None,
|
||||
speed: int | None = None, wait_complete: bool = True,
|
||||
timeout: float = 10.0) -> GripperStatus:
|
||||
try:
|
||||
self._require_client()
|
||||
except DHModbusError:
|
||||
return self._status
|
||||
assert self._client is not None
|
||||
target = int(np.clip(position_per_mille, 0, 1000))
|
||||
f = int(np.clip(force or self.cfg.default_finger_force, 20, 100))
|
||||
s = int(np.clip(speed or self.cfg.default_finger_speed, 1, 100))
|
||||
self._client.set_speed_pct(s)
|
||||
self._client.set_force_pct(f)
|
||||
self._client.set_position_per_mille(target)
|
||||
self._last_finger_per_mille = target
|
||||
self._last_finger_force = f
|
||||
self._last_finger_speed = s
|
||||
if wait_complete:
|
||||
start = time.monotonic()
|
||||
time.sleep(0.05)
|
||||
while time.monotonic() - start < timeout:
|
||||
state = self._client.get_gripper_state()
|
||||
pos = self._client.get_position_per_mille()
|
||||
self._status.gripper_state = state
|
||||
self._status.finger_position_per_mille = float(pos)
|
||||
if state in (1, 2):
|
||||
break
|
||||
if state == 3:
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = "夹持后物体掉落"
|
||||
break
|
||||
time.sleep(self.cfg.status_poll_interval)
|
||||
return self.get_status()
|
||||
|
||||
def open_fingers(self, **kwargs) -> GripperStatus:
|
||||
return self.move_fingers(self.cfg.finger_open_per_mille, **kwargs)
|
||||
|
||||
def close_fingers(self, **kwargs) -> GripperStatus:
|
||||
return self.move_fingers(self.cfg.finger_close_per_mille, **kwargs)
|
||||
|
||||
# -----------------------------
|
||||
# 旋转
|
||||
# -----------------------------
|
||||
def rotate_to(self, angle_deg: float, speed: int | None = None, torque: int | None = None,
|
||||
wait_complete: bool = True, timeout: float = 10.0, **kwargs) -> GripperStatus:
|
||||
try:
|
||||
self._require_client()
|
||||
except DHModbusError:
|
||||
return self._status
|
||||
assert self._client is not None
|
||||
|
||||
angle = float(np.clip(angle_deg, self.cfg.angle_min, self.cfg.angle_max))
|
||||
s = int(np.clip(speed or self.cfg.default_speed, 1, 100))
|
||||
t = int(np.clip(torque or self.cfg.default_torque, self.cfg.torque_min, self.cfg.torque_max))
|
||||
|
||||
self._client.set_rotation_speed_pct(s)
|
||||
self._client.set_rotation_force_pct(t)
|
||||
target = self._client.set_rotation_angle_deg(angle)
|
||||
|
||||
self._last_angle = float(target)
|
||||
self._last_speed = s
|
||||
self._last_torque = t
|
||||
self._status.state = GripperState.MOVING
|
||||
self._status.error_msg = ""
|
||||
|
||||
if wait_complete:
|
||||
self._wait_complete(float(target), timeout)
|
||||
return self.get_status()
|
||||
|
||||
def rotate_by(self, delta_deg: float, **kwargs) -> GripperStatus:
|
||||
current = self.get_status().rotation_deg
|
||||
return self.rotate_to(current + float(delta_deg), **kwargs)
|
||||
|
||||
def rotate_revolutions(self, revolutions: float, **kwargs) -> GripperStatus:
|
||||
"""旋转指定圈数；超过单步角度范围时自动分段，并按实际反馈角度结算。"""
|
||||
remaining = float(revolutions) * 360.0
|
||||
status = self.get_status()
|
||||
max_step = abs(float(self.cfg.max_step_deg)) or 30000.0
|
||||
while abs(remaining) > 1e-6:
|
||||
before = float(self.get_status().rotation_deg)
|
||||
step = float(np.clip(remaining, -max_step, max_step))
|
||||
status = self.rotate_to(before + step, **kwargs)
|
||||
if status.state == GripperState.ERROR:
|
||||
return status
|
||||
after = float(status.rotation_deg)
|
||||
achieved = after - before
|
||||
if abs(achieved) < max(1.0, abs(step) * 0.2):
|
||||
status.state = GripperState.ERROR
|
||||
status.error_msg = (
|
||||
"旋转未达到预期步进，可能超出角度寄存器范围或反馈异常："
|
||||
f"step={step:.3f}, before={before:.3f}, after={after:.3f}"
|
||||
)
|
||||
return status
|
||||
remaining -= achieved
|
||||
return status
|
||||
|
||||
def home_rotation(self, **kwargs) -> GripperStatus:
|
||||
return self.rotate_to(0.0, **kwargs)
|
||||
|
||||
def set_rotation_speed(self, speed_pct: int) -> None:
|
||||
self._require_client()
|
||||
assert self._client is not None
|
||||
self._last_speed = self._client.set_rotation_speed_pct(speed_pct)
|
||||
|
||||
def set_rotation_torque(self, torque_pct: int) -> None:
|
||||
self._require_client()
|
||||
assert self._client is not None
|
||||
self._last_torque = self._client.set_rotation_force_pct(torque_pct)
|
||||
|
||||
def set_finger_speed(self, speed_pct: int) -> None:
|
||||
self._require_client()
|
||||
assert self._client is not None
|
||||
self._last_finger_speed = self._client.set_speed_pct(speed_pct)
|
||||
|
||||
def set_finger_force(self, force_pct: int) -> None:
|
||||
self._require_client()
|
||||
assert self._client is not None
|
||||
self._last_finger_force = self._client.set_force_pct(force_pct)
|
||||
|
||||
def open(self, **kwargs) -> GripperStatus:
|
||||
"""对于 RGI，open 定义为张开手指；旋转请用 rotate_to/rotate_revolutions。"""
|
||||
return self.open_fingers(**kwargs)
|
||||
|
||||
def close(self, **kwargs) -> GripperStatus:
|
||||
"""对于 RGI，close 定义为闭合手指；旋转请用 rotate_to/rotate_revolutions。"""
|
||||
return self.close_fingers(**kwargs)
|
||||
|
||||
def stop(self) -> None:
|
||||
if self._client is not None:
|
||||
self._client.hold_current_all()
|
||||
self._status.state = GripperState.IDLE
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 双臂夹爪管理器
|
||||
# =============================================================================
|
||||
|
||||
@dataclass
|
||||
class DualArmGripperConfig:
|
||||
"""双臂夹爪配置。
|
||||
|
||||
这里**没有** ``left_arm_sn`` / ``right_arm_sn``：它们曾经存在但从来没有被读过
|
||||
（只写不读的死字段，R5.5），而且默认值是本机的 SN —— 换台臂就会误导读者。
|
||||
哪个侧别是哪台臂，由 ``load_from_site()`` 从 site.yaml 的 ``robots[].arm`` 决定。
|
||||
"""
|
||||
|
||||
left_gripper: ParallelGripperConfig = field(default_factory=ParallelGripperConfig)
|
||||
right_gripper: RotaryGripperConfig = field(default_factory=RotaryGripperConfig)
|
||||
|
||||
|
||||
class DualArmGripperManager:
|
||||
"""双臂夹爪管理器：统一控制左右臂夹爪。"""
|
||||
|
||||
def __init__(self, config: DualArmGripperConfig | None = None) -> None:
|
||||
self.cfg = config or DualArmGripperConfig()
|
||||
self._left: ParallelGripper | None = None
|
||||
self._right: RotaryGripper | None = None
|
||||
|
||||
@property
|
||||
def left(self) -> ParallelGripper:
|
||||
if self._left is None:
|
||||
self._left = ParallelGripper(self.cfg.left_gripper)
|
||||
return self._left
|
||||
|
||||
@property
|
||||
def right(self) -> RotaryGripper:
|
||||
if self._right is None:
|
||||
self._right = RotaryGripper(self.cfg.right_gripper)
|
||||
return self._right
|
||||
|
||||
def connect_all(self) -> None:
|
||||
self.left.connect()
|
||||
self.right.connect()
|
||||
|
||||
def disconnect_all(self) -> None:
|
||||
if self._left:
|
||||
self._left.disconnect()
|
||||
if self._right:
|
||||
self._right.disconnect()
|
||||
|
||||
def initialize_all(self, full: bool = False) -> None:
|
||||
self.left.initialize(full=full)
|
||||
self.right.initialize(full=full)
|
||||
|
||||
def get_all_status(self) -> dict[str, GripperStatus]:
|
||||
return {
|
||||
"left": self.left.get_status(),
|
||||
"right": self.right.get_status(),
|
||||
}
|
||||
|
||||
def emergency_stop(self) -> None:
|
||||
self.left.stop()
|
||||
self.right.stop()
|
||||
|
||||
|
||||
def load_dual_arm_gripper_config(path: str | Path) -> DualArmGripperConfig:
|
||||
"""从 configs/dual_arm_robot.yaml 加载双臂夹爪配置。"""
|
||||
import yaml
|
||||
|
||||
data = yaml.safe_load(Path(path).read_text(encoding="utf-8")) or {}
|
||||
left = data.get("left_arm", {}) or {}
|
||||
right = data.get("right_arm", {}) or {}
|
||||
grip = data.get("gripper", {}) or {}
|
||||
parallel = grip.get("parallel", {}) or {}
|
||||
rotary = grip.get("rotary", {}) or {}
|
||||
|
||||
left_cfg = ParallelGripperConfig(
|
||||
port=left.get("gripper_port", "/dev/ttyUSB0"),
|
||||
slave_id=int(left.get("gripper_slave_id", 1)),
|
||||
baudrate=int(left.get("gripper_baudrate", 115200)),
|
||||
stroke_min=float(parallel.get("stroke_min", 0.0)),
|
||||
stroke_max=float(parallel.get("stroke_max", 40.0)),
|
||||
default_speed=int(parallel.get("default_speed", 50)),
|
||||
default_force=int(parallel.get("default_force", 50)),
|
||||
force_min=int(parallel.get("force_min", 20)),
|
||||
force_max=int(parallel.get("force_max", 100)),
|
||||
auto_initialize=bool(parallel.get("auto_initialize", True)),
|
||||
)
|
||||
right_cfg = RotaryGripperConfig(
|
||||
port=right.get("gripper_port", "/dev/ttyUSB1"),
|
||||
slave_id=int(right.get("gripper_slave_id", 1)),
|
||||
baudrate=int(right.get("gripper_baudrate", 115200)),
|
||||
angle_min=float(rotary.get("angle_min", -32767.0)),
|
||||
angle_max=float(rotary.get("angle_max", 32767.0)),
|
||||
max_step_deg=float(rotary.get("max_step_deg", 30000.0)),
|
||||
finger_open_per_mille=int(rotary.get("finger_open_per_mille", 1000)),
|
||||
finger_close_per_mille=int(rotary.get("finger_close_per_mille", 0)),
|
||||
default_speed=int(rotary.get("default_speed", 50)),
|
||||
default_torque=int(rotary.get("default_torque", 50)),
|
||||
default_finger_speed=int(rotary.get("default_finger_speed", 50)),
|
||||
default_finger_force=int(rotary.get("default_finger_force", 50)),
|
||||
torque_min=int(rotary.get("torque_min", 20)),
|
||||
torque_max=int(rotary.get("torque_max", 100)),
|
||||
auto_initialize=bool(rotary.get("auto_initialize", True)),
|
||||
)
|
||||
return DualArmGripperConfig(
|
||||
left_gripper=left_cfg,
|
||||
right_gripper=right_cfg,
|
||||
)
|
||||
|
||||
|
||||
def load_from_site(data: dict) -> DualArmGripperConfig:
|
||||
"""从 ``~/ros2/config/site.yaml`` 构建 ``DualArmGripperConfig``。
|
||||
|
||||
左右靠 ``robots[].arm`` 认，**不靠 SN 里的数字**：``"063989 就是左臂"``
|
||||
是站点数据，写进代码后换个站点就会把左右夹爪搞反
|
||||
（症状是"右臂的旋转爪去开左臂的平行爪"，串口对不上才报错）。
|
||||
"""
|
||||
robots = list((data.get("robot") or {}).get("robots") or [])
|
||||
left = next((r for r in robots if str(r.get("arm") or "").lower() == "left"), {})
|
||||
right = next((r for r in robots if str(r.get("arm") or "").lower() == "right"), {})
|
||||
g = data.get("gripper") or {}
|
||||
parallel = g.get("parallel") or {}
|
||||
rotary = g.get("rotary") or {}
|
||||
lg = left.get("gripper") or {}
|
||||
rg = right.get("gripper") or {}
|
||||
stroke = float(lg.get("stroke_mm") or parallel.get("stroke_max") or g.get("stroke_mm") or 20.0)
|
||||
left_cfg = ParallelGripperConfig(
|
||||
port=str(lg.get("port") or "/dev/ttyUSB0"),
|
||||
slave_id=int(lg.get("slave_id") or g.get("slave_id") or 1),
|
||||
baudrate=int(lg.get("baud") or g.get("baud") or 115200),
|
||||
stroke_min=float(parallel.get("stroke_min") or 0.0),
|
||||
stroke_max=stroke,
|
||||
default_speed=int(parallel.get("default_speed") or g.get("speed_pct") or 50),
|
||||
default_force=int(parallel.get("default_force") or g.get("force_pct") or 50),
|
||||
force_min=int(parallel.get("force_min") or 20),
|
||||
force_max=int(parallel.get("force_max") or 100),
|
||||
auto_initialize=bool(lg.get("auto_init", parallel.get("auto_initialize", True))),
|
||||
init_timeout=float(parallel.get("init_timeout") or 10.0),
|
||||
)
|
||||
right_cfg = RotaryGripperConfig(
|
||||
port=str(rg.get("port") or "/dev/ttyUSB1"),
|
||||
slave_id=int(rg.get("slave_id") or 1),
|
||||
baudrate=int(rg.get("baud") or g.get("baud") or 115200),
|
||||
angle_min=float(rotary.get("angle_min") or -32767.0),
|
||||
angle_max=float(rotary.get("angle_max") or 32767.0),
|
||||
max_step_deg=float(rotary.get("max_step_deg") or 30000.0),
|
||||
finger_open_per_mille=int(rotary.get("finger_open_per_mille") or 1000),
|
||||
finger_close_per_mille=int(rotary.get("finger_close_per_mille") or 0),
|
||||
default_speed=int(rotary.get("default_speed") or 50),
|
||||
default_torque=int(rotary.get("default_torque") or 50),
|
||||
default_finger_speed=int(rotary.get("default_finger_speed") or 50),
|
||||
default_finger_force=int(rotary.get("default_finger_force") or 50),
|
||||
torque_min=int(rotary.get("torque_min") or 20),
|
||||
torque_max=int(rotary.get("torque_max") or 100),
|
||||
auto_initialize=bool(rg.get("auto_init", rotary.get("auto_initialize", False))),
|
||||
init_timeout=float(rotary.get("init_timeout") or 15.0),
|
||||
)
|
||||
return DualArmGripperConfig(
|
||||
left_gripper=left_cfg,
|
||||
right_gripper=right_cfg,
|
||||
)
|
||||
+190
@@ -0,0 +1,190 @@
|
||||
#!/usr/bin/env python3
|
||||
"""DH PGEA（左臂）与 RGI（右臂）的命令行工具。从 site.yaml 读取串口。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import yaml
|
||||
|
||||
from arm_application.dual_gripper import DualArmGripperManager, load_from_site
|
||||
|
||||
|
||||
def _site_path() -> Path:
|
||||
env = os.environ.get("SITE_CONFIG", "").strip()
|
||||
if env:
|
||||
return Path(env).expanduser()
|
||||
return Path.home() / "ros2" / "config" / "site.yaml"
|
||||
|
||||
|
||||
def _print_status(label: str, status) -> None:
|
||||
print(
|
||||
f" {label}: state={status.state.name} "
|
||||
f"pos={status.position:.2f} finger={status.finger_position_per_mille:.0f}‰ "
|
||||
f"rot={status.rotation_deg:.2f}° moving={status.is_moving} "
|
||||
f"gripped={status.is_gripped} init={status.initialized} "
|
||||
f"err={status.error_msg or '-'}"
|
||||
)
|
||||
|
||||
|
||||
def _load_site_exporter():
|
||||
"""复用 ``scripts/lib/export_site_env.py`` —— 它才是 --arm 别名的权威定义。
|
||||
|
||||
这里**不自己再抄一份** ``89 -> SN`` 的映射：抄一份就多一处会漂移的地方（M2）。
|
||||
"""
|
||||
root = Path(__file__).resolve().parents[3]
|
||||
lib = str(root / "scripts" / "lib")
|
||||
if lib not in sys.path:
|
||||
sys.path.insert(0, lib)
|
||||
import export_site_env # type: ignore
|
||||
|
||||
return export_site_env
|
||||
|
||||
|
||||
def _normalize_arm(raw: str, data: dict) -> str:
|
||||
"""把 --arm 的写法归一成 ``left`` / ``right`` / ``both``。
|
||||
|
||||
别名（``89`` 这类）由 ``export_site_env`` 从 site.yaml 的 ``robots[]`` 现算
|
||||
（臂号 = 该臂 SN 末两位）。代码里**不写死** SN，否则换台机器人后会指错臂，
|
||||
而症状是"给右臂发命令，左臂动了"。
|
||||
"""
|
||||
t = (raw or "").strip()
|
||||
if t.lower() in ("both", "dual", "all"):
|
||||
return "both"
|
||||
site_env = _load_site_exporter()
|
||||
sn, matched = site_env.resolve_robot_sn(data, t)
|
||||
if not matched:
|
||||
raise SystemExit(
|
||||
f"未知的 --arm {raw!r}：不在 site.yaml 的 robot.robots[] 里。\n"
|
||||
f"可用：\n{site_env.describe_arms(data)}\n"
|
||||
f"也可以直接写 left|right|both。"
|
||||
)
|
||||
for row in (data.get("robot") or {}).get("robots") or []:
|
||||
if str(row.get("sn") or "").strip() != sn:
|
||||
continue
|
||||
arm = str(row.get("arm") or "").strip().lower()
|
||||
if arm in ("left", "right"):
|
||||
return arm
|
||||
raise SystemExit(
|
||||
f"--arm {raw!r} 选中了 {sn}，但 site.yaml 里这台臂没写 arm: left|right，"
|
||||
f"无法判断操作哪个夹爪。请补上这个键。"
|
||||
)
|
||||
raise SystemExit(
|
||||
f"--arm {raw!r} 解析出 {sn}，但它不在 site.yaml 的 robot.robots[] 里，"
|
||||
f"无法判断是哪个夹爪。"
|
||||
)
|
||||
|
||||
|
||||
def test_parallel(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
|
||||
gripper = manager.left
|
||||
print(f"=== PGEA 平行夹爪（{gripper.cfg.port}）===")
|
||||
gripper.connect()
|
||||
_print_status("已连接", gripper.get_status())
|
||||
if args.test in ("init", "probe", "all"):
|
||||
if args.test != "probe":
|
||||
gripper.initialize(full=args.init_full)
|
||||
_print_status("初始化", gripper.get_status())
|
||||
if args.test in ("open", "all"):
|
||||
_print_status("张开", gripper.open(force=args.force, speed=args.speed))
|
||||
if args.test in ("close", "all"):
|
||||
_print_status("闭合", gripper.close(force=args.force, speed=args.speed))
|
||||
if args.test in ("move", "pos") and args.pos is not None:
|
||||
_print_status("移动", gripper.move_to(args.pos, force=args.force, speed=args.speed))
|
||||
if args.test in ("status", "probe"):
|
||||
_print_status("状态", gripper.get_status())
|
||||
|
||||
|
||||
def test_rotary(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
|
||||
gripper = manager.right
|
||||
print(f"=== RGI 旋转夹爪（{gripper.cfg.port}）===")
|
||||
gripper.connect()
|
||||
_print_status("已连接", gripper.get_status())
|
||||
if args.test in ("init", "all"):
|
||||
gripper.initialize(full=args.init_full)
|
||||
_print_status("初始化", gripper.get_status())
|
||||
if args.test in ("open", "all"):
|
||||
_print_status("张开", gripper.open_fingers(force=args.force, speed=args.speed))
|
||||
if args.test in ("close", "all"):
|
||||
_print_status("闭合", gripper.close_fingers(force=args.force, speed=args.speed))
|
||||
if args.test in ("move", "pos") and args.pos is not None:
|
||||
_print_status("手指", gripper.move_fingers(int(args.pos), force=args.force, speed=args.speed))
|
||||
if args.test in ("rotate", "rotate_to", "all"):
|
||||
if args.revolutions is not None:
|
||||
_print_status(
|
||||
"圈数",
|
||||
gripper.rotate_revolutions(args.revolutions, speed=args.speed, torque=args.force),
|
||||
)
|
||||
else:
|
||||
angle = args.angle if args.angle is not None else 90.0
|
||||
_print_status("旋转", gripper.rotate_to(angle, speed=args.speed, torque=args.force))
|
||||
if args.test == "rotate_by":
|
||||
delta = args.angle if args.angle is not None else 45.0
|
||||
_print_status("相对旋转", gripper.rotate_by(delta, speed=args.speed, torque=args.force))
|
||||
if args.test in ("status", "probe"):
|
||||
_print_status("状态", gripper.get_status())
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser(description="DH PGEA / RGI 命令行工具（串口来自 site.yaml）")
|
||||
parser.add_argument(
|
||||
"--arm",
|
||||
# 默认取当前臂（parse_arm_args.sh / load_site_config.sh 已经把它放进环境）。
|
||||
# **不**兜底成某个具体臂号：那等于"没选臂时也能跑，跑的是哪台看运气"。
|
||||
default=(
|
||||
os.environ.get("SITE_ROBOT_ARM")
|
||||
or os.environ.get("ROBOT_SN")
|
||||
or os.environ.get("SITE_ROBOT_SN")
|
||||
or ""
|
||||
),
|
||||
help="left|right|both，或该臂的臂号（= SN 末两位，由 site.yaml 决定）",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--test",
|
||||
default="status",
|
||||
help="probe|init|open|close|move|pos|rotate|rotate_by|status",
|
||||
)
|
||||
parser.add_argument("--pos", type=float, default=None, help="PGEA 为 mm；RGI 为手指 0-1000")
|
||||
parser.add_argument("--angle", type=float, default=None)
|
||||
parser.add_argument("--revolutions", type=float, default=None)
|
||||
parser.add_argument("--speed", type=int, default=25)
|
||||
parser.add_argument("--force", type=int, default=33)
|
||||
parser.add_argument("--init-full", action="store_true")
|
||||
parser.add_argument("--site", default="")
|
||||
args = parser.parse_args()
|
||||
|
||||
site = Path(args.site).expanduser() if args.site else _site_path()
|
||||
if not site.is_file():
|
||||
print(f"错误：site.yaml 缺失：{site}", file=sys.stderr)
|
||||
return 1
|
||||
data = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
|
||||
manager = DualArmGripperManager(load_from_site(data))
|
||||
arm = _normalize_arm(str(args.arm), data)
|
||||
print(f"site={site} arm={arm} test={args.test}")
|
||||
|
||||
try:
|
||||
if arm == "left":
|
||||
test_parallel(manager, args)
|
||||
elif arm == "right":
|
||||
test_rotary(manager, args)
|
||||
else:
|
||||
test_parallel(manager, args)
|
||||
test_rotary(manager, args)
|
||||
except KeyboardInterrupt:
|
||||
print("收到中断 —— 停止")
|
||||
manager.emergency_stop()
|
||||
return 130
|
||||
except Exception as exc:
|
||||
print(f"错误：{exc}", file=sys.stderr)
|
||||
return 1
|
||||
finally:
|
||||
manager.disconnect_all()
|
||||
time.sleep(0.05)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
+320
@@ -0,0 +1,320 @@
|
||||
"""ROS 2 双臂 DH 夹爪节点（左臂 PGEA + 右臂 RGI）。使用 arm_msgs，而非 Flexiv RDK。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import threading
|
||||
from pathlib import Path
|
||||
|
||||
import rclpy
|
||||
import yaml
|
||||
from rclpy.callback_groups import ReentrantCallbackGroup
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
from rclpy.node import Node
|
||||
|
||||
from arm_application.dual_gripper import (
|
||||
DualArmGripperManager,
|
||||
GripperState,
|
||||
load_dual_arm_gripper_config,
|
||||
load_from_site,
|
||||
)
|
||||
from arm_msgs.msg import GripperCommand as GripperCommandMsg
|
||||
from arm_msgs.msg import GripperStatus as GripperStatusMsg
|
||||
from arm_msgs.srv import GripperExecute
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
|
||||
def _site_path() -> Path:
|
||||
env = os.environ.get("SITE_CONFIG", "").strip()
|
||||
if env:
|
||||
return Path(env).expanduser()
|
||||
return Path.home() / "ros2" / "config" / "site.yaml"
|
||||
|
||||
|
||||
def _as_bool(val, default: bool = True) -> bool:
|
||||
if val is None:
|
||||
return default
|
||||
if isinstance(val, str):
|
||||
return val.strip().lower() in ("1", "true", "yes", "on")
|
||||
return bool(val)
|
||||
|
||||
|
||||
class DualGripperNode(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("dual_gripper_driver")
|
||||
self.declare_parameter("config_file", "")
|
||||
self.declare_parameter("auto_connect", True)
|
||||
self.declare_parameter("allow_offline", True)
|
||||
self.declare_parameter("publish_hz", 10.0)
|
||||
self.declare_parameter("sides", "left,right")
|
||||
|
||||
self._cb_group = ReentrantCallbackGroup()
|
||||
self._locks = {"left": threading.Lock(), "right": threading.Lock()}
|
||||
sides_raw = str(self.get_parameter("sides").value or "left,right")
|
||||
self._sides = [s.strip().lower() for s in sides_raw.split(",") if s.strip()]
|
||||
if not self._sides:
|
||||
self._sides = ["left", "right"]
|
||||
|
||||
cfg_path = self.get_parameter("config_file").get_parameter_value().string_value.strip()
|
||||
if cfg_path:
|
||||
self._manager = DualArmGripperManager(load_dual_arm_gripper_config(cfg_path))
|
||||
else:
|
||||
site = _site_path()
|
||||
data = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
|
||||
self._manager = DualArmGripperManager(load_from_site(data))
|
||||
self.get_logger().info(f"夹爪配置来自 site.yaml：{site}")
|
||||
|
||||
self._connect_errors: dict[str, str] = {}
|
||||
if _as_bool(self.get_parameter("auto_connect").value, True):
|
||||
self._connect_all()
|
||||
|
||||
self._left_pub = self.create_publisher(GripperStatusMsg, "~/left/status", 10)
|
||||
self._right_pub = self.create_publisher(GripperStatusMsg, "~/right/status", 10)
|
||||
if "left" in self._sides:
|
||||
self.create_subscription(
|
||||
GripperCommandMsg,
|
||||
"~/left/command",
|
||||
lambda msg: self._on_command("left", msg),
|
||||
10,
|
||||
callback_group=self._cb_group,
|
||||
)
|
||||
self.create_service(
|
||||
GripperExecute,
|
||||
"~/left/execute",
|
||||
lambda req, resp: self._on_execute("left", req, resp),
|
||||
callback_group=self._cb_group,
|
||||
)
|
||||
if "right" in self._sides:
|
||||
self.create_subscription(
|
||||
GripperCommandMsg,
|
||||
"~/right/command",
|
||||
lambda msg: self._on_command("right", msg),
|
||||
10,
|
||||
callback_group=self._cb_group,
|
||||
)
|
||||
self.create_service(
|
||||
GripperExecute,
|
||||
"~/right/execute",
|
||||
lambda req, resp: self._on_execute("right", req, resp),
|
||||
callback_group=self._cb_group,
|
||||
)
|
||||
|
||||
period = 1.0 / max(1e-3, float(self.get_parameter("publish_hz").value))
|
||||
self.create_timer(period, self._publish_status, callback_group=self._cb_group)
|
||||
self.get_logger().info(
|
||||
f"dual_gripper_driver ready sides={self._sides} "
|
||||
"~/left|right/command status execute"
|
||||
)
|
||||
|
||||
def _connect_all(self) -> None:
|
||||
mapping = (("left", self._manager.left), ("right", self._manager.right))
|
||||
for side, gripper in mapping:
|
||||
if side not in self._sides:
|
||||
continue
|
||||
try:
|
||||
gripper.connect()
|
||||
self.get_logger().info(f"{side} 夹爪已连接：{gripper.cfg.port}")
|
||||
except Exception as exc:
|
||||
self._connect_errors[side] = str(exc)
|
||||
self.get_logger().error(f"{side} 夹爪连接失败：{exc}")
|
||||
if self._connect_errors and not _as_bool(self.get_parameter("allow_offline").value, True):
|
||||
raise RuntimeError(f"夹爪连接失败：{self._connect_errors}")
|
||||
|
||||
def _gripper(self, side: str):
|
||||
return self._manager.left if side == "left" else self._manager.right
|
||||
|
||||
def _execute(self, side: str, msg: GripperCommandMsg):
|
||||
gripper = self._gripper(side)
|
||||
command = (msg.command or "").strip().lower()
|
||||
timeout = float(msg.timeout or 10.0)
|
||||
wait = bool(msg.wait_complete)
|
||||
|
||||
if side in self._connect_errors and not gripper.is_connected():
|
||||
status = gripper.get_status()
|
||||
status.error_msg = self._connect_errors[side]
|
||||
return status
|
||||
|
||||
if command == "connect":
|
||||
gripper.connect()
|
||||
self._connect_errors.pop(side, None)
|
||||
return gripper.get_status()
|
||||
if command == "init":
|
||||
gripper.initialize(full=False)
|
||||
return gripper.get_status()
|
||||
if command == "init_full":
|
||||
gripper.initialize(full=True)
|
||||
return gripper.get_status()
|
||||
if command == "stop":
|
||||
gripper.stop()
|
||||
return gripper.get_status()
|
||||
|
||||
if side == "left":
|
||||
if command == "open":
|
||||
return gripper.open(
|
||||
force=int(msg.force) if msg.force else None,
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command == "close":
|
||||
return gripper.close(
|
||||
force=int(msg.force) if msg.force else None,
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command == "move":
|
||||
return gripper.move_to(
|
||||
float(msg.position),
|
||||
force=int(msg.force) if msg.force else None,
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command == "set_force":
|
||||
gripper.set_force(int(msg.force))
|
||||
return gripper.get_status()
|
||||
if command == "set_speed":
|
||||
gripper.set_speed(int(msg.speed))
|
||||
return gripper.get_status()
|
||||
else:
|
||||
if command == "open":
|
||||
return gripper.open_fingers(
|
||||
force=int(msg.force) if msg.force else None,
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command == "close":
|
||||
return gripper.close_fingers(
|
||||
force=int(msg.force) if msg.force else None,
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command == "move":
|
||||
return gripper.move_fingers(
|
||||
int(msg.position),
|
||||
force=int(msg.force) if msg.force else None,
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command in ("rotate", "rotate_to"):
|
||||
return gripper.rotate_to(
|
||||
float(msg.position),
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
torque=int(msg.force) if msg.force else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command == "rotate_by":
|
||||
return gripper.rotate_by(
|
||||
float(msg.position),
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
torque=int(msg.force) if msg.force else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command == "rotate_revolutions":
|
||||
return gripper.rotate_revolutions(
|
||||
float(msg.position),
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
torque=int(msg.force) if msg.force else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command == "home_rotation":
|
||||
return gripper.home_rotation(
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
torque=int(msg.force) if msg.force else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command == "set_force":
|
||||
gripper.set_rotation_torque(int(msg.force))
|
||||
return gripper.get_status()
|
||||
if command == "set_speed":
|
||||
gripper.set_rotation_speed(int(msg.speed))
|
||||
return gripper.get_status()
|
||||
if command == "set_finger_force":
|
||||
gripper.set_finger_force(int(msg.force))
|
||||
return gripper.get_status()
|
||||
if command == "set_finger_speed":
|
||||
gripper.set_finger_speed(int(msg.speed))
|
||||
return gripper.get_status()
|
||||
|
||||
status = gripper.get_status()
|
||||
status.state = GripperState.ERROR
|
||||
status.error_msg = f"不支持的命令 {command!r}（side={side}）"
|
||||
return status
|
||||
|
||||
def _on_command(self, side: str, msg: GripperCommandMsg) -> None:
|
||||
with self._locks[side]:
|
||||
status = self._execute(side, msg)
|
||||
if status.state == GripperState.ERROR:
|
||||
self.get_logger().warn(f"{side} 命令 {msg.command!r} 失败：{status.error_msg}")
|
||||
|
||||
def _on_execute(self, side: str, request: GripperExecute.Request, response: GripperExecute.Response):
|
||||
with self._locks[side]:
|
||||
status = self._execute(side, request.command)
|
||||
response.success = status.state != GripperState.ERROR and status.connected
|
||||
response.message = status.error_msg or "OK"
|
||||
response.status = self._to_status_msg(side, status)
|
||||
return response
|
||||
|
||||
def _to_status_msg(self, side: str, status) -> GripperStatusMsg:
|
||||
gripper = self._gripper(side)
|
||||
msg = GripperStatusMsg()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = f"{side}_gripper"
|
||||
msg.name = gripper.name
|
||||
msg.gripper_type = gripper.gripper_type.name.lower()
|
||||
msg.state = status.state.name
|
||||
msg.connected = bool(status.connected)
|
||||
msg.initialized = bool(status.initialized)
|
||||
msg.is_moving = bool(status.is_moving)
|
||||
msg.is_gripped = bool(status.is_gripped)
|
||||
msg.position = float(status.position)
|
||||
msg.finger_position_per_mille = float(status.finger_position_per_mille)
|
||||
msg.rotation_deg = float(status.rotation_deg)
|
||||
msg.gripper_state = int(status.gripper_state)
|
||||
msg.rotation_state = int(status.rotation_state)
|
||||
msg.velocity = float(status.velocity)
|
||||
msg.force = float(status.force)
|
||||
msg.error_msg = status.error_msg or ""
|
||||
return msg
|
||||
|
||||
def _publish_status(self) -> None:
|
||||
if "left" in self._sides:
|
||||
left_status = self._manager.left.get_status()
|
||||
if "left" in self._connect_errors and not left_status.connected:
|
||||
left_status.error_msg = self._connect_errors["left"]
|
||||
self._left_pub.publish(self._to_status_msg("left", left_status))
|
||||
if "right" in self._sides:
|
||||
right_status = self._manager.right.get_status()
|
||||
if "right" in self._connect_errors and not right_status.connected:
|
||||
right_status.error_msg = self._connect_errors["right"]
|
||||
self._right_pub.publish(self._to_status_msg("right", right_status))
|
||||
|
||||
def destroy_node(self) -> bool:
|
||||
try:
|
||||
self._manager.disconnect_all()
|
||||
finally:
|
||||
return super().destroy_node()
|
||||
|
||||
|
||||
def main() -> None:
|
||||
rclpy.init()
|
||||
node = DualGripperNode()
|
||||
executor = MultiThreadedExecutor(num_threads=4)
|
||||
try:
|
||||
rclpy.spin(node, executor=executor)
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
+200
@@ -0,0 +1,200 @@
|
||||
#!/usr/bin/env python3
|
||||
"""跟踪点的三道守卫：**位置合理性** + **每帧位移上限** + **远框重捕获确认**。
|
||||
|
||||
为什么需要它们（2026-10-08 真机 arm 89，一阶段跑 150s 没锁上）
|
||||
------------------------------------------------------------
|
||||
``/tmp/pipeline_stage1.log`` 原话。真 k0 一开始是对的::
|
||||
|
||||
764.79 跟踪 keypoint conf=0.87 uv=(410,246) z=0.299 ← 真 k0（画面中偏右）
|
||||
|
||||
然后在 1 秒内被一串**假 k0** 顶掉（全都是高置信度）::
|
||||
|
||||
766.96 重新捕获 conf=0.58 dpx=480
|
||||
766.96 跟踪 keypoint conf=0.58 uv=(76,75) z=0.392 ← 贴左上角
|
||||
767.75 跟踪 keypoint conf=0.23 uv=(169,107) z_ok=0 深度过期 box_z hold_near
|
||||
768.16 跟踪 keypoint conf=0.53 uv=(143,92) z=0.376
|
||||
768.78 跟踪 keypoint conf=0.73 uv=(12,30) k0(ok=1551 near=620 借深度) z=0.276
|
||||
769.66 重新捕获 conf=0.22 dpx=207
|
||||
770.07 跟踪 keypoint conf=0.73 uv=(431,226) k0(ok=19440 near=7776 借深度) z=0.271
|
||||
|
||||
伺服于是以 0.45m/s 追着它们跑：``ee_xy`` home ``(0.686,-0.113)`` → ``(0.354,-0.294)``
|
||||
→ ``(0.083,-0.515)``（工作盒角落），1.2s 后看门狗急停；工件已在半米外，
|
||||
剩下 148s 全是"检测漏检"。三条结论：
|
||||
|
||||
1. **置信度挡不住**：假 k0 的 conf 是 0.53~0.73，真 k0 是 0.87 —— 没有可分区间。
|
||||
但假 k0 有一个共同特征：**落在画面边上**（``uv=(12,30)`` / ``(76,75)`` /
|
||||
``(143,92)``），而孔心长在工件上、不可能贴在画面边缘；
|
||||
2. **借深度把"没有深度"伪装成"有深度"**：``k0(… 借深度)`` 的 z 是从工件框里
|
||||
借的，看着很合理（0.27~0.39m），于是"物理量程"这道守卫也失效了。而
|
||||
**借来的深度意味着该像素处读不到自己的深度** ⇒ 那个点的绝对 XY 不可靠，
|
||||
不该用来**重定位**（换一个跟踪位置）；
|
||||
3. **位移不受限**：真 k0 到假 k0 一帧能差 480px / 400mm，伺服一次就全速扑过去。
|
||||
|
||||
三道守卫各挡一层，全都是"能真的失败"的判据（见 test_feature_guard_*.py 的负向用例）：
|
||||
|
||||
``kpt_plausible``
|
||||
k0 必须落在自己的检测框内（含少量余量）、且不许贴画面边缘。违反 ⇒ 本帧的
|
||||
k0 **位置**不可信（不是"没有目标"），交回调用方走"按框上相对偏移重建"的
|
||||
连续性路径（``perception_node._keypoint_feature`` 本来就有这条路）。
|
||||
``TrackSlewGuard``
|
||||
已发布跟踪点的**每帧位移上限**。工件不动、相机 30Hz，真实观测一帧不可能走
|
||||
4cm（``feature_jump_guard`` 实测真实运动 ≤15mm/帧）。超过上限：观测可信就
|
||||
**夹到上限**（限速跟随，不丢目标），观测来自借深度就**整帧不采信**。
|
||||
``FarReacquireConfirm``
|
||||
"远框重新捕获"（``dpx`` 大于门限）必须连续 ``confirm_frames`` 帧指向同一处
|
||||
才采信 —— 一帧的远框更可能是模型在别的东西上误报，而不是工件真的被挪走了。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Optional, Sequence, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def _xy_dist(a: np.ndarray, b: np.ndarray) -> float:
|
||||
"""两个 XY 点的距离（米/像素都行，单位跟随入参）。单一实现，别各写一份。"""
|
||||
return float(np.hypot(float(a[0]) - float(b[0]), float(a[1]) - float(b[1])))
|
||||
|
||||
|
||||
def kpt_plausible(
|
||||
u: float,
|
||||
v: float,
|
||||
box: Optional[Sequence[float]],
|
||||
w: int,
|
||||
h: int,
|
||||
*,
|
||||
border_px: float = 6.0,
|
||||
box_margin_frac: float = 0.10,
|
||||
) -> Optional[str]:
|
||||
"""k0 的**像素位置**是否可信。返回 None = 可信；否则返回拒绝原因。
|
||||
|
||||
``box`` 是同一帧的工件检测框 ``(x0,y0,x1,y1)``；``w,h`` 是彩色图宽高。
|
||||
"""
|
||||
if not (np.isfinite(u) and np.isfinite(v)):
|
||||
return "k0 像素非有限值"
|
||||
if box is None:
|
||||
# 没有框就没法判断 k0 是否落在工件上 —— 宁可当不可信（走连续性路径）。
|
||||
return "没有检测框，无法判断 k0 是否落在工件上"
|
||||
b = np.asarray(box, dtype=float).reshape(-1)
|
||||
if b.size < 4:
|
||||
return "检测框字段不完整"
|
||||
x0, y0, x1, y1 = (float(vv) for vv in b[:4])
|
||||
bw, bh = abs(x1 - x0), abs(y1 - y0)
|
||||
if not (np.isfinite(bw) and np.isfinite(bh)) or bw <= 1e-6 or bh <= 1e-6:
|
||||
return "检测框尺寸无效"
|
||||
mx, my = box_margin_frac * bw, box_margin_frac * bh
|
||||
if u < x0 - mx or u > x1 + mx or v < y0 - my or v > y1 + my:
|
||||
return (
|
||||
f"k0 ({u:.0f},{v:.0f}) 落在检测框 "
|
||||
f"[{x0:.0f},{y0:.0f},{x1:.0f},{y1:.0f}] 之外"
|
||||
)
|
||||
if w > 0 and h > 0:
|
||||
if u <= border_px or v <= border_px or u >= w - 1 - border_px or v >= h - 1 - border_px:
|
||||
return (
|
||||
f"k0 ({u:.0f},{v:.0f}) 贴画面边缘（{w}x{h}，余量 {border_px:.0f}px）"
|
||||
"—— 孔心长在工件上，不可能在画面边上"
|
||||
)
|
||||
return None
|
||||
|
||||
|
||||
class TrackSlewGuard:
|
||||
"""已发布跟踪点的每帧位移上限。
|
||||
|
||||
单实例服务**一个**跟踪点；调用方负责在"没有目标/重置"时 :meth:`reset`。
|
||||
"""
|
||||
|
||||
def __init__(self, max_step_m: float) -> None:
|
||||
# 只有**一个**门限，故意的：观测可信就限速跟随，借深度就整帧不采信。
|
||||
# 不做成两个门限是因为"借深度"只影响"能不能重定位"，不影响"小步跟不跟" ——
|
||||
# 两个门限会立刻变成"哪个该调、调到多少"的扯皮（且没有实测支撑）。
|
||||
self.max_step_m = float(max_step_m)
|
||||
self._last: Optional[np.ndarray] = None
|
||||
self.clamped_total = 0
|
||||
self.rejected_borrowed_total = 0
|
||||
|
||||
def reset(self) -> None:
|
||||
self._last = None
|
||||
|
||||
def update(
|
||||
self, p_xy: Optional[Sequence[float]], *, borrowed: bool = False
|
||||
) -> Tuple[Optional[np.ndarray], Optional[str]]:
|
||||
"""喂一次观测，返回 ``(采信的点, 拒绝原因)``。
|
||||
|
||||
- 第一次观测（没有历史）：原样采信（冷启动没有更好的信息，见服务节点的
|
||||
照常起步）。
|
||||
- 位移 ≤ 上限：原样采信。
|
||||
- 位移 > 上限且 ``borrowed``：整帧不采信（返回 ``None`` + 原因）——
|
||||
借来的深度意味着这个像素读不到自己的深度，绝对位置不足以用来重定位。
|
||||
- 位移 > 上限且观测可信：**夹到上限**（限速跟随），不丢目标。
|
||||
"""
|
||||
if p_xy is None:
|
||||
return None, "本帧没有观测"
|
||||
p = np.asarray(p_xy, dtype=float).reshape(2)
|
||||
if not np.all(np.isfinite(p)):
|
||||
return None, "跟踪点非有限值"
|
||||
if self._last is None:
|
||||
self._last = p.copy()
|
||||
return p, None
|
||||
d = _xy_dist(p, self._last)
|
||||
if d <= self.max_step_m:
|
||||
self._last = p.copy()
|
||||
return p, None
|
||||
if borrowed:
|
||||
self.rejected_borrowed_total += 1
|
||||
return None, (
|
||||
f"借深度的观测要求位移 {d*1000:.1f}mm > 上限 "
|
||||
f"{self.max_step_m*1000:.0f}mm —— 判误检，本帧不采信"
|
||||
)
|
||||
step = self._last + (p - self._last) * (self.max_step_m / d)
|
||||
self._last = step.copy()
|
||||
self.clamped_total += 1
|
||||
return step, (
|
||||
f"位移 {d*1000:.1f}mm > 上限 {self.max_step_m*1000:.0f}mm —— "
|
||||
"夹到上限限速跟随"
|
||||
)
|
||||
|
||||
|
||||
class FarReacquireConfirm:
|
||||
"""远框"重新捕获"的多帧确认。
|
||||
|
||||
``dpx`` 是候选框中心与上一帧框中心的像素距离。超过 ``far_px`` 的候选必须
|
||||
连续 ``confirm_frames`` 帧落在同一处（相互间距 ≤ ``far_px``）才放行。
|
||||
"""
|
||||
|
||||
def __init__(self, far_px: float, confirm_frames: int = 3) -> None:
|
||||
self.far_px = float(far_px)
|
||||
self.confirm_frames = max(1, int(confirm_frames))
|
||||
self._cand: Optional[np.ndarray] = None
|
||||
self._count = 0
|
||||
|
||||
def reset(self) -> None:
|
||||
self._cand = None
|
||||
self._count = 0
|
||||
|
||||
def allow(
|
||||
self, center_xy: Optional[Sequence[float]], dpx: float, *, near_px: float
|
||||
) -> Tuple[bool, Optional[str]]:
|
||||
"""返回 ``(是否放行, 拒绝原因)``。``near_px`` 是"不算远框"的门限。"""
|
||||
if center_xy is None:
|
||||
self.reset()
|
||||
return True, None
|
||||
c = np.asarray(center_xy, dtype=float).reshape(2)
|
||||
if not np.all(np.isfinite(c)):
|
||||
return False, "候选框中心非有限值"
|
||||
if float(dpx) <= float(near_px):
|
||||
# 近框：正常跟踪，不需要确认
|
||||
self.reset()
|
||||
return True, None
|
||||
if self._cand is None or _xy_dist(c, self._cand) > self.far_px:
|
||||
self._cand = c.copy()
|
||||
self._count = 1
|
||||
else:
|
||||
self._count += 1
|
||||
if self._count >= self.confirm_frames:
|
||||
self.reset()
|
||||
return True, None
|
||||
return False, (
|
||||
f"远框重捕获 dpx={float(dpx):.0f}px 需连续 {self.confirm_frames} 帧一致"
|
||||
f"（当前 {self._count} 帧）—— 本帧不采信"
|
||||
)
|
||||
+98
@@ -0,0 +1,98 @@
|
||||
#!/usr/bin/env python3
|
||||
"""跟踪点"假跳变"的确认门限：单帧大跳不采信，连续 N 帧一致才认。
|
||||
|
||||
为什么需要它（2026-10-08 真机实测，一阶段跑 150s 没锁上）
|
||||
--------------------------------------------------------
|
||||
arm 89 的日志原话：
|
||||
|
||||
0659.551 死区保持 |e_xy|=3.0mm ← 其实已经锁定！误差只剩 3mm
|
||||
0659.598 k0 置信度 0.01 < 0.30，本帧不用它
|
||||
0659.604 HANDEYE snap feat_base=(0.675,-0.099) ← 跟踪点瞬间横移 89mm
|
||||
0660.121 v=(-0.206,+0.400) |e_xy|=130mm ← 伺服以 0.45m/s 去追这个假点
|
||||
0660.561 过期保持 —— 不跟踪冻结的检测框 ← 画面糊掉，目标彻底丢失
|
||||
|
||||
原来的写法是"跳变超过 ``predict_jump_m`` 就 ``reset()``" —— 等于**单帧就采信**。
|
||||
可是相机 30Hz、工件不动，真实特征点一帧不可能跳 89mm（实测的真实运动
|
||||
在 15mm/帧 以内）。所以"大跳"几乎必然是检测/兜底产生的假点，不该一路追过去。
|
||||
|
||||
判据
|
||||
----
|
||||
- 位移 ≤ ``jump_m``：正常观测（``near``），照常滤波更新。
|
||||
- 位移 > ``jump_m``：进入"待确认"。此后**连续 ``confirm_frames`` 帧**都要落在
|
||||
同一个新位置附近（相互间距 ≤ ``jump_m``），才判为真实重定位（``jump``）。
|
||||
- 只要中间有一帧不对上（乱跳），计数清零、重新开始 —— 乱跳永远等不到确认。
|
||||
|
||||
这也顺手挡住"感知层 k0 掉信后乱跳"那类现象：宁可原地不动，也不要追着假点跑。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
# ``servo_node._update_feat_track`` 的三种判定结果。
|
||||
# 为什么要显式区分"没有目标"：perception 会把误检点也发过来（2026-10-08 真机，
|
||||
# 模型在运动模糊下把特征点丢到基座后方 -0.10,-0.65，工件其实在 0.5,-0.12），
|
||||
# 这种点在物理上不可能 —— 必须当"没看到目标"处理，而不是"钉在原地"。
|
||||
ACCEPT = "accept" # 采信这次观测
|
||||
HOLD = "hold" # 疑似假跳 —— 位置钉住不动
|
||||
REJECT = "reject" # 物理上不合理（工作盒外）—— 当"没有目标"处理
|
||||
|
||||
|
||||
class FeatureJumpGate:
|
||||
"""有状态的跳变确认门限。线程安全由调用方保证（servo 在锁内调用）。"""
|
||||
|
||||
NEAR = "near" # 正常观测：照常滤波更新
|
||||
HOLD = "hold" # 疑似假跳：本次不采信，位置保持不动
|
||||
JUMP = "jump" # 已确认的真实重定位：允许 reset 到新位置
|
||||
|
||||
def __init__(self, jump_m: float, confirm_frames: int = 3) -> None:
|
||||
self.jump_m = float(jump_m)
|
||||
# 至少 1 帧才谈得上"确认"；<=1 就等于退回旧的"单帧采信"，但那是显式选择，
|
||||
# 不是静默降级 —— 这里仍然保留参数以方便现场调节。
|
||||
self.confirm_frames = max(1, int(confirm_frames))
|
||||
self._cand: Optional[np.ndarray] = None
|
||||
self._count = 0
|
||||
# 诊断计数：连续 hold 了多少帧（供日志/排查，见 servo_node 的告警）。
|
||||
self.hold_frames = 0
|
||||
self.hold_total = 0
|
||||
|
||||
def reset(self) -> None:
|
||||
"""清空待确认状态（正常观测、或已确认后都要清）。"""
|
||||
self._cand = None
|
||||
self._count = 0
|
||||
self.hold_frames = 0
|
||||
|
||||
@staticmethod
|
||||
def _dist(a: np.ndarray, b: np.ndarray) -> float:
|
||||
return float(np.hypot(float(a[0]) - float(b[0]), float(a[1]) - float(b[1])))
|
||||
|
||||
def update(self, p_xy: np.ndarray, ref_xy: np.ndarray) -> str:
|
||||
"""喂一次观测。
|
||||
|
||||
参数
|
||||
----
|
||||
p_xy : 本帧测到的 XY（base 系，米）
|
||||
ref_xy : 当前滤波后的位置 XY（米）—— 判断"跳没跳"的基准
|
||||
|
||||
返回 :data:`NEAR` / :data:`HOLD` / :data:`JUMP` 之一。
|
||||
"""
|
||||
p = np.asarray(p_xy, dtype=float).reshape(2)
|
||||
ref = np.asarray(ref_xy, dtype=float).reshape(2)
|
||||
if self._dist(p, ref) <= self.jump_m:
|
||||
self.reset()
|
||||
return self.NEAR
|
||||
|
||||
# 大跳：本次观测只算"候选"。候选之间互相对不上就从头再数。
|
||||
if self._cand is None or self._dist(p, self._cand) > self.jump_m:
|
||||
self._cand = p.copy()
|
||||
self._count = 1
|
||||
else:
|
||||
self._count += 1
|
||||
if self._count >= self.confirm_frames:
|
||||
self.reset()
|
||||
return self.JUMP
|
||||
self.hold_frames += 1
|
||||
self.hold_total += 1
|
||||
return self.HOLD
|
||||
+2623
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,405 @@
|
||||
"""S00 开机自检：把"能不能跑"这件事变成一张**可打印的清单**（纯逻辑部分）。
|
||||
|
||||
为什么要有这一阶段
|
||||
------------------
|
||||
看板 S00 的要求是"各服务就绪后才开指令窗，任一不就绪即中止并报告"。它挡的是
|
||||
这个仓库最常见的一类事故：**流程跑起来了，只是没接上硬件**。例如：
|
||||
|
||||
* 触觉没起 → 三阶段的力阈值全是 0 → 搜孔"一直没找到"（实际是没数据）；
|
||||
* 相机没起 → 精拍拿不到图 → 报"精拍未拿到图像"（离真正原因差三层）；
|
||||
* 夹爪没起 → S08 松爪/夹紧静默失败 → 棘轮循环空转，看起来像"拧不动"。
|
||||
|
||||
放纯逻辑里的原因
|
||||
----------------
|
||||
"要检查哪些东西"是**事实**，会随现场（两台臂、两种夹爪后端、D435 有没有标定）
|
||||
变化；"怎么检查"是 ROS 机制。前者做成纯函数，就能被测试钉住：
|
||||
|
||||
* 少一项检查 → 测试红（比如"忘了查触觉"）；
|
||||
* 检查项的顺序无关紧要，但**必检项**和**选检项**的划分必须明确 ——
|
||||
把核心判据依赖的触觉标成选检，就会在没传感器时一路跑到力判据才失败。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Dict, Iterable, List, Mapping, Optional, Sequence, Tuple
|
||||
|
||||
#: 探针类型 —— 决定节点用哪种方式确认"就绪"。
|
||||
#:
|
||||
#: ``service`` 服务在线且类型匹配
|
||||
#: ``action`` action server 在线
|
||||
#: ``topic`` 话题在，且**真的收到过消息**（只看 topic list 会假阳性）
|
||||
#: ``tactile`` 触觉状态话题在发，且 ``connected=true``
|
||||
#: ``camera`` 图像话题在发（收到首帧）
|
||||
#: ``detector`` 算法权重能加载（推理预热）
|
||||
PROBES = ("service", "action", "topic", "tactile", "camera", "detector")
|
||||
|
||||
#: ``detector`` 自检除了"权重文件在"，还要求这些 Python 模块能 import。
|
||||
#: 为什么必须查：权重文件在 ≠ 能推理。实测现场 —— 仿真环境（ros_gz_sim）里
|
||||
#: 没有 torch/ultralytics，S00 的"检测算法预热"照样报 OK，然后 S04 空转 150s
|
||||
#: 才报"没锁定"，看起来像算法或标定问题（真正的错在很远的另一处）。
|
||||
#: ``ultralytics`` 会连带把 torch 拉进来，查它就够；真要细分再看 yolo_detector。
|
||||
DETECTOR_MODULES = ("ultralytics",)
|
||||
|
||||
#: 感知来源（``site.yaml`` 顶层 ``perception.source``）—— 谁产生"视觉测量"。
|
||||
#:
|
||||
#: ``yolo`` 本体 perception_node（要 torch/ultralytics）
|
||||
#: ``truth`` 仿真真值：按场景里已知的物体位姿直接算目标点，不跑推理
|
||||
#: ``external`` 外部节点按同一话题（``visual_servo/filtered_point``）提供
|
||||
#:
|
||||
#: ⚠️ 别和 ``yolo.backend`` 混：那个是"**怎么**推理"（tensorrt/onnx）。
|
||||
PERCEPTION_SOURCES = ("yolo", "truth", "external")
|
||||
|
||||
|
||||
def perception_source_of(perception_cfg: Optional[Mapping] = None) -> str:
|
||||
"""读 ``perception.source`` 并校验。**未知值直接报错，不默默当 yolo**。
|
||||
|
||||
默默退回 yolo 的后果：仿真里 perception_node 起不来，S00 又去查
|
||||
"推理依赖"，报出来的错跟真正的原因（source 拼错）隔着两层。
|
||||
"""
|
||||
src = str(((perception_cfg or {}) or {}).get("source") or "yolo").strip().lower()
|
||||
if src not in PERCEPTION_SOURCES:
|
||||
raise ValueError(
|
||||
f"site.yaml 的 perception.source = {src!r} 不认识。"
|
||||
f"只支持 {' / '.join(PERCEPTION_SOURCES)}"
|
||||
"（yolo=本体推理，truth=仿真真值，external=外部节点提供）。"
|
||||
)
|
||||
return src
|
||||
|
||||
|
||||
def missing_modules(modules: Sequence[str] = DETECTOR_MODULES) -> List[str]:
|
||||
"""哪些模块当前解释器里 import 不到（纯逻辑，可离线测试）。"""
|
||||
import importlib.util
|
||||
|
||||
out: List[str] = []
|
||||
for m in modules:
|
||||
try:
|
||||
if importlib.util.find_spec(m) is None:
|
||||
out.append(m)
|
||||
except (ImportError, ValueError):
|
||||
# find_spec 对"父包不存在/名字非法"会抛，这同样等于"用不了"
|
||||
out.append(m)
|
||||
return out
|
||||
|
||||
|
||||
def dependency_advice(missing: Sequence[str]) -> str:
|
||||
"""缺依赖时给**两条**出路：装它、或换掉这个后端。
|
||||
|
||||
只写"请安装 X"是不够的：仿真环境常常是离线的（这台机器实测 pip 直接
|
||||
SSL 失败），那样的话现场需要的是"换一条不依赖它的路"，而不是反复重试安装。
|
||||
"""
|
||||
mods = ", ".join(missing)
|
||||
return (
|
||||
f"缺推理依赖 {mods} —— 权重文件在也没用，导入不了就跑不了推理。两条路：\n"
|
||||
f" 1) 装上：pip install {' '.join(missing)}（需要联网；本机仿真环境离线时做不到）\n"
|
||||
" 2) 换后端：仿真里可以不用 YOLO —— 由外部按同一话题/同一类型提供感知结果\n"
|
||||
" （见 devices/sim/gazebo/README.md 的「仿真里哪些模块用真值替代」一节）"
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class BootCheck:
|
||||
"""一项自检。"""
|
||||
|
||||
key: str
|
||||
"""稳定标识（日志/结果 JSON 里用它，改名要同步文档）。"""
|
||||
|
||||
label: str
|
||||
"""人读的名字。"""
|
||||
|
||||
probe: str
|
||||
"""见 :data:`PROBES`。"""
|
||||
|
||||
target: str
|
||||
"""要探测的对象：服务名 / action 名 / 话题名 / 权重路径。"""
|
||||
|
||||
why: str
|
||||
"""**缺了会怎样**。自检失败的提示必须写这个 —— 只报"未就绪"没人知道该不该急。"""
|
||||
|
||||
required: bool = True
|
||||
"""必检 / 选检。选检失败只告警，不拦流程。"""
|
||||
|
||||
expect_type: Optional[str] = None
|
||||
"""期望的消息/服务类型（有值时类型不符即算失败）。"""
|
||||
|
||||
|
||||
#: 需要"绝对名字"的探针：相对名字会按节点命名空间解析，
|
||||
#: 节点被放进 namespace 之后 ``home`` 会变成 ``/ns/home`` —— 自检就会去探一个
|
||||
#: 不存在的名字，报"未就绪"。统一补前导斜杠，把这件事变成不可能发生。
|
||||
_ABSOLUTE_PROBES = frozenset({"service", "action", "topic", "tactile", "camera"})
|
||||
|
||||
|
||||
def abs_name(name: str) -> str:
|
||||
"""把 ROS 名字规范成绝对名（``home`` → ``/home``）。
|
||||
|
||||
对外公开：调用方（节点）也要用同一套规范化去比较名字，否则会出现
|
||||
"清单里是 /foo、参数里是 foo，比较不相等"这类只在运行时暴露的错。
|
||||
"""
|
||||
n = (name or "").strip()
|
||||
if not n:
|
||||
return n
|
||||
return n if n.startswith("/") else "/" + n
|
||||
|
||||
|
||||
def _topic(prefix: str, suffix: str) -> str:
|
||||
"""拼话题名，容忍 ``prefix`` 带不带前导斜杠。"""
|
||||
p = "/" + prefix.strip("/") if prefix else ""
|
||||
return f"{p}/{suffix.strip('/')}"
|
||||
|
||||
|
||||
def build_checks(
|
||||
*,
|
||||
joint_action: str,
|
||||
camera: str,
|
||||
tactile_topic: str,
|
||||
gripper_open: str,
|
||||
gripper_close: str,
|
||||
gripper_status: str,
|
||||
gripper_status_type: str,
|
||||
d435_camera: Optional[str] = None,
|
||||
tool_name: str = "",
|
||||
weights_path: str = "",
|
||||
perception_source: str = "yolo",
|
||||
) -> List[BootCheck]:
|
||||
"""按现场情况生成检查清单。
|
||||
|
||||
:param joint_action: JTC action 名（``rizon_arm_controller/follow_joint_trajectory``）。
|
||||
:param camera: 腕部相机命名空间（``d405_left``）。
|
||||
:param tactile_topic: 触觉状态话题（``/tactile/left/status``）。
|
||||
:param gripper_status: 夹爪状态话题，随后端不同（``/gripper/status`` 或 ``/gripper/states``）。
|
||||
:param d435_camera: 手外相机命名空间。给 None 表示这台臂没有手外相机 →
|
||||
不生成 S03 相关的检查项（**不生成**比"生成了一个必然失败的项"更诚实：
|
||||
后者会让每次启动都报一条无关的错，很快所有人都不看自检输出了）。
|
||||
:param tool_name: ``site.yaml`` 的 ``robot.tool_name``。空 = 没指定工具系，
|
||||
则 S01 的"工具系垂直向下锁定"无法确认 → 单独列为选检告警。
|
||||
:param weights_path: 检测权重路径。空 = 不检查推理预热。
|
||||
:param perception_source: ``site.yaml`` 的 ``perception.source``。**只有
|
||||
``yolo`` 才查推理预热**：truth/external 根本不跑推理，查了必然是
|
||||
假阴性（"缺 ultralytics"），而那是预期之内的。这里刻意**不生成**
|
||||
这一项，而不是生成一个永远 OK 的项 —— 后者会让人以为"感知也验过了"。
|
||||
|
||||
清单的顺序 = 打印顺序 = 排障顺序：先臂、再夹爪、再相机、再触觉、最后算法。
|
||||
"""
|
||||
checks: List[BootCheck] = [
|
||||
BootCheck(
|
||||
key="arm_jtc",
|
||||
label="机械臂关节轨迹控制器",
|
||||
probe="action",
|
||||
target=joint_action,
|
||||
why="没有它，S01/S06/S07/S10 的所有运动都发不出去",
|
||||
# 类型必须是 **control_msgs**：本流程（和 stage2/stage3/pipeline 一样）
|
||||
# 直接把轨迹发给 ros2_control 的 JTC 服务端，不经过 arm_control 的
|
||||
# 轨迹适配器（那个用的是 arm_msgs/action/FollowJointTrajectory）。
|
||||
# 写成 arm_msgs 的会得到一个"永远探不到服务端"的自检 —— 假阴性，
|
||||
# 而且看起来像控制器没起来。
|
||||
expect_type="control_msgs/action/FollowJointTrajectory",
|
||||
),
|
||||
BootCheck(
|
||||
key="arm_home",
|
||||
label="回零服务",
|
||||
probe="service",
|
||||
target="/home",
|
||||
why="S01 回 home、S11 复位都靠它",
|
||||
expect_type="std_srvs/srv/Trigger",
|
||||
),
|
||||
BootCheck(
|
||||
key="arm_named_pose",
|
||||
label="命名位姿服务",
|
||||
probe="service",
|
||||
target="/set_named_pose",
|
||||
why="/home 内部转发给它；它不在时回零会超时",
|
||||
expect_type="arm_msgs/srv/SetNamedPose",
|
||||
),
|
||||
BootCheck(
|
||||
key="arm_compliant",
|
||||
label="柔顺下压 action",
|
||||
probe="action",
|
||||
target="/compliant_descent",
|
||||
# 选检：S07 现在走节点内的"步进 + 力突变停"（与这个 action 内部
|
||||
# 同一套做法），不依赖它；有它时驱动侧原语更顺，属于加分项。
|
||||
# 标成必检会让仿真（arm_driver_flexiv 不参与）永远过不了自检。
|
||||
why="有它时 S07 用驱动侧柔顺原语；没有则用节点内步进（不影响正确性）",
|
||||
required=False,
|
||||
expect_type="arm_msgs/action/CompliantDescent",
|
||||
),
|
||||
BootCheck(
|
||||
key="gripper_open",
|
||||
label="夹爪张开服务",
|
||||
probe="service",
|
||||
target=gripper_open,
|
||||
why="S08 棘轮的『松爪』一步靠它；失败会让回转把规拧回去",
|
||||
),
|
||||
BootCheck(
|
||||
key="gripper_close",
|
||||
label="夹爪夹紧服务",
|
||||
probe="service",
|
||||
target=gripper_close,
|
||||
why="S08 的『夹紧』一步靠它；失败会让慢转时规打滑（磨螺纹）",
|
||||
),
|
||||
BootCheck(
|
||||
key="camera_color",
|
||||
label="腕部相机彩图",
|
||||
probe="camera",
|
||||
target=_topic(camera, "color/image_raw"),
|
||||
why="S05 精拍定位 k1/k2 靠它；缺了只能靠 S03 粗定位（精度不够）",
|
||||
expect_type="sensor_msgs/msg/Image",
|
||||
),
|
||||
BootCheck(
|
||||
key="camera_info",
|
||||
label="腕部相机内参",
|
||||
probe="topic",
|
||||
target=_topic(camera, "color/camera_info"),
|
||||
why="没有内参就无法把像素反投影成基座坐标（会退化成 60° 默认视场）",
|
||||
expect_type="sensor_msgs/msg/CameraInfo",
|
||||
),
|
||||
BootCheck(
|
||||
key="tactile_status",
|
||||
label="触觉三向力",
|
||||
probe="tactile",
|
||||
target=tactile_topic,
|
||||
why="S07 切向力突变停、S08 拧紧判据、E1 掉件都依赖它；"
|
||||
"缺了这几个判据全部失效（流程能走完但结果不可信）",
|
||||
expect_type="arm_msgs/msg/TactileStatus",
|
||||
),
|
||||
BootCheck(
|
||||
key="gripper_status",
|
||||
label="夹爪状态反馈",
|
||||
probe="topic",
|
||||
target=gripper_status,
|
||||
why="E1 掉件检测靠它；缺了掉件只能靠人看",
|
||||
expect_type=gripper_status_type,
|
||||
),
|
||||
]
|
||||
|
||||
if d435_camera:
|
||||
checks.append(
|
||||
BootCheck(
|
||||
key="coarse_camera",
|
||||
label="手外粗定位相机",
|
||||
probe="camera",
|
||||
target=_topic(d435_camera, "color/image_raw"),
|
||||
why="S03 兜底粗定位用；缺了只能在 S04 跟踪上多试几次",
|
||||
required=False,
|
||||
expect_type="sensor_msgs/msg/Image",
|
||||
)
|
||||
)
|
||||
if tool_name:
|
||||
checks.append(
|
||||
BootCheck(
|
||||
key="tool_frame",
|
||||
label="工具坐标系已指定",
|
||||
probe="service", # 具体内容由节点按 tool_name 复核（只查服务在不在）
|
||||
target="/switch_tool",
|
||||
why=f"要求 tool1（垂直向下、位姿锁定），site.yaml 里配的是 {tool_name!r}",
|
||||
required=False,
|
||||
expect_type="arm_msgs/srv/SwitchTool",
|
||||
)
|
||||
)
|
||||
# 配了工具系就必须能切：S01 切不过去会直接 TIMEOUT 中止，
|
||||
# 所以在自检里按**必检**拦下（早报比跑到 S01 再报少一次上电动作）。
|
||||
checks.append(
|
||||
BootCheck(
|
||||
key="arm_switch_tool",
|
||||
label="工具系切换服务",
|
||||
probe="service",
|
||||
target="/switch_tool",
|
||||
why=f"site.yaml 指定了工具 {tool_name!r}，切不过去后面全是歪的",
|
||||
expect_type="arm_msgs/srv/SwitchTool",
|
||||
)
|
||||
)
|
||||
else:
|
||||
# 没配工具系：S01 只会告警不会切，所以这条只算选检 ——
|
||||
# 真机没配工具系是**配置问题**（看板要求 tool1），但仿真里根本没有
|
||||
# 工具系切换服务，标必检会让仿真永远过不去。
|
||||
checks.append(
|
||||
BootCheck(
|
||||
key="arm_switch_tool",
|
||||
label="工具系切换服务",
|
||||
probe="service",
|
||||
target="/switch_tool",
|
||||
why="本机没配 robot.tool_name：真机应配 tool1 并确认这个服务在；仿真不需要",
|
||||
required=False,
|
||||
expect_type="arm_msgs/srv/SwitchTool",
|
||||
)
|
||||
)
|
||||
if weights_path and perception_source == "yolo":
|
||||
checks.append(
|
||||
BootCheck(
|
||||
key="detector",
|
||||
label="检测算法预热",
|
||||
probe="detector",
|
||||
target=weights_path,
|
||||
why="S04 跟踪、S03 粗定位都要它；预热失败会在第一次跟踪时才炸",
|
||||
)
|
||||
)
|
||||
|
||||
# 网络端点统一成绝对名（见 _ABSOLUTE_PROBES 的说明）。
|
||||
# detector 的 target 是文件路径，不参与规范化。
|
||||
return [
|
||||
c if c.probe not in _ABSOLUTE_PROBES
|
||||
else BootCheck(
|
||||
key=c.key, label=c.label, probe=c.probe, target=abs_name(c.target),
|
||||
why=c.why, required=c.required, expect_type=c.expect_type,
|
||||
)
|
||||
for c in checks
|
||||
]
|
||||
|
||||
|
||||
def format_report(results: Sequence[Tuple[BootCheck, bool, str]]) -> Tuple[str, List[str]]:
|
||||
"""把逐项结果排成一张表，返回 ``(表文本, 失败项描述)``。
|
||||
|
||||
``results`` 每项是 ``(检查项, 是否就绪, 细节)``。
|
||||
|
||||
两张输出分开是为了让调用方能按"必检失败"决定是否中止：表用来给人看，
|
||||
失败项描述用来进日志/结果 JSON。
|
||||
"""
|
||||
lines: List[str] = ["S00 自检："]
|
||||
failures: List[str] = []
|
||||
for item, ok, detail in results:
|
||||
mark = "OK " if ok else ("缺失" if item.required else "告警")
|
||||
tag = "" if item.required else "（选检）"
|
||||
lines.append(f" [{mark}] {item.label}{tag} {item.target}")
|
||||
if detail:
|
||||
lines.append(f" {detail}")
|
||||
if not ok:
|
||||
prefix = "必检项未就绪" if item.required else "选检项未就绪"
|
||||
failures.append(f"{prefix}：{item.label}（{item.target}）—— {item.why}")
|
||||
if failures:
|
||||
lines.append(f" 共 {len(failures)} 项未就绪")
|
||||
else:
|
||||
lines.append(" 全部就绪，开指令窗")
|
||||
return "\n".join(lines), failures
|
||||
|
||||
|
||||
def blocking_failures(results: Iterable[Tuple[BootCheck, bool, str]]) -> List[str]:
|
||||
"""只挑出**必检且未就绪**的项 —— 决定要不要中止的就是它们。
|
||||
|
||||
和 :func:`format_report` 的 ``failures`` 的区别：那里两项都收（含选检），
|
||||
这里只收必检。合成一个函数的话，调用方一不留神就会拿选检失败去中止流程，
|
||||
于是"D435 没接"变成"装配跑不起来"。
|
||||
"""
|
||||
return [
|
||||
f"必检项未就绪：{item.label}（{item.target}）—— {item.why}"
|
||||
for item, ok, _detail in results
|
||||
if (not ok) and item.required
|
||||
]
|
||||
|
||||
|
||||
def optional_warnings(results: Iterable[Tuple[BootCheck, bool, str]]) -> List[str]:
|
||||
"""选检未就绪的告警（不拦流程，但必须打出来）。"""
|
||||
return [
|
||||
f"选检项未就绪：{item.label}（{item.target}）—— {item.why}"
|
||||
for item, ok, _detail in results
|
||||
if (not ok) and not item.required
|
||||
]
|
||||
|
||||
|
||||
def summary_counts(results: Sequence[Tuple[BootCheck, bool, str]]) -> Dict[str, int]:
|
||||
"""统计``必检/选检``各自通过多少，给启动日志一行摘要。"""
|
||||
out = {"required_ok": 0, "required_total": 0, "optional_ok": 0, "optional_total": 0}
|
||||
for item, ok, _detail in results:
|
||||
slot = "required" if item.required else "optional"
|
||||
out[f"{slot}_total"] += 1
|
||||
if ok:
|
||||
out[f"{slot}_ok"] += 1
|
||||
return out
|
||||
@@ -0,0 +1,591 @@
|
||||
"""通止规装配的配置：从 ``site.yaml`` 的 ``gauge_assembly:`` 段解析 + 校验。
|
||||
|
||||
为什么单独一个模块
|
||||
------------------
|
||||
这一段的键**全是会动的量**（力阈值、角度、悬高、超时），也是现场最常改的地方。
|
||||
散在节点里读的话会有三个已知坑（都被这个仓库踩过）：
|
||||
|
||||
1. **名字与单位不一致** —— ``hover`` 填了毫米、代码按米用（规则 M3）；
|
||||
2. **键没接线** —— YAML 里写着 ``screw_step_deg``，代码读的是 ``screw_steps``，
|
||||
于是改了没效果，而报错也没有（``config_guard`` 模块的头注释就是这件事）；
|
||||
3. **拼错键名** —— YAML 静默接受，行为退回默认值，现场"改了参数没反应"。
|
||||
|
||||
所以：所有键在一张表里声明（含单位），解析时**逐条校验**，多出来的键直接
|
||||
报错并给出最相近的候选名，缺的键按 required/default 明确处理。
|
||||
``describe()`` 把**派生**结果（比如"逆时针最多能转 147°"）打出来，让现场
|
||||
看到的是"实际生效的值"，而不是"我填的值"。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import difflib
|
||||
from dataclasses import dataclass, fields
|
||||
from typing import Any, Dict, Iterable, List, Mapping, Optional, Tuple
|
||||
|
||||
from arm_application.gauge_fsm import descend_tol_m
|
||||
|
||||
#: S05 稳定判据可用的特征来源。``auto`` 按 ``perception.source`` 自动选。
|
||||
#:
|
||||
#: 为什么需要有"motion"这一路：判据原本是**工件检测框中心**的抖动（像素），
|
||||
#: 那要求检测器真的出框。仿真里合成图过不了真机训练出来的模型（实测最高分
|
||||
#: 0.017），于是 S05 会永远"不稳定"地空转 —— 而真正该等的是"相机不再动"。
|
||||
#: 用法兰位移（mm）当特征既直接又不依赖模型，且判据本身（到均值的最远距离）不变。
|
||||
SNAP_STABLE_FEATURES = ("auto", "bbox", "motion")
|
||||
|
||||
def snap_feature_of(want: str, perception_source: str) -> str:
|
||||
"""把 ``snap_stable_feature``（可能是 ``auto``）+ 感知来源 → 实际用哪个特征。
|
||||
|
||||
抽成纯函数是为了能脱离 ROS 节点测：这个选择错了，S05 会在真机上"永远不稳定"
|
||||
或者在仿真里空转到超时，而两种表现都像是"机械臂在晃"，很难往配置上想。
|
||||
"""
|
||||
want = str(want or "auto")
|
||||
if want != "auto":
|
||||
return want
|
||||
return "bbox" if perception_source == "yolo" else "motion"
|
||||
|
||||
|
||||
#: 合法的单位后缀。键名必须以 ``_<单位>`` 结尾（``au`` = Orisys 任意单位增量）。
|
||||
_UNIT_SUFFIX: Dict[str, str] = {
|
||||
"s": "_s",
|
||||
"deg": "_deg",
|
||||
"mm": "_mm",
|
||||
"m": "_m",
|
||||
"px": "_px",
|
||||
"pct": "_pct",
|
||||
"au": "_delta",
|
||||
# 腕部 FT 的**真牛顿**。与 au 分开一个后缀是刻意的：两个口径混用不会报错，
|
||||
# 只会"判据不触发"或"一碰就炸"，看起来像算法问题（见 site.yaml 的注释）。
|
||||
"N": "_n",
|
||||
}
|
||||
|
||||
#: 显式无量纲的键（布尔、计数、比例、枚举）。**必须在这里列出来** ——
|
||||
#: 没有单位又不登记的键，会让"单位自检"变成一句空话（规则 M1）。
|
||||
_UNITLESS = frozenset(
|
||||
{
|
||||
"extreme_direction",
|
||||
"grip_check",
|
||||
"coarse_enabled",
|
||||
"coarse_camera",
|
||||
"coarse_conf",
|
||||
"snap_min_frames",
|
||||
"snap_stable_feature",
|
||||
"screw_max_cycles",
|
||||
"unscrew_extra_cycles",
|
||||
"holes_total",
|
||||
"dry_run",
|
||||
}
|
||||
)
|
||||
|
||||
#: 每个键的**类型**（用于类型校验，不做隐式转换：``"20"`` 不当 20 用）。
|
||||
_TYPES: Dict[str, type] = {
|
||||
"boot_timeout_s": float,
|
||||
"extreme_angle_deg": float,
|
||||
"extreme_speed_deg_s": float,
|
||||
"extreme_direction": int,
|
||||
"extreme_margin_deg": float,
|
||||
"grip_check": bool,
|
||||
"grip_hold_s": float,
|
||||
"coarse_enabled": bool,
|
||||
"coarse_camera": str,
|
||||
"coarse_hover_m": float,
|
||||
"coarse_conf": float,
|
||||
"coarse_timeout_s": float,
|
||||
"snap_stable_s": float,
|
||||
"snap_tol_px": float,
|
||||
"snap_tol_mm": float,
|
||||
"snap_min_frames": int,
|
||||
"snap_stable_feature": str,
|
||||
"align_tol_mm": float,
|
||||
"contact_speed_m_s": float,
|
||||
"contact_step_mm": float,
|
||||
"contact_max_depth_mm": float,
|
||||
"tool_tip_below_flange_m": float,
|
||||
"workpiece_surface_z_m": float,
|
||||
"contact_tangential_delta": float,
|
||||
"contact_axial_delta_n": float,
|
||||
"contact_relief_mm": float,
|
||||
"contact_hold_n": float,
|
||||
"contact_safe_delta": float,
|
||||
"screw_step_deg": float,
|
||||
"screw_speed_deg_s": float,
|
||||
"screw_back_speed_deg_s": float,
|
||||
"screw_max_cycles": int,
|
||||
"screw_normal_delta": float,
|
||||
"screw_shear_delta": float,
|
||||
"screw_grip_force_pct": float,
|
||||
"screw_grip_speed_pct": float,
|
||||
"screw_release_wait_s": float,
|
||||
"unscrew_extra_cycles": int,
|
||||
"holes_total": int,
|
||||
"dry_run": bool,
|
||||
"reload_timeout_s": float,
|
||||
}
|
||||
|
||||
#: 每个键的单位。``""`` 表示无量纲（须同时登记在 :data:`_UNITLESS`）。
|
||||
_UNITS: Dict[str, str] = {
|
||||
"boot_timeout_s": "s",
|
||||
"extreme_angle_deg": "deg",
|
||||
"extreme_speed_deg_s": "deg/s",
|
||||
"extreme_direction": "",
|
||||
"extreme_margin_deg": "deg",
|
||||
"grip_check": "",
|
||||
"grip_hold_s": "s",
|
||||
"coarse_enabled": "",
|
||||
"coarse_camera": "",
|
||||
"coarse_hover_m": "m",
|
||||
"coarse_conf": "",
|
||||
"coarse_timeout_s": "s",
|
||||
"snap_stable_s": "s",
|
||||
"snap_tol_px": "px",
|
||||
"snap_tol_mm": "mm",
|
||||
"snap_min_frames": "",
|
||||
"snap_stable_feature": "",
|
||||
"align_tol_mm": "mm",
|
||||
"contact_speed_m_s": "m/s",
|
||||
"contact_step_mm": "mm",
|
||||
"contact_max_depth_mm": "mm",
|
||||
# 这两个是**长度**不是"毫米量"：用 m 口径，与 hover_z 同一把尺
|
||||
"tool_tip_below_flange_m": "m",
|
||||
"workpiece_surface_z_m": "m",
|
||||
"contact_tangential_delta": "au",
|
||||
# 腕部 FT 是**真牛顿**（与触觉的 au 不是一个口径，所以键名也不带 taut 前缀）
|
||||
"contact_axial_delta_n": "N",
|
||||
# 卸载回退是**长度**（与 step_mm 同一把尺）
|
||||
"contact_relief_mm": "mm",
|
||||
# 残余轴向力是**牛顿**（腕部 FT 口径，不是触觉的 au）
|
||||
"contact_hold_n": "N",
|
||||
"contact_safe_delta": "au",
|
||||
"screw_step_deg": "deg",
|
||||
"screw_speed_deg_s": "deg/s",
|
||||
"screw_back_speed_deg_s": "deg/s",
|
||||
"screw_max_cycles": "",
|
||||
"screw_normal_delta": "au",
|
||||
"screw_shear_delta": "au",
|
||||
"screw_grip_force_pct": "pct",
|
||||
"screw_grip_speed_pct": "pct",
|
||||
"screw_release_wait_s": "s",
|
||||
"unscrew_extra_cycles": "",
|
||||
"holes_total": "",
|
||||
"dry_run": "",
|
||||
"reload_timeout_s": "s",
|
||||
}
|
||||
|
||||
#: 键名后缀与单位必须一致（防"填毫米当米用"）。``contact_speed_m_s`` 这类
|
||||
#: 复合单位只检查主单位段。
|
||||
_SUFFIX_OVERRIDE: Dict[str, str] = {
|
||||
# 复合单位：后缀取"最后一个单位段"，键名里写全
|
||||
"contact_speed_m_s": "_m_s",
|
||||
"extreme_speed_deg_s": "_deg_s",
|
||||
"screw_speed_deg_s": "_deg_s",
|
||||
"screw_back_speed_deg_s": "_deg_s",
|
||||
}
|
||||
|
||||
|
||||
class GaugeConfigError(ValueError):
|
||||
"""``gauge_assembly:`` 段配置错误。消息里必须给出**改哪里**。"""
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 默认值
|
||||
# =============================================================================
|
||||
#: 缺省值。**只有真的有现场依据的才写默认值**：力阈值这类必须现场标定的，
|
||||
#: 默认值只是"能跑起来的起点"，必须在 ``describe()`` 里标出来让现场知道要改。
|
||||
DEFAULTS: Dict[str, Any] = {
|
||||
"boot_timeout_s": 30.0,
|
||||
# S01：逆时针预绕。符号靠 extreme_direction 定，不靠改代码里的负号。
|
||||
"extreme_angle_deg": 150.0,
|
||||
"extreme_speed_deg_s": 20.0,
|
||||
"extreme_direction": 1,
|
||||
"extreme_margin_deg": 3.0,
|
||||
# S02（预留）
|
||||
"grip_check": False,
|
||||
"grip_hold_s": 0.5,
|
||||
# S03
|
||||
"coarse_enabled": True,
|
||||
"coarse_camera": "d435",
|
||||
"coarse_hover_m": 0.25,
|
||||
"coarse_conf": 0.35,
|
||||
"coarse_timeout_s": 20.0,
|
||||
# S05
|
||||
"snap_stable_s": 1.0,
|
||||
"snap_tol_px": 2.0,
|
||||
"snap_tol_mm": 0.4,
|
||||
"snap_min_frames": 6,
|
||||
"snap_stable_feature": "auto",
|
||||
# S06
|
||||
"align_tol_mm": 1.0,
|
||||
# S07
|
||||
"contact_speed_m_s": 0.02,
|
||||
"contact_step_mm": 0.5,
|
||||
"contact_max_depth_mm": 35.0,
|
||||
"tool_tip_below_flange_m": 0.243,
|
||||
"workpiece_surface_z_m": 0.0, # 0 = 未知，运行期用 stage2 实测孔位兜底
|
||||
"contact_tangential_delta": 300.0,
|
||||
# 腕部 FT 的轴向力阈值 [N]。**这是 S07 的首选判据**，触觉只是兜底。
|
||||
#
|
||||
# 为什么默认是 8N 这么小：这是"接触了没有"的判据，不是"压到位了没有"。
|
||||
# 台肩一落面，接触刚度 5e4 N/m 意味着过压 0.1mm 就有 5N、0.2mm 有 10N ——
|
||||
# 所以 8N 对应约 0.16mm 的过压，**在打滑之前**就能判到（打滑上限由
|
||||
# µ·N 决定：µ=0.25、N≈46N → 两指合计 ~23N ≈ 0.46mm）。留了 3 倍余量。
|
||||
# 现场标定法：空载慢慢压到台肩落面，看日志里"腕部轴向"从基线涨到多少，
|
||||
# 取那个值的 1/3 作为阈值（要能区分"擦到孔壁"和"台肩落面"）。
|
||||
"contact_axial_delta_n": 8.0,
|
||||
# 接触到之后的**卸载回退** [mm]。
|
||||
#
|
||||
# 为什么必须有这一步（2026-09-24 一整轮排查换来的）：
|
||||
# 位置控制的臂在"撞上硬面"时会把力顶到自己的**出力上限**才停住 ——
|
||||
# 仿真实测：一步 0.5mm 的指令压在工件上，腕部 FT 读数直接跳到 **+244N**，
|
||||
# 而臂这一步根本没走完（"实测只走了 +0.09mm"）。这不是仿真特有的：
|
||||
# 接触刚度越高、步长越大，峰值力就越大，而位置环**不限制**它。
|
||||
#
|
||||
# 规是**靠夹持摩擦**抱住的，轴向能传的力上限是 µ·N（两指合计 ~73N@µ=0.8）。
|
||||
# 244N 远超这个上限 ⇒ 规被轴向力**顶出爪外**（本次就是：S08 一转，
|
||||
# 规直接掉到地上，报 E1）。所以"判到接触"之后不能就那样顶着，
|
||||
# 必须先把下压量放掉，再交给 S08 拧入。
|
||||
#
|
||||
# 0.5mm 的来历：判到接触时的**真实**过压量通常只有 0.0x mm（刚度 5e4 N/m
|
||||
# 时，10N 对应 0.2mm），回退 0.5mm 足够把力放干净，而规尖仍在孔里
|
||||
# （通端穿板 18mm），不会脱出。
|
||||
"contact_relief_mm": 0.5,
|
||||
# 卸载后允许的残余轴向力 [N]。
|
||||
# 超了不失败（可能只是回退量不够），但要**大声报**：它说明"夹持摩擦力不够
|
||||
# 承受这次接触"，接着 S08 一转就会掉件。调法只有两条 —— 降峰值（调小
|
||||
# contact_step_mm / contact_relief_mm）或加握力（改夹爪合拢力度/指垫）。
|
||||
"contact_hold_n": 5.0,
|
||||
"contact_safe_delta": 2000.0,
|
||||
# S08
|
||||
# 步长必须满足 2×step + 2×余量 ≤ 关节7 行程（Rizon4/10 ±175° ⇒ step ≤ 167°）：
|
||||
# 180° 会必然贴限位（棘轮一个来回要 360° > 350°），实测第 3 轮 ABORT。
|
||||
"screw_step_deg": 120.0,
|
||||
"screw_speed_deg_s": 30.0,
|
||||
"screw_back_speed_deg_s": 120.0,
|
||||
"screw_max_cycles": 12,
|
||||
"screw_normal_delta": 400.0,
|
||||
"screw_shear_delta": 600.0,
|
||||
"screw_grip_force_pct": 50.0,
|
||||
"screw_grip_speed_pct": 50.0,
|
||||
"screw_release_wait_s": 0.3,
|
||||
# S09
|
||||
"unscrew_extra_cycles": 1,
|
||||
# 全局
|
||||
"holes_total": 2,
|
||||
"dry_run": False,
|
||||
# E1（掉件）重放：等操作员放好并确认 /gauge/reload_ok 的最长时间。
|
||||
# 超时 = 中止（臂在 home，人不在旁边时"继续等"比"停"更危险）。
|
||||
"reload_timeout_s": 180.0,
|
||||
}
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GaugeConfig:
|
||||
"""通止规装配的全部可调量（解析并校验之后）。"""
|
||||
|
||||
boot_timeout_s: float
|
||||
extreme_angle_deg: float
|
||||
extreme_speed_deg_s: float
|
||||
extreme_direction: int
|
||||
extreme_margin_deg: float
|
||||
grip_check: bool
|
||||
grip_hold_s: float
|
||||
coarse_enabled: bool
|
||||
coarse_camera: str
|
||||
coarse_hover_m: float
|
||||
coarse_conf: float
|
||||
coarse_timeout_s: float
|
||||
snap_stable_s: float
|
||||
snap_tol_px: float
|
||||
snap_tol_mm: float
|
||||
snap_min_frames: int
|
||||
snap_stable_feature: str
|
||||
align_tol_mm: float
|
||||
contact_speed_m_s: float
|
||||
contact_step_mm: float
|
||||
contact_max_depth_mm: float
|
||||
tool_tip_below_flange_m: float
|
||||
workpiece_surface_z_m: float
|
||||
contact_tangential_delta: float
|
||||
contact_axial_delta_n: float
|
||||
contact_relief_mm: float
|
||||
contact_hold_n: float
|
||||
contact_safe_delta: float
|
||||
screw_step_deg: float
|
||||
screw_speed_deg_s: float
|
||||
screw_back_speed_deg_s: float
|
||||
screw_max_cycles: int
|
||||
screw_normal_delta: float
|
||||
screw_shear_delta: float
|
||||
screw_grip_force_pct: float
|
||||
screw_grip_speed_pct: float
|
||||
screw_release_wait_s: float
|
||||
unscrew_extra_cycles: int
|
||||
holes_total: int
|
||||
dry_run: bool
|
||||
reload_timeout_s: float
|
||||
|
||||
# -- 派生量（不占 YAML 键，免得两处各写一份，规则 M2）-------------------
|
||||
@property
|
||||
def screw_step_rad(self) -> float:
|
||||
import math
|
||||
|
||||
return math.radians(self.screw_step_deg)
|
||||
|
||||
@property
|
||||
def total_screw_deg(self) -> float:
|
||||
"""一根规要拧进去的总角度 = 单次转角 × 循环数。"""
|
||||
return self.screw_step_deg * self.screw_max_cycles
|
||||
|
||||
@property
|
||||
def unscrew_cycles(self) -> int:
|
||||
"""拧出要多转几轮：多留 ``unscrew_extra_cycles`` 轮，确保螺纹完全脱开。"""
|
||||
return self.screw_max_cycles + max(0, self.unscrew_extra_cycles)
|
||||
|
||||
def describe(self) -> List[str]:
|
||||
"""启动时打印的行：**实际生效的**值与派生结论。
|
||||
|
||||
特别是 S01 的"能转多少度"和 S08 的"要拧多少轮"—— 这两个数是现场
|
||||
判断"参数是否真的生效"最直接的依据，比打印"我读了 site.yaml"有用。
|
||||
"""
|
||||
return [
|
||||
f"通止规装配：每孔孔数 {self.holes_total}，干跑={self.dry_run}",
|
||||
f" S01 预绕 {self.extreme_angle_deg:.1f}°（方向={'逆时针' if self.extreme_direction > 0 else '顺时针'}，"
|
||||
f"限位余量 {self.extreme_margin_deg:.1f}°，关节速度 {self.extreme_speed_deg_s:.1f}°/s）",
|
||||
f" S03 粗定位={'开' if self.coarse_enabled else '关'} 相机={self.coarse_camera} "
|
||||
f"悬高 {self.coarse_hover_m * 1000:.0f}mm 置信度≥{self.coarse_conf:.2f}",
|
||||
f" S05 稳定 {self.snap_stable_s:.1f}s / 至少 {self.snap_min_frames} 帧 / "
|
||||
f"特征={self.snap_stable_feature}"
|
||||
f"（检测框 ≤{self.snap_tol_px:.1f}px、法兰位移 ≤{self.snap_tol_mm:.2f}mm）"
|
||||
"（稳定判据用「到均值的最远距离」，不是相邻帧之差）",
|
||||
# 悬停高度**不在这里**：它取自 servo.hole_hover_z_m（stage2 那一层），
|
||||
# 这里只印本段的容差 —— 一段配置里出现"另一个段的键"会让人改错地方。
|
||||
f" S06 对位容差 {self.align_tol_mm:.1f}mm（悬停高取自 servo.hole_hover_z_m）",
|
||||
f" S07 腕部轴向突变阈值 +{self.contact_axial_delta_n:.0f}N（**首选判据**，"
|
||||
"腕部 FT 在夹持摩擦的上游，工具打滑也测得准）,"
|
||||
f"切向突变阈值 +{self.contact_tangential_delta:.0f}au（触觉兜底，两通道任一，现场标定），"
|
||||
f"安全上限 +{self.contact_safe_delta:.0f}，最多下探 {self.contact_max_depth_mm:.0f}mm",
|
||||
f" S07 接触后卸载：回退 {self.contact_relief_mm:.2f}mm，"
|
||||
f"残余轴向力要求 ≤{self.contact_hold_n:.1f}N"
|
||||
"（位置控制的臂撞上硬面会把力顶到出力上限再停住，实测能到 200N+；"
|
||||
"规靠夹持摩擦抱住，轴向上限只有 µ·N≈73N，顶着不放 S08 一转就掉件）",
|
||||
f" S07 工具几何：尖端在法兰下方 {self.tool_tip_below_flange_m * 1000:.1f}mm，"
|
||||
f"工件上表面 z={self.workpiece_surface_z_m:.3f}m"
|
||||
"（启动时会按这两个值 + servo.hole_hover_z_m 算「悬停时工具到底入没入工件」，"
|
||||
"入了会**明确报错**——见 gauge_fsm.tool_clearance_m）",
|
||||
f" S07 步进 {self.contact_step_mm:.2f}mm @ {self.contact_speed_m_s * 1000:.0f}mm/s，"
|
||||
f"到位容差 {descend_tol_m(self.contact_step_mm / 1000.0) * 1000:.3f}mm"
|
||||
"（=步长的 40%，**推导出来的、不是可调项**：容差 ≥ 步长会让每一步都被判成"
|
||||
"「已在容差内」，臂一步都不走）",
|
||||
f" S08 棘轮 {self.screw_step_deg:.0f}°×{self.screw_max_cycles} 轮 = 累计 "
|
||||
f"{self.total_screw_deg:.0f}°；停止判据「切向 >+{self.screw_shear_delta:.0f} 且 "
|
||||
f"法向 >+{self.screw_normal_delta:.0f}」",
|
||||
f" S09 拧出 {self.unscrew_cycles} 轮（比拧入多 {self.unscrew_extra_cycles} 轮）",
|
||||
]
|
||||
|
||||
|
||||
def _resolve_one(key: str, raw: Mapping[str, Any]) -> Tuple[Any, bool]:
|
||||
"""取一个键的值，返回 ``(值, 是否来自 site.yaml)``。"""
|
||||
if key in raw:
|
||||
value = raw[key]
|
||||
elif key in DEFAULTS:
|
||||
return DEFAULTS[key], False
|
||||
else:
|
||||
raise GaugeConfigError(
|
||||
f"gauge_assembly.{key} 既没在 site.yaml 里给，也没有默认值。"
|
||||
f"请在 config/site.yaml 的 gauge_assembly: 段补上"
|
||||
f"（改完跑一次 bash scripts/show_site_config.sh 确认解析）"
|
||||
)
|
||||
|
||||
want = _TYPES[key]
|
||||
if want is bool and not isinstance(value, bool):
|
||||
raise GaugeConfigError(_type_msg(key, value, "布尔 true/false"))
|
||||
if want is int and (isinstance(value, bool) or not isinstance(value, int)):
|
||||
raise GaugeConfigError(_type_msg(key, value, "整数"))
|
||||
if want is float and (isinstance(value, bool) or not isinstance(value, (int, float))):
|
||||
raise GaugeConfigError(_type_msg(key, value, "数字（可带小数）"))
|
||||
if want is str and not isinstance(value, str):
|
||||
raise GaugeConfigError(_type_msg(key, value, "字符串"))
|
||||
return (float(value) if want is float else value), True
|
||||
|
||||
|
||||
def _type_msg(key: str, value: Any, human: str) -> str:
|
||||
unit = _UNITS[key]
|
||||
hint = f"，单位 {unit}" if unit else ""
|
||||
return (
|
||||
f"gauge_assembly.{key} 的类型不对：期望 {human}{hint}，"
|
||||
f"实际读到 {value!r}（{type(value).__name__}）。"
|
||||
"本模块不做隐式转换 —— \"20\" 不会被当成 20 用，"
|
||||
"因为那正是「填了字符串、跑起来是默认值」这类静默失效的入口"
|
||||
)
|
||||
|
||||
|
||||
def from_site(site: Mapping[str, Any], *, overrides: Optional[Mapping[str, Any]] = None) -> GaugeConfig:
|
||||
"""解析并校验 ``site.yaml`` 的 ``gauge_assembly:`` 段。
|
||||
|
||||
:param site: ``site.yaml`` 解析后的整份字典。
|
||||
:param overrides: CLI/launch 覆盖（只允许覆盖已知键，未知键报错）。
|
||||
:raises GaugeConfigError: 缺键、类型错、未知键、单位与键名不符。
|
||||
|
||||
整段缺失**不报错**（全部走默认值），但会在 :meth:`GaugeConfig.describe`
|
||||
之外由调用方打一条告警；理由是"默认值能不能用"取决于现场，
|
||||
由节点决定要不要拒绝启动，配置模块只负责如实解析。
|
||||
"""
|
||||
raw_any = site.get("gauge_assembly") or {}
|
||||
if not isinstance(raw_any, Mapping):
|
||||
raise GaugeConfigError(
|
||||
"site.yaml 的 gauge_assembly: 应该是一段映射（键值对），"
|
||||
f"实际是 {type(raw_any).__name__}"
|
||||
)
|
||||
raw: Dict[str, Any] = dict(raw_any)
|
||||
|
||||
if overrides:
|
||||
bad = [k for k in overrides if k not in _TYPES]
|
||||
if bad:
|
||||
raise GaugeConfigError(
|
||||
f"未知的覆盖参数: {', '.join(sorted(bad))}。"
|
||||
f"可在 gauge_assembly 段覆盖的键见 gauge_config._TYPES"
|
||||
)
|
||||
raw.update({k: v for k, v in overrides.items() if v is not None})
|
||||
|
||||
_reject_unknown(raw)
|
||||
values = {key: _resolve_one(key, raw)[0] for key in _TYPES}
|
||||
cfg = GaugeConfig(**values)
|
||||
_sanity(cfg)
|
||||
return cfg
|
||||
|
||||
|
||||
def _reject_unknown(raw: Mapping[str, Any]) -> None:
|
||||
"""多余的键直接报错，并给出最相近的候选名。
|
||||
|
||||
静默忽略多余键的代价是：现场写 ``screw_step_degs``，改参数没反应，
|
||||
而日志里一切正常。报错里带候选名，是因为拼错比写错新键常见得多。
|
||||
"""
|
||||
unknown = sorted(set(raw) - set(_TYPES))
|
||||
if not unknown:
|
||||
return
|
||||
lines: List[str] = []
|
||||
for key in unknown:
|
||||
close = difflib.get_close_matches(key, list(_TYPES), n=1, cutoff=0.6)
|
||||
hint = f"，是不是想写 {close[0]}？" if close else ""
|
||||
lines.append(f" - {key}{hint}")
|
||||
raise GaugeConfigError(
|
||||
"site.yaml 的 gauge_assembly: 段里有本框架不认识的键：\n"
|
||||
+ "\n".join(lines)
|
||||
+ "\n多余的键不会被忽略，因为「写了没生效」比「写错报错」难查得多。"
|
||||
"确认要新增配置项时，先在 gauge_config._TYPES / _UNITS 里登记"
|
||||
)
|
||||
|
||||
|
||||
def _sanity(cfg: GaugeConfig) -> None:
|
||||
"""跨键的合理性检查：单键合法、组合起来没意义的，在这里挡住。"""
|
||||
if cfg.holes_total < 1:
|
||||
raise GaugeConfigError(f"gauge_assembly.holes_total 至少为 1，收到 {cfg.holes_total}")
|
||||
if cfg.extreme_direction not in (-1, 1):
|
||||
raise GaugeConfigError(
|
||||
f"gauge_assembly.extreme_direction 只能是 +1（逆时针）或 -1（顺时针），"
|
||||
f"收到 {cfg.extreme_direction}"
|
||||
)
|
||||
if cfg.screw_step_deg <= 0.0:
|
||||
raise GaugeConfigError(
|
||||
f"gauge_assembly.screw_step_deg 必须为正，收到 {cfg.screw_step_deg}"
|
||||
)
|
||||
if cfg.screw_max_cycles < 1:
|
||||
raise GaugeConfigError(
|
||||
f"gauge_assembly.screw_max_cycles 至少为 1，收到 {cfg.screw_max_cycles}"
|
||||
)
|
||||
if cfg.extreme_angle_deg <= 0.0:
|
||||
raise GaugeConfigError(
|
||||
f"gauge_assembly.extreme_angle_deg 必须为正（方向由 extreme_direction 决定，"
|
||||
f"不要用负角度表方向），收到 {cfg.extreme_angle_deg}"
|
||||
)
|
||||
if cfg.contact_safe_delta <= cfg.contact_tangential_delta:
|
||||
raise GaugeConfigError(
|
||||
f"contact_safe_delta({cfg.contact_safe_delta}) 必须大于 "
|
||||
f"contact_tangential_delta({cfg.contact_tangential_delta})："
|
||||
"前者是「立刻抬臂」的安全上限，后者只是「停止下压」。"
|
||||
"两者相等的话，接触一开始就会触发安全上限"
|
||||
)
|
||||
if cfg.tool_tip_below_flange_m <= 0.0:
|
||||
raise GaugeConfigError(
|
||||
f"tool_tip_below_flange_m 必须为正（工具尖端在法兰**下方**多少米），"
|
||||
f"收到 {cfg.tool_tip_below_flange_m}。"
|
||||
"填 0 会让「悬停时工具会不会已经插进工件」这个判断失效"
|
||||
)
|
||||
if cfg.snap_tol_px <= 0.0:
|
||||
raise GaugeConfigError(f"gauge_assembly.snap_tol_px 必须为正，收到 {cfg.snap_tol_px}")
|
||||
if cfg.contact_relief_mm < 0.0:
|
||||
raise GaugeConfigError(
|
||||
f"gauge_assembly.contact_relief_mm 不能为负（0 = 接触到之后不卸载），"
|
||||
f"收到 {cfg.contact_relief_mm}。\n"
|
||||
" 它为什么值得一条检查：位置控制的臂撞上硬面会把轴向下压顶到**出力上限**"
|
||||
"才停住（实测 200N+），而规靠夹持摩擦抱住、轴向上限只有 µ·N（≈73N）。"
|
||||
"卸载这一步省掉，S08 一转就掉件（报 E1）—— 而那时的日志只会说"
|
||||
"「通止规掉了」，看不到真因。"
|
||||
)
|
||||
if cfg.contact_hold_n <= 0.0:
|
||||
raise GaugeConfigError(
|
||||
f"gauge_assembly.contact_hold_n 必须为正（卸载后的残余轴向力上限 [N]），"
|
||||
f"收到 {cfg.contact_hold_n}。0 会让「卸载干不干净」永远判不过。"
|
||||
)
|
||||
if cfg.snap_tol_mm <= 0.0:
|
||||
raise GaugeConfigError(f"gauge_assembly.snap_tol_mm 必须为正，收到 {cfg.snap_tol_mm}")
|
||||
if cfg.snap_stable_feature not in SNAP_STABLE_FEATURES:
|
||||
raise GaugeConfigError(
|
||||
f"gauge_assembly.snap_stable_feature={cfg.snap_stable_feature!r} 不认识，"
|
||||
f"只支持 {sorted(SNAP_STABLE_FEATURES)}。\n"
|
||||
f" auto = 按 perception.source 自动选（真机 yolo→bbox、仿真/外部→motion）\n"
|
||||
f" bbox = 用工件检测框中心（像素），需要检测器真的出框\n"
|
||||
f" motion = 用法兰位移（mm），不依赖检测器"
|
||||
)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 自检：键名必须体现单位（防"填了毫米按米用"）
|
||||
# =============================================================================
|
||||
def unit_violations(keys: Optional[Iterable[str]] = None) -> List[str]:
|
||||
"""返回"键名与单位不符"的清单（空 = 全部合规）。
|
||||
|
||||
规则：单位非空的键，**键名必须以对应的单位后缀结尾**（见 :data:`_UNIT_SUFFIX`）。
|
||||
例外只有 :data:`_SUFFIX_OVERRIDE` 里显式登记的复合单位。
|
||||
|
||||
为什么值得一条专门的检查：``hover`` 填毫米、代码按米用，会给出一个
|
||||
"看起来完全合理"的运动（悬停 5 变成悬停 5 米或 5 毫米），
|
||||
而 :mod:`arm_interfaces.config_guard` 的头注释记录的正是这类事故。
|
||||
"""
|
||||
bad: List[str] = []
|
||||
for key in keys if keys is not None else _UNITS:
|
||||
unit = _UNITS[key]
|
||||
if not unit:
|
||||
if key not in _UNITLESS:
|
||||
bad.append(f"{key}: 无量纲但未登记在 _UNITLESS")
|
||||
continue
|
||||
suffix = _SUFFIX_OVERRIDE.get(key)
|
||||
if suffix is None:
|
||||
base = unit.split("/")[0]
|
||||
suffix = _UNIT_SUFFIX.get(base)
|
||||
if suffix is None:
|
||||
bad.append(f"{key}: 单位 {unit} 没有登记后缀（补 _UNIT_SUFFIX 或 _SUFFIX_OVERRIDE）")
|
||||
continue
|
||||
if not key.endswith(suffix):
|
||||
bad.append(f"{key}: 单位是 {unit}，键名应以 {suffix} 结尾")
|
||||
return bad
|
||||
|
||||
|
||||
def missing_entries() -> List[str]:
|
||||
"""三张表（类型/单位/默认值）是否互相覆盖完整。"""
|
||||
problems: List[str] = []
|
||||
for key in _TYPES:
|
||||
if key not in _UNITS:
|
||||
problems.append(f"{key}: 缺 _UNITS 登记")
|
||||
if key not in DEFAULTS:
|
||||
problems.append(f"{key}: 缺 DEFAULTS（required 的键也应写一个起点值并在文档里标注必标定）")
|
||||
for key in _UNITS:
|
||||
if key not in _TYPES:
|
||||
problems.append(f"{key}: 在 _UNITS 里但不在 _TYPES 里")
|
||||
for key in DEFAULTS:
|
||||
if key not in _TYPES:
|
||||
problems.append(f"{key}: 在 DEFAULTS 里但不在 _TYPES 里")
|
||||
return problems
|
||||
|
||||
|
||||
def dataclass_keys() -> List[str]:
|
||||
""":class:`GaugeConfig` 的字段名 —— 与配置表比对，防"解析了但没存进对象"。"""
|
||||
return [f.name for f in fields(GaugeConfig)]
|
||||
File diff suppressed because it is too large
Load Diff
+228
@@ -0,0 +1,228 @@
|
||||
"""夹爪客户端：把"真机今天的接口"和"框架契约里的接口"收敛到一个出入口。
|
||||
|
||||
为什么要这一层（这不是多余抽象）
|
||||
--------------------------------
|
||||
现场查证（2026-09-24）发现**同一个服务名后面挂着两种类型**：
|
||||
|
||||
=============== ========================== ================================
|
||||
服务 A：真机今天跑的 B：框架契约 / 仿真
|
||||
=============== ========================== ================================
|
||||
``/gripper/open`` ``std_srvs/Trigger`` ``arm_msgs/srv/GripperExecute``
|
||||
``/gripper/close`` ``std_srvs/Trigger`` ``arm_msgs/srv/GripperExecute``
|
||||
状态 ``/gripper/states`` ``/gripper/status``
|
||||
（Float32MultiArray，5 元） （arm_msgs/GripperStatus）
|
||||
=============== ========================== ================================
|
||||
|
||||
A 来自 ``arm_application/dh_pgea_node.py``（真机在跑的那个节点，grip_state 直接
|
||||
带"咬住/掉落"）；B 来自 ``devices/grippers/dh_pgea/manifest.yaml`` 声明的
|
||||
``arm_driver_dh_gripper`` —— **那个包还不存在**（``src/`` 下没有），目前只有
|
||||
仿真适配器实现了它。
|
||||
|
||||
于是有两个错法，都得避免：
|
||||
|
||||
* 只按契约（B）写：真机上服务类型对不上，装配流程一步都走不了，
|
||||
而且报错会像是"服务没起"（实际起了，只是类型不同）；
|
||||
* 只按真机（A）写：仿真跑不了，而且把"临时接口"固化成事实（规则 M2）。
|
||||
|
||||
所以：**运行时看服务类型自己选**（:func:`pick_backend`），装配节点只依赖
|
||||
:class:`GripperClient` 的 ``open/close/wait/is_gripped`` 四个方法。
|
||||
等 ``arm_driver_dh_gripper`` 落地后，把 ``gauge_assembly.gripper_backend``
|
||||
改回 ``auto`` 即可自动切过去，应用层不用改一行。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Dict, List, Mapping, Optional, Sequence
|
||||
|
||||
#: 契约后端用到的服务名（``arm_msgs/srv/GripperExecute``）。
|
||||
CONTRACT_TYPE = "arm_msgs/srv/GripperExecute"
|
||||
#: 真机今天用的后端（``std_srvs/srv/Trigger``）。
|
||||
LEGACY_TYPE = "std_srvs/srv/Trigger"
|
||||
|
||||
CONTRACT_OPEN = "/gripper/open"
|
||||
CONTRACT_CLOSE = "/gripper/close"
|
||||
CONTRACT_STATUS_TOPIC = "/gripper/status"
|
||||
LEGACY_STATUS_TOPIC = "/gripper/states"
|
||||
|
||||
#: DH 状态字（与 ``arm_msgs/GripperStatus.msg`` 里的注释一致）。
|
||||
GRIP_STATE_MOVING = 0
|
||||
GRIP_STATE_REACHED = 1
|
||||
GRIP_STATE_CAUGHT = 2
|
||||
GRIP_STATE_DROPPED = 3
|
||||
GRIP_STATE_NAME = {
|
||||
GRIP_STATE_MOVING: "运动中",
|
||||
GRIP_STATE_REACHED: "到位",
|
||||
GRIP_STATE_CAUGHT: "咬住物体",
|
||||
GRIP_STATE_DROPPED: "物体掉落",
|
||||
}
|
||||
|
||||
BACKENDS = ("contract", "legacy")
|
||||
|
||||
|
||||
class GripperInterfaceError(RuntimeError):
|
||||
"""夹爪接口无法确定。消息里必须写清"现在是什么状态、怎么修"。"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class BackendChoice:
|
||||
"""选择结果。"""
|
||||
|
||||
backend: str
|
||||
"""``contract`` 或 ``legacy``。"""
|
||||
|
||||
reason: str
|
||||
"""为什么这么选 —— 要能直接打进启动日志。"""
|
||||
|
||||
status_topic: str
|
||||
"""该后端下状态从哪个话题读。"""
|
||||
|
||||
|
||||
def pick_backend(
|
||||
services: Mapping[str, Sequence[str]], prefer: str = "auto"
|
||||
) -> BackendChoice:
|
||||
"""按**实际服务类型**决定用哪种夹爪接口。
|
||||
|
||||
:param services: ``{服务名: [类型, ...]}``，直接来自
|
||||
``Node.get_service_names_and_types()``。
|
||||
:param prefer: ``auto`` / ``contract`` / ``legacy``。
|
||||
非 auto 时若实际类型与之不符，**不静默降级**，而是报错 ——
|
||||
现场明确指定了后端却跑在另一个上，是最难查的一类"改了没生效"。
|
||||
:raises GripperInterfaceError: 认不出来、两种都有、或指定的后端对不上。
|
||||
|
||||
用类型而不是名字判断，是因为两种后端的**服务名完全一样**（都是
|
||||
``/gripper/open``）：只看名字必然选不出来，而"随便选一个"的后果是
|
||||
调用一个类型不匹配的服务，报出来的错与真实原因无关（规则 M1）。
|
||||
"""
|
||||
if prefer not in ("auto",) + BACKENDS:
|
||||
raise GripperInterfaceError(
|
||||
f"gripper_backend 只能是 auto / {' / '.join(BACKENDS)}，收到 {prefer!r}"
|
||||
)
|
||||
|
||||
open_types = list(services.get(CONTRACT_OPEN, ()))
|
||||
close_types = list(services.get(CONTRACT_CLOSE, ()))
|
||||
has_contract = CONTRACT_TYPE in open_types and CONTRACT_TYPE in close_types
|
||||
has_legacy = LEGACY_TYPE in open_types and LEGACY_TYPE in close_types
|
||||
|
||||
if prefer == "contract" and not has_contract:
|
||||
raise GripperInterfaceError(
|
||||
f"配置要求 contract 后端，但 {CONTRACT_OPEN} 的类型是 "
|
||||
f"{open_types or '（服务不存在）'}，不是 {CONTRACT_TYPE}。\n"
|
||||
" 怎么修：① 起仿真（它有契约服务）或等 arm_driver_dh_gripper 落地；"
|
||||
"② 改用 gauge_assembly.gripper_backend: legacy（真机今天的接口）；"
|
||||
"③ 改回 auto 让本函数自己选"
|
||||
)
|
||||
if prefer == "legacy" and not has_legacy:
|
||||
raise GripperInterfaceError(
|
||||
f"配置要求 legacy 后端，但 {CONTRACT_OPEN} 的类型是 "
|
||||
f"{open_types or '（服务不存在）'}，不是 {LEGACY_TYPE}。\n"
|
||||
" 怎么修：确认真机侧起的是 arm_application 的 dh_pgea 节点"
|
||||
"（bash scripts/run_gripper.sh --arm 89）"
|
||||
)
|
||||
|
||||
if has_contract and has_legacy:
|
||||
# 同名服务两种类型同时在线：ROS 2 允许，但调用方只能撞运气。
|
||||
raise GripperInterfaceError(
|
||||
f"{CONTRACT_OPEN} 同时存在两种类型：{open_types}。"
|
||||
"是仿真适配器和真机节点同时起着（或旧的没停干净）。\n"
|
||||
" 怎么修：只留一个 —— 仿真用 scripts/stop_sim_gz.sh 停干净，"
|
||||
"真机 pkill -f dh_pgea"
|
||||
)
|
||||
|
||||
if prefer == "auto":
|
||||
if has_contract:
|
||||
return BackendChoice("contract", f"{CONTRACT_OPEN} 是 {CONTRACT_TYPE}", CONTRACT_STATUS_TOPIC)
|
||||
if has_legacy:
|
||||
return BackendChoice(
|
||||
"legacy",
|
||||
f"{CONTRACT_OPEN} 是 {LEGACY_TYPE}（真机 dh_pgea 节点；"
|
||||
"契约驱动 arm_driver_dh_gripper 还没落地）",
|
||||
LEGACY_STATUS_TOPIC,
|
||||
)
|
||||
raise GripperInterfaceError(_unrecognised_msg(services, open_types))
|
||||
|
||||
if prefer == "contract":
|
||||
return BackendChoice("contract", "配置指定 contract", CONTRACT_STATUS_TOPIC)
|
||||
return BackendChoice("legacy", "配置指定 legacy", LEGACY_STATUS_TOPIC)
|
||||
|
||||
|
||||
def _unrecognised_msg(
|
||||
services: Mapping[str, Sequence[str]], open_types: Sequence[str]
|
||||
) -> str:
|
||||
"""认不出来时，把"服务不在"和"服务在但类型不对"分开说。
|
||||
|
||||
两者都报成"服务没起"是这个仓库反复踩过的坑：现场明明起了夹爪节点，
|
||||
却被提示"去起夹爪节点"，然后开始怀疑人生（规则 M1、R5.4）。
|
||||
"""
|
||||
missing = [n for n in (CONTRACT_OPEN, CONTRACT_CLOSE) if n not in services]
|
||||
if missing:
|
||||
return (
|
||||
f"{' / '.join(missing)} 不在，夹爪节点没起来。\n"
|
||||
" 怎么修：真机 bash scripts/run_gripper.sh --arm 89；"
|
||||
"仿真 bash scripts/run_sim_gz.sh（仿真自带契约服务）"
|
||||
)
|
||||
return (
|
||||
f"{CONTRACT_OPEN} 在，但类型是 {list(open_types)}，"
|
||||
f"既不认识 {CONTRACT_TYPE}（契约）也不认识 {LEGACY_TYPE}（真机 dh_pgea）。\n"
|
||||
" 常见原因：这个服务名被别的节点占用了。查是谁：\n"
|
||||
" ros2 service list -t | grep gripper\n"
|
||||
" 怎么修：停掉那个节点，或改 gauge_assembly.gripper_backend 明确指定后端"
|
||||
)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 状态解读（纯函数）
|
||||
# =============================================================================
|
||||
def is_gripped_from_state(grip_state: int) -> Optional[bool]:
|
||||
"""DH 状态字 → "爪里有没有东西"。
|
||||
|
||||
``2`` 咬住 / ``3`` 掉落 是明确的；``0`` 运动中、``1`` 到位（空爪）都要
|
||||
小心：**"到位"不等于"夹住了"** —— 空爪闭合也会到位。所以只有 2/3 给
|
||||
明确答案，其余返回 None（"不知道"），由调用方决定要不要等。
|
||||
|
||||
返回 None 而不是 False 很重要：把"不知道"当成"没夹住"会让 E1 掉件检测
|
||||
在任何一次开合过程中误报。
|
||||
"""
|
||||
if grip_state == GRIP_STATE_CAUGHT:
|
||||
return True
|
||||
if grip_state == GRIP_STATE_DROPPED:
|
||||
return False
|
||||
return None
|
||||
|
||||
|
||||
def is_dropped_from_state(grip_state: int, was_gripped: bool) -> bool:
|
||||
"""E1 掉件判据：**曾经夹住过**，现在状态字变成"掉落"才是掉件。
|
||||
|
||||
为什么要 ``was_gripped``：装配前爪里本来就是空的，那时 ``grip_state``
|
||||
可能是 3（掉落/无物），直接判成掉件会让流程一开始就报 E1。
|
||||
"""
|
||||
return was_gripped and grip_state == GRIP_STATE_DROPPED
|
||||
|
||||
|
||||
def legacy_states_to_fields(data: Sequence[float]) -> Dict[str, float]:
|
||||
"""``/gripper/states`` 的 5 元素数组 → 具名字段。
|
||||
|
||||
超出长度或为空时返回空字典（而不是抛异常）：这是**状态话题**，
|
||||
收到畸形数据时应该让上层走"不知道"分支，而不是让整个节点崩掉。
|
||||
"""
|
||||
if not data or len(data) < 5:
|
||||
return {}
|
||||
return {
|
||||
"initialized": float(data[0]),
|
||||
"grip_state": float(data[1]),
|
||||
"position": float(data[2]),
|
||||
"target_position": float(data[3]),
|
||||
"target_force": float(data[4]),
|
||||
}
|
||||
|
||||
|
||||
def describe_backend_choice(choice: BackendChoice) -> List[str]:
|
||||
"""给启动日志用的多行说明（含"契约驱动还没落地"这类现场事实）。"""
|
||||
lines = [f"夹爪后端 = {choice.backend}（{choice.reason}）"]
|
||||
if choice.backend == "legacy":
|
||||
lines.append(
|
||||
" 注意：legacy 后端下 /gripper/open|close 是 std_srvs/Trigger，"
|
||||
"开口是 1000‰ 的百分比语义，不是毫米；装配流程只用它开合，"
|
||||
"不靠它量开口"
|
||||
)
|
||||
return lines
|
||||
@@ -0,0 +1,244 @@
|
||||
"""通止规装配用的小几何/解析工具 —— **纯函数**，可离线单测。
|
||||
|
||||
放这里的都是"算错了不会抛异常、只会让动作做错"的东西（规则 M3、R5.1）：
|
||||
|
||||
* ``parse_joint_limits``：从 URDF 文本里抠关节限位，给 S01 的"转到极限角"
|
||||
做软夹取。为什么不用 ``urdf_parser_py``：本机没装，多一个依赖就多一个
|
||||
"在真机上 import 失败"的机会；限位信息在 URDF 里是极规整的一行
|
||||
``<limit lower=... upper=...>``，用标准库解析足够，而且**可控**。
|
||||
* ``prewind``：算"逆时针转到不触发关节极限的极限角"到底能转多少度。
|
||||
* ``rotation_about_axis_deg``：事后验证那个动作**真的**是绕工具 Z 转的。
|
||||
这条是 S01 的验收判据：不验证的话，"绕错轴"会表现成"转是转了，孔位
|
||||
偏了"，而且要到几个阶段之后才看出来。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import re
|
||||
import xml.etree.ElementTree as ET
|
||||
from dataclasses import dataclass
|
||||
from typing import Dict, Iterable, Optional, Sequence, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
#: 限位夹取时留的余量（度）。贴到限位上再下发，驱动器会因为"位置超出限位"
|
||||
#: 直接拒绝整条轨迹 —— 那不是我们要的"极限角"，是报错。
|
||||
DEFAULT_LIMIT_MARGIN_DEG = 3.0
|
||||
|
||||
_JOINT_RE = re.compile(r"<joint\s+name\s*=\s*\"([^\"]+)\"")
|
||||
_ATTR_RE = re.compile(r"(lower|upper)\s*=\s*\"([-+0-9.eE]+)\"")
|
||||
|
||||
|
||||
def _limits_by_lines(text: str) -> Dict[str, Tuple[float, float]]:
|
||||
"""逐行兜底解析：记住最近一个 ``<joint name=...>``，紧跟的 ``<limit>`` 归它。
|
||||
|
||||
为什么要"记住"而不是"要求同一行"：正常格式化的 URDF 里关节名和 limit
|
||||
**不在同一行**，只认可同一行的兜底等于永远不生效（一个不会触发的兜底，
|
||||
和没有兜底是一样的）。属性顺序也按出现顺序读，不假设 lower 在 upper 前。
|
||||
"""
|
||||
out: Dict[str, Tuple[float, float]] = {}
|
||||
current: Optional[str] = None
|
||||
for line in text.splitlines():
|
||||
m = _JOINT_RE.search(line)
|
||||
if m:
|
||||
current = m.group(1)
|
||||
if current is None or "<limit" not in line:
|
||||
continue
|
||||
attrs = dict(_ATTR_RE.findall(line))
|
||||
if "lower" in attrs and "upper" in attrs:
|
||||
out[current] = (float(attrs["lower"]), float(attrs["upper"]))
|
||||
current = None
|
||||
return out
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class PrewindResult:
|
||||
"""S01 预绕量的计算结果。"""
|
||||
|
||||
target_rad: float
|
||||
"""目标关节角 [rad]。"""
|
||||
|
||||
applied_deg: float
|
||||
"""实际能转的角度（带符号，符合"逆时针为正"的约定）[deg]。"""
|
||||
|
||||
want_deg: float
|
||||
"""希望转的角度 [deg]。"""
|
||||
|
||||
clamped: bool
|
||||
"""是否因为关节限位被夹取过。被夹取时 ``applied_deg < want_deg``。"""
|
||||
|
||||
has_limits: bool
|
||||
"""是否拿到了可信的关节限位。False 表示没有任何夹取依据。"""
|
||||
|
||||
|
||||
def parse_joint_limits(urdf_xml: str) -> Dict[str, Tuple[float, float]]:
|
||||
"""从 URDF 文本里解析 ``关节名 -> (lower, upper)``（弧度）。
|
||||
|
||||
关节名**原样保留**（含 ``Rizon4-063989_`` 前缀），调用方自己决定怎么匹配；
|
||||
这里不做"去前缀"之类的猜测 —— 猜测会在换臂时静默失配，然后限位夹取
|
||||
悄悄失效（所有关节都"查不到限位"，于是不夹取，于是贴着限位下发）。
|
||||
|
||||
解析失败的关节直接不进结果集：``continuous`` 关节本来就没有限位，
|
||||
不该被当成"解析失败"。调用方用"查不到 = 不夹取"来处理。
|
||||
"""
|
||||
out: Dict[str, Tuple[float, float]] = {}
|
||||
if not urdf_xml or "<robot" not in urdf_xml:
|
||||
return out
|
||||
try:
|
||||
root = ET.fromstring(urdf_xml)
|
||||
except ET.ParseError:
|
||||
# URDF 里出现未展开的 xacro 或未转义的 ``&`` 是常见的（拿错话题就是
|
||||
# 这种内容）。降级成逐行扫描仍能救回限位；救不回就返回空表（= 不夹取），
|
||||
# 由调用方告警 —— **不会**返回半套数据。
|
||||
return _limits_by_lines(urdf_xml)
|
||||
|
||||
for joint in root.findall(".//joint"):
|
||||
name = joint.get("name")
|
||||
limit = joint.find("limit")
|
||||
if not name or limit is None:
|
||||
continue
|
||||
lo, up = limit.get("lower"), limit.get("upper")
|
||||
if lo is None or up is None:
|
||||
continue
|
||||
try:
|
||||
out[name] = (float(lo), float(up))
|
||||
except ValueError:
|
||||
continue
|
||||
return out
|
||||
|
||||
|
||||
def match_limits(
|
||||
limits: Dict[str, Tuple[float, float]], joint_names: Sequence[str]
|
||||
) -> Optional[Tuple[float, float]]:
|
||||
"""按名字给一串关节名找限位，**要求全部找到**才返回。
|
||||
|
||||
只有一部分找到时返回 None 而不是"把找到的那部分用上"：半套限位会让
|
||||
"能不能转到极限角"的结论变得不可解释（有些关节夹了、有些没夹），
|
||||
与其给出一个说不清的边界，不如明确地说"这次没有限位依据"。
|
||||
"""
|
||||
found = [limits.get(n) for n in joint_names]
|
||||
if any(v is None for v in found):
|
||||
return None
|
||||
return found[-1] # 最后一个 = 我们要绕的那根轴（调用方传的顺序有意义）
|
||||
|
||||
|
||||
def prewind(
|
||||
q_now_rad: float,
|
||||
want_deg: float,
|
||||
direction: int,
|
||||
limits: Optional[Tuple[float, float]] = None,
|
||||
margin_deg: float = DEFAULT_LIMIT_MARGIN_DEG,
|
||||
) -> PrewindResult:
|
||||
"""算 S01 的预绕目标角。
|
||||
|
||||
:param q_now_rad: 当前关节角 [rad]。
|
||||
:param want_deg: 想转的角度（**正值**）[deg]。
|
||||
:param direction: ``+1`` = 绕 +Z 逆时针（右手定则）；``-1`` = 顺时针。
|
||||
符号到底对应现场的哪个方向，用真机确认一次后改 ``site.yaml`` 即可，
|
||||
不要靠"改代码里的负号"—— 那种改动没人 review 得到（规则 M2）。
|
||||
:param limits: ``(lower, upper)`` [rad]。给 None 时不做夹取。
|
||||
:param margin_deg: 距离限位保留的余量 [deg]。
|
||||
|
||||
:returns: :class:`PrewindResult`。**无论有没有限位，都返回目标角**，
|
||||
因为"贴到限位再继续转"在现场是预期行为（用户要求转到极限角），
|
||||
驱动器真拒绝时会由 :func:`arm_application.gauge_fsm.is_joint_limit_error`
|
||||
认成 JOINT_LIMIT 事件继续流程。
|
||||
|
||||
为什么要在这里夹取（而不是直接下发、等驱动器报错）：
|
||||
驱动器拒绝一条轨迹时，整个 action 失败，连带把已经转过的那部分**也**
|
||||
变成"没转过"的语义（角度实际到位了，但流程认为失败）。夹取到
|
||||
"限位 − 余量"能拿到一个明确成功、且确实接近极限的姿态度。
|
||||
"""
|
||||
if want_deg <= 0.0:
|
||||
raise ValueError(f"预绕角度必须为正，收到 {want_deg}")
|
||||
if direction not in (-1, 1):
|
||||
raise ValueError(f"方向只能是 +1 / -1，收到 {direction}")
|
||||
|
||||
signed = direction * abs(want_deg)
|
||||
target = q_now_rad + math.radians(signed)
|
||||
has_limits = limits is not None
|
||||
clamped = False
|
||||
if limits is not None:
|
||||
lo = limits[0] + math.radians(margin_deg)
|
||||
up = limits[1] - math.radians(margin_deg)
|
||||
if lo > up: # 限位本身比余量还窄（不该发生，但别让它变成静默走偏）
|
||||
lo = up = 0.5 * (limits[0] + limits[1])
|
||||
if target > up:
|
||||
target, clamped = up, True
|
||||
elif target < lo:
|
||||
target, clamped = lo, True
|
||||
applied = math.degrees(target - q_now_rad)
|
||||
return PrewindResult(
|
||||
target_rad=target,
|
||||
applied_deg=applied,
|
||||
want_deg=abs(want_deg),
|
||||
clamped=clamped,
|
||||
has_limits=has_limits,
|
||||
)
|
||||
|
||||
|
||||
def rotation_about_axis_deg(R_before: np.ndarray, R_after: np.ndarray, axis: np.ndarray) -> float:
|
||||
"""从两个旋转矩阵算出"绕 ``axis`` 转了多少度"（带符号）。
|
||||
|
||||
S01 的验收判据就靠它：绕工具 Z 转 Δ 之后，如果实际绕的是别的轴，
|
||||
这个函数给出的角度会与 Δ 明显不符（或者 axis 分量不占优）。
|
||||
做法是把相对旋转投影到 ``axis`` 上：
|
||||
|
||||
``θ = atan2( axis·(R_b·R_a⁻¹ 的反对称部分) , (trace-1)/2 )``
|
||||
|
||||
:param axis: **单位**轴（工具 Z 在世界系下的方向 = ``R[:, 2]``）。
|
||||
"""
|
||||
a = np.asarray(axis, dtype=float)
|
||||
n = float(np.linalg.norm(a))
|
||||
if n < 1e-9:
|
||||
raise ValueError("轴向量长度为 0，无法判定旋转")
|
||||
a = a / n
|
||||
|
||||
Rb = np.asarray(R_before, dtype=float)[:3, :3]
|
||||
Ra = np.asarray(R_after, dtype=float)[:3, :3]
|
||||
Rrel = Ra @ Rb.T
|
||||
# 反对称部分：sin(θ)·[axis]_×
|
||||
v = np.array([Rrel[2, 1] - Rrel[1, 2], Rrel[0, 2] - Rrel[2, 0], Rrel[1, 0] - Rrel[0, 1]])
|
||||
sin_t = 0.5 * float(np.dot(v, a))
|
||||
cos_t = 0.5 * (float(np.trace(Rrel)) - 1.0)
|
||||
return math.degrees(math.atan2(sin_t, cos_t))
|
||||
|
||||
|
||||
def axis_misalignment_deg(R_before: np.ndarray, R_after: np.ndarray, axis: np.ndarray) -> float:
|
||||
"""相对旋转的转轴与 ``axis`` 的夹角 [deg]。
|
||||
|
||||
纯绕轴旋转时这个值≈0；转错轴时它会接近 90°。用作 S01 的判错依据：
|
||||
只看"转了多少度"是不够的 —— 绕正确的轴转 90° 和绕错误的轴转 90°
|
||||
给出的角度读数一样。
|
||||
"""
|
||||
Rb = np.asarray(R_before, dtype=float)[:3, :3]
|
||||
Ra = np.asarray(R_after, dtype=float)[:3, :3]
|
||||
Rrel = Ra @ Rb.T
|
||||
# 旋转轴 = 反对称部分对应的向量（sinθ 因子不影响方向）
|
||||
v = np.array([Rrel[2, 1] - Rrel[1, 2], Rrel[0, 2] - Rrel[2, 0], Rrel[1, 0] - Rrel[0, 1]])
|
||||
nv = float(np.linalg.norm(v))
|
||||
if nv < 1e-9:
|
||||
return 0.0 # 几乎没转，谈不上"转错轴"
|
||||
a = np.asarray(axis, dtype=float)
|
||||
a = a / float(np.linalg.norm(a))
|
||||
cos = abs(float(np.dot(v / nv, a))) # 取绝对值：轴线的两个方向等价
|
||||
return math.degrees(math.acos(max(-1.0, min(1.0, cos))))
|
||||
|
||||
|
||||
def unit_tool_z(R: np.ndarray) -> np.ndarray:
|
||||
"""工具坐标系的 Z 轴在世界系下的方向（旋转矩阵的第三列）。"""
|
||||
return np.asarray(R, dtype=float)[:3, 2]
|
||||
|
||||
|
||||
def vertical_down_error_deg(R: np.ndarray, world_down: Iterable[float] = (0.0, 0.0, -1.0)) -> float:
|
||||
"""工具 Z 与"垂直向下"的夹角 [deg]。0 = 完全垂直向下。
|
||||
|
||||
看板里"工具系全程垂直向下、位姿全局锁定"这句话，只有能算出这个数才算
|
||||
真的在执行 —— 否则每个阶段都得靠人眼看画面判断臂歪没歪。
|
||||
"""
|
||||
z = unit_tool_z(R)
|
||||
d = np.asarray(list(world_down), dtype=float)
|
||||
d = d / float(np.linalg.norm(d))
|
||||
cos = float(np.dot(z / max(1e-12, float(np.linalg.norm(z))), d))
|
||||
return math.degrees(math.acos(max(-1.0, min(1.0, cos))))
|
||||
@@ -0,0 +1,40 @@
|
||||
#!/usr/bin/env python3
|
||||
"""向后兼容 shim —— 实现已移到 :mod:`arm_kinematics.geom_ik`。
|
||||
|
||||
为什么搬走
|
||||
----------
|
||||
这套几何雅可比 + DLS 逆解同时被应用层（视觉伺服、搜孔装配）和驱动层
|
||||
（``arm_driver_flexiv`` 的柔顺下压）需要。留在 ``arm_application`` 里会让驱动层
|
||||
反过来依赖应用层，属于层次倒置。搬到不依赖 ROS 的 ``arm_kinematics`` 后，
|
||||
两边都依赖一个底层包。
|
||||
|
||||
为什么保留这个文件
|
||||
------------------
|
||||
``stage2_locate_node`` / ``stage3_search_node`` / ``servo_node`` 等现有代码都在
|
||||
``from arm_application.geom_ik import ...``。保留 shim 就不用改动这些已经在跑的
|
||||
文件 —— 重构不应牵连能工作的代码。
|
||||
|
||||
新代码请直接 ``from arm_kinematics import dls_twist``。
|
||||
"""
|
||||
|
||||
from arm_kinematics.geom_ik import ( # noqa: F401
|
||||
RIZON_LINK_AXES,
|
||||
dls_position_step,
|
||||
dls_translate_hold_ori,
|
||||
dls_twist,
|
||||
geometric_jacobian,
|
||||
load_handeye_T,
|
||||
position_jacobian,
|
||||
quat_xyzw_to_R,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"RIZON_LINK_AXES",
|
||||
"quat_xyzw_to_R",
|
||||
"geometric_jacobian",
|
||||
"position_jacobian",
|
||||
"dls_position_step",
|
||||
"dls_twist",
|
||||
"dls_translate_hold_ori",
|
||||
"load_handeye_T",
|
||||
]
|
||||
+457
@@ -0,0 +1,457 @@
|
||||
#!/usr/bin/env python3
|
||||
"""感知/伺服启动前的开机回初始位流程。
|
||||
|
||||
流程：
|
||||
[A] 若法兰低于 ``servo.safe_z_m``，先让它竖直上升（闭环 Z 小步跳），
|
||||
以免回 home 时扫到墙面/工装；
|
||||
[B] 调用 ``/set_named_pose``（Flexiv 命名位姿，默认 "home"）—— 由 Flexiv
|
||||
自己的规划器把机械臂送回 Flexiv 初始位姿；
|
||||
[C] 保持（pin）住，让伺服从一个干净、静止的位姿接手。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from arm_msgs.srv import SetNamedPose
|
||||
from control_msgs.action import FollowJointTrajectory
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
from tf2_ros import Buffer, TransformListener
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
|
||||
from arm_application.geom_ik import (
|
||||
RIZON_LINK_AXES,
|
||||
dls_translate_hold_ori,
|
||||
dls_twist,
|
||||
geometric_jacobian,
|
||||
quat_xyzw_to_R,
|
||||
)
|
||||
from arm_application.site_io import (
|
||||
base_frame_default,
|
||||
ee_frame_default,
|
||||
joint_prefix_default,
|
||||
)
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
|
||||
|
||||
def _load_ee_cfg() -> dict:
|
||||
root = Path(__file__).resolve().parents[3]
|
||||
sys.path.insert(0, str(root / "scripts" / "lib"))
|
||||
from export_site_env import load_site, resolve_robot # type: ignore
|
||||
|
||||
data = load_site()
|
||||
servo = (data.get("servo") or {})
|
||||
robot = resolve_robot(data)
|
||||
ee_z = float(servo.get("ee_z_m") or 0.24)
|
||||
return {
|
||||
"prefix": str(robot.get("joint_prefix") or joint_prefix_default()),
|
||||
"base": str(robot.get("base_frame") or base_frame_default()),
|
||||
"ee": str(robot.get("ee_frame") or ee_frame_default()),
|
||||
"z": ee_z,
|
||||
# 回 home 前先竖直升到这个 Z（避免撞到墙面/工装）；
|
||||
# 然后再回到 Flexiv 的命名初始位姿。
|
||||
"safe_z": float(servo.get("safe_z_m") or max(0.40, ee_z + 0.10)),
|
||||
"home_pose": str(servo.get("home_pose_name") or "home"),
|
||||
"home_speed": float(servo.get("home_pose_speed") or 0.30),
|
||||
"v_z": float(servo.get("v_max_z") or 0.04),
|
||||
# 仅用于（目前未使用的）verticalize 规划路径
|
||||
"kp": float(servo.get("ee_hold_kp") or 2.0),
|
||||
"w_max": float(servo.get("ee_w_max") or 0.50),
|
||||
}
|
||||
|
||||
|
||||
def _pt(q: np.ndarray, qdot: np.ndarray, t_sec: float) -> JointTrajectoryPoint:
|
||||
pt = JointTrajectoryPoint()
|
||||
pt.positions = [float(v) for v in q.tolist()]
|
||||
pt.velocities = [float(v) for v in qdot.tolist()]
|
||||
pt.accelerations = [0.0] * int(q.size)
|
||||
ns = int(round(max(0.0, float(t_sec)) * 1e9))
|
||||
pt.time_from_start.sec = ns // 1_000_000_000
|
||||
pt.time_from_start.nanosec = ns % 1_000_000_000
|
||||
return pt
|
||||
|
||||
|
||||
class GotoEePlane(TolerantNode):
|
||||
def __init__(self, cfg: dict) -> None:
|
||||
super().__init__("goto_ee_plane")
|
||||
self.cfg = cfg
|
||||
prefix = cfg["prefix"]
|
||||
self.joint_names = [f"{prefix}joint{i}" for i in range(1, 8)]
|
||||
self.link_frames = [f"{prefix}link{i}" for i in range(1, 8)]
|
||||
self._js: Optional[JointState] = None
|
||||
self.create_subscription(JointState, "joint_states", self._on_js, 10)
|
||||
self._tf = Buffer()
|
||||
TransformListener(self._tf, self, spin_thread=True)
|
||||
self._cli = ActionClient(
|
||||
self, FollowJointTrajectory, "rizon_arm_controller/follow_joint_trajectory"
|
||||
)
|
||||
|
||||
def _on_js(self, msg: JointState) -> None:
|
||||
self._js = msg
|
||||
|
||||
def _spin(self, dt: float = 0.05) -> None:
|
||||
rclpy.spin_once(self, timeout_sec=dt)
|
||||
|
||||
def _lookup(self, parent: str, child: str) -> Optional[np.ndarray]:
|
||||
try:
|
||||
tf = self._tf.lookup_transform(
|
||||
parent, child, rclpy.time.Time(),
|
||||
timeout=rclpy.duration.Duration(nanoseconds=80_000_000),
|
||||
)
|
||||
except Exception:
|
||||
return None
|
||||
t = tf.transform.translation
|
||||
o = tf.transform.rotation
|
||||
T = np.eye(4)
|
||||
T[:3, :3] = quat_xyzw_to_R([o.x, o.y, o.z, o.w])
|
||||
T[:3, 3] = [t.x, t.y, t.z]
|
||||
return T
|
||||
|
||||
def _q(self) -> Optional[np.ndarray]:
|
||||
if self._js is None:
|
||||
return None
|
||||
m = {n: p for n, p in zip(self._js.name, self._js.position)}
|
||||
try:
|
||||
return np.array([float(m[n]) for n in self.joint_names], dtype=float)
|
||||
except KeyError:
|
||||
return None
|
||||
|
||||
def _jacobian(self, p_ee: np.ndarray) -> Optional[np.ndarray]:
|
||||
link_R: List[np.ndarray] = []
|
||||
link_p: List[np.ndarray] = []
|
||||
for fr in self.link_frames:
|
||||
Tl = self._lookup(self.cfg["base"], fr)
|
||||
if Tl is None:
|
||||
return None
|
||||
link_R.append(Tl[:3, :3])
|
||||
link_p.append(Tl[:3, 3])
|
||||
return geometric_jacobian(link_R, link_p, p_ee, RIZON_LINK_AXES)
|
||||
|
||||
def _w_down(self, R: np.ndarray) -> Tuple[np.ndarray, float]:
|
||||
z_now = R[:, 2]
|
||||
z_des = np.array([0.0, 0.0, -1.0])
|
||||
c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0))
|
||||
ang = float(np.arccos(c))
|
||||
if ang < 0.02:
|
||||
return np.zeros(3), ang
|
||||
axis = np.cross(z_now, z_des)
|
||||
n = float(np.linalg.norm(axis))
|
||||
axis = axis / n if n > 1e-9 else np.array([1.0, 0.0, 0.0])
|
||||
mag = float(np.clip(self.cfg["kp"] * ang, -self.cfg["w_max"], self.cfg["w_max"]))
|
||||
return axis * mag, ang
|
||||
|
||||
def _target_err(
|
||||
self, p: np.ndarray, R: np.ndarray, target: np.ndarray, verticalize: bool
|
||||
) -> Tuple[np.ndarray, float, float]:
|
||||
dp = np.asarray(target, dtype=float).reshape(3) - np.asarray(p, dtype=float).reshape(3)
|
||||
_, ang = (self._w_down(R) if verticalize else (np.zeros(3), 0.0))
|
||||
xy = float(np.hypot(dp[0], dp[1]))
|
||||
return dp, ang, xy
|
||||
|
||||
def _wait_state(self, timeout_s: float) -> Tuple[Optional[np.ndarray], Optional[np.ndarray]]:
|
||||
t0 = time.monotonic()
|
||||
q = None
|
||||
Tee = None
|
||||
while time.monotonic() - t0 < timeout_s:
|
||||
self._spin(0.05)
|
||||
q = self._q()
|
||||
Tee = self._lookup(self.cfg["base"], self.cfg["ee"])
|
||||
if q is not None and Tee is not None and self._jacobian(Tee[:3, 3]) is not None:
|
||||
return q, Tee
|
||||
return q, Tee
|
||||
|
||||
def _send_action(self, traj: JointTrajectory, wait_s: float) -> bool:
|
||||
goal = FollowJointTrajectory.Goal()
|
||||
goal.trajectory = traj
|
||||
fut = self._cli.send_goal_async(goal)
|
||||
t0 = time.monotonic()
|
||||
while not fut.done() and time.monotonic() - t0 < 4.0:
|
||||
self._spin(0.05)
|
||||
handle = fut.result() if fut.done() else None
|
||||
if handle is None or not handle.accepted:
|
||||
print(" 错误：JTC 拒绝了抬升轨迹", flush=True)
|
||||
return False
|
||||
res = handle.get_result_async()
|
||||
t0 = time.monotonic()
|
||||
last = 0.0
|
||||
while not res.done() and time.monotonic() - t0 < wait_s:
|
||||
self._spin(0.05)
|
||||
now = time.monotonic()
|
||||
if now - last > 0.4:
|
||||
Tee = self._lookup(self.cfg["base"], self.cfg["ee"])
|
||||
if Tee is not None:
|
||||
p = Tee[:3, 3]
|
||||
print(
|
||||
f" 运动中 ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f}) "
|
||||
f"z_err={(self.cfg['z'] - p[2]) * 1000:+.0f}mm",
|
||||
flush=True,
|
||||
)
|
||||
last = now
|
||||
if not res.done():
|
||||
print(" 警告：JTC 结果超时 —— 仍然继续检查 TF", flush=True)
|
||||
return False
|
||||
return True
|
||||
|
||||
def _plan_and_send(
|
||||
self,
|
||||
q0: np.ndarray,
|
||||
Tee: np.ndarray,
|
||||
target: np.ndarray,
|
||||
verticalize: bool,
|
||||
tag: str = "",
|
||||
max_seg: float = 0.08,
|
||||
v: float = 0.05,
|
||||
) -> bool:
|
||||
"""规划并执行一段朝向 target 的短笛卡尔位移（≤ max_seg）。
|
||||
长距离移动由调用方以闭环小步跳的方式完成，因为雅可比是在当前位姿
|
||||
处冻结的 —— 超过 10 cm 后线性化误差会让一次性规划严重偏离目标。"""
|
||||
p = Tee[:3, 3].copy()
|
||||
R = Tee[:3, :3]
|
||||
dp, ang, _ = self._target_err(p, R, target, verticalize)
|
||||
dist = float(np.linalg.norm(dp))
|
||||
if dist > max_seg: # 限制本次小步跳的长度；调用方会用最新的 TF 循环执行
|
||||
dp = dp * (max_seg / dist)
|
||||
dist = max_seg
|
||||
print(
|
||||
f" 规划 {tag} Δxyz=({dp[0]*1000:+.0f},{dp[1]*1000:+.0f},{dp[2]*1000:+.0f})mm "
|
||||
f"倾角={ang * 180 / np.pi:.1f}deg",
|
||||
flush=True,
|
||||
)
|
||||
J = self._jacobian(p)
|
||||
if J is None:
|
||||
print("错误：缺少用于雅可比的 link TF", flush=True)
|
||||
return False
|
||||
|
||||
q = q0.copy()
|
||||
waypoints = [q.copy()]
|
||||
remain = dp.copy()
|
||||
w_step, _ = (self._w_down(R) if verticalize else (np.zeros(3), 0.0))
|
||||
n_max = int(np.ceil(dist / 0.012)) + 4
|
||||
for _ in range(n_max):
|
||||
if float(np.linalg.norm(remain)) < 0.0015 and (
|
||||
(not verticalize) or ang < 0.03
|
||||
):
|
||||
break
|
||||
nseg = max(1, int(np.ceil(float(np.linalg.norm(remain)) / 0.012)))
|
||||
step = remain / nseg
|
||||
if verticalize and ang > 0.03:
|
||||
qdot = dls_twist(
|
||||
J, step / 0.12, w_base=w_step, damping=0.08, max_qdot=0.45
|
||||
)
|
||||
dq = np.clip(qdot * 0.12, -0.06, 0.06)
|
||||
else:
|
||||
dq = dls_translate_hold_ori(J, step, damping=0.08, max_dq=0.06)
|
||||
if float(np.max(np.abs(dq))) < 1e-5:
|
||||
break
|
||||
q = q + dq
|
||||
waypoints.append(q.copy())
|
||||
remain = remain - (J[:3, :] @ dq)
|
||||
ang = max(0.0, ang - 0.05)
|
||||
|
||||
planned = dist - float(np.linalg.norm(remain))
|
||||
if len(waypoints) < 2 or planned < 0.4 * dist:
|
||||
print(
|
||||
f" 警告 {tag}：规划器停滞 "
|
||||
f"（已规划 {planned*1000:.0f}/{dist*1000:.0f}mm）—— 跳过本次小步跳",
|
||||
flush=True,
|
||||
)
|
||||
return False
|
||||
# 关节偏移量的合理性闸门：拒绝过于夸张的规划
|
||||
if float(np.max(np.abs(waypoints[-1] - q0))) > 1.0:
|
||||
print(f" 错误 {tag}：关节偏移 >1 rad —— 放弃本次小步跳", flush=True)
|
||||
return False
|
||||
|
||||
duration = max(1.6, dist / max(min(v, 0.06), 1e-3))
|
||||
n = len(waypoints) - 1
|
||||
traj = JointTrajectory()
|
||||
traj.header.stamp.sec = 0
|
||||
traj.header.stamp.nanosec = 0
|
||||
traj.joint_names = list(self.joint_names)
|
||||
traj.points.append(_pt(waypoints[0], np.zeros(7), 0.08))
|
||||
zeros = np.zeros(7)
|
||||
for i, qi in enumerate(waypoints[1:], start=1):
|
||||
t = 0.08 + duration * (i / n)
|
||||
qdot = zeros if i == n else (waypoints[min(i + 1, n)] - qi) / max(
|
||||
duration / n, 0.05
|
||||
)
|
||||
qdot = np.clip(qdot, -0.40, 0.40)
|
||||
traj.points.append(_pt(qi, qdot, t))
|
||||
# 强制最后一个点的速度为零
|
||||
traj.points[-1].velocities = [0.0] * 7
|
||||
print(
|
||||
f" 发送 JTC action {tag}，{n} 步 T={duration:.1f}s",
|
||||
flush=True,
|
||||
)
|
||||
return self._send_action(traj, wait_s=duration + 6.0)
|
||||
|
||||
def _goto(
|
||||
self,
|
||||
q: np.ndarray,
|
||||
Tee: np.ndarray,
|
||||
target: np.ndarray,
|
||||
verticalize: bool,
|
||||
tol_m: float,
|
||||
tol_tilt: float,
|
||||
tag: str,
|
||||
v: float,
|
||||
max_hops: int = 8,
|
||||
) -> Tuple[bool, np.ndarray, np.ndarray]:
|
||||
"""闭环点到点：规划 ≤8 cm 的小步跳 → 执行 → 重新测量 TF → 重复，
|
||||
直到落在容差内。这样对长距离下的雅可比线性化误差更鲁棒。"""
|
||||
for hop in range(max_hops):
|
||||
p = Tee[:3, 3]
|
||||
dp, ang, _ = self._target_err(p, Tee[:3, :3], target, verticalize)
|
||||
dist = float(np.linalg.norm(dp))
|
||||
if dist < tol_m and ((not verticalize) or ang < tol_tilt):
|
||||
return True, q, Tee
|
||||
print(
|
||||
f" {tag} 第 {hop + 1} 跳 remain={dist*1000:.0f}mm "
|
||||
f"倾角={ang * 180 / np.pi:.1f}deg",
|
||||
flush=True,
|
||||
)
|
||||
if not self._plan_and_send(q, Tee, target, verticalize, tag, v=v):
|
||||
return False, q, Tee
|
||||
t_end = time.monotonic() + 0.5
|
||||
while time.monotonic() < t_end:
|
||||
self._spin(0.05)
|
||||
q2, Tee2 = self._wait_state(2.0)
|
||||
if q2 is None or Tee2 is None:
|
||||
print("错误：移动过程中丢失状态", flush=True)
|
||||
return False, q, Tee
|
||||
q, Tee = q2, Tee2
|
||||
p = Tee[:3, 3]
|
||||
dp, ang, _ = self._target_err(p, Tee[:3, :3], target, verticalize)
|
||||
ok = float(np.linalg.norm(dp)) < (tol_m * 2.0) and (
|
||||
(not verticalize) or ang < (tol_tilt * 2.0)
|
||||
)
|
||||
return ok, q, Tee
|
||||
|
||||
def _pin(self, q: np.ndarray) -> None:
|
||||
if not self._cli.wait_for_server(timeout_sec=3.0):
|
||||
return
|
||||
traj = JointTrajectory()
|
||||
traj.header.stamp.sec = 0
|
||||
traj.header.stamp.nanosec = 0
|
||||
traj.joint_names = list(self.joint_names)
|
||||
traj.points = [_pt(q, np.zeros(7), 0.08), _pt(q, np.zeros(7), 0.45)]
|
||||
self._send_action(traj, wait_s=3.0)
|
||||
|
||||
def _named_pose(self, name: str, speed: float) -> bool:
|
||||
"""调用 Flexiv 的 ``/set_named_pose``，并等待机械臂静止。"""
|
||||
cli = self.create_client(SetNamedPose, "/set_named_pose")
|
||||
if not cli.wait_for_server(timeout_sec=8.0):
|
||||
print("错误：``/set_named_pose`` 不可用 —— 请启动 run_real.sh", flush=True)
|
||||
return False
|
||||
req = SetNamedPose.Request()
|
||||
req.pose_name = name
|
||||
req.velocity_scale = float(min(max(speed, 0.05), 1.0))
|
||||
req.acceleration_scale = 0.0
|
||||
print(f" [B] set_named_pose '{name}' speed={req.velocity_scale:.2f}", flush=True)
|
||||
fut = cli.call_async(req)
|
||||
t0 = time.monotonic()
|
||||
while not fut.done() and time.monotonic() - t0 < 90.0:
|
||||
self._spin(0.1)
|
||||
if not fut.done():
|
||||
print("错误：set_named_pose 超时", flush=True)
|
||||
return False
|
||||
resp = fut.result()
|
||||
print(
|
||||
f" [B] 响应 success={resp.success} msg={resp.message}",
|
||||
flush=True,
|
||||
)
|
||||
if not resp.success:
|
||||
return False
|
||||
# 等待法兰停止运动（约 0.8 s 内 Δp < 1 mm）
|
||||
last = None
|
||||
still = 0.0
|
||||
t0 = time.monotonic()
|
||||
while time.monotonic() - t0 < 30.0:
|
||||
self._spin(0.2)
|
||||
Tee = self._lookup(self.cfg["base"], self.cfg["ee"])
|
||||
if Tee is not None:
|
||||
p = Tee[:3, 3]
|
||||
if last is not None and float(np.linalg.norm(p - last)) < 0.001:
|
||||
still += 0.2
|
||||
if still >= 0.8:
|
||||
return True
|
||||
else:
|
||||
still = 0.0
|
||||
last = p.copy()
|
||||
print(" 警告：等待静止超时 —— 仍然继续", flush=True)
|
||||
return True
|
||||
|
||||
def run(self, timeout_s: float = 25.0) -> int:
|
||||
cfg = self.cfg
|
||||
q, Tee = self._wait_state(4.0)
|
||||
if q is None or Tee is None:
|
||||
print("错误：没有 joint_states / TF —— 请启动 run_real.sh", flush=True)
|
||||
return 2
|
||||
if not self._cli.wait_for_server(timeout_sec=8.0):
|
||||
print("错误：JTC action 不可用 —— 请启动 run_real.sh", flush=True)
|
||||
return 3
|
||||
|
||||
p = Tee[:3, 3]
|
||||
safe_z = float(cfg["safe_z"])
|
||||
print(
|
||||
f"=== 回到 Flexiv 初始位姿 '{cfg['home_pose']}' "
|
||||
f"safe_z={safe_z*1000:.0f}mm ===",
|
||||
flush=True,
|
||||
)
|
||||
print(
|
||||
f" 当前 ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f})",
|
||||
flush=True,
|
||||
)
|
||||
|
||||
# --- 阶段 A：回 home 前先沿纯 Z 方向升到安全高度 ---
|
||||
if p[2] < safe_z - 0.01:
|
||||
lift_tgt = np.array([p[0], p[1], safe_z], dtype=float)
|
||||
print(f" [A] 抬升 Z {p[2]*1000:.0f} → {safe_z*1000:.0f}mm", flush=True)
|
||||
ok, q, Tee = self._goto(
|
||||
q, Tee, lift_tgt, False, 0.010, 0.0, "[A]lift", v=0.05
|
||||
)
|
||||
if not ok:
|
||||
print("错误：抬升阶段失败 —— 就地 pin 住", flush=True)
|
||||
self._pin(q)
|
||||
return 4
|
||||
|
||||
# --- 阶段 B：Flexiv 命名位姿 → 真正的初始位姿，姿态良好 ---
|
||||
if not self._named_pose(cfg["home_pose"], cfg["home_speed"]):
|
||||
print("错误：回命名位姿失败 —— 就地 pin 住", flush=True)
|
||||
self._pin(q)
|
||||
return 4
|
||||
|
||||
# --- 阶段 C：保持住，让伺服从一个静止位姿接手 ---
|
||||
q, Tee = self._wait_state(2.0)
|
||||
if q is not None:
|
||||
self._pin(q)
|
||||
if Tee is not None:
|
||||
p = Tee[:3, 3]
|
||||
print(
|
||||
f" 已到初始位姿 ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f})",
|
||||
flush=True,
|
||||
)
|
||||
return 0
|
||||
|
||||
def main() -> int:
|
||||
cfg = _load_ee_cfg()
|
||||
rclpy.init()
|
||||
node = GotoEePlane(cfg)
|
||||
try:
|
||||
return node.run()
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
+771
@@ -0,0 +1,771 @@
|
||||
#!/usr/bin/env python3
|
||||
"""完整手眼标定流水线：移动 → 棋盘稳定判定 → 采集 → 求解 → 清理。
|
||||
|
||||
支持 eye_in_hand / eye_to_hand。
|
||||
|
||||
相机内参优先级：
|
||||
1) ROS 的 CameraInfo 话题（若正在发布）
|
||||
2) 通过 pyrealsense2 读取 RealSense D405/D435 的 EEPROM（camera_source）
|
||||
3) YAML 中手写的 camera_fx/fy/cx/cy
|
||||
|
||||
棋盘尺寸（board_cols / board_rows / square_size_m）可由用户配置。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import shutil
|
||||
import time
|
||||
from datetime import datetime, timezone
|
||||
from pathlib import Path
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import CameraInfo, Image, JointState
|
||||
from std_msgs.msg import String
|
||||
|
||||
from arm_application.handeye_lib import (
|
||||
BoardConfig,
|
||||
CameraIntrinsics,
|
||||
ChessboardStabilizer,
|
||||
T_from_flexiv_tcp,
|
||||
calibrate_hand_eye,
|
||||
cam_to_flange_via_tool,
|
||||
detect_chessboard,
|
||||
format_matrix_yaml,
|
||||
format_transform_yaml,
|
||||
is_pose_filled,
|
||||
pose_to_Rt,
|
||||
solve_board_pose,
|
||||
)
|
||||
from arm_application.handeye_realsense import (
|
||||
is_realsense_source,
|
||||
query_realsense_color_intrinsics,
|
||||
resolve_realsense_topics,
|
||||
)
|
||||
from arm_application.site_io import joint_prefix_default
|
||||
|
||||
from arm_msgs.action import FollowJointTrajectory
|
||||
from arm_msgs.srv import GetArmState
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
DEFAULT_JOINTS = [
|
||||
'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
|
||||
]
|
||||
|
||||
|
||||
def _image_to_bgr(msg: Image) -> np.ndarray:
|
||||
h, w = int(msg.height), int(msg.width)
|
||||
enc = (msg.encoding or '').lower()
|
||||
data = np.frombuffer(msg.data, dtype=np.uint8)
|
||||
|
||||
if enc in ('bgr8', 'rgb8'):
|
||||
img = data.reshape(h, w, 3)
|
||||
return img[:, :, ::-1].copy() if enc == 'rgb8' else img
|
||||
if enc in ('mono8', '8uc1'):
|
||||
gray = data.reshape(h, w)
|
||||
return np.stack([gray, gray, gray], axis=-1)
|
||||
if enc in ('bgra8', 'rgba8'):
|
||||
img = data.reshape(h, w, 4)
|
||||
bgr = img[:, :, :3]
|
||||
return bgr[:, :, ::-1].copy() if enc == 'rgba8' else bgr
|
||||
raise RuntimeError(f'不支持的图像编码：{msg.encoding!r}')
|
||||
|
||||
|
||||
def _camera_info_to_intrinsics(msg: CameraInfo) -> CameraIntrinsics:
|
||||
K = np.array(msg.k, dtype=float).reshape(3, 3)
|
||||
dist = np.array(msg.d, dtype=float).reshape(-1, 1) if msg.d else np.zeros((5, 1))
|
||||
return CameraIntrinsics(K=K, dist=dist)
|
||||
|
||||
|
||||
class HandeyeCalibrate(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('handeye_calibrate')
|
||||
|
||||
self.declare_parameter('mode', 'eye_in_hand') # eye_in_hand | eye_to_hand
|
||||
self.declare_parameter('joint_prefix', joint_prefix_default())
|
||||
self.declare_parameter('joint_names', DEFAULT_JOINTS)
|
||||
self.declare_parameter('velocity_scale', 0.10)
|
||||
self.declare_parameter('action_name', 'follow_joint_trajectory')
|
||||
self.declare_parameter('move_time_sec', 4.0)
|
||||
self.declare_parameter('start_index', 0)
|
||||
|
||||
self.declare_parameter('num_poses', 12)
|
||||
self.declare_parameter('waypoint_positions', [0.0] * (12 * 7))
|
||||
self.declare_parameter('min_poses', 5)
|
||||
|
||||
# 相机来源：manual | d405 | d435
|
||||
self.declare_parameter('camera_source', 'manual')
|
||||
self.declare_parameter('camera_serial', '')
|
||||
self.declare_parameter('camera_namespace', '')
|
||||
self.declare_parameter('realsense_width', 1280)
|
||||
self.declare_parameter('realsense_height', 720)
|
||||
self.declare_parameter('image_topic', '')
|
||||
self.declare_parameter('camera_info_topic', '')
|
||||
self.declare_parameter('camera_fx', 0.0)
|
||||
self.declare_parameter('camera_fy', 0.0)
|
||||
self.declare_parameter('camera_cx', 0.0)
|
||||
self.declare_parameter('camera_cy', 0.0)
|
||||
self.declare_parameter('camera_dist', [0.0, 0.0, 0.0, 0.0, 0.0])
|
||||
|
||||
# 棋盘（可自定义）
|
||||
# 默认值必须与 site.yaml 的 handeye.board_* 及结果文件头里的实际板子一致。
|
||||
# 真实使用的板：A5 内角点 11x8，方格 15mm（site.yaml handeye.boards.eye_in_hand）。
|
||||
# 这里原先写的是 9x6 / 0.025，后来是 8x11；漏配就会静默用错尺寸。
|
||||
self.declare_parameter('board_cols', 11) # 内角点
|
||||
self.declare_parameter('board_rows', 8)
|
||||
self.declare_parameter('square_size_m', 0.015)
|
||||
|
||||
self.declare_parameter('stable_window', 8)
|
||||
self.declare_parameter('stable_pixel_std', 0.45)
|
||||
self.declare_parameter('stable_timeout_sec', 25.0)
|
||||
self.declare_parameter('settle_sec', 0.4)
|
||||
# 到位后的"真的停住了"判据 —— 直接看关节速度，比固定 sleep 可靠。
|
||||
# 这几个参数原先 site.yaml 和 run_handeye.sh 一直在传，但本节点从未声明，
|
||||
# ROS 2 会静默忽略未声明的覆盖参数，于是"等静止再拍照"这件事其实没实现，
|
||||
# 实际只靠一次固定 settle_sec。现在补上。
|
||||
self.declare_parameter('settle_qd_max', 0.015) # |qd|_max [rad/s] 低于此值算静
|
||||
self.declare_parameter('settle_stable_sec', 0.6) # 需要连续静多久
|
||||
self.declare_parameter('settle_timeout_sec', 8.0) # 等静止的总超时
|
||||
self.declare_parameter('pin_holds', 2) # 静止后再保持几拍
|
||||
self.declare_parameter('pin_hold_sec', 0.45) # 每拍多长
|
||||
self.declare_parameter('skip_unstable_poses', True)
|
||||
|
||||
from arm_application.site_io import ros2_root
|
||||
|
||||
self.declare_parameter('output_dir', str(ros2_root() / 'handeye' / 'tmp'))
|
||||
self.declare_parameter('result_file', '')
|
||||
self.declare_parameter('cleanup_images', True)
|
||||
# 眼在手上：AX=XB 的 gripper 用 Studio 工具 TCP，再折成 cam→法兰写 T_4x4。
|
||||
# 空 = 旧口径（ee_pose 当 gripper，T_4x4 原样写出）。配了就必须带 TCP。
|
||||
self.declare_parameter('gripper_tool', '')
|
||||
self.declare_parameter('gripper_tcp_xyz', [0.0, 0.0, 0.0])
|
||||
self.declare_parameter('gripper_tcp_qwxyz', [1.0, 0.0, 0.0, 0.0])
|
||||
|
||||
prefix = str(self.get_parameter('joint_prefix').value or '')
|
||||
base = list(
|
||||
self.get_parameter('joint_names').get_parameter_value().string_array_value
|
||||
or DEFAULT_JOINTS
|
||||
)
|
||||
self.joint_names = [f'{prefix}{n}' for n in base]
|
||||
self.mode = str(self.get_parameter('mode').value).strip().lower()
|
||||
self.velocity_scale = float(self.get_parameter('velocity_scale').value)
|
||||
self.action_name = str(self.get_parameter('action_name').value)
|
||||
self.move_time_sec = float(self.get_parameter('move_time_sec').value)
|
||||
self.start_index = max(0, int(self.get_parameter('start_index').value))
|
||||
self.min_poses = int(self.get_parameter('min_poses').value)
|
||||
self.cleanup_images = bool(self.get_parameter('cleanup_images').value)
|
||||
self.gripper_tool = str(self.get_parameter('gripper_tool').value or '').strip()
|
||||
tcp_xyz = list(
|
||||
self.get_parameter('gripper_tcp_xyz').get_parameter_value().double_array_value
|
||||
or [0.0, 0.0, 0.0]
|
||||
)
|
||||
tcp_q = list(
|
||||
self.get_parameter('gripper_tcp_qwxyz').get_parameter_value().double_array_value
|
||||
or [1.0, 0.0, 0.0, 0.0]
|
||||
)
|
||||
self.T_flange_to_tool = T_from_flexiv_tcp(tcp_xyz[:3], tcp_q[:4])
|
||||
if self.gripper_tool and float(np.linalg.norm(self.T_flange_to_tool[:3, 3])) < 1e-4:
|
||||
raise RuntimeError(
|
||||
f'gripper_tool={self.gripper_tool!r} 但 gripper_tcp_xyz 几乎是零：'
|
||||
'等于没把 Studio TCP 传进来。不许按法兰口径假装在用工具系。'
|
||||
)
|
||||
|
||||
self.camera_source = str(self.get_parameter('camera_source').value).strip().lower()
|
||||
self.camera_serial = str(self.get_parameter('camera_serial').value or '').strip()
|
||||
self.camera_namespace = str(
|
||||
self.get_parameter('camera_namespace').value or ''
|
||||
).strip()
|
||||
self.rs_width = int(self.get_parameter('realsense_width').value)
|
||||
self.rs_height = int(self.get_parameter('realsense_height').value)
|
||||
|
||||
self.board = BoardConfig(
|
||||
cols=int(self.get_parameter('board_cols').value),
|
||||
rows=int(self.get_parameter('board_rows').value),
|
||||
square_size=float(self.get_parameter('square_size_m').value),
|
||||
)
|
||||
self.stabilizer = ChessboardStabilizer(
|
||||
window=int(self.get_parameter('stable_window').value),
|
||||
max_pixel_std=float(self.get_parameter('stable_pixel_std').value),
|
||||
)
|
||||
self.stable_timeout = float(self.get_parameter('stable_timeout_sec').value)
|
||||
self.settle_sec = float(self.get_parameter('settle_sec').value)
|
||||
self.settle_qd_max = float(self.get_parameter('settle_qd_max').value)
|
||||
self.settle_stable_sec = float(self.get_parameter('settle_stable_sec').value)
|
||||
self.settle_timeout = float(self.get_parameter('settle_timeout_sec').value)
|
||||
self.pin_holds = max(0, int(self.get_parameter('pin_holds').value))
|
||||
self.pin_hold_sec = max(0.0, float(self.get_parameter('pin_hold_sec').value))
|
||||
self.skip_unstable = bool(self.get_parameter('skip_unstable_poses').value)
|
||||
|
||||
out = Path(str(self.get_parameter('output_dir').value)).expanduser()
|
||||
self.output_dir = out
|
||||
self.images_dir = out / 'images'
|
||||
rf = str(self.get_parameter('result_file').value or '').strip()
|
||||
self.result_file = Path(rf).expanduser() if rf else (out / 'handeye_result.yaml')
|
||||
|
||||
self._client = ActionClient(self, FollowJointTrajectory, self.action_name)
|
||||
self._get_state = self.create_client(GetArmState, 'get_arm_state')
|
||||
self._capture_pub = self.create_publisher(String, 'handeye/capture', 10)
|
||||
|
||||
self._latest_image: Optional[Image] = None
|
||||
self._latest_info: Optional[CameraInfo] = None
|
||||
self._latest_js: Optional[JointState] = None
|
||||
self._intrinsics: Optional[CameraIntrinsics] = None
|
||||
|
||||
img_topic = str(self.get_parameter('image_topic').value or '').strip()
|
||||
info_topic = str(self.get_parameter('camera_info_topic').value or '').strip()
|
||||
if is_realsense_source(self.camera_source):
|
||||
img_topic, info_topic = resolve_realsense_topics(
|
||||
self.camera_source,
|
||||
camera_namespace=self.camera_namespace,
|
||||
image_topic=img_topic,
|
||||
camera_info_topic=info_topic,
|
||||
)
|
||||
self._info_topic = info_topic
|
||||
|
||||
self.create_subscription(JointState, '/joint_states', self._on_js, 10)
|
||||
|
||||
if not img_topic:
|
||||
self.get_logger().error(
|
||||
'image_topic 为空。请设置 camera_source:=d405|d435，或填写 image_topic。'
|
||||
)
|
||||
else:
|
||||
self.create_subscription(
|
||||
Image, img_topic, self._on_image, qos_profile_sensor_data
|
||||
)
|
||||
self.get_logger().info(f'已订阅图像：{img_topic}（BEST_EFFORT）')
|
||||
if info_topic:
|
||||
self.create_subscription(
|
||||
CameraInfo, info_topic, self._on_info, qos_profile_sensor_data
|
||||
)
|
||||
self.get_logger().info(f'已订阅 camera_info：{info_topic}（BEST_EFFORT）')
|
||||
|
||||
def _on_js(self, msg: JointState) -> None:
|
||||
self._latest_js = msg
|
||||
|
||||
def _on_image(self, msg: Image) -> None:
|
||||
self._latest_image = msg
|
||||
|
||||
def _on_info(self, msg: CameraInfo) -> None:
|
||||
self._latest_info = msg
|
||||
|
||||
def run(self) -> int:
|
||||
if self.mode not in ('eye_in_hand', 'eye_to_hand'):
|
||||
self.get_logger().error(
|
||||
f'未知的 mode={self.mode!r}；请使用 eye_in_hand 或 eye_to_hand'
|
||||
)
|
||||
return 1
|
||||
|
||||
poses = self._load_poses()
|
||||
if len(poses) < self.min_poses:
|
||||
self.get_logger().error(
|
||||
f'只有 {len(poses)} 个已填写位姿（需要 >= {self.min_poses}）。'
|
||||
'示教命令：bash ~/ros2/scripts/run_handeye.sh teach && merge。'
|
||||
)
|
||||
return 1
|
||||
if self.start_index >= len(poses):
|
||||
self.get_logger().error(
|
||||
f'start_index {self.start_index} >= 已填写位姿数 {len(poses)}'
|
||||
)
|
||||
return 1
|
||||
|
||||
if not self._resolve_intrinsics():
|
||||
return 1
|
||||
|
||||
if not self._client.wait_for_server(timeout_sec=15.0):
|
||||
self.get_logger().error(
|
||||
f'Action [{self.action_name}] 不可用。'
|
||||
'请启动：bash ~/ros2/scripts/run_real.sh'
|
||||
)
|
||||
return 2
|
||||
|
||||
self.output_dir.mkdir(parents=True, exist_ok=True)
|
||||
self.images_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
eye_to_hand = self.mode == 'eye_to_hand'
|
||||
grip = self.gripper_tool or '（ee_pose 原样，无工具折算）'
|
||||
self.get_logger().info(
|
||||
f'手眼标定 [{self.mode}] camera_source={self.camera_source}：'
|
||||
f'{len(poses)} 个位姿，棋盘={self.board.cols}x{self.board.rows} '
|
||||
f'方格={self.board.square_size}m，v={self.velocity_scale} '
|
||||
f'gripper_tool={grip}'
|
||||
)
|
||||
self.get_logger().warn('清理工作空间；确认急停就绪。Ctrl+C 可中止。')
|
||||
|
||||
Rg: List[np.ndarray] = []
|
||||
tg: List[np.ndarray] = []
|
||||
Rc: List[np.ndarray] = []
|
||||
tc: List[np.ndarray] = []
|
||||
meta = []
|
||||
|
||||
try:
|
||||
for i in range(self.start_index, len(poses)):
|
||||
q = poses[i]
|
||||
self._log_progress(i, len(poses), '移动中...')
|
||||
code = self._move_one(q, self.move_time_sec)
|
||||
if code != 0:
|
||||
return code
|
||||
|
||||
if not self._verify_joints(q, tol=0.08):
|
||||
self.get_logger().error(
|
||||
f'[{i + 1}] 机械臂未到达目标关节位置 '
|
||||
'（控制器报告成功，但关节没有变化）。'
|
||||
'请重启 run_real.sh；示教器切 AUTO Remote；清除故障。'
|
||||
'不要相信这次标定结果。'
|
||||
)
|
||||
return 10
|
||||
|
||||
end = time.monotonic() + max(0.0, self.settle_sec)
|
||||
while rclpy.ok() and time.monotonic() < end:
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
|
||||
# 固定 settle_sec 之后再看关节速度：真停了才算数，然后 pin 一拍。
|
||||
self._wait_settled()
|
||||
self._pin_hold()
|
||||
|
||||
ok, std, corners, pattern, bgr = self._wait_stable()
|
||||
if not ok or corners is None or bgr is None:
|
||||
msg = f'[{i + 1}] 棋盘不稳定（std={std:.3f}）'
|
||||
if self.skip_unstable:
|
||||
self.get_logger().warn(f'{msg} —— 跳过')
|
||||
continue
|
||||
self.get_logger().error(f'{msg}。中止。')
|
||||
return 7
|
||||
|
||||
ee = self._read_ee_pose()
|
||||
if ee is None:
|
||||
self.get_logger().error('读取 ee_pose 失败')
|
||||
return 8
|
||||
|
||||
assert self._intrinsics is not None
|
||||
R_t2c, t_t2c = solve_board_pose(
|
||||
corners, self.board, self._intrinsics, pattern=pattern
|
||||
)
|
||||
xyz = [ee.pose.position.x, ee.pose.position.y, ee.pose.position.z]
|
||||
quat = [
|
||||
ee.pose.orientation.x,
|
||||
ee.pose.orientation.y,
|
||||
ee.pose.orientation.z,
|
||||
ee.pose.orientation.w,
|
||||
]
|
||||
R_g2b, t_g2b = pose_to_Rt(xyz, quat)
|
||||
|
||||
stamp = datetime.now(timezone.utc).strftime('%Y%m%d_%H%M%S')
|
||||
img_path = self.images_dir / f'pose_{i:02d}_{stamp}.png'
|
||||
self._save_png(bgr, img_path)
|
||||
|
||||
msg = String()
|
||||
msg.data = f'capture:{i}:{len(poses)}'
|
||||
self._capture_pub.publish(msg)
|
||||
|
||||
Rg.append(R_g2b)
|
||||
tg.append(t_g2b)
|
||||
Rc.append(R_t2c)
|
||||
tc.append(t_t2c)
|
||||
meta.append(
|
||||
{
|
||||
'index': i,
|
||||
'joints': list(q),
|
||||
'ee_xyz': xyz,
|
||||
'ee_quat_xyzw': quat,
|
||||
'image': str(img_path),
|
||||
'corner_std_px': std,
|
||||
}
|
||||
)
|
||||
self.get_logger().info(
|
||||
f'[{i + 1}/{len(poses)}] 已采集 '
|
||||
f'(corner_std={std:.3f}px) -> {img_path.name}'
|
||||
)
|
||||
except KeyboardInterrupt:
|
||||
self.get_logger().info('已中断')
|
||||
return 0
|
||||
|
||||
if len(Rg) < self.min_poses:
|
||||
self.get_logger().error(
|
||||
f'只采集到 {len(Rg)} 次；求解至少需要 {self.min_poses} 次'
|
||||
)
|
||||
return 9
|
||||
|
||||
self.get_logger().info(f'用 {len(Rg)} 个视角求解手眼标定...')
|
||||
R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=eye_to_hand)
|
||||
extra = ''
|
||||
if eye_to_hand:
|
||||
name = 'T_cam_to_base (eye_to_hand)'
|
||||
yaml_txt = format_transform_yaml(R, t, name)
|
||||
if self.gripper_tool:
|
||||
extra = (
|
||||
f'gripper_tool: {self.gripper_tool}\n'
|
||||
+ format_matrix_yaml(self.T_flange_to_tool, 'T_flange_to_tool')
|
||||
)
|
||||
self.get_logger().info(
|
||||
f'gripper={self.gripper_tool}：AX=XB 的手是 TCP；'
|
||||
f'T_4x4 是 cam→基座，不折 t={t.reshape(3)*1000} mm'
|
||||
)
|
||||
elif self.gripper_tool:
|
||||
T_cam_tool = np.eye(4)
|
||||
T_cam_tool[:3, :3] = R
|
||||
T_cam_tool[:3, 3] = t.reshape(3)
|
||||
T_cam_flange = cam_to_flange_via_tool(T_cam_tool, self.T_flange_to_tool)
|
||||
delta = float(np.linalg.norm(T_cam_flange[:3, 3] - T_cam_tool[:3, 3]))
|
||||
tool_len = float(np.linalg.norm(self.T_flange_to_tool[:3, 3]))
|
||||
if tool_len > 0.05 and delta < 1e-4:
|
||||
self.get_logger().error(
|
||||
'T_cam_to_flange 与 T_cam_to_tool 平移几乎相同，'
|
||||
'折算没生效。结果作废。'
|
||||
)
|
||||
return 11
|
||||
# 腕部相机在法兰前方（法兰 +Z）。乘反了 Z 会落到 −200 mm 一带。
|
||||
if (
|
||||
self.T_flange_to_tool[2, 3] > 0.05
|
||||
and float(T_cam_flange[2, 3]) < 0.0
|
||||
):
|
||||
self.get_logger().error(
|
||||
f'T_cam_to_flange tz={float(T_cam_flange[2, 3])*1000:.1f} mm '
|
||||
'在法兰后方，乘序反了。结果作废。'
|
||||
)
|
||||
return 12
|
||||
R, t = T_cam_flange[:3, :3], T_cam_flange[:3, 3].reshape(3, 1)
|
||||
name = 'T_cam_to_flange (eye_in_hand，由工具 TCP 折算)'
|
||||
yaml_txt = format_transform_yaml(R, t, name)
|
||||
extra = (
|
||||
f'gripper_tool: {self.gripper_tool}\n'
|
||||
+ format_matrix_yaml(self.T_flange_to_tool, 'T_flange_to_tool')
|
||||
+ format_matrix_yaml(T_cam_tool, 'T_cam_to_tool')
|
||||
)
|
||||
self.get_logger().info(
|
||||
f'gripper={self.gripper_tool}：'
|
||||
f'T_cam_to_tool t={T_cam_tool[:3, 3]*1000} mm → '
|
||||
f'T_cam_to_flange t={T_cam_flange[:3, 3]*1000} mm'
|
||||
)
|
||||
else:
|
||||
name = 'T_cam_to_gripper (eye_in_hand)'
|
||||
yaml_txt = format_transform_yaml(R, t, name)
|
||||
header = (
|
||||
f'# 手眼标定结果\n'
|
||||
f'# mode: {self.mode}\n'
|
||||
f'# camera_source: {self.camera_source}\n'
|
||||
f'# views: {len(Rg)}\n'
|
||||
f'# utc: {datetime.now(timezone.utc).isoformat()}\n'
|
||||
f'# board: {self.board.cols}x{self.board.rows} '
|
||||
f'square_m={self.board.square_size}\n'
|
||||
)
|
||||
if self.gripper_tool and eye_to_hand:
|
||||
header += (
|
||||
f'# gripper_tool: {self.gripper_tool}\n'
|
||||
f'# T_4x4 = cam→基座（AX=XB 的手是 TCP，不折到法兰）\n'
|
||||
f'# 旧结果备份在 results/backup/\n'
|
||||
)
|
||||
elif self.gripper_tool:
|
||||
header += (
|
||||
f'# gripper_tool: {self.gripper_tool}\n'
|
||||
f'# T_4x4 = cam→法兰（AX=XB 在 TCP 上解，再左乘 T_flange_to_tool）\n'
|
||||
f'# T_cam_to_tool 是原始求解；旧的法兰/tool1 备份在 results/backup/\n'
|
||||
)
|
||||
self.result_file.parent.mkdir(parents=True, exist_ok=True)
|
||||
self.result_file.write_text(header + extra + yaml_txt, encoding='utf-8')
|
||||
(self.output_dir / 'captures.json').write_text(
|
||||
json.dumps(meta, indent=2), encoding='utf-8'
|
||||
)
|
||||
|
||||
self.get_logger().info('======== 手眼标定结果 ========')
|
||||
for line in (header + yaml_txt).strip().splitlines():
|
||||
self.get_logger().info(line)
|
||||
self.get_logger().info(f'已保存 -> {self.result_file}')
|
||||
|
||||
if self.cleanup_images:
|
||||
if self.images_dir.exists():
|
||||
shutil.rmtree(self.images_dir, ignore_errors=True)
|
||||
self.get_logger().info('标定图像已删除')
|
||||
else:
|
||||
self.get_logger().info(f'图像保留在 {self.images_dir}')
|
||||
return 0
|
||||
|
||||
def _load_poses(self) -> List[List[float]]:
|
||||
flat = list(self.get_parameter('waypoint_positions').value)
|
||||
n_j = len(self.joint_names)
|
||||
n_cfg = int(self.get_parameter('num_poses').value)
|
||||
if n_cfg < 1:
|
||||
return []
|
||||
need = n_cfg * n_j
|
||||
if len(flat) < need:
|
||||
self.get_logger().warn(
|
||||
f'waypoint_positions 长度 {len(flat)} < num_poses*{n_j}={need}；'
|
||||
'用零补齐'
|
||||
)
|
||||
flat = flat + [0.0] * (need - len(flat))
|
||||
poses: List[List[float]] = []
|
||||
for i in range(n_cfg):
|
||||
q = [float(v) for v in flat[i * n_j : (i + 1) * n_j]]
|
||||
if is_pose_filled(q):
|
||||
poses.append(q)
|
||||
else:
|
||||
self.get_logger().warn(f'位姿槽 {i} 为空（全零）—— 已跳过')
|
||||
return poses
|
||||
|
||||
def _resolve_intrinsics(self) -> bool:
|
||||
# 1) CameraInfo
|
||||
deadline = time.monotonic() + 3.0
|
||||
while rclpy.ok() and self._latest_info is None and time.monotonic() < deadline:
|
||||
rclpy.spin_once(self, timeout_sec=0.1)
|
||||
|
||||
if self._latest_info is not None:
|
||||
self._intrinsics = _camera_info_to_intrinsics(self._latest_info)
|
||||
K = self._intrinsics.K
|
||||
self.get_logger().info(
|
||||
f'内参来自 CameraInfo（{self._info_topic}）：'
|
||||
f'fx={K[0, 0]:.2f} fy={K[1, 1]:.2f} cx={K[0, 2]:.2f} cy={K[1, 2]:.2f}'
|
||||
)
|
||||
return True
|
||||
|
||||
# 2) RealSense EEPROM
|
||||
if is_realsense_source(self.camera_source):
|
||||
try:
|
||||
intr = query_realsense_color_intrinsics(
|
||||
self.camera_source,
|
||||
serial=self.camera_serial,
|
||||
width=self.rs_width,
|
||||
height=self.rs_height,
|
||||
)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().error(f'RealSense 内参查询失败：{exc}')
|
||||
else:
|
||||
self._intrinsics = CameraIntrinsics(K=intr.as_K(), dist=intr.as_dist())
|
||||
self.get_logger().info(
|
||||
f'内参来自 RealSense {intr.model.upper()} '
|
||||
f'serial={intr.serial} {intr.width}x{intr.height}：'
|
||||
f'fx={intr.fx:.2f} fy={intr.fy:.2f} '
|
||||
f'cx={intr.cx:.2f} cy={intr.cy:.2f}'
|
||||
)
|
||||
if (intr.width, intr.height) != (self.rs_width, self.rs_height):
|
||||
self.get_logger().warn(
|
||||
f'请求 {self.rs_width}x{self.rs_height}，'
|
||||
f'实际得到 {intr.width}x{intr.height}（最接近的 profile）'
|
||||
)
|
||||
return True
|
||||
|
||||
# 3) Manual YAML
|
||||
fx = float(self.get_parameter('camera_fx').value)
|
||||
fy = float(self.get_parameter('camera_fy').value)
|
||||
cx = float(self.get_parameter('camera_cx').value)
|
||||
cy = float(self.get_parameter('camera_cy').value)
|
||||
if fx <= 0 or fy <= 0:
|
||||
self.get_logger().error(
|
||||
'没有内参。请设置 camera_source:=d405|d435，'
|
||||
'或提供 camera_info_topic，或填写 camera_fx/fy/cx/cy。'
|
||||
)
|
||||
return False
|
||||
dist = np.array(
|
||||
list(self.get_parameter('camera_dist').value), dtype=float
|
||||
).reshape(-1, 1)
|
||||
K = np.array([[fx, 0.0, cx], [0.0, fy, cy], [0.0, 0.0, 1.0]], dtype=float)
|
||||
self._intrinsics = CameraIntrinsics(K=K, dist=dist)
|
||||
self.get_logger().info(
|
||||
f'内参来自 YAML：fx={fx:.2f} fy={fy:.2f} cx={cx:.2f} cy={cy:.2f}'
|
||||
)
|
||||
return True
|
||||
|
||||
def _log_progress(self, i: int, total: int, suffix: str = '') -> None:
|
||||
"""带进度条的日志。
|
||||
|
||||
自动标定是无人值守的长流程（十几个点位 × 移动+稳定+拍照），
|
||||
单看 `[3/12]` 很难有"还要多久"的直觉，加一条可视化进度条。
|
||||
"""
|
||||
frac = (i + 1) / max(1, total)
|
||||
width = 24
|
||||
filled = int(round(frac * width))
|
||||
bar = '█' * filled + '░' * (width - filled)
|
||||
self.get_logger().info(
|
||||
f'[{i + 1}/{total}] [{bar}] {frac * 100:5.1f}% {suffix}'
|
||||
)
|
||||
|
||||
def _wait_settled(self) -> bool:
|
||||
"""等机械臂真的停下来，再进入拍照。
|
||||
|
||||
判据是关节速度 |qd|_max 连续 ``settle_stable_sec`` 低于 ``settle_qd_max``。
|
||||
为什么不用固定 sleep：`settle_sec` 是拍脑袋的时长，臂负载/姿态不同，
|
||||
刹车距离差别很大 —— 等太短会在臂还在动时拍照（棋盘虚 → 角点抖动），
|
||||
等太长则整轮标定被无谓拉长。直接看速度更准。
|
||||
|
||||
超时不算致命：记一条警告后继续，让后面的棋盘稳定性检查去把关。
|
||||
"""
|
||||
if self.settle_qd_max <= 0.0:
|
||||
return True
|
||||
deadline = time.monotonic() + self.settle_timeout
|
||||
stable_since: Optional[float] = None
|
||||
while rclpy.ok() and time.monotonic() < deadline:
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
js = self._latest_js
|
||||
if js is None or not list(js.velocity):
|
||||
continue
|
||||
try:
|
||||
vmax = float(np.max(np.abs(np.asarray(js.velocity, dtype=float))))
|
||||
except Exception: # noqa: BLE001
|
||||
continue
|
||||
now = time.monotonic()
|
||||
if vmax <= self.settle_qd_max:
|
||||
if stable_since is None:
|
||||
stable_since = now
|
||||
elif now - stable_since >= self.settle_stable_sec:
|
||||
return True
|
||||
else:
|
||||
stable_since = None # 又动了，重新计时
|
||||
self.get_logger().warn(
|
||||
f'wait_settled 超时 ({self.settle_timeout:.1f}s)，臂可能仍在微动；'
|
||||
'交给棋盘稳定性检查兜底'
|
||||
)
|
||||
return False
|
||||
|
||||
def _pin_hold(self) -> None:
|
||||
"""静止后再保持几拍，抑制到位后的微小回弹/漂移。
|
||||
|
||||
语义取自 site.yaml 的注释"零速 pin 后再拍照"：不是重新规划轨迹，
|
||||
而是纯粹等一段时间让机械臂自己稳下来（控制器在到位后仍会做少量补偿）。
|
||||
"""
|
||||
total = self.pin_holds * self.pin_hold_sec
|
||||
end = time.monotonic() + total
|
||||
while rclpy.ok() and time.monotonic() < end:
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
|
||||
def _wait_stable(
|
||||
self,
|
||||
) -> Tuple[bool, float, Optional[np.ndarray], Optional[np.ndarray]]:
|
||||
self.stabilizer.reset()
|
||||
deadline = time.monotonic() + self.stable_timeout
|
||||
last_std = float('inf')
|
||||
last_corners = None
|
||||
last_pattern = None
|
||||
last_bgr = None
|
||||
last_log = 0.0
|
||||
while rclpy.ok() and time.monotonic() < deadline:
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
if self._latest_image is None:
|
||||
continue
|
||||
try:
|
||||
bgr = _image_to_bgr(self._latest_image)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().error(f'图像转换失败：{exc}')
|
||||
return False, last_std, None, None, None
|
||||
detected = detect_chessboard(bgr, self.board)
|
||||
corners = detected[0] if detected is not None else None
|
||||
pattern = detected[1] if detected is not None else None
|
||||
ok, std = self.stabilizer.push(corners)
|
||||
last_std = std
|
||||
if corners is not None:
|
||||
last_corners = corners
|
||||
last_pattern = pattern
|
||||
last_bgr = bgr
|
||||
now = time.monotonic()
|
||||
if now - last_log > 1.0:
|
||||
seen = '有' if corners is not None else '无'
|
||||
self.get_logger().info(
|
||||
f' 棋盘={seen} 角点标准差='
|
||||
f'{std if std < 1e8 else float("nan"):.3f}px'
|
||||
)
|
||||
last_log = now
|
||||
if ok:
|
||||
return True, std, last_corners, last_pattern, last_bgr
|
||||
return False, last_std, last_corners, last_pattern, last_bgr
|
||||
|
||||
|
||||
def _verify_joints(self, target: List[float], tol: float = 0.08) -> bool:
|
||||
"""确认 ``joint_states`` 真的走到了目标位置。
|
||||
|
||||
用来识破"假的成功"：有些执行接口会立刻返回成功，但关节其实没动
|
||||
（或只动了一半）。这里在 2 秒内轮询关节角，逐轴比对容差 ``tol``（弧度），
|
||||
只有全部落在容差内才算真到位。
|
||||
"""
|
||||
deadline = time.monotonic() + 2.0
|
||||
while rclpy.ok() and time.monotonic() < deadline:
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
if self._latest_js is None:
|
||||
continue
|
||||
name_to_q = dict(zip(list(self._latest_js.name), list(self._latest_js.position)))
|
||||
actual = []
|
||||
for n in self.joint_names:
|
||||
if n not in name_to_q:
|
||||
break
|
||||
actual.append(float(name_to_q[n]))
|
||||
if len(actual) != len(target):
|
||||
continue
|
||||
err = float(np.sqrt(np.mean([(a - b) ** 2 for a, b in zip(actual, target)])))
|
||||
self.get_logger().info(
|
||||
f' 关节相对目标的 RMSE：{err:.4f} rad（tol={tol}）'
|
||||
)
|
||||
return err <= tol
|
||||
self.get_logger().error('校验运动时没有 joint_states')
|
||||
return False
|
||||
|
||||
def _move_one(self, positions: List[float], duration: float) -> int:
|
||||
goal = FollowJointTrajectory.Goal()
|
||||
goal.joint_names = self.joint_names
|
||||
goal.num_points = 1
|
||||
goal.velocity_scale = self.velocity_scale
|
||||
goal.positions = list(positions)
|
||||
goal.time_from_start = [float(duration)]
|
||||
|
||||
send = self._client.send_goal_async(goal)
|
||||
if not self._wait_future(send, 10.0, '发送目标'):
|
||||
return 3
|
||||
handle = send.result()
|
||||
if handle is None or not handle.accepted:
|
||||
self.get_logger().error('目标被拒绝')
|
||||
return 4
|
||||
res_fut = handle.get_result_async()
|
||||
if not self._wait_future(res_fut, max(60.0, duration * 3.0), '结果'):
|
||||
return 5
|
||||
result = res_fut.result().result
|
||||
if not result.success:
|
||||
self.get_logger().error(f'移动失败：{result.message}')
|
||||
return 6
|
||||
return 0
|
||||
|
||||
def _wait_future(self, fut, timeout: float, label: str) -> bool:
|
||||
deadline = time.monotonic() + timeout
|
||||
while rclpy.ok() and not fut.done():
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
if time.monotonic() > deadline:
|
||||
self.get_logger().error(f'等待 {label} 超时')
|
||||
return False
|
||||
return fut.done()
|
||||
|
||||
def _read_ee_pose(self) -> Optional[PoseStamped]:
|
||||
if not self._get_state.wait_for_service(timeout_sec=2.0):
|
||||
return None
|
||||
fut = self._get_state.call_async(GetArmState.Request())
|
||||
if not self._wait_future(fut, 2.0, 'get_arm_state'):
|
||||
return None
|
||||
res = fut.result()
|
||||
if res is None or not res.success:
|
||||
return None
|
||||
return res.ee_pose
|
||||
|
||||
def _save_png(self, bgr: np.ndarray, path: Path) -> None:
|
||||
import cv2
|
||||
|
||||
if not cv2.imwrite(str(path), bgr):
|
||||
raise RuntimeError(f'cv2.imwrite 失败：{path}')
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = HandeyeCalibrate()
|
||||
code = 1
|
||||
try:
|
||||
code = node.run()
|
||||
except KeyboardInterrupt:
|
||||
code = 0
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
raise SystemExit(code)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,404 @@
|
||||
#!/usr/bin/env python3
|
||||
"""手眼标定辅助工具：棋盘稳定判定 + AX=XB 求解。
|
||||
|
||||
有 ``cv2.calibrateHandEye`` 时用它，否则用 Tsai-Lenz（NumPy 实现）。
|
||||
棋盘检测始终依赖 OpenCV。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from collections import deque
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Deque, List, Optional, Sequence, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
try:
|
||||
import cv2
|
||||
except Exception: # noqa: BLE001
|
||||
cv2 = None # type: ignore
|
||||
|
||||
|
||||
@dataclass
|
||||
class BoardConfig:
|
||||
cols: int # 内角点数（宽）
|
||||
rows: int # 内角点数（高）
|
||||
square_size: float # 米
|
||||
|
||||
|
||||
@dataclass
|
||||
class CameraIntrinsics:
|
||||
K: np.ndarray
|
||||
dist: np.ndarray
|
||||
|
||||
|
||||
def R_to_quat_xyzw(R: np.ndarray) -> Tuple[float, float, float, float]:
|
||||
"""3×3 旋转 → ``(x, y, z, w)`` 四元数（Shepperd 法，按最大分量选分支）。
|
||||
|
||||
为什么放在库里而不是各个 launch 里：这段换算**已经在两份 launch 里各抄了
|
||||
一遍**，而且抄错过（对角分支的公式被写歪，症状是手眼 TF 差了 30°，
|
||||
定位看起来"总能到附近"但永远差一点）。换算只有一份才可能被测试：
|
||||
``test_handeye_lib_roundtrip.py`` 拿真实标定文件验证
|
||||
``quat_xyzw_to_R(R_to_quat_xyzw(R)) == R``。
|
||||
"""
|
||||
R = np.asarray(R, dtype=float).reshape(3, 3)
|
||||
tr = float(R[0, 0] + R[1, 1] + R[2, 2])
|
||||
if tr > 0.0:
|
||||
s = math.sqrt(tr + 1.0) * 2.0
|
||||
w = 0.25 * s
|
||||
x = (R[2, 1] - R[1, 2]) / s
|
||||
y = (R[0, 2] - R[2, 0]) / s
|
||||
z = (R[1, 0] - R[0, 1]) / s
|
||||
elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
|
||||
s = math.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2.0
|
||||
w = (R[2, 1] - R[1, 2]) / s
|
||||
x = 0.25 * s
|
||||
y = (R[0, 1] + R[1, 0]) / s
|
||||
z = (R[0, 2] + R[2, 0]) / s
|
||||
elif R[1, 1] > R[2, 2]:
|
||||
s = math.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2.0
|
||||
w = (R[0, 2] - R[2, 0]) / s
|
||||
x = (R[0, 1] + R[1, 0]) / s
|
||||
y = 0.25 * s
|
||||
z = (R[1, 2] + R[2, 1]) / s
|
||||
else:
|
||||
s = math.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2.0
|
||||
w = (R[1, 0] - R[0, 1]) / s
|
||||
x = (R[0, 2] + R[2, 0]) / s
|
||||
y = (R[1, 2] + R[2, 1]) / s
|
||||
z = 0.25 * s
|
||||
n = math.sqrt(x * x + y * y + z * z + w * w)
|
||||
if n <= 0.0:
|
||||
raise ValueError("旋转矩阵无法转成四元数（矩阵为零？）")
|
||||
return x / n, y / n, z / n, w / n
|
||||
|
||||
|
||||
def read_transform_yaml(path: str | Path) -> Tuple[np.ndarray, Tuple[float, float, float, float]]:
|
||||
"""读手眼标定产物 → ``(xyz_m, quat_xyzw)``。
|
||||
|
||||
文件格式见 :func:`format_transform_yaml`：``T_4x4`` 是 4 行 4 列的列表。
|
||||
读取时**必须校验形状**：手眼文件是现场人手拷来拷去的，形状不对时
|
||||
直接报错，比"取到一堆 nan 然后发了个全零 TF"好得多（后者会让定位
|
||||
永远差一个常量，而且看起来像相机标定不准）。
|
||||
"""
|
||||
import yaml # 局部导入：本模块其它函数不需要 yaml（标定计算本身不依赖）
|
||||
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
data = yaml.safe_load(f) or {}
|
||||
T = data.get("T_4x4")
|
||||
if T is None:
|
||||
raise ValueError(f"{path}: 缺少 T_4x4 字段")
|
||||
arr = np.asarray(T, dtype=float)
|
||||
if arr.shape != (4, 4):
|
||||
raise ValueError(f"{path}: T_4x4 应为 4x4，实际 {arr.shape}")
|
||||
xyz = np.array([arr[0, 3], arr[1, 3], arr[2, 3]], dtype=float)
|
||||
return xyz, R_to_quat_xyzw(arr[:3, :3])
|
||||
|
||||
|
||||
def quat_xyzw_to_R(q: Sequence[float]) -> np.ndarray:
|
||||
x, y, z, w = [float(v) for v in q]
|
||||
n = np.sqrt(x * x + y * y + z * z + w * w) + 1e-12
|
||||
x, y, z, w = x / n, y / n, z / n, w / n
|
||||
return np.array(
|
||||
[
|
||||
[1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
|
||||
[2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
|
||||
[2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
|
||||
|
||||
def pose_to_Rt(
|
||||
xyz: Sequence[float], quat_xyzw: Sequence[float]
|
||||
) -> Tuple[np.ndarray, np.ndarray]:
|
||||
R = quat_xyzw_to_R(quat_xyzw)
|
||||
t = np.array([[float(xyz[0])], [float(xyz[1])], [float(xyz[2])]], dtype=float)
|
||||
return R, t
|
||||
|
||||
|
||||
def Rt_to_T(R: np.ndarray, t: np.ndarray) -> np.ndarray:
|
||||
T = np.eye(4)
|
||||
T[:3, :3] = R
|
||||
T[:3, 3] = t.reshape(3)
|
||||
return T
|
||||
|
||||
|
||||
def cam_to_base_T(
|
||||
T_handeye: np.ndarray, T_ee: np.ndarray, mode: str
|
||||
) -> np.ndarray:
|
||||
"""相机系 → 基座系的 4×4。
|
||||
|
||||
* ``eye_in_hand``：``T_handeye`` 是 cam→法兰，再左乘 ``T_ee``（base→法兰）。
|
||||
* ``eye_to_hand``：``T_handeye`` **已经是** cam→基座。再乘 ``T_ee`` 会把
|
||||
固定相机错挂到法兰上 —— 定位总能到附近、永远差一截，看起来像标定不准。
|
||||
"""
|
||||
T_he = np.asarray(T_handeye, dtype=float).reshape(4, 4)
|
||||
tag = str(mode or "").strip().lower()
|
||||
if tag == "eye_to_hand":
|
||||
return T_he
|
||||
if tag != "eye_in_hand":
|
||||
raise ValueError(
|
||||
f"手眼模式只能是 eye_in_hand 或 eye_to_hand，收到 {mode!r}"
|
||||
)
|
||||
return np.asarray(T_ee, dtype=float).reshape(4, 4) @ T_he
|
||||
|
||||
|
||||
def cam_point_to_base(
|
||||
p_cam: np.ndarray,
|
||||
T_handeye: np.ndarray,
|
||||
T_ee: np.ndarray,
|
||||
mode: str,
|
||||
) -> np.ndarray:
|
||||
"""工件三维点（相机系）→ 基座系 XYZ。"""
|
||||
T_bc = cam_to_base_T(T_handeye, T_ee, mode)
|
||||
xyz = np.asarray(p_cam, dtype=float).reshape(3)
|
||||
ph = np.array([float(xyz[0]), float(xyz[1]), float(xyz[2]), 1.0], dtype=float)
|
||||
return (T_bc @ ph)[:3]
|
||||
|
||||
|
||||
def invert_Rt(R: np.ndarray, t: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
|
||||
Ri = R.T
|
||||
ti = -Ri @ t.reshape(3, 1)
|
||||
return Ri, ti
|
||||
|
||||
|
||||
def board_object_points(cfg: BoardConfig) -> np.ndarray:
|
||||
obj = np.zeros((cfg.rows * cfg.cols, 3), np.float32)
|
||||
xs, ys = np.meshgrid(np.arange(cfg.cols), np.arange(cfg.rows))
|
||||
obj[:, 0] = xs.reshape(-1) * cfg.square_size
|
||||
obj[:, 1] = ys.reshape(-1) * cfg.square_size
|
||||
return obj
|
||||
|
||||
|
||||
def detect_chessboard(
|
||||
image_bgr: np.ndarray, cfg: BoardConfig
|
||||
) -> Optional[Tuple[np.ndarray, Tuple[int, int]]]:
|
||||
"""返回 (角点, pattern_size) 或 None。pattern 为实际使用的 (cols, rows)。"""
|
||||
if cv2 is None:
|
||||
raise RuntimeError('棋盘检测需要 OpenCV（cv2）')
|
||||
gray = cv2.cvtColor(image_bgr, cv2.COLOR_BGR2GRAY)
|
||||
pattern = (int(cfg.cols), int(cfg.rows))
|
||||
flags = cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_NORMALIZE_IMAGE
|
||||
ok, corners = cv2.findChessboardCorners(gray, pattern, flags)
|
||||
used = pattern
|
||||
if not ok:
|
||||
ok, corners = cv2.findChessboardCorners(
|
||||
gray, (pattern[1], pattern[0]), flags
|
||||
)
|
||||
used = (pattern[1], pattern[0])
|
||||
if not ok or corners is None:
|
||||
return None
|
||||
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 40, 1e-3)
|
||||
corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
|
||||
return np.asarray(corners, dtype=np.float32), used
|
||||
|
||||
|
||||
def solve_board_pose(
|
||||
corners: np.ndarray,
|
||||
cfg: BoardConfig,
|
||||
cam: CameraIntrinsics,
|
||||
pattern: Optional[Tuple[int, int]] = None,
|
||||
) -> Tuple[np.ndarray, np.ndarray]:
|
||||
if cv2 is None:
|
||||
raise RuntimeError('solvePnP 需要 OpenCV（cv2）')
|
||||
if pattern is None:
|
||||
pattern = (cfg.cols, cfg.rows)
|
||||
board = BoardConfig(pattern[0], pattern[1], cfg.square_size)
|
||||
obj = board_object_points(board)
|
||||
ok, rvec, tvec = cv2.solvePnP(
|
||||
obj, corners, cam.K, cam.dist, flags=cv2.SOLVEPNP_ITERATIVE
|
||||
)
|
||||
if not ok:
|
||||
raise RuntimeError('solvePnP 失败')
|
||||
R, _ = cv2.Rodrigues(rvec)
|
||||
return R.astype(float), np.asarray(tvec, dtype=float).reshape(3, 1)
|
||||
|
||||
|
||||
class ChessboardStabilizer:
|
||||
def __init__(self, window: int = 8, max_pixel_std: float = 0.45) -> None:
|
||||
self.window = int(window)
|
||||
self.max_pixel_std = float(max_pixel_std)
|
||||
self._buf: Deque[np.ndarray] = deque(maxlen=self.window)
|
||||
|
||||
def reset(self) -> None:
|
||||
self._buf.clear()
|
||||
|
||||
def push(self, corners: Optional[np.ndarray]) -> Tuple[bool, float]:
|
||||
if corners is None:
|
||||
self._buf.clear()
|
||||
return False, float('inf')
|
||||
self._buf.append(corners.reshape(-1, 2).astype(float))
|
||||
if len(self._buf) < self.window:
|
||||
return False, float('inf')
|
||||
stack = np.stack(list(self._buf), axis=0)
|
||||
std = float(np.mean(np.std(stack, axis=0)))
|
||||
return std <= self.max_pixel_std, std
|
||||
|
||||
|
||||
def _skew(v: np.ndarray) -> np.ndarray:
|
||||
x, y, z = v.reshape(3)
|
||||
return np.array([[0.0, -z, y], [z, 0.0, -x], [-y, x, 0.0]])
|
||||
|
||||
|
||||
def _mat_log(R: np.ndarray) -> np.ndarray:
|
||||
c = float(np.clip((np.trace(R) - 1.0) * 0.5, -1.0, 1.0))
|
||||
theta = float(np.arccos(c))
|
||||
if theta < 1e-8:
|
||||
return np.zeros(3)
|
||||
w = np.array(
|
||||
[R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]], dtype=float
|
||||
)
|
||||
return w * (theta / (2.0 * np.sin(theta)))
|
||||
|
||||
|
||||
def _rodrigues(rvec: np.ndarray) -> np.ndarray:
|
||||
r = rvec.reshape(3)
|
||||
theta = float(np.linalg.norm(r))
|
||||
if theta < 1e-12:
|
||||
return np.eye(3)
|
||||
k = r / theta
|
||||
K = _skew(k)
|
||||
return np.eye(3) + np.sin(theta) * K + (1.0 - np.cos(theta)) * (K @ K)
|
||||
|
||||
|
||||
def calibrate_hand_eye_tsai(
|
||||
R_gripper2base: List[np.ndarray],
|
||||
t_gripper2base: List[np.ndarray],
|
||||
R_target2cam: List[np.ndarray],
|
||||
t_target2cam: List[np.ndarray],
|
||||
) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""Tsai-Lenz 方法。返回 R_cam2gripper、t_cam2gripper。"""
|
||||
n = len(R_gripper2base)
|
||||
if n < 3:
|
||||
raise ValueError('至少需要 3 个位姿')
|
||||
|
||||
Hg = [Rt_to_T(R, t) for R, t in zip(R_gripper2base, t_gripper2base)]
|
||||
Hc = [Rt_to_T(R, t) for R, t in zip(R_target2cam, t_target2cam)]
|
||||
|
||||
A_rows: List[np.ndarray] = []
|
||||
b_rows: List[np.ndarray] = []
|
||||
for i in range(n - 1):
|
||||
A = np.linalg.inv(Hg[i + 1]) @ Hg[i]
|
||||
B = Hc[i + 1] @ np.linalg.inv(Hc[i])
|
||||
ra = _mat_log(A[:3, :3])
|
||||
rb = _mat_log(B[:3, :3])
|
||||
A_rows.append(_skew(ra + rb))
|
||||
b_rows.append(rb - ra)
|
||||
A_mat = np.vstack(A_rows)
|
||||
b_vec = np.concatenate(b_rows)
|
||||
p, _, _, _ = np.linalg.lstsq(A_mat, b_vec, rcond=None)
|
||||
denom = np.sqrt(1.0 + float(p @ p))
|
||||
P = 2.0 * p / denom
|
||||
Rx = _rodrigues(P)
|
||||
|
||||
C_rows: List[np.ndarray] = []
|
||||
d_rows: List[np.ndarray] = []
|
||||
for i in range(n - 1):
|
||||
A = np.linalg.inv(Hg[i + 1]) @ Hg[i]
|
||||
B = Hc[i + 1] @ np.linalg.inv(Hc[i])
|
||||
Ra, ta = A[:3, :3], A[:3, 3:4]
|
||||
_Rb, tb = B[:3, :3], B[:3, 3:4]
|
||||
C_rows.append(Ra - np.eye(3))
|
||||
d_rows.append(Rx @ tb - ta)
|
||||
C = np.vstack(C_rows)
|
||||
d = np.vstack(d_rows)
|
||||
tx, _, _, _ = np.linalg.lstsq(C, d, rcond=None)
|
||||
return Rx.astype(float), tx.reshape(3, 1).astype(float)
|
||||
|
||||
|
||||
def calibrate_hand_eye(
|
||||
R_gripper2base: List[np.ndarray],
|
||||
t_gripper2base: List[np.ndarray],
|
||||
R_target2cam: List[np.ndarray],
|
||||
t_target2cam: List[np.ndarray],
|
||||
*,
|
||||
eye_to_hand: bool = False,
|
||||
) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""
|
||||
eye_in_hand -> X = cam -> gripper（相机装在法兰上）
|
||||
eye_to_hand -> X = cam -> base（相机固定安装）
|
||||
"""
|
||||
Rg = list(R_gripper2base)
|
||||
tg = list(t_gripper2base)
|
||||
if eye_to_hand:
|
||||
Rg = []
|
||||
tg = []
|
||||
for R, t in zip(R_gripper2base, t_gripper2base):
|
||||
Ri, ti = invert_Rt(R, t)
|
||||
Rg.append(Ri)
|
||||
tg.append(ti)
|
||||
|
||||
if cv2 is not None and hasattr(cv2, 'calibrateHandEye'):
|
||||
try:
|
||||
R, t = cv2.calibrateHandEye(
|
||||
Rg, tg, R_target2cam, t_target2cam, method=cv2.CALIB_HAND_EYE_TSAI
|
||||
)
|
||||
return np.asarray(R, float), np.asarray(t, float).reshape(3, 1)
|
||||
except Exception:
|
||||
pass # 退回 NumPy 版 Tsai
|
||||
return calibrate_hand_eye_tsai(Rg, tg, R_target2cam, t_target2cam)
|
||||
|
||||
|
||||
def T_from_flexiv_tcp(xyz_m, qwxyz) -> np.ndarray:
|
||||
"""Studio ``tcp_location`` → 法兰→工具 4×4。
|
||||
|
||||
Flexiv 顺序是 ``[x, y, z, q_w, q_x, q_y, q_z]``，四元数是 wxyz，
|
||||
和本仓库其余地方的 xyzw 不一样。只在这里换一次。
|
||||
"""
|
||||
xyz = np.asarray(xyz_m, dtype=float).reshape(3)
|
||||
q = np.asarray(qwxyz, dtype=float).reshape(7) if np.asarray(qwxyz).size == 7 else np.asarray(qwxyz, dtype=float).reshape(4)
|
||||
if q.size == 7:
|
||||
qw, qx, qy, qz = (float(q[3]), float(q[4]), float(q[5]), float(q[6]))
|
||||
else:
|
||||
qw, qx, qy, qz = (float(q[0]), float(q[1]), float(q[2]), float(q[3]))
|
||||
R = quat_xyzw_to_R((qx, qy, qz, qw))
|
||||
return Rt_to_T(R, xyz.reshape(3, 1))
|
||||
|
||||
|
||||
def cam_to_flange_via_tool(T_cam_to_tool: np.ndarray, T_flange_to_tool: np.ndarray) -> np.ndarray:
|
||||
"""AX=XB 在工具 TCP 上解出 cam→工具后，再折成 cam→法兰。
|
||||
|
||||
``T_flange_to_tool`` 是 Studio TCP：``p_法兰 = T_flange_to_tool · p_工具``。
|
||||
OpenCV 的 ``T_cam_to_tool`` 是 ``p_工具 = T_cam_to_tool · p_相机``。
|
||||
所以 ``T_cam_to_flange = T_flange_to_tool · T_cam_to_tool``。
|
||||
|
||||
乘反了（``T_cam_to_tool · inv(T_ft)``）旋转看着还像，平移会把相机
|
||||
扔到法兰后方约 30 cm。忘了乘则 ``T_4x4`` 停在 TCP 上，沿工具偏 ~225 mm。
|
||||
两种都不报错。
|
||||
"""
|
||||
T_ct = np.asarray(T_cam_to_tool, dtype=float).reshape(4, 4)
|
||||
T_ft = np.asarray(T_flange_to_tool, dtype=float).reshape(4, 4)
|
||||
return T_ft @ T_ct
|
||||
|
||||
|
||||
def format_transform_yaml(R: np.ndarray, t: np.ndarray, name: str) -> str:
|
||||
T = Rt_to_T(R, t)
|
||||
lines = [
|
||||
f'# {name}',
|
||||
f'translation_m: [{t[0, 0]:.8f}, {t[1, 0]:.8f}, {t[2, 0]:.8f}]',
|
||||
'T_4x4:',
|
||||
]
|
||||
for row in T:
|
||||
lines.append(' - [' + ', '.join(f'{v:.8f}' for v in row) + ']')
|
||||
return '\n'.join(lines) + '\n'
|
||||
|
||||
|
||||
def format_matrix_yaml(T: np.ndarray, key: str) -> str:
|
||||
arr = np.asarray(T, dtype=float).reshape(4, 4)
|
||||
lines = [f'{key}:']
|
||||
for row in arr:
|
||||
lines.append(' - [' + ', '.join(f'{v:.8f}' for v in row) + ']')
|
||||
return '\n'.join(lines) + '\n'
|
||||
|
||||
|
||||
def is_pose_filled(joints: Sequence[float]) -> bool:
|
||||
"""若全为零 / NaN 占位则返回 False。"""
|
||||
arr = np.asarray(joints, dtype=float)
|
||||
if arr.size != 7:
|
||||
return False
|
||||
if np.any(np.isnan(arr)):
|
||||
return False
|
||||
return float(np.linalg.norm(arr)) > 1e-6
|
||||
+251
@@ -0,0 +1,251 @@
|
||||
#!/usr/bin/env python3
|
||||
"""通过 pyrealsense2 查询 RealSense（D405 / D435）的出厂彩色内参。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import List, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass
|
||||
class RealsenseIntrinsics:
|
||||
model: str
|
||||
serial: str
|
||||
width: int
|
||||
height: int
|
||||
fx: float
|
||||
fy: float
|
||||
cx: float
|
||||
cy: float
|
||||
dist: List[float] # OpenCV 顺序，长度 5
|
||||
|
||||
def as_K(self) -> np.ndarray:
|
||||
return np.array(
|
||||
[[self.fx, 0.0, self.cx], [0.0, self.fy, self.cy], [0.0, 0.0, 1.0]],
|
||||
dtype=float,
|
||||
)
|
||||
|
||||
def as_dist(self) -> np.ndarray:
|
||||
d = list(self.dist) + [0.0] * 5
|
||||
return np.array(d[:5], dtype=float).reshape(-1, 1)
|
||||
|
||||
|
||||
# camera_source 为 d405/d435 时的默认 ROS 话题命名空间。
|
||||
REALSENSE_TOPIC_DEFAULTS = {
|
||||
'd405': {
|
||||
'image_topic': '/d405/color/image_raw',
|
||||
'camera_info_topic': '/d405/color/camera_info',
|
||||
},
|
||||
'd435': {
|
||||
'image_topic': '/d435/color/image_raw',
|
||||
'camera_info_topic': '/d435/color/camera_info',
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def _norm_model(model: str) -> str:
|
||||
m = model.strip().lower().replace(' ', '').replace('-', '')
|
||||
if m.startswith('d405') or m in ('realsensed405', 'intelrealsensed405'):
|
||||
return 'd405'
|
||||
if m.startswith('d435') or m in ('realsensed435', 'intelrealsensed435'):
|
||||
return 'd435'
|
||||
return m
|
||||
|
||||
|
||||
def list_realsense_devices() -> List[Tuple[str, str]]:
|
||||
"""返回 [(name, serial), ...]。"""
|
||||
try:
|
||||
import pyrealsense2 as rs
|
||||
except Exception as exc: # noqa: BLE001
|
||||
raise RuntimeError(
|
||||
'未安装 pyrealsense2。'
|
||||
'请在 ros_flexiv 环境中安装：pip/conda install pyrealsense2'
|
||||
) from exc
|
||||
out: List[Tuple[str, str]] = []
|
||||
for d in rs.context().devices:
|
||||
name = d.get_info(rs.camera_info.name)
|
||||
serial = d.get_info(rs.camera_info.serial_number)
|
||||
out.append((name, serial))
|
||||
return out
|
||||
|
||||
|
||||
def query_realsense_color_intrinsics(
|
||||
model: str,
|
||||
*,
|
||||
serial: str = '',
|
||||
width: int = 1280,
|
||||
height: int = 720,
|
||||
) -> RealsenseIntrinsics:
|
||||
"""
|
||||
在给定分辨率下读取 D405/D435 设备上的彩色内参。
|
||||
|
||||
不会启动流管道（只读取 profile 元数据）。
|
||||
"""
|
||||
try:
|
||||
import pyrealsense2 as rs
|
||||
except Exception as exc: # noqa: BLE001
|
||||
raise RuntimeError(
|
||||
'未安装 pyrealsense2。'
|
||||
'请在 ros_flexiv 环境中安装：pip/conda install pyrealsense2'
|
||||
) from exc
|
||||
|
||||
want = _norm_model(model)
|
||||
if want not in ('d405', 'd435'):
|
||||
raise ValueError(f'不支持的型号 {model!r}；请使用 d405 或 d435')
|
||||
|
||||
hint = want.upper() # D405 / D435
|
||||
serial = (serial or '').strip()
|
||||
devices = list(rs.context().devices)
|
||||
if not devices:
|
||||
raise RuntimeError('未连接任何 RealSense 设备')
|
||||
|
||||
chosen = None
|
||||
for d in devices:
|
||||
name = d.get_info(rs.camera_info.name)
|
||||
sn = d.get_info(rs.camera_info.serial_number)
|
||||
if serial and sn != serial:
|
||||
continue
|
||||
name_u = name.upper().replace(' ', '')
|
||||
if hint in name_u or hint in name.upper():
|
||||
chosen = d
|
||||
break
|
||||
|
||||
if chosen is None:
|
||||
avail = ', '.join(
|
||||
f'{d.get_info(rs.camera_info.name)}'
|
||||
f'({d.get_info(rs.camera_info.serial_number)})'
|
||||
for d in devices
|
||||
)
|
||||
raise RuntimeError(
|
||||
f'没有匹配 model={want!r} serial={serial!r} 的 RealSense。'
|
||||
f'已连接的设备：{avail or "无"}'
|
||||
)
|
||||
|
||||
name = chosen.get_info(rs.camera_info.name)
|
||||
sn = chosen.get_info(rs.camera_info.serial_number)
|
||||
|
||||
best = None # (评分, width, height, fx, fy, cx, cy, 畸变系数)
|
||||
for s in chosen.sensors:
|
||||
for p in s.profiles:
|
||||
if p.stream_type() != rs.stream.color:
|
||||
continue
|
||||
vp = p.as_video_stream_profile()
|
||||
try:
|
||||
intr = vp.get_intrinsics()
|
||||
except Exception: # noqa: BLE001
|
||||
continue
|
||||
w, h = int(intr.width), int(intr.height)
|
||||
if w == int(width) and h == int(height):
|
||||
score = 0
|
||||
else:
|
||||
score = abs(w * h - int(width) * int(height)) + 1
|
||||
cand = (
|
||||
score,
|
||||
w,
|
||||
h,
|
||||
float(intr.fx),
|
||||
float(intr.fy),
|
||||
float(intr.ppx),
|
||||
float(intr.ppy),
|
||||
[float(c) for c in list(intr.coeffs)[:5]],
|
||||
)
|
||||
if best is None or cand[0] < best[0]:
|
||||
best = cand
|
||||
|
||||
if best is None:
|
||||
raise RuntimeError(f'{name} ({sn}) 上没有彩色流 profile')
|
||||
|
||||
_, w, h, fx, fy, cx, cy, coeffs = best
|
||||
while len(coeffs) < 5:
|
||||
coeffs.append(0.0)
|
||||
return RealsenseIntrinsics(
|
||||
model=want,
|
||||
serial=sn,
|
||||
width=w,
|
||||
height=h,
|
||||
fx=fx,
|
||||
fy=fy,
|
||||
cx=cx,
|
||||
cy=cy,
|
||||
dist=coeffs[:5],
|
||||
)
|
||||
|
||||
|
||||
def is_realsense_source(name: str) -> bool:
|
||||
"""对 d405 / d435 及带命名空间的别名（d405_left、d405_right）返回 True。"""
|
||||
return _norm_model(name or "") in ("d405", "d435")
|
||||
|
||||
|
||||
def resolve_realsense_topics(
|
||||
model: str,
|
||||
*,
|
||||
camera_namespace: str = '',
|
||||
image_topic: str = '',
|
||||
camera_info_topic: str = '',
|
||||
) -> Tuple[str, str]:
|
||||
"""为空时，为 d405/d435 填充默认的 image/camera_info 话题。
|
||||
|
||||
``model`` 可以是 ``d405_left`` / ``d405_right`` 这类话题命名空间。
|
||||
"""
|
||||
ns = (camera_namespace or model or 'd405').strip().strip('/')
|
||||
img = (image_topic or '').strip()
|
||||
info = (camera_info_topic or '').strip()
|
||||
if not img and ns:
|
||||
img = f'/{ns}/color/image_raw'
|
||||
if not info and ns:
|
||||
info = f'/{ns}/color/camera_info'
|
||||
return img, info
|
||||
|
||||
|
||||
def apply_realsense_exposure(
|
||||
device,
|
||||
*,
|
||||
auto_exposure: bool = True,
|
||||
ae_limit_us: float = 3000.0,
|
||||
emitter_enabled: bool = False,
|
||||
laser_power: float = 0.0,
|
||||
exposure_us: float | None = None,
|
||||
gain: float | None = None,
|
||||
log=None,
|
||||
) -> list[str]:
|
||||
"""限制自动曝光 / 关闭投影器。腕部 D405 过曝通常是自动曝光过长 + 发射器导致。"""
|
||||
import pyrealsense2 as rs
|
||||
|
||||
notes: list[str] = []
|
||||
|
||||
def _set(sensor, opt, val) -> bool:
|
||||
try:
|
||||
if sensor.supports(opt):
|
||||
sensor.set_option(opt, float(val))
|
||||
return True
|
||||
except Exception:
|
||||
return False
|
||||
return False
|
||||
|
||||
for sensor in device.query_sensors():
|
||||
if _set(sensor, rs.option.emitter_enabled, 1.0 if emitter_enabled else 0.0):
|
||||
notes.append(f"emitter={int(bool(emitter_enabled))}")
|
||||
laser_opt = getattr(rs.option, "laser_power", None)
|
||||
if laser_opt is not None and not emitter_enabled:
|
||||
_set(sensor, laser_opt, float(laser_power))
|
||||
# 必须在写曝光*之前*关闭自动曝光；D405 固件在自动曝光开启时会忽略曝光设置。
|
||||
if _set(sensor, rs.option.enable_auto_exposure, 1.0 if auto_exposure else 0.0):
|
||||
notes.append(f"ae={int(bool(auto_exposure))}")
|
||||
pri = getattr(rs.option, "auto_exposure_priority", None)
|
||||
# priority 0 = 保帧压暗；无上限时不要设，让自动曝光自己变亮
|
||||
if pri is not None and ae_limit_us and auto_exposure:
|
||||
_set(sensor, pri, 0.0)
|
||||
limit = getattr(rs.option, "auto_exposure_limit", None)
|
||||
if auto_exposure and limit is not None and ae_limit_us and ae_limit_us > 0:
|
||||
if _set(sensor, limit, float(ae_limit_us)):
|
||||
notes.append(f"ae_limit={int(ae_limit_us)}us")
|
||||
if not auto_exposure:
|
||||
if exposure_us is not None and _set(sensor, rs.option.exposure, float(exposure_us)):
|
||||
notes.append(f"exposure={int(exposure_us)}us")
|
||||
if gain is not None and _set(sensor, rs.option.gain, float(gain)):
|
||||
notes.append(f"gain={gain}")
|
||||
if log and notes:
|
||||
log("RealSense 曝光设置：" + ", ".join(dict.fromkeys(notes)))
|
||||
return notes
|
||||
+679
@@ -0,0 +1,679 @@
|
||||
#!/usr/bin/env python3
|
||||
"""手眼示教：实时预览 + Free Drive + 按 Enter 拍照/记录关节。
|
||||
|
||||
两种后端
|
||||
------------
|
||||
ros（默认，整套栈正在运行）：
|
||||
- 关节来自 ``/get_arm_state``
|
||||
- 拖拽：示教器 Free Drive + 按住使能键
|
||||
需保持：bash ~/ros2/scripts/run_real.sh
|
||||
|
||||
rdk（软件 Free Drive）：
|
||||
- 先停掉 run_real.sh（RDK 独占）
|
||||
- 按 'd' 通过 RDK 启用 FloatingJoint 拖拽
|
||||
- 关节来自 RDK 的 robot.states
|
||||
|
||||
source ~/ros2/scripts/env.sh
|
||||
ros2 run arm_application handeye_teach --ros-args -p backend:=rdk
|
||||
|
||||
界面按键（预览窗口）
|
||||
Enter / Space 拍照 + 记录关节
|
||||
d 启用 Free Drive（rdk）/ 显示示教器提示（ros）
|
||||
s 停止 Free Drive（rdk）/ 保持
|
||||
q / Esc 结束
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import threading
|
||||
import time
|
||||
from datetime import datetime, timezone
|
||||
from pathlib import Path
|
||||
from typing import List, Optional
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rcl_interfaces.msg import ParameterDescriptor
|
||||
from rclpy.node import Node
|
||||
|
||||
from arm_application.ros_node import TolerantNode, param_as_str
|
||||
|
||||
HELP = """
|
||||
================================================
|
||||
手眼示教 TEACH：预览 -> 拖拽 -> 拍照+位姿
|
||||
================================================
|
||||
窗口按键：
|
||||
Enter / Space = 拍照 + 保存关节
|
||||
d = 开启 Free Drive
|
||||
s = 关闭 Free Drive / 保持
|
||||
q / Esc = 结束
|
||||
提示：让棋盘清晰，并占到画面的约 30-70%
|
||||
建议采 8-12 个角度有变化的位姿
|
||||
================================================
|
||||
"""
|
||||
|
||||
|
||||
def _setup_matplotlib_font():
|
||||
"""避免中文乱码：优先选用存在的中日韩（CJK）字体。"""
|
||||
import matplotlib
|
||||
|
||||
matplotlib.use('QtAgg')
|
||||
from matplotlib import font_manager, rcParams
|
||||
|
||||
candidates = [
|
||||
'Noto Sans CJK SC',
|
||||
'Noto Sans CJK JP',
|
||||
'Noto Serif CJK SC',
|
||||
'AR PL UMing CN',
|
||||
'AR PL UKai CN',
|
||||
'WenQuanYi Micro Hei',
|
||||
'Source Han Sans CN',
|
||||
'DejaVu Sans',
|
||||
]
|
||||
available = {f.name for f in font_manager.fontManager.ttflist}
|
||||
chosen = None
|
||||
for name in candidates:
|
||||
if name in available:
|
||||
chosen = name
|
||||
break
|
||||
if chosen is None:
|
||||
# 按文件路径再试一次
|
||||
for path in (
|
||||
'/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc',
|
||||
'/usr/share/fonts/opentype/noto/NotoSansCJK-Medium.ttc',
|
||||
'/usr/share/fonts/opentype/noto/NotoSansCJK-DemiLight.ttc',
|
||||
'/usr/share/fonts/truetype/arphic/uming.ttc',
|
||||
):
|
||||
if os.path.isfile(path):
|
||||
font_manager.fontManager.addfont(path)
|
||||
prop = font_manager.FontProperties(fname=path)
|
||||
chosen = prop.get_name()
|
||||
break
|
||||
if chosen:
|
||||
rcParams['font.family'] = chosen
|
||||
rcParams['axes.unicode_minus'] = False
|
||||
return chosen
|
||||
|
||||
|
||||
class RdkBridge:
|
||||
"""通过 rdk_py310 子进程（Python 3.10 ABI）与 Flexiv RDK 通信。"""
|
||||
|
||||
def __init__(self, robot_sn: str, logger=None) -> None:
|
||||
import json
|
||||
import subprocess
|
||||
|
||||
self._json = json
|
||||
self._logger = logger
|
||||
self.robot_sn = robot_sn
|
||||
self._last_q: Optional[List[float]] = None
|
||||
self._io = threading.Lock()
|
||||
from arm_application.site_io import ros2_root
|
||||
|
||||
py = os.environ.get('RDK_PYTHON', '').strip() or str(
|
||||
Path.home() / 'rdk_py310' / 'bin' / 'python'
|
||||
)
|
||||
bridge = ros2_root() / 'scripts' / 'rdk_drag_bridge.py'
|
||||
if not bridge.is_file():
|
||||
raise FileNotFoundError(f'找不到 rdk_drag_bridge.py：{bridge}')
|
||||
|
||||
# 只保留 Flexiv 的库 —— 不要继承 conda 的 LD_LIBRARY_PATH（会让 RDK 卡死）。
|
||||
flexiv_lib = os.path.expanduser('~/flexiv_install/lib')
|
||||
env = {
|
||||
'HOME': os.environ.get('HOME', ''),
|
||||
'PATH': f'{Path(py).parent}:/usr/bin:/bin',
|
||||
'LD_LIBRARY_PATH': flexiv_lib,
|
||||
'LANG': 'C.UTF-8',
|
||||
'SITE_PC_IP': os.environ.get('SITE_PC_IP', ''),
|
||||
'SITE_NET_IFACE': os.environ.get('SITE_NET_IFACE', ''),
|
||||
}
|
||||
# 不要继承 conda 的 PYTHONPATH。
|
||||
# 重要：stderr 不能用 PIPE —— RDK 日志会填满管道并让桥接进程死锁
|
||||
# （stdout 永远不返回）。改成写入日志文件。
|
||||
log_path = Path('/tmp/rdk_drag_bridge.log')
|
||||
self._err_file = open(log_path, 'w', encoding='utf-8')
|
||||
self._proc = subprocess.Popen(
|
||||
[py, str(bridge)],
|
||||
stdin=subprocess.PIPE,
|
||||
stdout=subprocess.PIPE,
|
||||
stderr=self._err_file,
|
||||
text=True,
|
||||
bufsize=1,
|
||||
env=env,
|
||||
)
|
||||
if logger:
|
||||
logger.info(f'RDK 桥接已启动；日志 → {log_path}')
|
||||
pong = self._ask({'cmd': 'ping'}, timeout=8.0)
|
||||
if not pong.get('ok'):
|
||||
raise RuntimeError(
|
||||
f'RDK 桥接 ping 失败：{pong}。参见 {log_path}'
|
||||
)
|
||||
if logger:
|
||||
logger.info('RDK 桥接 ping 正常，正在连接机器人...')
|
||||
conn = self._ask(
|
||||
{
|
||||
'cmd': 'connect',
|
||||
'sn': robot_sn,
|
||||
'iface': os.environ.get('SITE_NET_IFACE', ''),
|
||||
},
|
||||
timeout=45.0,
|
||||
)
|
||||
if not conn.get('ok'):
|
||||
raise RuntimeError(
|
||||
f'RDK 连接失败：{conn.get("error")}。'
|
||||
f'请先停掉 run_real.sh；切 AUTO Remote 并清除故障。参见 {log_path}'
|
||||
)
|
||||
if logger:
|
||||
logger.info(f'RDK 桥接就绪（py={pong.get("py")}）sn={robot_sn}')
|
||||
q0 = conn.get('q') or []
|
||||
if len(q0) >= 7:
|
||||
self._last_q = [float(v) for v in q0[:7]]
|
||||
|
||||
def _ask(self, msg: dict, timeout: float = 10.0) -> dict:
|
||||
import select
|
||||
|
||||
# 同一时刻只允许一条在途命令：相机/GUI 绝不能共用这个管道。
|
||||
if not self._io.acquire(timeout=max(timeout, 0.2)):
|
||||
return {'ok': False, 'error': '桥接忙'}
|
||||
try:
|
||||
assert self._proc.stdin and self._proc.stdout
|
||||
self._proc.stdin.write(self._json.dumps(msg) + '\n')
|
||||
self._proc.stdin.flush()
|
||||
end = time.monotonic() + timeout
|
||||
while time.monotonic() < end:
|
||||
if self._proc.poll() is not None:
|
||||
tip = '/tmp/rdk_drag_bridge.log'
|
||||
return {
|
||||
'ok': False,
|
||||
'error': f'桥接已退出 rc={self._proc.returncode}；参见 {tip}',
|
||||
}
|
||||
ready, _, _ = select.select([self._proc.stdout], [], [], 0.05)
|
||||
if not ready:
|
||||
continue
|
||||
line = self._proc.stdout.readline()
|
||||
if not line:
|
||||
continue
|
||||
line = line.strip()
|
||||
if not line.startswith('{'):
|
||||
continue
|
||||
try:
|
||||
return self._json.loads(line)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
return {'ok': False, 'error': f'应答异常：{exc} {line!r}'}
|
||||
return {'ok': False, 'error': '桥接超时'}
|
||||
finally:
|
||||
self._io.release()
|
||||
|
||||
def drag_on(self, primitive: str = 'FloatingJoint', mode: str = 'soft') -> dict:
|
||||
return self._ask(
|
||||
{'cmd': 'drag_on', 'primitive': primitive, 'mode': mode},
|
||||
timeout=20.0,
|
||||
)
|
||||
|
||||
def drag_off(self) -> dict:
|
||||
return self._ask({'cmd': 'drag_off'}, timeout=6.0)
|
||||
|
||||
def joints(self) -> Optional[List[float]]:
|
||||
res = self._ask({'cmd': 'joints'}, timeout=2.0)
|
||||
if res.get('ok'):
|
||||
q = res.get('q') or []
|
||||
if len(q) >= 7:
|
||||
self._last_q = [float(v) for v in q[:7]]
|
||||
return self._last_q
|
||||
return self._last_q
|
||||
|
||||
def move_joints(self, joints: List[float], speed: float = 0.15) -> dict:
|
||||
"""按关节阻抗把臂缓慢送到已记录的 7 个关节角。speed 单位 rad/s。"""
|
||||
return self._ask(
|
||||
{'cmd': 'move_joints', 'q': [float(v) for v in joints[:7]], 'speed': float(speed)},
|
||||
timeout=180.0,
|
||||
)
|
||||
|
||||
def close(self) -> None:
|
||||
try:
|
||||
self._ask({'cmd': 'quit'}, timeout=3.0)
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
self._proc.terminate()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
if getattr(self, '_err_file', None) is not None:
|
||||
self._err_file.close()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
class HandeyeTeach(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('handeye_teach')
|
||||
from arm_application.site_io import ros2_root
|
||||
|
||||
self.declare_parameter('output_dir', str(ros2_root() / 'handeye' / 'tmp'))
|
||||
self.declare_parameter('width', 1280)
|
||||
self.declare_parameter('height', 720)
|
||||
self.declare_parameter('fps', 15)
|
||||
self.declare_parameter('backend', 'ros') # ros | rdk
|
||||
self.declare_parameter('robot_sn', os.environ.get('SITE_ROBOT_SN', '').strip())
|
||||
self.declare_parameter('drag_primitive', 'FloatingJoint') # or FloatingCartesian
|
||||
self._serial = param_as_str(self, 'camera_serial', '')
|
||||
self.declare_parameter('auto_exposure', True)
|
||||
self.declare_parameter('ae_limit_us', 1800.0)
|
||||
self.declare_parameter('emitter_enabled', False)
|
||||
self.declare_parameter('exposure_us', 0.0)
|
||||
self.declare_parameter('gain', 0.0)
|
||||
# 目标采集点数。0 = 不限（按 q 结束）。
|
||||
# 设了就只是个"提醒"：采够时提示可以收工，不强制关窗 ——
|
||||
# 常常采到一半发现某个角度还没覆盖，强制退出反而要重开。
|
||||
self.declare_parameter('target_points', 0)
|
||||
# 必须和 run_handeye.sh 的 TAUGHT 是同一个文件。只写 taught_poses.txt、
|
||||
# 脚本去读 taught_poses_<模式>.txt，merge 就会「文件在、内容空」。
|
||||
self.declare_parameter('poses_file', '')
|
||||
|
||||
self.backend = str(self.get_parameter('backend').value).strip().lower()
|
||||
self.robot_sn = str(self.get_parameter('robot_sn').value)
|
||||
self.drag_primitive = str(self.get_parameter('drag_primitive').value).strip()
|
||||
|
||||
self.output_dir = Path(str(self.get_parameter('output_dir').value)).expanduser()
|
||||
self.images_dir = self.output_dir / 'images'
|
||||
_pf = str(self.get_parameter('poses_file').value).strip()
|
||||
self.poses_file = (
|
||||
Path(_pf).expanduser() if _pf else self.output_dir / 'taught_poses.txt'
|
||||
)
|
||||
self.yaml_file = self.output_dir / 'taught_waypoint_positions.yaml'
|
||||
self._last_capture_t = 0.0
|
||||
self.images_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
self._captures: List[dict] = []
|
||||
self._latest_bgr: Optional[np.ndarray] = None
|
||||
self._frame_lock = threading.Lock()
|
||||
self._quit = False
|
||||
self._busy = False
|
||||
self._cmd_pending = False
|
||||
self._drag_on = False
|
||||
self._robot = None
|
||||
self._rdk = None
|
||||
self._get_state = None
|
||||
self._cam_thread: Optional[threading.Thread] = None
|
||||
self._pipe = None
|
||||
self._fps = 15
|
||||
|
||||
if self.backend == 'rdk':
|
||||
self._init_rdk()
|
||||
else:
|
||||
self._init_ros_state()
|
||||
|
||||
self._start_camera()
|
||||
font = _setup_matplotlib_font()
|
||||
self._start_gui()
|
||||
print(HELP)
|
||||
if self.backend == 'rdk':
|
||||
print('[backend=rdk] 按 d = 软拖拽（在 AUTO Remote 下可用，无需 Floating 许可）')
|
||||
print(' run_real.sh 必须已停止。')
|
||||
print(' 拖着就能按 Enter 拍照，不必先按 s')
|
||||
else:
|
||||
print('[backend=ros] 用示教器 Free Drive + 按住使能键来拖拽。')
|
||||
print(' 或停掉 run_real.sh 后改用：-p backend:=rdk')
|
||||
if font:
|
||||
self.get_logger().info(f'界面字体：{font}')
|
||||
self.create_timer(1.0 / max(1, self._fps), self._tick)
|
||||
|
||||
def _init_ros_state(self) -> None:
|
||||
from arm_msgs.srv import GetArmState
|
||||
|
||||
self._get_state = self.create_client(GetArmState, 'get_arm_state')
|
||||
if not self._get_state.wait_for_service(timeout_sec=10.0):
|
||||
raise RuntimeError(
|
||||
'get_arm_state 不可用。请启动 run_real.sh，'
|
||||
'或改用：-p backend:=rdk（需先停掉整套栈）'
|
||||
)
|
||||
|
||||
def _init_rdk(self) -> None:
|
||||
self.get_logger().info(f'正在把 RDK 桥接连到 {self.robot_sn} ...')
|
||||
self._rdk = RdkBridge(self.robot_sn, logger=self.get_logger())
|
||||
self._robot = True # 标记：RDK 后端已就绪
|
||||
self.get_logger().info('RDK 已连接。按 d 启用 Free Drive。')
|
||||
|
||||
def _start_camera(self) -> None:
|
||||
import pyrealsense2 as rs
|
||||
|
||||
w = int(self.get_parameter('width').value)
|
||||
h = int(self.get_parameter('height').value)
|
||||
fps = int(self.get_parameter('fps').value)
|
||||
pipe = rs.pipeline()
|
||||
profile = None
|
||||
last_err = None
|
||||
for ww, hh, ff in [
|
||||
(w, h, fps), (w, h, 15), (w, h, 10),
|
||||
(640, 480, 30), (640, 480, 15),
|
||||
]:
|
||||
try:
|
||||
cfg = rs.config()
|
||||
if self._serial:
|
||||
cfg.enable_device(self._serial)
|
||||
cfg.enable_stream(rs.stream.color, ww, hh, rs.format.bgr8, ff)
|
||||
profile = pipe.start(cfg)
|
||||
self._fps = ff
|
||||
self.get_logger().info(f'相机 {ww}x{hh}@{ff}Hz')
|
||||
break
|
||||
except Exception as exc: # noqa: BLE001
|
||||
last_err = exc
|
||||
try:
|
||||
pipe.stop()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
pipe = rs.pipeline()
|
||||
if profile is None:
|
||||
raise RuntimeError(
|
||||
f'无法打开 D405 —— 请先停掉 realsense_color_pub。（{last_err}）'
|
||||
)
|
||||
self._pipe = pipe
|
||||
try:
|
||||
from arm_application.handeye_realsense import apply_realsense_exposure
|
||||
|
||||
apply_realsense_exposure(
|
||||
profile.get_device(),
|
||||
auto_exposure=bool(self.get_parameter('auto_exposure').value),
|
||||
ae_limit_us=float(self.get_parameter('ae_limit_us').value),
|
||||
emitter_enabled=bool(self.get_parameter('emitter_enabled').value),
|
||||
exposure_us=(
|
||||
float(self.get_parameter('exposure_us').value) or None
|
||||
),
|
||||
gain=(float(self.get_parameter('gain').value) or None),
|
||||
log=self.get_logger().info,
|
||||
)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(f'曝光设置应用失败：{exc}')
|
||||
self._cam_thread = threading.Thread(target=self._cam_loop, name='d405', daemon=True)
|
||||
self._cam_thread.start()
|
||||
|
||||
def _cam_loop(self) -> None:
|
||||
while not self._quit:
|
||||
try:
|
||||
frames = self._pipe.wait_for_frames(timeout_ms=400)
|
||||
color = frames.get_color_frame()
|
||||
except Exception: # noqa: BLE001
|
||||
time.sleep(0.02)
|
||||
continue
|
||||
if not color:
|
||||
continue
|
||||
bgr = np.asanyarray(color.get_data()).copy()
|
||||
with self._frame_lock:
|
||||
self._latest_bgr = bgr
|
||||
|
||||
def _start_gui(self) -> None:
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
self._plt = plt
|
||||
self._fig, self._ax = plt.subplots(1, 1, figsize=(11, 7))
|
||||
self._fig.canvas.manager.set_window_title(
|
||||
'handeye_teach | Enter=拍照 d=拖拽 s=停止 q=退出'
|
||||
)
|
||||
self._im = None
|
||||
self._status = self._ax.text(
|
||||
0.02,
|
||||
0.98,
|
||||
self._status_text(),
|
||||
transform=self._ax.transAxes,
|
||||
color='lime',
|
||||
fontsize=13,
|
||||
va='top',
|
||||
fontweight='bold',
|
||||
bbox=dict(boxstyle='round', facecolor='black', alpha=0.6),
|
||||
)
|
||||
self._ax.axis('off')
|
||||
self._fig.canvas.mpl_connect('key_press_event', self._on_key)
|
||||
plt.ion()
|
||||
plt.show(block=False)
|
||||
|
||||
def _status_text(self) -> str:
|
||||
drag = '拖拽开启' if self._drag_on else '拖拽关闭'
|
||||
if self.backend == 'rdk':
|
||||
tip = 'd=软拖拽 s=保持'
|
||||
else:
|
||||
tip = '示教器 FreeDrive+使能'
|
||||
return (
|
||||
f'已采集:{len(self._captures)} [{drag}] '
|
||||
f'{tip} | Enter=拍照 q=退出'
|
||||
)
|
||||
|
||||
def _on_key(self, event) -> None:
|
||||
key = (event.key or '').lower()
|
||||
if key in ('q', 'escape'):
|
||||
self._quit = True
|
||||
self.get_logger().info('收到退出请求')
|
||||
return
|
||||
if self._cmd_pending:
|
||||
return
|
||||
if key in ('enter', ' ', 'space', 'd', 's'):
|
||||
self._cmd_pending = True
|
||||
threading.Thread(target=self._run_key, args=(key,), daemon=True).start()
|
||||
|
||||
def _run_key(self, key: str) -> None:
|
||||
try:
|
||||
if key in ('enter', ' ', 'space'):
|
||||
self._capture_once()
|
||||
elif key == 'd':
|
||||
self._enable_drag()
|
||||
elif key == 's':
|
||||
self._disable_drag()
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().error(f'按键 {key}：{exc}')
|
||||
finally:
|
||||
self._cmd_pending = False
|
||||
|
||||
def _enable_drag(self) -> None:
|
||||
if self.backend != 'rdk':
|
||||
self._drag_on = True
|
||||
self.get_logger().warn(
|
||||
'ROS 后端：请用示教器的 Free Drive + 按住使能键。'
|
||||
'想用软件拖拽：先停掉 run_real.sh，然后执行 '
|
||||
'ros2 run arm_application handeye_teach --ros-args -p backend:=rdk'
|
||||
)
|
||||
self._status.set_text(self._status_text())
|
||||
return
|
||||
self.get_logger().info('正在启动软拖拽（预览保持实时刷新）...')
|
||||
# soft = AUTO Remote 模式下的关节阻抗拖拽（不需要 Floating 许可）
|
||||
res = self._rdk.drag_on(self.drag_primitive, mode='soft')
|
||||
if res.get('ok'):
|
||||
self._drag_on = True
|
||||
self.get_logger().info(
|
||||
f'拖拽已开启 mode={res.get("mode")} —— 推着机械臂走，Enter=拍照（s=保持，可选）'
|
||||
)
|
||||
if res.get('hint'):
|
||||
self.get_logger().info(str(res.get('hint')))
|
||||
else:
|
||||
self._drag_on = False
|
||||
self.get_logger().error(f'拖拽失败：{res.get("error")}')
|
||||
# 状态文字由 GUI 线程里的 _tick 刷新
|
||||
|
||||
def _disable_drag(self) -> None:
|
||||
if self.backend == 'rdk' and getattr(self, '_rdk', None) is not None:
|
||||
res = self._rdk.drag_off()
|
||||
if not res.get('ok'):
|
||||
self.get_logger().warn(f'停止拖拽：{res.get("error")}')
|
||||
self._drag_on = False
|
||||
self.get_logger().info('Free Drive 已关闭 / 保持')
|
||||
|
||||
def _tick(self) -> None:
|
||||
if self._quit:
|
||||
return
|
||||
with self._frame_lock:
|
||||
bgr = None if self._latest_bgr is None else self._latest_bgr.copy()
|
||||
if bgr is None:
|
||||
try:
|
||||
self._fig.canvas.flush_events()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
return
|
||||
rgb = bgr[:, :, ::-1]
|
||||
if self._im is None:
|
||||
self._im = self._ax.imshow(rgb)
|
||||
else:
|
||||
self._im.set_data(rgb)
|
||||
try:
|
||||
self._status.set_text(self._status_text())
|
||||
self._fig.canvas.draw_idle()
|
||||
self._fig.canvas.flush_events()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
|
||||
def _read_joints(self) -> Optional[List[float]]:
|
||||
if self.backend == 'rdk' and getattr(self, '_rdk', None) is not None:
|
||||
q = self._rdk.joints()
|
||||
if q is None:
|
||||
self.get_logger().error('读取 RDK 关节失败')
|
||||
return q
|
||||
from arm_msgs.srv import GetArmState
|
||||
|
||||
fut = self._get_state.call_async(GetArmState.Request())
|
||||
end = time.monotonic() + 2.0
|
||||
while rclpy.ok() and not fut.done() and time.monotonic() < end:
|
||||
rclpy.spin_once(self, timeout_sec=0.02)
|
||||
res = fut.result() if fut.done() else None
|
||||
if res is None or not res.success:
|
||||
return None
|
||||
q = list(res.joint_positions)
|
||||
return [float(v) for v in q[:7]] if len(q) >= 7 else None
|
||||
|
||||
def _capture_once(self) -> None:
|
||||
if self._busy:
|
||||
return
|
||||
self._busy = True
|
||||
try:
|
||||
with self._frame_lock:
|
||||
bgr = None if self._latest_bgr is None else self._latest_bgr.copy()
|
||||
if bgr is None:
|
||||
self.get_logger().warn('还没有相机帧')
|
||||
return
|
||||
q = self._read_joints()
|
||||
if q is None:
|
||||
self.get_logger().error('读取关节失败')
|
||||
return
|
||||
now = time.monotonic()
|
||||
if now - self._last_capture_t < 0.8:
|
||||
self.get_logger().warn('按太快，忽略（防 Enter 连发）')
|
||||
return
|
||||
if self._captures:
|
||||
last = self._captures[-1]['joints']
|
||||
delta = max(abs(a - b) for a, b in zip(q, last))
|
||||
if delta < 0.015:
|
||||
self.get_logger().warn(
|
||||
f'关节几乎没动（Δ={delta:.4f} rad），忽略这次拍照。'
|
||||
'先拖到另一个姿态再按 Enter。'
|
||||
)
|
||||
return
|
||||
self._last_capture_t = now
|
||||
|
||||
idx = len(self._captures)
|
||||
stamp = datetime.now(timezone.utc).strftime('%Y%m%d_%H%M%S')
|
||||
img_path = self.images_dir / f'teach_{idx:02d}_{stamp}.png'
|
||||
try:
|
||||
import cv2
|
||||
|
||||
cv2.imwrite(str(img_path), bgr)
|
||||
except Exception: # noqa: BLE001
|
||||
from PIL import Image as PILImage
|
||||
|
||||
PILImage.fromarray(bgr[:, :, ::-1]).save(str(img_path))
|
||||
|
||||
line = ', '.join(f'{v:.6f}' for v in q) + ','
|
||||
self._captures.append(
|
||||
{'index': idx, 'joints': q, 'image': str(img_path), 'line': line}
|
||||
)
|
||||
with self.poses_file.open('a', encoding='utf-8') as f:
|
||||
f.write(f'# pose {idx} {stamp}\n')
|
||||
f.write(line + '\n')
|
||||
f.write(f'# image {img_path}\n')
|
||||
print(f'\n# pose {idx} {img_path.name}')
|
||||
print(line)
|
||||
n = idx + 1
|
||||
try:
|
||||
target = int(self.get_parameter('target_points').value)
|
||||
except Exception: # noqa: BLE001
|
||||
target = 0
|
||||
if target > 0:
|
||||
if n < target:
|
||||
self.get_logger().info(
|
||||
f'[{n}/{target}] 已保存照片+关节 —— 还差 {target - n} 个点'
|
||||
)
|
||||
elif n == target:
|
||||
self.get_logger().info(
|
||||
f'[{n}/{target}] 已采够 {target} 个点。'
|
||||
' 建议按 q 结束，然后跑 merge + calibrate；'
|
||||
' 想多采几个点也可以继续按 Enter。'
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
f'[{n}/{target}] 已保存照片+关节 —— 已超出目标 '
|
||||
f'{n - target} 个点（会用全部点求解）'
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(f'[{n}] 已保存照片+关节')
|
||||
self._write_yaml_snippet()
|
||||
finally:
|
||||
self._busy = False
|
||||
|
||||
def _write_yaml_snippet(self) -> None:
|
||||
lines = [
|
||||
'# 粘贴到 handeye_calibrate.yaml -> waypoint_positions',
|
||||
f'# num_poses: {len(self._captures)}',
|
||||
'waypoint_positions: [',
|
||||
]
|
||||
for c in self._captures:
|
||||
lines.append(f' # pose {c["index"]}')
|
||||
lines.append(f' {c["line"]}')
|
||||
lines.append(']')
|
||||
self.yaml_file.write_text('\n'.join(lines) + '\n', encoding='utf-8')
|
||||
|
||||
def finish(self) -> None:
|
||||
self._quit = True
|
||||
if self._cam_thread is not None:
|
||||
self._cam_thread.join(timeout=1.5)
|
||||
if self._drag_on:
|
||||
self._disable_drag()
|
||||
if getattr(self, '_rdk', None) is not None:
|
||||
try:
|
||||
self._rdk.close()
|
||||
except Exception:
|
||||
pass
|
||||
n = len(self._captures)
|
||||
print('\n========== 示教完成 ==========')
|
||||
print(f'位姿: {n}')
|
||||
print(f'关节: {self.poses_file}')
|
||||
print(f'yaml: {self.yaml_file}')
|
||||
print(f'图像: {self.images_dir}')
|
||||
if n >= 1:
|
||||
print('\n--- 粘贴到 handeye_calibrate.yaml ---\n')
|
||||
print(self.yaml_file.read_text(encoding='utf-8'))
|
||||
print('同时设置 num_poses:', n)
|
||||
if n < 5:
|
||||
self.get_logger().warn(f'只有 {n} 个位姿；至少需要 5 个（建议 8-12 个）。')
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = None
|
||||
try:
|
||||
node = HandeyeTeach()
|
||||
while rclpy.ok() and not node._quit:
|
||||
rclpy.spin_once(node, timeout_sec=0.05)
|
||||
except SystemExit:
|
||||
pass
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if node is not None:
|
||||
node.finish()
|
||||
try:
|
||||
node._pipe.stop()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
+525
@@ -0,0 +1,525 @@
|
||||
#!/usr/bin/env python3
|
||||
"""手眼标定路点执行器。
|
||||
|
||||
逐个移动到每个关节路点，等到位后停留一段时间，
|
||||
让相机（或操作者）采集一张标定图像。
|
||||
|
||||
capture_mode:
|
||||
auto — 停留 dwell_sec 后进入下一个位姿
|
||||
keypress — 在终端按 Enter 继续
|
||||
preview — OpenCV 实时相机 + Enter/Space 确认（示教时推荐）
|
||||
|
||||
处于 ``preview`` 模式时，可在预览窗口打开期间用示教器的 **Free Drive**
|
||||
拖拽微调（ros2_control 保持 AUTO Remote；拖拽经由示教器，而非 RDK）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import array
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from datetime import datetime, timezone
|
||||
from pathlib import Path
|
||||
from typing import List, Optional
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import Image
|
||||
from std_msgs.msg import String
|
||||
|
||||
from arm_application.handeye_realsense import (
|
||||
is_realsense_source,
|
||||
resolve_realsense_topics,
|
||||
)
|
||||
from arm_application.site_io import joint_prefix_default
|
||||
|
||||
from arm_msgs.action import FollowJointTrajectory
|
||||
from arm_msgs.srv import GetArmState
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
try:
|
||||
import cv2
|
||||
except Exception: # noqa: BLE001
|
||||
cv2 = None # type: ignore
|
||||
|
||||
DEFAULT_JOINTS = [
|
||||
'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
|
||||
]
|
||||
|
||||
DEFAULT_TIMES = [4.0] * 12
|
||||
DEFAULT_POSITIONS = [
|
||||
0.00, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
|
||||
0.35, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
|
||||
-0.35, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
|
||||
0.00, -0.90, 0.00, 1.70, 0.00, 0.85, 0.00,
|
||||
0.00, -0.50, 0.00, 1.40, 0.00, 0.55, 0.00,
|
||||
0.20, -0.70, 0.15, 1.57, 0.40, 0.70, 0.30,
|
||||
-0.20, -0.70, -0.15, 1.57, -0.40, 0.70, -0.30,
|
||||
0.25, -0.80, 0.10, 1.65, 0.25, 0.90, -0.40,
|
||||
-0.25, -0.80, -0.10, 1.65, -0.25, 0.90, 0.40,
|
||||
0.15, -0.60, 0.20, 1.50, -0.35, 0.60, 0.50,
|
||||
-0.15, -0.60, -0.20, 1.50, 0.35, 0.60, -0.50,
|
||||
0.00, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
|
||||
]
|
||||
|
||||
PREVIEW_HELP_GUI = """
|
||||
=== 位姿预览 / 拖拽调整（OpenCV 窗口）===
|
||||
1. 示教器：Free Drive + 按住使能键拖拽微调
|
||||
2. 预览窗口：确认棋盘在画面内且清晰
|
||||
3. 键盘：Enter / Space = 下一个位姿；q / Esc = 退出
|
||||
"""
|
||||
|
||||
PREVIEW_HELP_HEADLESS = """
|
||||
=== 位姿预览 / 拖拽调整（无 OpenCV 窗口）===
|
||||
1. 另开终端查看画面：
|
||||
ros2 run rqt_image_view rqt_image_view /handeye/preview_image
|
||||
2. 示教器：Free Drive + 按住使能键拖拽微调
|
||||
3. 本终端：Enter = 确认下一个位姿；输入 q 回车 = 退出
|
||||
"""
|
||||
|
||||
|
||||
def _cv_gui_available() -> bool:
|
||||
if cv2 is None:
|
||||
return False
|
||||
try:
|
||||
cv2.namedWindow('_handeye_gui_probe', cv2.WINDOW_NORMAL)
|
||||
cv2.destroyWindow('_handeye_gui_probe')
|
||||
return True
|
||||
except Exception: # noqa: BLE001
|
||||
return False
|
||||
|
||||
|
||||
def _image_to_bgr(msg: Image) -> np.ndarray:
|
||||
h, w = int(msg.height), int(msg.width)
|
||||
enc = (msg.encoding or '').lower()
|
||||
data = np.frombuffer(msg.data, dtype=np.uint8)
|
||||
if enc in ('bgr8', 'rgb8'):
|
||||
img = data.reshape(h, w, 3)
|
||||
return img[:, :, ::-1].copy() if enc == 'rgb8' else img
|
||||
if enc in ('mono8', '8uc1'):
|
||||
gray = data.reshape(h, w)
|
||||
return np.stack([gray, gray, gray], axis=-1)
|
||||
if enc in ('bgra8', 'rgba8'):
|
||||
img = data.reshape(h, w, 4)
|
||||
bgr = img[:, :, :3]
|
||||
return bgr[:, :, ::-1].copy() if enc == 'rgba8' else bgr
|
||||
raise RuntimeError(f'不支持的图像编码：{msg.encoding!r}')
|
||||
|
||||
|
||||
class HandeyeWaypoints(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('handeye_waypoints')
|
||||
self.declare_parameter('joint_prefix', joint_prefix_default())
|
||||
self.declare_parameter('joint_names', DEFAULT_JOINTS)
|
||||
self.declare_parameter('velocity_scale', 0.12)
|
||||
self.declare_parameter('action_name', 'follow_joint_trajectory')
|
||||
self.declare_parameter('waypoint_times', DEFAULT_TIMES)
|
||||
self.declare_parameter('waypoint_positions', DEFAULT_POSITIONS)
|
||||
self.declare_parameter('dwell_sec', 2.5)
|
||||
# auto | keypress | preview
|
||||
self.declare_parameter('capture_mode', 'auto')
|
||||
self.declare_parameter('start_index', 0)
|
||||
self.declare_parameter('log_csv', '')
|
||||
self.declare_parameter('capture_topic', 'handeye/capture')
|
||||
self.declare_parameter('camera_source', 'd405')
|
||||
self.declare_parameter('camera_namespace', '')
|
||||
self.declare_parameter('image_topic', '')
|
||||
self.declare_parameter('preview_window', True)
|
||||
self.declare_parameter('append_poses_file', '/tmp/handeye_taught_poses.txt')
|
||||
|
||||
prefix = str(self.get_parameter('joint_prefix').value or '')
|
||||
base = list(
|
||||
self.get_parameter('joint_names').get_parameter_value().string_array_value
|
||||
or DEFAULT_JOINTS
|
||||
)
|
||||
self.joint_names = [f'{prefix}{n}' for n in base]
|
||||
self.velocity_scale = float(self.get_parameter('velocity_scale').value)
|
||||
self.action_name = str(self.get_parameter('action_name').value)
|
||||
self.dwell_sec = float(self.get_parameter('dwell_sec').value)
|
||||
self.capture_mode = str(self.get_parameter('capture_mode').value).strip().lower()
|
||||
self.start_index = max(0, int(self.get_parameter('start_index').value))
|
||||
self.log_csv = str(self.get_parameter('log_csv').value or '').strip()
|
||||
topic = str(self.get_parameter('capture_topic').value)
|
||||
self.preview_window = bool(self.get_parameter('preview_window').value)
|
||||
self.append_poses_file = str(
|
||||
self.get_parameter('append_poses_file').value or ''
|
||||
).strip()
|
||||
|
||||
cam_src = str(self.get_parameter('camera_source').value).strip().lower()
|
||||
img_topic = str(self.get_parameter('image_topic').value or '').strip()
|
||||
ns = str(self.get_parameter('camera_namespace').value or '').strip()
|
||||
if is_realsense_source(cam_src) and not img_topic:
|
||||
img_topic, _ = resolve_realsense_topics(
|
||||
cam_src, camera_namespace=ns, image_topic='', camera_info_topic=''
|
||||
)
|
||||
self.image_topic = img_topic
|
||||
|
||||
self._client = ActionClient(self, FollowJointTrajectory, self.action_name)
|
||||
self._get_state = self.create_client(GetArmState, 'get_arm_state')
|
||||
self._capture_pub = self.create_publisher(String, topic, 10)
|
||||
self._preview_pub = self.create_publisher(Image, 'handeye/preview_image', 10)
|
||||
self._latest_bgr: Optional[np.ndarray] = None
|
||||
self._cv_gui = _cv_gui_available() and self.preview_window
|
||||
if self.capture_mode == 'preview' and not self._cv_gui:
|
||||
self.get_logger().warn(
|
||||
'OpenCV 没有 GUI（无头环境）。请在 /handeye/preview_image 上预览，'
|
||||
'在本终端按 Enter 确认。'
|
||||
)
|
||||
if self.image_topic and self.capture_mode == 'preview':
|
||||
self.create_subscription(
|
||||
Image, self.image_topic, self._on_image, qos_profile_sensor_data
|
||||
)
|
||||
self.get_logger().info(f'预览图像：{self.image_topic}')
|
||||
|
||||
def _on_image(self, msg: Image) -> None:
|
||||
try:
|
||||
self._latest_bgr = _image_to_bgr(msg)
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
|
||||
def run(self) -> int:
|
||||
if self.capture_mode == 'preview':
|
||||
if cv2 is None:
|
||||
self.get_logger().error('preview 模式需要 OpenCV（cv2）')
|
||||
return 1
|
||||
if not self.image_topic:
|
||||
self.get_logger().error('preview 模式需要 image_topic 或 camera_source')
|
||||
return 1
|
||||
|
||||
times = list(self.get_parameter('waypoint_times').value)
|
||||
positions = list(self.get_parameter('waypoint_positions').value)
|
||||
n_j = len(self.joint_names)
|
||||
n_pts = len(times)
|
||||
if n_pts < 1:
|
||||
self.get_logger().error('waypoint_times 为空')
|
||||
return 1
|
||||
if len(positions) != n_pts * n_j:
|
||||
self.get_logger().error(
|
||||
f'waypoint_positions 长度 {len(positions)} != {n_pts}*{n_j}'
|
||||
)
|
||||
return 1
|
||||
if self.start_index >= n_pts:
|
||||
self.get_logger().error(
|
||||
f'start_index {self.start_index} >= 路点总数 {n_pts}'
|
||||
)
|
||||
return 1
|
||||
|
||||
if not self._client.wait_for_server(timeout_sec=15.0):
|
||||
self.get_logger().error(
|
||||
f'Action [{self.action_name}] 不可用。'
|
||||
'请先启动整套栈：bash scripts/run_real.sh'
|
||||
)
|
||||
return 2
|
||||
|
||||
csv_file = None
|
||||
csv_writer: Optional[csv.writer] = None
|
||||
if self.log_csv:
|
||||
path = Path(self.log_csv).expanduser()
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
csv_file = path.open('w', newline='', encoding='utf-8')
|
||||
csv_writer = csv.writer(csv_file)
|
||||
csv_writer.writerow(
|
||||
['index', 'utc_iso', 'move_duration_s']
|
||||
+ [f'target_{n}' for n in self.joint_names]
|
||||
+ [f'measured_{n}' for n in self.joint_names]
|
||||
)
|
||||
self.get_logger().info(f'正在记录采集数据 → {path}')
|
||||
|
||||
if self.capture_mode == 'preview':
|
||||
print(PREVIEW_HELP_GUI if self._cv_gui else PREVIEW_HELP_HEADLESS)
|
||||
|
||||
self.get_logger().info(
|
||||
f'手眼标定：{n_pts} 个位姿，start={self.start_index}，'
|
||||
f'v_scale={self.velocity_scale}，dwell={self.dwell_sec}s，'
|
||||
f'mode={self.capture_mode}'
|
||||
)
|
||||
self.get_logger().warn(
|
||||
'清理工作空间；让棋盘保持在视野内。Ctrl+C 可中止。'
|
||||
)
|
||||
|
||||
win = 'handeye_preview'
|
||||
try:
|
||||
for i in range(self.start_index, n_pts):
|
||||
q = positions[i * n_j : (i + 1) * n_j]
|
||||
move_t = max(1.0, float(times[i]))
|
||||
self.get_logger().info(
|
||||
f'[{i + 1}/{n_pts}] 移动中（约 {move_t:.1f}s）...'
|
||||
)
|
||||
code = self._move_one(q, move_t)
|
||||
if code != 0:
|
||||
return code
|
||||
|
||||
measured = self._read_joints()
|
||||
self._announce_capture(i, n_pts, q, measured)
|
||||
|
||||
if csv_writer is not None and csv_file is not None:
|
||||
csv_writer.writerow(
|
||||
[i, datetime.now(timezone.utc).isoformat(), move_t]
|
||||
+ list(q)
|
||||
+ list(measured if measured else [float('nan')] * n_j)
|
||||
)
|
||||
csv_file.flush()
|
||||
|
||||
if not self._wait_capture(i, n_pts):
|
||||
self.get_logger().info('用户已中止')
|
||||
return 0
|
||||
|
||||
# 在 preview 模式下确认后，保存示教关节
|
||||
if self.capture_mode == 'preview':
|
||||
taught = self._read_joints()
|
||||
if taught:
|
||||
self._save_taught_pose(i, taught)
|
||||
finally:
|
||||
if csv_file is not None:
|
||||
csv_file.close()
|
||||
if cv2 is not None:
|
||||
try:
|
||||
cv2.destroyWindow(win)
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
|
||||
self.get_logger().info('手眼标定路点序列执行完毕')
|
||||
if self.append_poses_file and self.capture_mode == 'preview':
|
||||
self.get_logger().info(
|
||||
f'示教位姿已保存 → {self.append_poses_file}'
|
||||
)
|
||||
return 0
|
||||
|
||||
def _save_taught_pose(self, index: int, joints: List[float]) -> None:
|
||||
line = ', '.join(f'{v:.6f}' for v in joints[:7]) + ','
|
||||
print(f'# pose {index}')
|
||||
print(line)
|
||||
if not self.append_poses_file:
|
||||
return
|
||||
path = Path(self.append_poses_file).expanduser()
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with path.open('a', encoding='utf-8') as f:
|
||||
f.write(f'# pose {index}\n')
|
||||
f.write(line + '\n')
|
||||
self.get_logger().info(f'已保存位姿 {index} → {path}')
|
||||
|
||||
def _move_one(self, positions: List[float], duration: float) -> int:
|
||||
goal = FollowJointTrajectory.Goal()
|
||||
goal.joint_names = self.joint_names
|
||||
goal.num_points = 1
|
||||
goal.velocity_scale = self.velocity_scale
|
||||
goal.positions = list(positions)
|
||||
goal.time_from_start = [float(duration)]
|
||||
|
||||
send = self._client.send_goal_async(goal)
|
||||
if not self._wait_future(send, 10.0, '发送目标'):
|
||||
return 3
|
||||
handle = send.result()
|
||||
if handle is None or not handle.accepted:
|
||||
self.get_logger().error('目标被拒绝')
|
||||
return 4
|
||||
|
||||
res_fut = handle.get_result_async()
|
||||
if not self._wait_future(res_fut, max(60.0, duration * 3.0), '结果'):
|
||||
return 5
|
||||
result = res_fut.result().result
|
||||
if not result.success:
|
||||
self.get_logger().error(f'移动失败：{result.message}')
|
||||
return 6
|
||||
return 0
|
||||
|
||||
def _wait_future(self, fut, timeout: float, label: str) -> bool:
|
||||
deadline = time.monotonic() + timeout
|
||||
while rclpy.ok() and not fut.done():
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
if time.monotonic() > deadline:
|
||||
self.get_logger().error(f'等待 {label} 超时')
|
||||
return False
|
||||
return fut.done()
|
||||
|
||||
def _read_joints(self) -> Optional[List[float]]:
|
||||
if not self._get_state.wait_for_service(timeout_sec=0.5):
|
||||
return None
|
||||
fut = self._get_state.call_async(GetArmState.Request())
|
||||
if not self._wait_future(fut, 2.0, 'get_arm_state'):
|
||||
return None
|
||||
res = fut.result()
|
||||
if res is None or not res.success:
|
||||
return None
|
||||
return list(res.joint_positions)
|
||||
|
||||
def _announce_capture(
|
||||
self,
|
||||
index: int,
|
||||
n_pts: int,
|
||||
target: List[float],
|
||||
measured: Optional[List[float]],
|
||||
) -> None:
|
||||
msg = String()
|
||||
msg.data = f'capture:{index}:{n_pts}'
|
||||
self._capture_pub.publish(msg)
|
||||
q_str = ', '.join(f'{v:.4f}' for v in (measured or target))
|
||||
if self.capture_mode == 'preview':
|
||||
self.get_logger().info(
|
||||
f'[{index + 1}/{n_pts}] 已到位 —— 用 Free Drive 调整，然后按 Enter/Space'
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
f'[{index + 1}/{n_pts}] 已到位 —— 现在采集 q=[{q_str}]'
|
||||
)
|
||||
|
||||
def _wait_capture(self, index: int, n_pts: int) -> bool:
|
||||
if self.capture_mode == 'preview':
|
||||
return self._wait_preview(index, n_pts)
|
||||
|
||||
if self.capture_mode in ('key', 'keypress', 'manual'):
|
||||
self.get_logger().info(
|
||||
'采集完成后按 Enter（q+Enter 退出）...'
|
||||
)
|
||||
for _ in range(5):
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
line = sys.stdin.readline()
|
||||
if not line or line.strip().lower() in ('q', 'quit', 'exit'):
|
||||
return False
|
||||
return True
|
||||
|
||||
end = time.monotonic() + max(0.0, self.dwell_sec)
|
||||
while rclpy.ok() and time.monotonic() < end:
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
return rclpy.ok()
|
||||
|
||||
def _annotate_preview(
|
||||
self, bgr: np.ndarray, index: int, n_pts: int
|
||||
) -> np.ndarray:
|
||||
vis = bgr.copy()
|
||||
if cv2 is None:
|
||||
return vis
|
||||
cv2.putText(
|
||||
vis,
|
||||
f'位姿 {index + 1}/{n_pts}',
|
||||
(12, 28),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.7,
|
||||
(0, 255, 0),
|
||||
2,
|
||||
)
|
||||
cv2.putText(
|
||||
vis,
|
||||
'Free Drive 拖拽 | Enter=下一个 q=退出',
|
||||
(12, 56),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.6,
|
||||
(0, 255, 255),
|
||||
2,
|
||||
)
|
||||
return vis
|
||||
|
||||
def _publish_preview(self, bgr: np.ndarray) -> None:
|
||||
msg = Image()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = 'camera'
|
||||
msg.height, msg.width = bgr.shape[0], bgr.shape[1]
|
||||
msg.encoding = 'bgr8'
|
||||
msg.is_bigendian = 0
|
||||
msg.step = msg.width * 3
|
||||
msg.data = array.array('B', bgr.tobytes()) # 快速路径；用 bytes 会多花约 40 ms
|
||||
self._preview_pub.publish(msg)
|
||||
|
||||
def _wait_preview(self, index: int, n_pts: int) -> bool:
|
||||
if self._cv_gui:
|
||||
return self._wait_preview_opencv(index, n_pts)
|
||||
return self._wait_preview_terminal(index, n_pts)
|
||||
|
||||
def _wait_preview_terminal(self, index: int, n_pts: int) -> bool:
|
||||
confirmed = threading.Event()
|
||||
quit_req = threading.Event()
|
||||
|
||||
def _stdin_reader() -> None:
|
||||
try:
|
||||
line = sys.stdin.readline()
|
||||
except Exception: # noqa: BLE001
|
||||
quit_req.set()
|
||||
return
|
||||
if not line or line.strip().lower() in ('q', 'quit', 'exit'):
|
||||
quit_req.set()
|
||||
else:
|
||||
confirmed.set()
|
||||
|
||||
self.get_logger().info(
|
||||
f'[{index + 1}/{n_pts}] 用 Free Drive 调整，然后按 Enter '
|
||||
'（q+Enter 退出）'
|
||||
)
|
||||
reader = threading.Thread(target=_stdin_reader, daemon=True)
|
||||
reader.start()
|
||||
|
||||
no_frame_warn = False
|
||||
while rclpy.ok():
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
if confirmed.is_set():
|
||||
return True
|
||||
if quit_req.is_set():
|
||||
return False
|
||||
if not reader.is_alive() and not confirmed.is_set() and not quit_req.is_set():
|
||||
return False
|
||||
|
||||
bgr = self._latest_bgr
|
||||
if bgr is None:
|
||||
if not no_frame_warn:
|
||||
self.get_logger().warn(
|
||||
f'正在等待 {self.image_topic} ...',
|
||||
throttle_duration_sec=3.0,
|
||||
)
|
||||
no_frame_warn = True
|
||||
continue
|
||||
vis = self._annotate_preview(bgr, index, n_pts)
|
||||
self._publish_preview(vis)
|
||||
return False
|
||||
|
||||
def _wait_preview_opencv(self, index: int, n_pts: int) -> bool:
|
||||
assert cv2 is not None
|
||||
win = 'handeye_preview'
|
||||
cv2.namedWindow(win, cv2.WINDOW_NORMAL)
|
||||
|
||||
no_frame_warn = False
|
||||
while rclpy.ok():
|
||||
rclpy.spin_once(self, timeout_sec=0.02)
|
||||
bgr = self._latest_bgr
|
||||
if bgr is None:
|
||||
if not no_frame_warn:
|
||||
self.get_logger().warn(
|
||||
f'正在等待 {self.image_topic} ...',
|
||||
throttle_duration_sec=3.0,
|
||||
)
|
||||
no_frame_warn = True
|
||||
key = cv2.waitKey(30) & 0xFF
|
||||
else:
|
||||
vis = self._annotate_preview(bgr, index, n_pts)
|
||||
self._publish_preview(vis)
|
||||
cv2.imshow(win, vis)
|
||||
key = cv2.waitKey(30) & 0xFF
|
||||
|
||||
if key in (13, 10, 32):
|
||||
return True
|
||||
if key in (27, ord('q'), ord('Q')):
|
||||
return False
|
||||
return False
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = HandeyeWaypoints()
|
||||
code = 1
|
||||
try:
|
||||
code = node.run()
|
||||
except KeyboardInterrupt:
|
||||
code = 0
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
raise SystemExit(code)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
+844
@@ -0,0 +1,844 @@
|
||||
"""工件二阶段检测：YOLO 定位工件 + HoughCircles 检测左右螺纹孔中心。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import yaml
|
||||
|
||||
_PKG_CONFIG = Path(__file__).resolve().parents[1] / "config" / "hole_detect.yaml"
|
||||
_WS_CONFIG = Path.home() / "ros2" / "src" / "arm_application" / "config" / "hole_detect.yaml"
|
||||
DEFAULT_CONFIG = _PKG_CONFIG if _PKG_CONFIG.is_file() else _WS_CONFIG
|
||||
|
||||
|
||||
def load_config(path: str | Path | None = None) -> dict[str, Any]:
|
||||
cfg_path = Path(path) if path else DEFAULT_CONFIG
|
||||
with open(cfg_path, encoding="utf-8") as f:
|
||||
data = yaml.safe_load(f) or {}
|
||||
if not isinstance(data, dict):
|
||||
raise ValueError(f"配置格式错误: {cfg_path}")
|
||||
return data
|
||||
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
def _fail(
|
||||
msg: str,
|
||||
draw_img: np.ndarray,
|
||||
bbox: tuple[int, int, int, int] | None = None,
|
||||
gate_block: bool = False,
|
||||
) -> dict[str, Any]:
|
||||
return {
|
||||
"status": False,
|
||||
"msg": msg,
|
||||
"hole_points": [],
|
||||
"hole_radii": [],
|
||||
"bbox": list(bbox) if bbox is not None else None,
|
||||
"gate_block": gate_block,
|
||||
"draw_img": draw_img,
|
||||
}
|
||||
|
||||
|
||||
def _draw_bbox(
|
||||
img: np.ndarray,
|
||||
bbox: tuple[int, int, int, int],
|
||||
color=(0, 255, 0),
|
||||
label: str = "",
|
||||
) -> None:
|
||||
"""画工件框。``label`` 是模型给出的类别名（由标签文件决定）。
|
||||
|
||||
这里刻意**不写死类名**：以前写死了 "gongjian"，但实际模型的类别是
|
||||
"workpiece"，图上的字和模型的类名对不上，排查时会被误导。
|
||||
现在用 ``Detection.cls_name``，也就是 ``weights.sh labels`` 里能读到的那份。
|
||||
"""
|
||||
x1, y1, x2, y2 = bbox
|
||||
cv2.rectangle(img, (x1, y1), (x2, y2), color, 2)
|
||||
text = (label or "").strip()
|
||||
if text:
|
||||
cv2.putText(img, text, (x1, max(20, y1 - 8)), cv2.FONT_HERSHEY_SIMPLEX, 0.7, color, 2)
|
||||
|
||||
|
||||
def _best_label(dets) -> str:
|
||||
"""取置信度最高那个检测的类名（由标签文件决定）。没有则返回空串。"""
|
||||
if not dets:
|
||||
return ""
|
||||
try:
|
||||
best = max(dets, key=lambda d: float(d.conf))
|
||||
except Exception: # noqa: BLE001
|
||||
return ""
|
||||
return str(getattr(best, "cls_name", "") or "").strip()
|
||||
|
||||
|
||||
def _draw_center_gate(img: np.ndarray, max_dx: float, max_dy: float, ok: bool) -> None:
|
||||
h, w = img.shape[:2]
|
||||
cx, cy = w * 0.5, h * 0.5
|
||||
x1 = int(round(cx - max_dx))
|
||||
y1 = int(round(cy - max_dy))
|
||||
x2 = int(round(cx + max_dx))
|
||||
y2 = int(round(cy + max_dy))
|
||||
color = (0, 255, 0) if ok else (0, 165, 255)
|
||||
cv2.rectangle(img, (x1, y1), (x2, y2), color, 1)
|
||||
cv2.drawMarker(img, (int(cx), int(cy)), color, markerType=cv2.MARKER_CROSS, markerSize=18, thickness=1)
|
||||
|
||||
|
||||
def _bbox_center_ok(
|
||||
bbox: tuple[int, int, int, int],
|
||||
img_w: int,
|
||||
img_h: int,
|
||||
cfg: dict[str, Any],
|
||||
) -> tuple[bool, float, float, float, float]:
|
||||
"""返回 (是否居中, bbox_cx, bbox_cy, max_dx, max_dy)。"""
|
||||
x1, y1, x2, y2 = bbox
|
||||
bx = 0.5 * (x1 + x2)
|
||||
by = 0.5 * (y1 + y2)
|
||||
max_dx = float(cfg.get("center_max_offset_ratio_x", 0.12)) * img_w
|
||||
max_dy = float(cfg.get("center_max_offset_ratio_y", 0.12)) * img_h
|
||||
ok = abs(bx - img_w * 0.5) <= max_dx and abs(by - img_h * 0.5) <= max_dy
|
||||
return ok, bx, by, max_dx, max_dy
|
||||
|
||||
|
||||
def _draw_holes(
|
||||
img: np.ndarray,
|
||||
points: list[list[float]],
|
||||
radii: list[float] | None = None,
|
||||
) -> None:
|
||||
"""画孔轮廓圆 + 中心点（十字+实心点）+ 坐标。"""
|
||||
for i, (cx, cy) in enumerate(points):
|
||||
px, py = int(round(cx)), int(round(cy))
|
||||
r = 12.0 if radii is None or i >= len(radii) else float(radii[i])
|
||||
rr = max(4, int(round(r)))
|
||||
# 轮廓
|
||||
cv2.circle(img, (px, py), rr, (0, 0, 255), 2)
|
||||
# 中心十字 + 黄点
|
||||
cv2.drawMarker(img, (px, py), (0, 0, 255), markerType=cv2.MARKER_CROSS, markerSize=16, thickness=2)
|
||||
cv2.circle(img, (px, py), 3, (0, 255, 255), -1)
|
||||
label = f"P{i + 1}({px},{py}) r={rr}"
|
||||
cv2.putText(img, label, (px + 12, py - 8), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 0, 255), 2)
|
||||
|
||||
|
||||
def _parse_best_bbox(dets, conf_thr: float) -> tuple[int, int, int, int] | None:
|
||||
"""最多取 1 个工件：取置信度最高且 >= conf 的框。"""
|
||||
if not dets:
|
||||
return None
|
||||
ordered = sorted(dets, key=lambda d: d.conf, reverse=True)
|
||||
for det in ordered:
|
||||
if float(det.conf) < conf_thr:
|
||||
continue
|
||||
return tuple(int(v) for v in det.xyxy) # type: ignore[return-value]
|
||||
return None
|
||||
|
||||
|
||||
def _keypoint_holes(
|
||||
dets,
|
||||
*,
|
||||
conf_thr: float,
|
||||
min_kconf: float,
|
||||
radius_px: float,
|
||||
min_dist_px: float,
|
||||
):
|
||||
"""pose 模型直接给出的 k1/k2 关键点当孔位。
|
||||
|
||||
现场约定（见 ``pose_detector``）：k0 跟踪、k1/k2 对孔。
|
||||
返回 ``((k1x,k1y),(k2x,k2y),r,r)``，拿不到可信关键点时返回 None
|
||||
（不是 pose 模型 / 关键点置信度不够 / 两点几乎重合），调用方回落到找圆。
|
||||
|
||||
为什么值得优先用关键点：Hough / 亮环那套是在**孔本身的图像特征**上找圆，
|
||||
光照、反光、通止规尖伸进视野都会把圆心拉偏；而关键点是模型在训练分布里
|
||||
直接回归出来的，对这几类干扰天然更稳。
|
||||
"""
|
||||
if not dets:
|
||||
return None
|
||||
for det in sorted(dets, key=lambda d: d.conf, reverse=True):
|
||||
if float(det.conf) < conf_thr:
|
||||
continue
|
||||
kpts = getattr(det, "kpts", None)
|
||||
if kpts is None:
|
||||
continue
|
||||
kpts = np.asarray(kpts, dtype=float)
|
||||
if kpts.ndim != 2 or kpts.shape[0] < 3 or kpts.shape[1] < 3:
|
||||
continue
|
||||
k1, k2 = kpts[1].copy(), kpts[2].copy()
|
||||
if not (np.isfinite(k1[:2]).all() and np.isfinite(k2[:2]).all()):
|
||||
continue
|
||||
if float(k1[2]) < min_kconf or float(k2[2]) < min_kconf:
|
||||
logger.info(
|
||||
"k1/k2 关键点置信度不足（%.2f / %.2f < %.2f）—— 回落到找圆",
|
||||
float(k1[2]),
|
||||
float(k2[2]),
|
||||
min_kconf,
|
||||
)
|
||||
continue
|
||||
dist = float(np.hypot(k1[0] - k2[0], k1[1] - k2[1]))
|
||||
if dist < float(min_dist_px):
|
||||
logger.info(
|
||||
"k1/k2 两关键点只差 %.1fpx（< %.1fpx）—— 视为不可信，回落到找圆",
|
||||
dist,
|
||||
min_dist_px,
|
||||
)
|
||||
continue
|
||||
r = float(radius_px)
|
||||
# **刻意不按图像 x 交换左右**。
|
||||
# Hough 路径的 k1/k2 是"画面左孔 / 右孔"（位置定义），但 pose 模型学出来的
|
||||
# k1/k2 是**固定语义**（训练时标注的那个物理孔）。工件只要转个角度，
|
||||
# 位置就会翻面；按 x 强制交换会让 `target_hole=k1` 时左时右地指向两个不同
|
||||
# 的物理孔 —— 那是最难查的一类现场问题。
|
||||
# 下游怎么认这两个点由 `stage2.k1_label` 决定：
|
||||
# hole_points[0] = 模型的 k1，hole_points[1] = 模型的 k2，
|
||||
# 所以 k1_label="left" ↔ k1 就是模型的 k1（见 stage2_locate_node._snap）。
|
||||
return (float(k1[0]), float(k1[1])), (float(k2[0]), float(k2[1])), r, r
|
||||
return None
|
||||
|
||||
|
||||
def _refine_circle_center(
|
||||
gray: np.ndarray,
|
||||
cx: float,
|
||||
cy: float,
|
||||
r: float,
|
||||
) -> tuple[float, float]:
|
||||
"""Hough 整数圆心 → 暗孔连通域一阶矩（亚像素）。失败则退回原圆心。"""
|
||||
h, w = gray.shape[:2]
|
||||
rad = max(5.0, float(r))
|
||||
half = int(round(2.2 * rad))
|
||||
x0 = max(0, int(round(cx)) - half)
|
||||
y0 = max(0, int(round(cy)) - half)
|
||||
x1 = min(w, int(round(cx)) + half)
|
||||
y1 = min(h, int(round(cy)) + half)
|
||||
if x1 - x0 < 8 or y1 - y0 < 8:
|
||||
return cx, cy
|
||||
roi = gray[y0:y1, x0:x1]
|
||||
blur = cv2.GaussianBlur(roi, (5, 5), 1.0)
|
||||
_, bw = cv2.threshold(blur, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
|
||||
n, labels, stats, cents = cv2.connectedComponentsWithStats(bw, 8)
|
||||
if n <= 1:
|
||||
return cx, cy
|
||||
ex, ey = cx - x0, cy - y0
|
||||
min_area = np.pi * (0.35 * rad) ** 2
|
||||
max_area = np.pi * (1.8 * rad) ** 2
|
||||
best_i = 0
|
||||
best_d = 1e18
|
||||
for i in range(1, n):
|
||||
area = float(stats[i, cv2.CC_STAT_AREA])
|
||||
if area < min_area or area > max_area:
|
||||
continue
|
||||
d = (float(cents[i][0]) - ex) ** 2 + (float(cents[i][1]) - ey) ** 2
|
||||
if d < best_d:
|
||||
best_d = d
|
||||
best_i = i
|
||||
if best_i <= 0 or best_d > (1.2 * rad) ** 2:
|
||||
return cx, cy
|
||||
mask = np.uint8(labels == best_i)
|
||||
M = cv2.moments(mask)
|
||||
if M["m00"] < 1.0:
|
||||
return cx, cy
|
||||
return x0 + M["m10"] / M["m00"], y0 + M["m01"] / M["m00"]
|
||||
|
||||
|
||||
# ==================== 螺纹嵌件对检测（亮环签名 + 关于中心孔对称）====================
|
||||
# 目标件是圆形法兰：中心一个大螺纹孔，两个目标螺纹嵌件在中心孔两侧、关于中心对称。
|
||||
# 旧 Hough"间距最大的一对"在这类多孔板上会锁到普通安装孔且逐帧跳变。
|
||||
# 新路径：金属嵌件有"暗孔芯 + 亮金属环 + 暗法兰背景"的明确签名；
|
||||
# 两个目标嵌件关于中心孔对称（中点≈中心孔、半径相近）——据此唯一锁定，抗干扰抗跳变。
|
||||
|
||||
|
||||
def _disc_kernel(r: float) -> np.ndarray:
|
||||
R = int(np.ceil(r))
|
||||
y, x = np.mgrid[-R:R + 1, -R:R + 1]
|
||||
k = (x * x + y * y <= r * r).astype(np.float32)
|
||||
return k / max(1.0, float(k.sum()))
|
||||
|
||||
|
||||
def _ring_kernel(a: float, b: float) -> np.ndarray:
|
||||
R = int(np.ceil(b))
|
||||
y, x = np.mgrid[-R:R + 1, -R:R + 1]
|
||||
d = x * x + y * y
|
||||
k = ((d >= a * a) & (d <= b * b)).astype(np.float32)
|
||||
return k / max(1.0, float(k.sum()))
|
||||
|
||||
|
||||
def _insert_score_map(gray: np.ndarray, scales) -> np.ndarray:
|
||||
"""亮环签名评分（多尺度取 max）：环越亮、芯越暗、外背景越暗，分越高。"""
|
||||
g = gray.astype(np.float32)
|
||||
best = None
|
||||
for ri, rr, ro in scales:
|
||||
inner = cv2.filter2D(g, -1, _disc_kernel(ri))
|
||||
ringm = cv2.filter2D(g, -1, _ring_kernel(ri + 1, rr))
|
||||
outer = cv2.filter2D(g, -1, _ring_kernel(rr + 2, ro))
|
||||
s = ringm - 0.5 * (inner + outer)
|
||||
best = s if best is None else np.maximum(best, s)
|
||||
return best
|
||||
|
||||
|
||||
def _refine_dark_bore(gray: np.ndarray, cx: float, cy: float, half: int = 26):
|
||||
"""暗孔连通域一阶矩 → 亚像素孔心 + 等效半径。失败退回原心。"""
|
||||
h, w = gray.shape
|
||||
x0 = max(0, int(round(cx)) - half)
|
||||
y0 = max(0, int(round(cy)) - half)
|
||||
x1 = min(w, int(round(cx)) + half)
|
||||
y1 = min(h, int(round(cy)) + half)
|
||||
if x1 - x0 < 12 or y1 - y0 < 12:
|
||||
return float(cx), float(cy), 12.0
|
||||
roi = gray[y0:y1, x0:x1].astype(np.float32)
|
||||
blur = cv2.GaussianBlur(roi, (5, 5), 1.0)
|
||||
thr = float(np.percentile(blur, 20))
|
||||
mask = (blur < thr).astype(np.uint8) * 255
|
||||
n, _lab, st, ce = cv2.connectedComponentsWithStats(mask, 8)
|
||||
best_i, best_d = 0, 1e18
|
||||
ex, ey = cx - x0, cy - y0
|
||||
for i in range(1, n):
|
||||
a = float(st[i, cv2.CC_STAT_AREA])
|
||||
if a < 15:
|
||||
continue
|
||||
d = (float(ce[i][0]) - ex) ** 2 + (float(ce[i][1]) - ey) ** 2
|
||||
if d < best_d:
|
||||
best_d, best_i = d, i
|
||||
if best_i <= 0:
|
||||
return float(cx), float(cy), 12.0
|
||||
area = float(st[best_i, cv2.CC_STAT_AREA])
|
||||
r_eq = float(np.sqrt(area / np.pi))
|
||||
return x0 + float(ce[best_i][0]), y0 + float(ce[best_i][1]), max(6.0, r_eq)
|
||||
|
||||
|
||||
def _detect_insert_pair(gray: np.ndarray, bbox, cfg, depth_m=None, intr=None):
|
||||
"""找"关于工件中心对称的一对螺纹嵌件"。返回 ((k1x,k1y),(k2x,k2y),r1,r2) 或 None。
|
||||
|
||||
尺度自适应：用深度把物理螺栓圆半径（~29mm，两孔间距~58mm）换算成像素半径，
|
||||
在中心 C 周围的圆环上搜候选，再选"中点≈C、都在环上、间距≈直径"的对称对。
|
||||
不依赖 YOLO 框是否盖住嵌件（框只用来定中心 C）。k1=左孔(x 小)，k2=右孔(x 大)。
|
||||
"""
|
||||
h, w = gray.shape
|
||||
x1, y1, x2, y2 = [int(v) for v in bbox]
|
||||
C = np.array([0.5 * (x1 + x2), 0.5 * (y1 + y2)], dtype=float)
|
||||
# ---- 螺栓圆像素半径 bolt_px ----
|
||||
bolt_mm = float(cfg.get("insert_bolt_radius_mm", 29.0))
|
||||
bolt_px = 0.0
|
||||
if depth_m is not None and intr is not None:
|
||||
# 工件表面深度：C 附近小邻域的浅分位（孔底更深，表面偏浅）
|
||||
r_s = max(6, int(round(0.25 * min(x2 - x1, y2 - y1))))
|
||||
cxi, cyi = int(round(C[0])), int(round(C[1]))
|
||||
sx1, sx2 = max(0, cxi - r_s), min(w, cxi + r_s)
|
||||
sy1, sy2 = max(0, cyi - r_s), min(h, cyi + r_s)
|
||||
zreg = depth_m[sy1:sy2, sx1:sx2]
|
||||
zreg = zreg[np.isfinite(zreg) & (zreg > 0.05)]
|
||||
if zreg.size >= 20:
|
||||
z_plate = float(np.percentile(zreg, 30))
|
||||
z_surf = max(0.05, z_plate - float(cfg.get("plate_thickness_m", 0.004)))
|
||||
bolt_px = bolt_mm / 1000.0 * float(intr["fx"]) / z_surf
|
||||
if bolt_px <= 0:
|
||||
# 无深度时退化为按框尺寸粗估（嵌件约在 0.7×框半宽处）
|
||||
bolt_px = 0.70 * 0.5 * max(x2 - x1, y2 - y1)
|
||||
bolt_px = float(np.clip(bolt_px, 24.0, 0.42 * min(h, w)))
|
||||
# ---- 亮环分数图 ----
|
||||
scales = cfg.get("insert_scales") or [
|
||||
(3, 6, 10), (5, 9, 14), (7, 12, 18), (10, 16, 23), (14, 21, 30),
|
||||
]
|
||||
score = _insert_score_map(gray, scales)
|
||||
# ---- 圆环掩码：嵌件在 [0.45, 1.55]×bolt_px 的环上 ----
|
||||
yy, xx = np.mgrid[0:h, 0:w]
|
||||
dist = np.hypot(xx - C[0], yy - C[1])
|
||||
ring = ((dist > 0.45 * bolt_px) & (dist < 1.55 * bolt_px)).astype(np.float32)
|
||||
scm = score * ring
|
||||
pos = scm[scm > 0]
|
||||
if pos.size < 20:
|
||||
return None
|
||||
thr = float(np.percentile(pos, float(cfg.get("insert_percentile", 96))))
|
||||
pk = (scm > thr).astype(np.uint8) * 255
|
||||
n, _lab, st, ce = cv2.connectedComponentsWithStats(pk, 8)
|
||||
cands = [] # (pos, score)
|
||||
for i in range(1, n):
|
||||
if st[i, cv2.CC_STAT_AREA] < 8:
|
||||
continue
|
||||
cx_i, cy_i = float(ce[i][0]), float(ce[i][1])
|
||||
cands.append((np.array([cx_i, cy_i], dtype=float),
|
||||
float(scm[int(round(cy_i)), int(round(cx_i))])))
|
||||
if len(cands) < 2:
|
||||
return None
|
||||
# ---- 对称对：两孔都在环上、间距≈直径、中点大致≈C（对称门）。
|
||||
# 在几何合格的对里选"亮环总分最高"的——真嵌件亮环最强，稳定选中真孔对，
|
||||
# 不被"更对称但更弱"的假对带偏（C 与真中心有几像素偏差时仍稳健，消除双稳态跳变）。
|
||||
best = None
|
||||
best_score = -1.0
|
||||
sym_gate = float(cfg.get("insert_sym_gate_ratio", 0.40)) * bolt_px
|
||||
for i in range(len(cands)):
|
||||
for j in range(i + 1, len(cands)):
|
||||
pi, si = cands[i]
|
||||
pj, sj = cands[j]
|
||||
di = float(np.linalg.norm(pi - C))
|
||||
dj = float(np.linalg.norm(pj - C))
|
||||
if not (0.40 * bolt_px < di < 1.65 * bolt_px):
|
||||
continue
|
||||
if not (0.40 * bolt_px < dj < 1.65 * bolt_px):
|
||||
continue
|
||||
sep = float(np.linalg.norm(pi - pj))
|
||||
if not (1.2 * bolt_px < sep < 2.9 * bolt_px):
|
||||
continue
|
||||
mid = 0.5 * (pi + pj)
|
||||
if float(np.linalg.norm(mid - C)) > sym_gate:
|
||||
continue
|
||||
tot = si + sj
|
||||
if tot > best_score:
|
||||
best_score = tot
|
||||
best = (pi, pj)
|
||||
if best is None:
|
||||
return None
|
||||
p, q = best
|
||||
# 稳定分配 k1/k2：沿主方向，避免竖直对时"x 小者"被噪声翻转。
|
||||
# 竖直对(|dy|>=|dx|) → k1=上(y 小)、k2=下；水平对 → k1=左(x 小)、k2=右。
|
||||
if abs(p[1] - q[1]) >= abs(p[0] - q[0]):
|
||||
k1raw, k2raw = (p, q) if p[1] < q[1] else (q, p)
|
||||
else:
|
||||
k1raw, k2raw = (p, q) if p[0] < q[0] else (q, p)
|
||||
k1x, k1y, r1 = _refine_dark_bore(gray, k1raw[0], k1raw[1])
|
||||
k2x, k2y, r2 = _refine_dark_bore(gray, k2raw[0], k2raw[1])
|
||||
return (k1x, k1y), (k2x, k2y), r1, r2
|
||||
|
||||
|
||||
def _pad_roi(
|
||||
img_w: int,
|
||||
img_h: int,
|
||||
bbox: tuple[int, int, int, int],
|
||||
padding: int,
|
||||
) -> tuple[int, int, int, int, int, int]:
|
||||
"""返回 x1,y1,x2,y2,offset_x,offset_y（裁剪左上角在原图坐标）。"""
|
||||
x1, y1, x2, y2 = bbox
|
||||
px1 = max(0, x1 - padding)
|
||||
py1 = max(0, y1 - padding)
|
||||
px2 = min(img_w, x2 + padding)
|
||||
py2 = min(img_h, y2 + padding)
|
||||
if px2 <= px1 or py2 <= py1:
|
||||
raise ValueError("ROI 无效（宽或高为 0）")
|
||||
return px1, py1, px2, py2, px1, py1
|
||||
|
||||
|
||||
def _render_success(
|
||||
image: np.ndarray,
|
||||
bbox: tuple[int, int, int, int],
|
||||
hole_points: list[list[float]],
|
||||
hole_radii: list[float],
|
||||
dist: float,
|
||||
gate: tuple[float, float] | None = None,
|
||||
label: str = "",
|
||||
) -> np.ndarray:
|
||||
draw_img = image.copy()
|
||||
if gate is not None:
|
||||
_draw_center_gate(draw_img, gate[0], gate[1], True)
|
||||
_draw_bbox(draw_img, bbox, label=label)
|
||||
_draw_holes(draw_img, hole_points, hole_radii)
|
||||
cv2.putText(
|
||||
draw_img,
|
||||
f"OK dist={dist:.1f}px",
|
||||
(16, 36),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.8,
|
||||
(0, 255, 0),
|
||||
2,
|
||||
)
|
||||
return draw_img
|
||||
|
||||
|
||||
def detect_holes(
|
||||
image: np.ndarray,
|
||||
detector,
|
||||
config: dict[str, Any] | None = None,
|
||||
device: str | int = 0,
|
||||
skip_center_gate: bool = False,
|
||||
depth_m: np.ndarray | None = None,
|
||||
intr: dict | None = None,
|
||||
allow_full_frame_roi: bool = False,
|
||||
) -> dict[str, Any]:
|
||||
"""
|
||||
二阶段检测螺纹孔中心。
|
||||
|
||||
detector: 需提供 detect(image_bgr, conf=..., iou=..., imgsz=...) -> list[Detection]
|
||||
（YOLODetector / FasterRCNNDetector / ONNX 均可）
|
||||
|
||||
:param allow_full_frame_roi: 检测器**没给出工件框**时，是否退回"整幅画面当 ROI"。
|
||||
默认 False（保持原行为：没有框就不做，报「未检测到工件」）。
|
||||
置 True 的场景是**调用方明确要试一把**：知道检测器可能漏检，但孔本身
|
||||
在画面里已经看得见（例如工件检测模型与当前相机/画面不匹配、或工件贴着
|
||||
画面中心）。此时 ROI 的约束由孔自身的判据接手 —— 环半径、两孔间距、
|
||||
居中门控都还在，所以不会凭空多出"随便一个圆"。
|
||||
|
||||
返回:
|
||||
status, msg, hole_points [[cx,cy],...], hole_radii [r1,r2], bbox, draw_img(BGR)
|
||||
"""
|
||||
try:
|
||||
if image is None or not isinstance(image, np.ndarray) or image.size == 0:
|
||||
return _fail("输入图像无效", np.zeros((480, 640, 3), dtype=np.uint8))
|
||||
|
||||
cfg = config if config is not None else load_config()
|
||||
draw_img = image.copy()
|
||||
h, w = image.shape[:2]
|
||||
|
||||
conf = float(cfg.get("conf", 0.25))
|
||||
iou = float(cfg.get("iou", 0.7))
|
||||
imgsz = int(cfg.get("imgsz", 640))
|
||||
padding = int(cfg.get("bbox_padding", 25))
|
||||
ksize = int(cfg.get("gaussian_ksize", 5))
|
||||
if ksize % 2 == 0:
|
||||
ksize += 1
|
||||
sigma = float(cfg.get("gaussian_sigma", 1.5))
|
||||
|
||||
# 1) 检测器定位工件 bbox（最多 1 个）— 兼容 YOLO / Faster R-CNN
|
||||
dets = detector.detect(image, conf=conf, iou=iou, imgsz=imgsz)
|
||||
bbox = _parse_best_bbox(dets, conf)
|
||||
if bbox is None and allow_full_frame_roi:
|
||||
# 退回整幅画面：ROI 从"检测器给的框"换成"整幅"，孔自身的判据
|
||||
#（环半径区间、两孔间距、居中门控）继续把关。
|
||||
bbox = (0, 0, int(w - 1), int(h - 1))
|
||||
logger.info(
|
||||
"工件检测器没给框，按调用方要求用整幅画面当 ROI 继续找孔"
|
||||
"（允许的原因见 allow_full_frame_roi 的说明）"
|
||||
)
|
||||
if bbox is None:
|
||||
return _fail("未检测到工件", draw_img)
|
||||
|
||||
# 类别名来自标签文件（见 model_labels / weights.sh labels），不再写死。
|
||||
label = _best_label(dets)
|
||||
_draw_bbox(draw_img, bbox, label=label)
|
||||
|
||||
# 1.5) 居中门控：bbox 中心接近画面中心才进入二阶检测。
|
||||
# 精对准 k1/k2 时工件会离开画面中心，调用方应 skip_center_gate=True。
|
||||
center_enable = bool(cfg.get("center_gate_enable", True)) and not skip_center_gate
|
||||
centered, bx, by, max_dx, max_dy = _bbox_center_ok(bbox, w, h, cfg)
|
||||
if bool(cfg.get("center_gate_draw", True)):
|
||||
_draw_center_gate(draw_img, max_dx, max_dy, centered if center_enable else True)
|
||||
if center_enable and not centered:
|
||||
dx = abs(bx - w * 0.5)
|
||||
dy = abs(by - h * 0.5)
|
||||
cv2.putText(
|
||||
draw_img,
|
||||
"MOVE TO CENTER",
|
||||
(16, 36),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.8,
|
||||
(0, 165, 255),
|
||||
2,
|
||||
)
|
||||
return _fail(
|
||||
f"工件未居中：offset=({dx:.0f},{dy:.0f})px，允许<=({max_dx:.0f},{max_dy:.0f})px",
|
||||
draw_img,
|
||||
bbox,
|
||||
gate_block=True,
|
||||
)
|
||||
|
||||
# ---- 首选：pose 模型直接给出 k1/k2 关键点（不必再找圆） ----
|
||||
if bool(cfg.get("keypoint_enable", True)):
|
||||
kp = _keypoint_holes(
|
||||
dets,
|
||||
conf_thr=conf,
|
||||
min_kconf=float(cfg.get("keypoint_min_conf", 0.30)),
|
||||
radius_px=float(cfg.get("keypoint_hole_radius_px", 12.0)),
|
||||
min_dist_px=float(cfg.get("keypoint_min_dist_px", 15.0)),
|
||||
)
|
||||
if kp is not None:
|
||||
(k1, k2, r1, r2) = kp
|
||||
hole_points = [[float(k1[0]), float(k1[1])], [float(k2[0]), float(k2[1])]]
|
||||
hole_radii = [float(r1), float(r2)]
|
||||
dist = float(np.hypot(k1[0] - k2[0], k1[1] - k2[1]))
|
||||
gate = (max_dx, max_dy) if bool(cfg.get("center_gate_draw", True)) else None
|
||||
draw_img = _render_success(
|
||||
image, bbox, hole_points, hole_radii, dist, gate=gate, label=label
|
||||
)
|
||||
return {
|
||||
"status": True,
|
||||
"msg": f"pose 关键点 k1/k2，两孔距离 {dist:.1f}px",
|
||||
"hole_points": hole_points,
|
||||
"hole_radii": hole_radii,
|
||||
"bbox": list(bbox),
|
||||
"label": label,
|
||||
"source": "keypoint",
|
||||
"draw_img": draw_img,
|
||||
}
|
||||
|
||||
# ---- 主路径：螺纹嵌件对（亮环签名 + 关于中心孔对称），抗多孔干扰、抗跳变 ----
|
||||
if bool(cfg.get("insert_pair_enable", True)):
|
||||
gray_full = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if image.ndim == 3 else image
|
||||
pair = _detect_insert_pair(gray_full, bbox, cfg, depth_m=depth_m, intr=intr)
|
||||
if pair is not None:
|
||||
(k1, k2, r1, r2) = pair
|
||||
hole_points = [[float(k1[0]), float(k1[1])], [float(k2[0]), float(k2[1])]]
|
||||
hole_radii = [float(r1), float(r2)]
|
||||
dist = float(np.hypot(k1[0] - k2[0], k1[1] - k2[1]))
|
||||
gate = (max_dx, max_dy) if bool(cfg.get("center_gate_draw", True)) else None
|
||||
draw_img = _render_success(
|
||||
image, bbox, hole_points, hole_radii, dist, gate=gate, label=label
|
||||
)
|
||||
return {
|
||||
"status": True,
|
||||
"msg": f"螺纹嵌件对，两孔距离 {dist:.1f}px",
|
||||
"hole_points": hole_points,
|
||||
"hole_radii": hole_radii,
|
||||
"bbox": list(bbox),
|
||||
"label": label,
|
||||
"draw_img": draw_img,
|
||||
}
|
||||
# ---- 退路：原 Hough 老路径 ----
|
||||
|
||||
# YOLO 框经常只包住半边法兰；扩到能同时容纳两个螺纹孔
|
||||
min_w = int(cfg.get("bbox_min_width", 280))
|
||||
min_h = int(cfg.get("bbox_min_height", 220))
|
||||
x1, y1, x2, y2 = bbox
|
||||
bw, bh = x2 - x1, y2 - y1
|
||||
if bw < min_w or bh < min_h:
|
||||
cxb, cyb = 0.5 * (x1 + x2), 0.5 * (y1 + y2)
|
||||
bw, bh = max(bw, min_w), max(bh, min_h)
|
||||
x1 = int(round(cxb - 0.5 * bw))
|
||||
y1 = int(round(cyb - 0.5 * bh))
|
||||
x2 = int(round(cxb + 0.5 * bw))
|
||||
y2 = int(round(cyb + 0.5 * bh))
|
||||
bbox = (
|
||||
max(0, x1), max(0, y1),
|
||||
min(w, x2), min(h, y2),
|
||||
)
|
||||
|
||||
# 2) padding ROI + 边界保护
|
||||
rx1, ry1, rx2, ry2, offset_x, offset_y = _pad_roi(w, h, bbox, padding)
|
||||
roi = image[ry1:ry2, rx1:rx2]
|
||||
if roi.size == 0:
|
||||
return _fail("ROI 裁剪失败", draw_img, bbox)
|
||||
|
||||
# 3) 灰度 + 高斯模糊
|
||||
gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY) if roi.ndim == 3 else roi
|
||||
blur = cv2.GaussianBlur(gray, (ksize, ksize), sigma)
|
||||
|
||||
# 4) HoughCircles
|
||||
circles = cv2.HoughCircles(
|
||||
blur,
|
||||
cv2.HOUGH_GRADIENT,
|
||||
dp=float(cfg.get("hough_dp", 1.2)),
|
||||
minDist=float(cfg.get("hough_min_dist", 40)),
|
||||
param1=float(cfg.get("hough_param1", 100)),
|
||||
param2=float(cfg.get("hough_param2", 28)),
|
||||
minRadius=int(cfg.get("hough_min_radius", 6)),
|
||||
maxRadius=int(cfg.get("hough_max_radius", 45)),
|
||||
)
|
||||
if circles is None:
|
||||
return _fail("HoughCircles 未检测到圆孔", draw_img, bbox)
|
||||
|
||||
# 5) ROI 坐标 -> 原图坐标，再亚像素细化孔心
|
||||
circles = np.asarray(circles[0], dtype=np.float64)
|
||||
gray_full = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if image.ndim == 3 else image
|
||||
do_refine = bool(cfg.get("refine_hole_centers", True))
|
||||
mapped: list[tuple[float, float, float]] = []
|
||||
for cx, cy, r in circles:
|
||||
fx = float(cx + offset_x)
|
||||
fy = float(cy + offset_y)
|
||||
fr = float(r)
|
||||
if do_refine:
|
||||
fx, fy = _refine_circle_center(gray_full, fx, fy, fr)
|
||||
mapped.append((fx, fy, fr))
|
||||
|
||||
# 6) 工业校验（含 bbox padding 区内的孔，避免螺纹孔落在框外）
|
||||
bx1, by1, bx2, by2 = bbox
|
||||
vx1 = max(0, bx1 - padding)
|
||||
vy1 = max(0, by1 - padding)
|
||||
vx2 = min(w, bx2 + padding)
|
||||
vy2 = min(h, by2 + padding)
|
||||
min_r = float(cfg.get("min_radius", 6))
|
||||
max_r = float(cfg.get("max_radius", 45))
|
||||
dist_min = float(cfg.get("hole_distance_min", 80))
|
||||
dist_max = float(cfg.get("hole_distance_max", 900))
|
||||
|
||||
valid: list[tuple[float, float, float]] = []
|
||||
for cx, cy, r in mapped:
|
||||
if not (vx1 <= cx <= vx2 and vy1 <= cy <= vy2):
|
||||
continue
|
||||
if not (min_r <= r <= max_r):
|
||||
continue
|
||||
valid.append((cx, cy, r))
|
||||
|
||||
# 丢掉法兰中心大孔：螺纹孔在工件两端，中心孔靠近 bbox 中心
|
||||
bcx = 0.5 * (bx1 + bx2)
|
||||
bcy = 0.5 * (by1 + by2)
|
||||
diag = max(40.0, float(np.hypot(bx2 - bx1, by2 - by1)))
|
||||
ring = [
|
||||
c for c in valid
|
||||
if float(np.hypot(c[0] - bcx, c[1] - bcy)) >= 0.18 * diag
|
||||
]
|
||||
if len(ring) >= 2:
|
||||
valid = ring
|
||||
|
||||
# 多于 2 个：在间距窗内取半径接近、间距最大的一对（左右螺纹孔，避开中心大孔边缘）
|
||||
if len(valid) >= 2:
|
||||
best = None
|
||||
best_d = -1.0
|
||||
for i in range(len(valid)):
|
||||
for j in range(i + 1, len(valid)):
|
||||
d = float(np.hypot(valid[i][0] - valid[j][0], valid[i][1] - valid[j][1]))
|
||||
if not (dist_min <= d <= dist_max):
|
||||
continue
|
||||
if abs(float(valid[i][2]) - float(valid[j][2])) > 10.0:
|
||||
continue
|
||||
if d > best_d:
|
||||
best_d = d
|
||||
best = (valid[i], valid[j])
|
||||
if best is None:
|
||||
return _fail(
|
||||
f"候选圆孔={len(valid)}，没有间距在 [{dist_min}, {dist_max}] 且半径接近的一对",
|
||||
draw_img,
|
||||
bbox,
|
||||
)
|
||||
valid = [best[0], best[1]]
|
||||
|
||||
if len(valid) != 2:
|
||||
return _fail(f"有效圆孔数量={len(valid)}，要求严格等于 2", draw_img, bbox)
|
||||
|
||||
ordered = sorted(valid, key=lambda c: c[0])
|
||||
(x1, y1, r1), (x2, y2, r2) = ordered
|
||||
dist = float(np.hypot(x1 - x2, y1 - y2))
|
||||
if not (dist_min <= dist <= dist_max):
|
||||
return _fail(
|
||||
f"两孔距离={dist:.1f}px，不在 [{dist_min}, {dist_max}]",
|
||||
draw_img,
|
||||
bbox,
|
||||
)
|
||||
|
||||
hole_points = [[x1, y1], [x2, y2]]
|
||||
hole_radii = [r1, r2]
|
||||
gate = (max_dx, max_dy) if bool(cfg.get("center_gate_draw", True)) else None
|
||||
draw_img = _render_success(image, bbox, hole_points, hole_radii, dist, gate=gate, label=label)
|
||||
return {
|
||||
"status": True,
|
||||
"msg": f"检测成功，两孔距离 {dist:.1f}px",
|
||||
"hole_points": hole_points,
|
||||
"hole_radii": hole_radii,
|
||||
"bbox": list(bbox),
|
||||
"label": label,
|
||||
"draw_img": draw_img,
|
||||
}
|
||||
except Exception as exc:
|
||||
fallback = image.copy() if isinstance(image, np.ndarray) and image.size else np.zeros((480, 640, 3), dtype=np.uint8)
|
||||
return _fail(f"异常: {exc}", fallback)
|
||||
|
||||
|
||||
class HoleStabilizer:
|
||||
"""时序稳帧：EMA 平滑中心/半径，短暂失败时沿用上一帧结果，抑制闪烁。"""
|
||||
|
||||
def __init__(self, config: dict[str, Any] | None = None) -> None:
|
||||
cfg = config or {}
|
||||
self.cfg = cfg
|
||||
self.hold_frames = int(cfg.get("stabilize_hold_frames", 15))
|
||||
self.ema_alpha = float(cfg.get("stabilize_ema_alpha", 0.35))
|
||||
self.max_jump = float(cfg.get("stabilize_max_jump", 60))
|
||||
self.reset()
|
||||
|
||||
def reset(self) -> None:
|
||||
self.points: list[list[float]] | None = None
|
||||
self.radii: list[float] | None = None
|
||||
self.bbox: tuple[int, int, int, int] | None = None
|
||||
self.miss = 0
|
||||
|
||||
def update(self, image: np.ndarray, result: dict[str, Any]) -> dict[str, Any]:
|
||||
try:
|
||||
# 未居中：不做二阶、不沿用旧孔，立即清空稳帧
|
||||
if result.get("gate_block"):
|
||||
self.reset()
|
||||
out = dict(result)
|
||||
out["hole_points"] = []
|
||||
out["hole_radii"] = []
|
||||
return out
|
||||
|
||||
bbox = result.get("bbox")
|
||||
if bbox is not None and len(bbox) == 4:
|
||||
self.bbox = (int(bbox[0]), int(bbox[1]), int(bbox[2]), int(bbox[3]))
|
||||
|
||||
if result.get("status") and len(result.get("hole_points") or []) == 2:
|
||||
pts = [[float(p[0]), float(p[1])] for p in result["hole_points"]]
|
||||
pts = sorted(pts, key=lambda p: p[0])
|
||||
rad = result.get("hole_radii") or [12.0, 12.0]
|
||||
rad = [float(rad[0]), float(rad[1])] if len(rad) >= 2 else [12.0, 12.0]
|
||||
|
||||
if self.points is not None:
|
||||
jump = max(
|
||||
float(np.hypot(pts[0][0] - self.points[0][0], pts[0][1] - self.points[0][1])),
|
||||
float(np.hypot(pts[1][0] - self.points[1][0], pts[1][1] - self.points[1][1])),
|
||||
)
|
||||
if jump > self.max_jump:
|
||||
# 跳变过大，当作本帧失败，沿用稳定值
|
||||
self.miss += 1
|
||||
else:
|
||||
a = self.ema_alpha
|
||||
self.points = [
|
||||
[
|
||||
(1 - a) * self.points[0][0] + a * pts[0][0],
|
||||
(1 - a) * self.points[0][1] + a * pts[0][1],
|
||||
],
|
||||
[
|
||||
(1 - a) * self.points[1][0] + a * pts[1][0],
|
||||
(1 - a) * self.points[1][1] + a * pts[1][1],
|
||||
],
|
||||
]
|
||||
self.radii = [
|
||||
(1 - a) * self.radii[0] + a * rad[0],
|
||||
(1 - a) * self.radii[1] + a * rad[1],
|
||||
]
|
||||
self.miss = 0
|
||||
else:
|
||||
self.points = pts
|
||||
self.radii = rad
|
||||
self.miss = 0
|
||||
else:
|
||||
self.miss += 1
|
||||
if self.miss > self.hold_frames:
|
||||
self.points = None
|
||||
self.radii = None
|
||||
|
||||
if self.points is None or self.radii is None:
|
||||
# 彻底丢失：只保留当前帧绘制（可能仅有 bbox）
|
||||
out = dict(result)
|
||||
out["hole_points"] = []
|
||||
out["hole_radii"] = []
|
||||
return out
|
||||
|
||||
dist = float(
|
||||
np.hypot(
|
||||
self.points[0][0] - self.points[1][0],
|
||||
self.points[0][1] - self.points[1][1],
|
||||
)
|
||||
)
|
||||
bbox_draw = self.bbox
|
||||
if bbox_draw is None and result.get("bbox") is not None:
|
||||
b = result["bbox"]
|
||||
bbox_draw = (int(b[0]), int(b[1]), int(b[2]), int(b[3]))
|
||||
if bbox_draw is None:
|
||||
# 无框时仍画孔
|
||||
draw_img = image.copy()
|
||||
_draw_holes(draw_img, self.points, self.radii)
|
||||
else:
|
||||
gate = None
|
||||
if bool(self.cfg.get("center_gate_draw", True)):
|
||||
ih, iw = image.shape[:2]
|
||||
max_dx = float(self.cfg.get("center_max_offset_ratio_x", 0.12)) * iw
|
||||
max_dy = float(self.cfg.get("center_max_offset_ratio_y", 0.12)) * ih
|
||||
gate = (max_dx, max_dy)
|
||||
draw_img = _render_success(
|
||||
image,
|
||||
bbox_draw,
|
||||
self.points,
|
||||
self.radii,
|
||||
dist,
|
||||
gate=gate,
|
||||
label=str(result.get("label") or ""),
|
||||
)
|
||||
|
||||
held = self.miss > 0
|
||||
return {
|
||||
"status": True,
|
||||
"msg": (
|
||||
f"检测成功，两孔距离 {dist:.1f}px"
|
||||
+ (f"（稳帧 hold={self.miss}）" if held else "")
|
||||
),
|
||||
"hole_points": [[p[0], p[1]] for p in self.points],
|
||||
"hole_radii": [float(self.radii[0]), float(self.radii[1])],
|
||||
"bbox": list(bbox_draw) if bbox_draw is not None else result.get("bbox"),
|
||||
"draw_img": draw_img,
|
||||
}
|
||||
except Exception as exc:
|
||||
fallback = image.copy() if isinstance(image, np.ndarray) and image.size else np.zeros((480, 640, 3), dtype=np.uint8)
|
||||
return _fail(f"稳帧异常: {exc}", fallback)
|
||||
+190
@@ -0,0 +1,190 @@
|
||||
#!/usr/bin/env python3
|
||||
"""在当前机械臂的 D405 彩色话题上做入孔检测。只检测，不做插入动作。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import Image
|
||||
from std_msgs.msg import Float32MultiArray
|
||||
|
||||
from arm_application.handeye_realsense import resolve_realsense_topics
|
||||
from arm_application.hole_detect_hough import HoleStabilizer
|
||||
from arm_application.hole_detector import (
|
||||
HoleDetection,
|
||||
HoleDetectResult,
|
||||
HoleDetector,
|
||||
config_from_site,
|
||||
weights_from_site,
|
||||
)
|
||||
from arm_application.site_io import load_site_dict
|
||||
from arm_msgs.msg import HoleDetection as HoleDetectionMsg
|
||||
from arm_msgs.msg import HoleDetections
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
|
||||
def _as_bool(val, default: bool = False) -> bool:
|
||||
if val is None:
|
||||
return default
|
||||
if isinstance(val, str):
|
||||
return val.strip().lower() in ("1", "true", "yes", "on")
|
||||
return bool(val)
|
||||
|
||||
|
||||
def _bgr_copy(msg: Image) -> np.ndarray:
|
||||
h, w = int(msg.height), int(msg.width)
|
||||
enc = (msg.encoding or "").lower()
|
||||
buf = np.frombuffer(msg.data, dtype=np.uint8)
|
||||
if enc == "bgr8":
|
||||
return buf.reshape(h, w, 3).copy()
|
||||
if enc == "rgb8":
|
||||
return buf.reshape(h, w, 3)[:, :, ::-1].copy()
|
||||
raise RuntimeError(f"Unsupported encoding {msg.encoding!r}")
|
||||
|
||||
|
||||
def _bgr_to_msg(image: np.ndarray, stamp, frame_id: str) -> Image:
|
||||
msg = Image()
|
||||
msg.header.stamp = stamp
|
||||
msg.header.frame_id = frame_id
|
||||
msg.height = int(image.shape[0])
|
||||
msg.width = int(image.shape[1])
|
||||
msg.encoding = "bgr8"
|
||||
msg.step = int(image.shape[1] * 3)
|
||||
msg.data = image.tobytes()
|
||||
return msg
|
||||
|
||||
|
||||
class HoleDetectNode(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("hole_detect")
|
||||
self.declare_parameter("camera_source", os.environ.get("SITE_CAM_NAME") or "d405")
|
||||
self.declare_parameter("image_topic", "")
|
||||
self.declare_parameter("weights", "")
|
||||
self.declare_parameter("config_file", "")
|
||||
self.declare_parameter("backend", "two_stage")
|
||||
self.declare_parameter("device", os.environ.get("SITE_YOLO_DEVICE") or "0")
|
||||
self.declare_parameter("preview", False)
|
||||
|
||||
site = load_site_dict()
|
||||
cam = str(self.get_parameter("camera_source").value or "d405")
|
||||
image_topic, _info = resolve_realsense_topics(cam, image_topic=str(self.get_parameter("image_topic").value or ""))
|
||||
cfg = config_from_site(site)
|
||||
param_backend = str(self.get_parameter("backend").value or "").strip().lower()
|
||||
if param_backend:
|
||||
cfg["backend"] = param_backend
|
||||
backend = str(cfg.get("backend") or "two_stage")
|
||||
weights = str(self.get_parameter("weights").value or "").strip() or weights_from_site(site)
|
||||
self._preview = _as_bool(self.get_parameter("preview").value, False)
|
||||
self._detector = HoleDetector(
|
||||
model_path=weights or None,
|
||||
config=cfg,
|
||||
device=str(self.get_parameter("device").value or "0"),
|
||||
)
|
||||
self._stabilizer = (
|
||||
HoleStabilizer(cfg) if backend in ("two_stage", "hough_roi") else None
|
||||
)
|
||||
self._pub = self.create_publisher(HoleDetections, "/hole_detect/result", 10)
|
||||
self._arr_pub = self.create_publisher(Float32MultiArray, "/hole_detect/holes", 10)
|
||||
self._overlay_pub = self.create_publisher(Image, "/hole_detect/overlay", 10)
|
||||
self.create_subscription(Image, image_topic, self._on_image, qos_profile_sensor_data)
|
||||
self.get_logger().info(
|
||||
f"hole_detect camera={cam} topic={image_topic} backend={backend} "
|
||||
f"weights={self._detector.model_path} site overlay=hole_detect"
|
||||
)
|
||||
|
||||
def _on_image(self, msg: Image) -> None:
|
||||
try:
|
||||
bgr = _bgr_copy(msg)
|
||||
except Exception as exc:
|
||||
self.get_logger().error(str(exc))
|
||||
return
|
||||
result = self._detector.detect(bgr)
|
||||
if self._stabilizer is not None:
|
||||
result = self._stabilize(bgr, result)
|
||||
out = HoleDetections()
|
||||
out.header = msg.header
|
||||
out.ok = bool(result.status)
|
||||
out.message = result.msg
|
||||
for hole in result.holes:
|
||||
hmsg = HoleDetectionMsg()
|
||||
hmsg.u = float(hole.center_px[0])
|
||||
hmsg.v = float(hole.center_px[1])
|
||||
hmsg.radius_px = float(hole.radius_px)
|
||||
hmsg.confidence = float(hole.confidence)
|
||||
hmsg.label = hole.label or ""
|
||||
if hole.bbox:
|
||||
hmsg.bbox = [int(v) for v in hole.bbox]
|
||||
out.holes.append(hmsg)
|
||||
self._pub.publish(out)
|
||||
|
||||
arr = Float32MultiArray()
|
||||
data = [1.0 if result.status else 0.0, float(len(result.holes))]
|
||||
for hole in result.holes:
|
||||
data.extend([hole.center_px[0], hole.center_px[1], hole.radius_px, hole.confidence])
|
||||
arr.data = data
|
||||
self._arr_pub.publish(arr)
|
||||
|
||||
overlay = result.draw_img if result.draw_img is not None else bgr
|
||||
self._overlay_pub.publish(_bgr_to_msg(overlay, msg.header.stamp, msg.header.frame_id))
|
||||
if self._preview:
|
||||
import cv2
|
||||
|
||||
cv2.imshow("hole_detect", overlay)
|
||||
cv2.waitKey(1)
|
||||
|
||||
def _stabilize(self, bgr, result: HoleDetectResult) -> HoleDetectResult:
|
||||
raw = result.raw if isinstance(result.raw, dict) else {}
|
||||
payload = {
|
||||
"status": result.status,
|
||||
"msg": result.msg,
|
||||
"hole_points": raw.get("hole_points")
|
||||
or [list(h.center_px) for h in result.holes],
|
||||
"hole_radii": raw.get("hole_radii") or [h.radius_px for h in result.holes],
|
||||
"bbox": raw.get("bbox") or (list(result.holes[0].bbox) if result.holes and result.holes[0].bbox else None),
|
||||
"draw_img": result.draw_img,
|
||||
"gate_block": bool(raw.get("gate_block")),
|
||||
}
|
||||
stab = self._stabilizer.update(bgr, payload)
|
||||
holes: list[HoleDetection] = []
|
||||
radii = stab.get("hole_radii") or []
|
||||
bbox = stab.get("bbox")
|
||||
bbox_t = tuple(int(v) for v in bbox) if bbox and len(bbox) == 4 else None
|
||||
for i, pt in enumerate(stab.get("hole_points") or []):
|
||||
rr = float(radii[i]) if i < len(radii) else 0.0
|
||||
holes.append(
|
||||
HoleDetection(
|
||||
center_px=(float(pt[0]), float(pt[1])),
|
||||
radius_px=rr,
|
||||
confidence=1.0,
|
||||
bbox=bbox_t,
|
||||
label="left" if i == 0 else "right" if i == 1 else f"h{i + 1}",
|
||||
)
|
||||
)
|
||||
return HoleDetectResult(
|
||||
bool(stab.get("status")),
|
||||
str(stab.get("msg") or ""),
|
||||
holes,
|
||||
stab.get("draw_img", result.draw_img),
|
||||
raw=stab,
|
||||
)
|
||||
|
||||
|
||||
def main() -> None:
|
||||
rclpy.init()
|
||||
node = HoleDetectNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
+368
@@ -0,0 +1,368 @@
|
||||
"""孔定位模块：加载已训练模型，输出孔中心像素坐标与置信度。
|
||||
|
||||
与工件定位（thread_gauge）完全分离，可独立替换模型与参数。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import yaml
|
||||
|
||||
_PKG_SHARE_FALLBACK = Path(__file__).resolve().parents[1] / "config"
|
||||
DEFAULT_CONFIG = _PKG_SHARE_FALLBACK / "hole_insert.yaml"
|
||||
|
||||
# 仓库里**实际存在**的孔/工件检测模型，相对仓库根。
|
||||
# 以前这个位置上写的是 ``models/hole_insert/best.pt`` —— 文件根本不存在，属于悬空引用：
|
||||
# 配置齐全时永远走不到这条分支，一旦 site.yaml 缺键就报"模型文件不存在"，
|
||||
# 而打印出来的路径看着特别合理，很容易被误判成"文件被人误删了"。
|
||||
# 改动这个常量时，顺手跑 ``bash scripts/weights.sh doctor`` 确认没有悬空引用。
|
||||
_CANONICAL_HOLE_MODEL = "models/yolo/gongjian0929.onnx"
|
||||
|
||||
|
||||
@dataclass
|
||||
class HoleDetection:
|
||||
"""单孔检测结果。"""
|
||||
|
||||
center_px: tuple[float, float]
|
||||
radius_px: float
|
||||
confidence: float
|
||||
bbox: tuple[int, int, int, int] | None = None
|
||||
cls_id: int = 0
|
||||
label: str = "" # left / right
|
||||
|
||||
|
||||
@dataclass
|
||||
class HoleDetectResult:
|
||||
"""孔定位整体结果。"""
|
||||
|
||||
status: bool
|
||||
msg: str
|
||||
holes: list[HoleDetection]
|
||||
draw_img: np.ndarray
|
||||
raw: dict[str, Any] | None = None
|
||||
|
||||
|
||||
def load_hole_config(path: str | Path | None = None) -> dict[str, Any]:
|
||||
cfg_path = Path(path) if path else DEFAULT_CONFIG
|
||||
with open(cfg_path, encoding="utf-8") as f:
|
||||
data = yaml.safe_load(f) or {}
|
||||
if not isinstance(data, dict):
|
||||
raise ValueError(f"孔定位配置格式错误: {cfg_path}")
|
||||
return data
|
||||
|
||||
|
||||
_HOLE_SITE_SKIP = {"weights", "workpiece_weights", "config"}
|
||||
|
||||
|
||||
def config_from_site(data: dict[str, Any] | None = None) -> dict[str, Any]:
|
||||
"""先取包内 yaml 默认值，再用 ``site.yaml`` 的 ``hole_detect:`` 覆盖。
|
||||
|
||||
以 ``site.yaml`` 为准（operator 的唯一数据源）；``data`` 参数用于测试时直接喂字典。
|
||||
"""
|
||||
from arm_application.site_io import deep_merge, load_site_dict, ros2_root
|
||||
|
||||
site = data if data is not None else load_site_dict()
|
||||
hd = dict(site.get("hole_detect") or {})
|
||||
backend = str(hd.get("backend") or "two_stage").strip().lower()
|
||||
cfg_path = str(hd.get("config") or "").strip()
|
||||
if not cfg_path:
|
||||
root = ros2_root()
|
||||
if backend in ("two_stage", "hough_roi"):
|
||||
cfg_path = str(root / "src" / "arm_application" / "config" / "hole_detect.yaml")
|
||||
else:
|
||||
cfg_path = str(root / "src" / "arm_application" / "config" / "hole_insert.yaml")
|
||||
base = load_hole_config(cfg_path) if Path(cfg_path).is_file() else {}
|
||||
overlay = {k: v for k, v in hd.items() if k not in _HOLE_SITE_SKIP and v != ""}
|
||||
merged = deep_merge(base, overlay)
|
||||
merged["backend"] = backend
|
||||
return merged
|
||||
|
||||
|
||||
def weights_from_site(data: dict[str, Any] | None = None) -> str:
|
||||
from arm_application.site_io import load_site_dict, ros2_root
|
||||
|
||||
site = data if data is not None else load_site_dict()
|
||||
hd = site.get("hole_detect") or {}
|
||||
paths = site.get("paths") or {}
|
||||
backend = str(hd.get("backend") or "two_stage").strip().lower()
|
||||
if backend in ("two_stage", "hough_roi"):
|
||||
return str(
|
||||
hd.get("workpiece_weights")
|
||||
or paths.get("hole_workpiece_weights")
|
||||
or paths.get("yolo_weights")
|
||||
or ""
|
||||
)
|
||||
return str(
|
||||
hd.get("weights")
|
||||
or paths.get("hole_weights")
|
||||
or str(ros2_root() / _CANONICAL_HOLE_MODEL)
|
||||
)
|
||||
|
||||
|
||||
def _default_model_path() -> Path:
|
||||
"""未显式给 ``model_path`` 时的兜底模型路径。
|
||||
|
||||
以前这里枚举了 ``hole_insert/best.pt`` / ``hole_best.pt`` / ``yolo/detect_best.pt``，
|
||||
三个文件在本仓库里**都不存在**，函数却会 ``return candidates[0]`` ——
|
||||
于是永远返回一个不存在的路径（同 ``weights_from_site`` 里那个悬空的 best.pt）。
|
||||
|
||||
现在以 site.yaml 为准（它才是真正的来源），它给的路径存在就用它；
|
||||
否则退到仓库里实际存在的规范模型。这样报错信息里的路径至少是对的。
|
||||
"""
|
||||
configured = weights_from_site()
|
||||
if configured:
|
||||
p = Path(configured).expanduser()
|
||||
if p.is_file():
|
||||
return p
|
||||
|
||||
from arm_application.site_io import ros2_root
|
||||
|
||||
return ros2_root() / _CANONICAL_HOLE_MODEL
|
||||
|
||||
|
||||
class HoleDetector:
|
||||
"""孔定位检测器：支持 YOLO / HoughCircles 双模式，可配置切换。"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
model_path: str | Path | None = None,
|
||||
config: dict[str, Any] | None = None,
|
||||
device: str | int = 0,
|
||||
) -> None:
|
||||
self.cfg = config or load_hole_config()
|
||||
self.device = device
|
||||
self.model_path = Path(model_path) if model_path else _default_model_path()
|
||||
self._detector = None
|
||||
self._backend = self.cfg.get("backend", "yolo").lower()
|
||||
|
||||
def _load_yolo(self):
|
||||
if self._detector is not None:
|
||||
return self._detector
|
||||
from arm_application.yolo_detector import build_detector, resolve_weights
|
||||
|
||||
weights = resolve_weights(
|
||||
self.model_path,
|
||||
backend=str(self.cfg.get("yolo_backend") or "auto"),
|
||||
)
|
||||
# build_detector 会自己识别 pose 模型（k0/k1/k2），不必在这里分支。
|
||||
self._detector = build_detector(
|
||||
weights,
|
||||
conf=float(self.cfg.get("conf", 0.25)),
|
||||
iou=float(self.cfg.get("iou", 0.7)),
|
||||
device=str(self.device),
|
||||
imgsz=int(self.cfg.get("imgsz", 640)),
|
||||
)
|
||||
return self._detector
|
||||
|
||||
def detect(self, image_bgr: np.ndarray) -> HoleDetectResult:
|
||||
"""检测图像中的孔，返回像素坐标列表。"""
|
||||
if image_bgr is None or image_bgr.size == 0:
|
||||
return HoleDetectResult(False, "输入图像无效", [], np.zeros((480, 640, 3), dtype=np.uint8))
|
||||
|
||||
draw_img = image_bgr.copy()
|
||||
backend = self._backend
|
||||
|
||||
if backend in {"yolo", "faster_rcnn"}:
|
||||
return self._detect_by_model(image_bgr, draw_img)
|
||||
if backend in {"two_stage", "hough_roi"}:
|
||||
return self._detect_two_stage(image_bgr, draw_img)
|
||||
if backend == "hough":
|
||||
return self._detect_by_hough(image_bgr, draw_img)
|
||||
return HoleDetectResult(False, f"未知 backend: {backend}", [], draw_img)
|
||||
|
||||
def _detect_two_stage(self, image: np.ndarray, draw_img: np.ndarray) -> HoleDetectResult:
|
||||
from arm_application.hole_detect_hough import detect_holes
|
||||
|
||||
try:
|
||||
detector = self._load_yolo()
|
||||
except Exception as exc:
|
||||
return HoleDetectResult(False, f"模型加载失败: {exc}", [], draw_img)
|
||||
|
||||
raw = detect_holes(image, detector, config=self.cfg, device=self.device)
|
||||
holes: list[HoleDetection] = []
|
||||
points = raw.get("hole_points") or []
|
||||
radii = raw.get("hole_radii") or []
|
||||
bbox = raw.get("bbox")
|
||||
bbox_t = tuple(int(v) for v in bbox) if bbox and len(bbox) == 4 else None
|
||||
for i, pt in enumerate(points):
|
||||
r = float(radii[i]) if i < len(radii) else 0.0
|
||||
label = "left" if i == 0 else "right" if i == 1 else f"h{i + 1}"
|
||||
holes.append(
|
||||
HoleDetection(
|
||||
center_px=(float(pt[0]), float(pt[1])),
|
||||
radius_px=r,
|
||||
confidence=1.0,
|
||||
bbox=bbox_t,
|
||||
label=label,
|
||||
)
|
||||
)
|
||||
draw = raw.get("draw_img", draw_img)
|
||||
return HoleDetectResult(bool(raw.get("status")), str(raw.get("msg") or ""), holes, draw, raw=raw)
|
||||
|
||||
def _detect_by_model(self, image: np.ndarray, draw_img: np.ndarray) -> HoleDetectResult:
|
||||
try:
|
||||
detector = self._load_yolo()
|
||||
except Exception as exc:
|
||||
return HoleDetectResult(False, f"模型加载失败: {exc}", [], draw_img)
|
||||
|
||||
conf = float(self.cfg.get("conf", 0.25))
|
||||
iou = float(self.cfg.get("iou", 0.7))
|
||||
imgsz = int(self.cfg.get("imgsz", 640))
|
||||
|
||||
dets = detector.detect(image, conf=conf, iou=iou, imgsz=imgsz)
|
||||
if not dets:
|
||||
return HoleDetectResult(False, "未检测到孔", [], draw_img)
|
||||
|
||||
holes: list[HoleDetection] = []
|
||||
for d in dets:
|
||||
x1, y1, x2, y2 = [int(v) for v in d.xyxy]
|
||||
cx = 0.5 * (x1 + x2)
|
||||
cy = 0.5 * (y1 + y2)
|
||||
r = 0.5 * max(x2 - x1, y2 - y1)
|
||||
holes.append(
|
||||
HoleDetection(
|
||||
center_px=(float(cx), float(cy)),
|
||||
radius_px=float(r),
|
||||
confidence=float(d.conf),
|
||||
bbox=(x1, y1, x2, y2),
|
||||
cls_id=int(getattr(d, "cls_id", 0)),
|
||||
)
|
||||
)
|
||||
return _finalize_holes(holes, draw_img, raw={"dets": dets}, cfg=self.cfg)
|
||||
|
||||
def _detect_by_hough(self, image: np.ndarray, draw_img: np.ndarray) -> HoleDetectResult:
|
||||
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if image.ndim == 3 else image
|
||||
ksize = int(self.cfg.get("gaussian_ksize", 5))
|
||||
if ksize % 2 == 0:
|
||||
ksize += 1
|
||||
blur = cv2.GaussianBlur(gray, (ksize, ksize), float(self.cfg.get("gaussian_sigma", 1.5)))
|
||||
|
||||
circles = cv2.HoughCircles(
|
||||
blur,
|
||||
cv2.HOUGH_GRADIENT,
|
||||
dp=float(self.cfg.get("hough_dp", 1.2)),
|
||||
minDist=float(self.cfg.get("hough_min_dist", 40)),
|
||||
param1=float(self.cfg.get("hough_param1", 100)),
|
||||
param2=float(self.cfg.get("hough_param2", 28)),
|
||||
minRadius=int(self.cfg.get("hough_min_radius", 6)),
|
||||
maxRadius=int(self.cfg.get("hough_max_radius", 45)),
|
||||
)
|
||||
if circles is None:
|
||||
return HoleDetectResult(False, "HoughCircles 未检测到孔", [], draw_img)
|
||||
|
||||
holes: list[HoleDetection] = []
|
||||
for c in np.asarray(circles[0], dtype=np.float64):
|
||||
cx, cy, r = float(c[0]), float(c[1]), float(c[2])
|
||||
holes.append(HoleDetection(center_px=(cx, cy), radius_px=r, confidence=1.0))
|
||||
return _finalize_holes(holes, draw_img, cfg=self.cfg)
|
||||
|
||||
def pixel_to_camera(
|
||||
self,
|
||||
hole: HoleDetection,
|
||||
depth_m: float | None = None,
|
||||
intrinsics: dict[str, float] | None = None,
|
||||
) -> np.ndarray:
|
||||
depth = float(depth_m if depth_m is not None else self.cfg.get("default_depth_m", 0.10))
|
||||
intr = intrinsics or self.cfg.get("camera_intrinsics") or {}
|
||||
return pixel_to_camera(hole, depth, intr)
|
||||
|
||||
|
||||
def pair_two_holes(holes: list[HoleDetection], cfg: dict[str, Any] | None = None) -> list[HoleDetection]:
|
||||
"""取左右两孔：优先最高置信度，再按图像 x 排序并打 left/right 标签。"""
|
||||
cfg = cfg or {}
|
||||
expected = int(cfg.get("expected_holes", 2))
|
||||
if expected != 2:
|
||||
ordered = sorted(holes, key=lambda h: h.center_px[0])
|
||||
for i, hole in enumerate(ordered):
|
||||
hole.label = f"h{i + 1}"
|
||||
return ordered
|
||||
|
||||
if len(holes) < 2:
|
||||
return holes
|
||||
|
||||
picked = sorted(holes, key=lambda h: h.confidence, reverse=True)
|
||||
if len(picked) > 2:
|
||||
by_x = sorted(picked, key=lambda h: h.center_px[0])
|
||||
picked = [by_x[0], by_x[-1]]
|
||||
else:
|
||||
picked = picked[:2]
|
||||
picked.sort(key=lambda h: h.center_px[0])
|
||||
dist = float(
|
||||
np.hypot(
|
||||
picked[1].center_px[0] - picked[0].center_px[0],
|
||||
picked[1].center_px[1] - picked[0].center_px[1],
|
||||
)
|
||||
)
|
||||
min_sep = float(cfg.get("min_hole_sep_px", 20.0))
|
||||
max_sep = float(cfg.get("max_hole_sep_px", 2000.0))
|
||||
if not (min_sep <= dist <= max_sep):
|
||||
return []
|
||||
picked[0].label = "left"
|
||||
picked[1].label = "right"
|
||||
return picked
|
||||
|
||||
|
||||
def _draw_holes(draw_img: np.ndarray, holes: list[HoleDetection]) -> None:
|
||||
for hole in holes:
|
||||
cx, cy = int(round(hole.center_px[0])), int(round(hole.center_px[1]))
|
||||
rr = max(4, int(round(hole.radius_px)))
|
||||
cv2.circle(draw_img, (cx, cy), rr, (0, 0, 255), 2)
|
||||
cv2.drawMarker(draw_img, (cx, cy), (0, 255, 255), cv2.MARKER_CROSS, 16, 2)
|
||||
cv2.circle(draw_img, (cx, cy), 3, (0, 255, 255), -1)
|
||||
name = hole.label or "hole"
|
||||
cv2.putText(
|
||||
draw_img,
|
||||
f"{name} ({cx},{cy}) {hole.confidence:.2f}",
|
||||
(cx + 12, cy - 8),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.55,
|
||||
(0, 0, 255),
|
||||
2,
|
||||
)
|
||||
|
||||
|
||||
def _finalize_holes(
|
||||
holes: list[HoleDetection],
|
||||
draw_img: np.ndarray,
|
||||
raw: dict[str, Any] | None = None,
|
||||
cfg: dict[str, Any] | None = None,
|
||||
) -> HoleDetectResult:
|
||||
cfg = cfg or {}
|
||||
paired = pair_two_holes(holes, cfg)
|
||||
if len(holes) >= 2 and not paired:
|
||||
_draw_holes(draw_img, holes)
|
||||
return HoleDetectResult(False, "两孔间距不在允许范围", holes, draw_img, raw=raw)
|
||||
if not paired:
|
||||
_draw_holes(draw_img, holes)
|
||||
return HoleDetectResult(False, f"有效孔数量={len(holes)}，需要 2 个", holes, draw_img, raw=raw)
|
||||
_draw_holes(draw_img, paired)
|
||||
dist = float(
|
||||
np.hypot(
|
||||
paired[1].center_px[0] - paired[0].center_px[0],
|
||||
paired[1].center_px[1] - paired[0].center_px[1],
|
||||
)
|
||||
)
|
||||
msg = f"检测到双孔 left={paired[0].center_px} right={paired[1].center_px} dist={dist:.1f}px"
|
||||
return HoleDetectResult(True, msg, paired, draw_img, raw=raw)
|
||||
|
||||
|
||||
def pixel_to_camera(
|
||||
hole: HoleDetection,
|
||||
depth_m: float,
|
||||
intrinsics: dict[str, float],
|
||||
) -> np.ndarray:
|
||||
"""像素坐标 + 深度 -> 相机系三维坐标（m）。"""
|
||||
fx = float(intrinsics["fx"])
|
||||
fy = float(intrinsics["fy"])
|
||||
cx = float(intrinsics["cx"])
|
||||
cy = float(intrinsics["cy"])
|
||||
u, v = hole.center_px
|
||||
z = float(depth_m)
|
||||
x = (u - cx) * z / fx
|
||||
y = (v - cy) * z / fy
|
||||
return np.array([x, y, z], dtype=float)
|
||||
@@ -0,0 +1,149 @@
|
||||
"""把 numpy 数组打包成 ``sensor_msgs/Image``。
|
||||
|
||||
为什么需要
|
||||
----------
|
||||
Orisys 视触觉传感器的数据分两类：
|
||||
|
||||
* **三力**（``fnormal`` / ``fshearx`` / ``fsheary``）—— 每帧几个标量，走子进程
|
||||
队列几乎没有开销。
|
||||
* **图像类**（形变图 / 深度图 / 力矢量场）—— 每帧几百 KB 到几 MB。原先
|
||||
``tactile_worker`` 为了性能**故意不过队列**，于是这些数据从未离开子进程、
|
||||
也从未发布到 ROS 话题上。
|
||||
|
||||
本模块只负责最后一公里：把已经拿到的数组正确打包成 ``Image``。放在单独文件
|
||||
里是为了能脱离相机单测 —— 编码、``step``、通道顺序这些细节一旦写错，下游
|
||||
拿到的图会静默错位（图像撕裂、颜色通道互换），而不是报错。
|
||||
|
||||
支持的形状与编码
|
||||
----------------
|
||||
=============== ============== ==========================================
|
||||
形状 dtype 编码
|
||||
=============== ============== ==========================================
|
||||
``(H, W)`` ``uint8`` ``mono8``
|
||||
``(H, W)`` ``uint16`` ``mono16``
|
||||
``(H, W)`` ``float32`` ``32FC1``
|
||||
``(H, W, 1)`` 任意 ``mono8``
|
||||
``(H, W, 2)`` ``float32`` ``32FC2``（力矢量场：每像素 dx, dy）
|
||||
``(H, W, 3)`` ``uint8`` ``rgb8``
|
||||
``(H, W, 4)`` ``uint8`` ``rgba8``
|
||||
=============== ============== ==========================================
|
||||
|
||||
2 通道的 ``32FC2`` 是 ROS 里表达矢量场的标准做法，``step = W * 4 * 2``。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
from sensor_msgs.msg import Image
|
||||
|
||||
__all__ = ["ndarray_to_image", "infer_encoding", "UnsupportedImageError"]
|
||||
|
||||
|
||||
class UnsupportedImageError(ValueError):
|
||||
"""数组的形状/类型没有对应的 ROS 编码。
|
||||
|
||||
宁可显式报错也不要猜 —— 猜错的结果是下游拿到一张静默错位的图。
|
||||
错误消息里带上实际形状与 dtype，方便对照本文档的表格。
|
||||
"""
|
||||
|
||||
|
||||
#: (通道数, dtype) -> ROS 编码
|
||||
_ENCODING_BY_CHANNELS = {
|
||||
(1, np.dtype(np.uint8)): "mono8",
|
||||
(1, np.dtype(np.uint16)): "mono16",
|
||||
(1, np.dtype(np.float32)): "32FC1",
|
||||
(2, np.dtype(np.float32)): "32FC2",
|
||||
(3, np.dtype(np.uint8)): "rgb8",
|
||||
(4, np.dtype(np.uint8)): "rgba8",
|
||||
}
|
||||
|
||||
|
||||
def infer_encoding(array: np.ndarray) -> str:
|
||||
"""推断 ROS 编码。推不出来时抛 :class:`UnsupportedImageError`。"""
|
||||
arr = np.asarray(array)
|
||||
if arr.ndim == 2:
|
||||
channels = 1
|
||||
elif arr.ndim == 3:
|
||||
channels = arr.shape[2]
|
||||
else:
|
||||
raise UnsupportedImageError(
|
||||
f"只支持 2D（灰度）或 3D（H,W,C）数组，收到 {arr.ndim}D，形状 {arr.shape}"
|
||||
)
|
||||
|
||||
key = (channels, arr.dtype)
|
||||
encoding = _ENCODING_BY_CHANNELS.get(key)
|
||||
if encoding is None:
|
||||
supported = ", ".join(
|
||||
f"{enc}({c}通道/{d.name})"
|
||||
for (c, d), enc in sorted(_ENCODING_BY_CHANNELS.items(), key=lambda kv: kv[1])
|
||||
)
|
||||
raise UnsupportedImageError(
|
||||
f"没有 {channels} 通道 + {arr.dtype.name} 对应的 ROS 编码。"
|
||||
f"支持: {supported}。"
|
||||
f"（收到形状 {arr.shape}）"
|
||||
)
|
||||
return encoding
|
||||
|
||||
|
||||
def _layout(array: np.ndarray) -> Tuple[int, int, int]:
|
||||
"""返回 ``(height, width, channels)``，并把 (H,W,1) 归一化成 2D 语义。"""
|
||||
arr = np.asarray(array)
|
||||
if arr.ndim == 2:
|
||||
h, w = arr.shape
|
||||
return int(h), int(w), 1
|
||||
h, w, c = arr.shape
|
||||
return int(h), int(w), int(c)
|
||||
|
||||
|
||||
def ndarray_to_image(
|
||||
array: np.ndarray,
|
||||
*,
|
||||
frame_id: str = "",
|
||||
stamp=None,
|
||||
encoding: Optional[str] = None,
|
||||
) -> Image:
|
||||
"""把数组打包成 ``sensor_msgs/Image``。
|
||||
|
||||
参数
|
||||
----
|
||||
array:
|
||||
图像或矢量场。必须是 C 连续（``np.ascontiguousarray`` 会自动处理）。
|
||||
encoding:
|
||||
显式指定编码以覆盖推断。仅在确实知道自己在做什么时使用 ——
|
||||
编码与 dtype 不匹配会让下游按错误的元素大小解读 ``data``。
|
||||
"""
|
||||
arr = np.asarray(array)
|
||||
if encoding is None:
|
||||
encoding = infer_encoding(arr)
|
||||
|
||||
height, width, channels = _layout(arr)
|
||||
|
||||
# (H,W,1) 在内存里与 (H,W) 连续一致，压平即可。
|
||||
flat = np.ascontiguousarray(arr).reshape(-1)
|
||||
|
||||
msg = Image()
|
||||
if stamp is not None:
|
||||
msg.header.stamp = stamp
|
||||
msg.header.frame_id = frame_id
|
||||
msg.height = height
|
||||
msg.width = width
|
||||
msg.encoding = encoding
|
||||
msg.is_bigendian = False
|
||||
msg.step = int(width * channels * flat.dtype.itemsize)
|
||||
msg.data = flat.tobytes()
|
||||
return msg
|
||||
|
||||
|
||||
def expected_step(array: np.ndarray, encoding: Optional[str] = None) -> int:
|
||||
"""算出应当写入 ``Image.step`` 的值。测试与自检用。"""
|
||||
arr = np.asarray(array)
|
||||
_, width, channels = _layout(arr)
|
||||
if encoding is not None:
|
||||
channels = {"mono8": 1, "mono16": 1, "32FC1": 1, "32FC2": 2,
|
||||
"rgb8": 3, "rgba8": 4}.get(encoding, channels)
|
||||
itemsize = {"mono8": 1, "mono16": 2, "32FC1": 4, "32FC2": 4,
|
||||
"rgb8": 1, "rgba8": 1}.get(encoding, arr.dtype.itemsize)
|
||||
return int(width * channels * itemsize)
|
||||
return int(width * channels * arr.dtype.itemsize)
|
||||
@@ -0,0 +1,97 @@
|
||||
#!/usr/bin/env python3
|
||||
"""三维点的匀速模型卡尔曼滤波（坐标系为相机系）。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
class KalmanFilter3D:
|
||||
"""状态量为 ``[x, y, z, vx, vy, vz]``；观测只有位置，没有速度。"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
q_pos: float = 1e-4,
|
||||
q_vel: float = 5e-3,
|
||||
r_pos: float = 4e-4,
|
||||
) -> None:
|
||||
self.q_pos = float(q_pos)
|
||||
self.q_vel = float(q_vel)
|
||||
self.r_pos = float(r_pos)
|
||||
self.x = np.zeros(6, dtype=float)
|
||||
self.P = np.eye(6, dtype=float)
|
||||
self._t: Optional[float] = None
|
||||
self.initialized = False
|
||||
|
||||
def reset(self, p: np.ndarray, t: Optional[float] = None) -> None:
|
||||
p = np.asarray(p, dtype=float).reshape(3)
|
||||
self.x[:] = 0.0
|
||||
self.x[:3] = p
|
||||
self.P = np.diag([1e-3, 1e-3, 1e-3, 1e-2, 1e-2, 1e-2])
|
||||
self._t = t
|
||||
self.initialized = True
|
||||
|
||||
@property
|
||||
def position(self) -> np.ndarray:
|
||||
return self.x[:3].copy()
|
||||
|
||||
@property
|
||||
def velocity(self) -> np.ndarray:
|
||||
return self.x[3:].copy()
|
||||
|
||||
def advance(self, t: float) -> None:
|
||||
"""把状态预测到时刻 ``t``（秒，必须单调递增）。
|
||||
|
||||
超过 0.5 秒的空档按"目标可能已经跑掉"处理：位置不动、速度清零，
|
||||
否则旧速度会把位置外推得很离谱。
|
||||
"""
|
||||
if not self.initialized or self._t is None:
|
||||
self._t = t
|
||||
return
|
||||
dt = float(t - self._t)
|
||||
if dt <= 1e-6:
|
||||
return
|
||||
if dt > 0.5:
|
||||
# 空档太长——位置保持不动，速度清零。
|
||||
self.x[3:] = 0.0
|
||||
self._t = t
|
||||
return
|
||||
self._predict(dt)
|
||||
self._t = t
|
||||
|
||||
def zero_velocity(self) -> None:
|
||||
"""把速度状态清零、位置保持不动。
|
||||
|
||||
用途：观测疑似假跳变（见 ``feature_jump_guard.FeatureJumpGate``）时本次不
|
||||
采信观测，但**不能**让上一次的速度继续把位置外推出去 —— 否则"不动"会变成
|
||||
"按旧速度匀速跑掉"，刚好是我们要避免的那件事。
|
||||
"""
|
||||
self.x[3:] = 0.0
|
||||
|
||||
def update(self, z: np.ndarray) -> None:
|
||||
"""用一次三维位置观测做量测更新。"""
|
||||
z = np.asarray(z, dtype=float).reshape(3)
|
||||
if not self.initialized:
|
||||
self.reset(z)
|
||||
return
|
||||
H = np.zeros((3, 6), dtype=float)
|
||||
H[0, 0] = H[1, 1] = H[2, 2] = 1.0
|
||||
R = np.eye(3, dtype=float) * self.r_pos
|
||||
y = z - H @ self.x
|
||||
S = H @ self.P @ H.T + R
|
||||
K = self.P @ H.T @ np.linalg.inv(S)
|
||||
self.x = self.x + K @ y
|
||||
I = np.eye(6)
|
||||
self.P = (I - K @ H) @ self.P
|
||||
|
||||
def _predict(self, dt: float) -> None:
|
||||
F = np.eye(6, dtype=float)
|
||||
F[0, 3] = F[1, 4] = F[2, 5] = dt
|
||||
q_p = self.q_pos
|
||||
q_v = self.q_vel
|
||||
Q = np.diag([q_p, q_p, q_p, q_v, q_v, q_v])
|
||||
self.x = F @ self.x
|
||||
self.P = F @ self.P @ F.T + Q
|
||||
@@ -0,0 +1,558 @@
|
||||
"""模型类别标签（class names）的统一读写。
|
||||
|
||||
## 为什么需要这个模块
|
||||
|
||||
推理代码需要把模型输出的**类别索引**（整数 0/1/2…）翻译成**人能看懂的名字**。
|
||||
这件事原先没有统一做法，导致同一个模型在不同后端下名字都不一样：
|
||||
|
||||
- `.pt`：从 Ultralytics 的 `model.names` 读（来源是 checkpoint 内嵌 metadata）
|
||||
- `.onnx`：ONNX 文件里有 `names` metadata，但代码没用，而是**硬编码** `{0: "workpiece"}`
|
||||
- `.engine`：**标签直接丢了**
|
||||
|
||||
最后一条是本模块存在的主要原因。TensorRT engine 要用 `trt.OnnxParser` 重新构建，
|
||||
而**当前安装的 TensorRT（10.13）Python API 根本没有 metadata 读写接口** ——
|
||||
`INetworkDefinition` / `IBuilderConfig` / `ICudaEngine` 上都没有任何 metadata 方法。
|
||||
所以 engine 构建完就只剩张量，标签无从附着。实测：
|
||||
|
||||
detect_best_0.engine 二进制里搜不到 "workpiece" → 标签已丢
|
||||
detect_hc90.engine 能搜到 "workpiece" → 旧版 TRT 建的，但当前环境已无法加载
|
||||
|
||||
（`detect_hc90` 已作为零引用模型移出仓库，这里保留这行是因为它同时印证了两件事：
|
||||
旧版 TRT 建的 engine 里其实是有标签的，以及 engine 与 TRT 版本绑死。）
|
||||
|
||||
后果很隐蔽：`yolo_detector` 读不到名字时会退化成 `str(cid)`，也就是类名变成字符串
|
||||
`"0"`；而预览界面又不绘制类名，所以**没有任何地方看得出标签丢了**。
|
||||
|
||||
## 解决办法：旁挂标签文件
|
||||
|
||||
在每个权重旁边放一个同名文本文件 `<模型名>.labels.yaml`：
|
||||
|
||||
models/yolo/detect_best_0.onnx
|
||||
models/yolo/detect_best_0.engine
|
||||
models/yolo/detect_best_0.labels.yaml ← 就是这个
|
||||
|
||||
选文本文件而不是想办法塞进 engine，理由有二：
|
||||
|
||||
1. **可修改**。标签是配置，不是模型的一部分。塞进 engine 就只能重建 engine 才能改。
|
||||
2. **跨格式统一**。engine / onnx / pt 三种格式共用同一份标签，不会出现
|
||||
"换后端名字就变了"。engine 丢标签的问题也一并绕过了。
|
||||
|
||||
标签文件是纯文本，进 git（体积小、属于配置）；权重二进制不进 git。
|
||||
|
||||
## 读取优先级
|
||||
|
||||
sidecar（旁挂文件） > onnx（ONNX metadata） > model（模型自身 metadata） > placeholder
|
||||
|
||||
`read_labels()` 会连**来源**一起返回。这点很重要：调用方可以据此提示
|
||||
"标签来自占位符，请补一个 .labels.yaml"，而不是悄悄给出一个错的类名。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Dict, List, Optional, Sequence, Tuple, Union
|
||||
|
||||
# 标签文件的扩展名。刻意用双后缀：既能被 *.yaml 工具识别，
|
||||
# 又能和模型自身的配置（如果有）区分开。
|
||||
LABELS_SUFFIX = ".labels.yaml"
|
||||
|
||||
# read_labels() 的来源标记。顺序即优先级（高 → 低）。
|
||||
SOURCE_SIDECAR = "sidecar" # 旁挂 <模型名>.labels.yaml（权威，用户可直接改）
|
||||
SOURCE_ONNX = "onnx" # ONNX 文件内嵌 metadata
|
||||
SOURCE_MODEL = "model" # 模型自身 metadata（Ultralytics 的 .names）
|
||||
SOURCE_PLACEHOLDER = "placeholder" # 都没有 → 占位符 class_0/class_1…
|
||||
|
||||
SOURCE_LABELS_ZH = {
|
||||
SOURCE_SIDECAR: "旁挂标签文件",
|
||||
SOURCE_ONNX: "ONNX 内嵌 metadata",
|
||||
SOURCE_MODEL: "模型内嵌 metadata",
|
||||
SOURCE_PLACEHOLDER: "占位符（缺标签）",
|
||||
}
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class LabelSet:
|
||||
"""一次标签读取的结果。
|
||||
|
||||
``names`` 是索引 → 名字；``source`` 说明它从哪来（见 SOURCE_* 常量）。
|
||||
|
||||
调用方**应该**检查 ``source``：如果是 ``placeholder``，说明这个模型缺标签，
|
||||
此时给出的是 ``class_0`` 这样的占位名，只适合占位显示，不该当成真名使用。
|
||||
"""
|
||||
|
||||
names: Dict[int, str]
|
||||
source: str
|
||||
path: Optional[Path] = None # 实际读到的文件（placeholder 时为 None）
|
||||
|
||||
@property
|
||||
def is_placeholder(self) -> bool:
|
||||
return self.source == SOURCE_PLACEHOLDER
|
||||
|
||||
def get(self, cid: int, default: Optional[str] = None) -> str:
|
||||
if cid in self.names:
|
||||
return self.names[cid]
|
||||
return default if default is not None else f"class_{cid}"
|
||||
|
||||
def sorted_names(self) -> List[str]:
|
||||
return [self.names[k] for k in sorted(self.names)]
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 路径
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def labels_path_for(weights: Union[str, Path]) -> Path:
|
||||
"""给出权重对应的旁挂标签文件路径。
|
||||
|
||||
只换后缀，目录不变：``models/yolo/detect_best_0.engine`` →
|
||||
``models/yolo/detect_best_0.labels.yaml``。
|
||||
|
||||
这样无论 ``resolve_weights()`` 最后选中了 .pt/.onnx/.engine 中的哪一个，
|
||||
算出来的标签路径都相同 —— 一份标签服务同一个模型的所有格式。
|
||||
"""
|
||||
p = Path(weights).expanduser()
|
||||
# 用 stem 手工拼接，而不是 with_suffix('.labels.yaml')：
|
||||
# 后者对 `a.b.onnx` 这类含点文件名容易踩坑，手工拼没有歧义。
|
||||
return p.parent / (p.stem + LABELS_SUFFIX)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 解析：把各种形状的标签定义统一成 {索引: 名字}
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _clean(text: object) -> str:
|
||||
# None 必须先拦掉：str(None) == "None" 是 truthy 的，
|
||||
# 直接 str() 会让空项变成一个字面量为 "None" 的类名。
|
||||
if text is None:
|
||||
return ""
|
||||
return str(text).strip()
|
||||
|
||||
|
||||
def _try_parse_structured(text: str) -> Optional[Dict[int, str]]:
|
||||
"""尝试把字符串当成 dict/list 的字面量解析。
|
||||
|
||||
为什么需要：ONNX metadata 里的 ``names`` 存的**不是** JSON，也不是逗号列表，
|
||||
而是 Python 字典的 repr（单引号）：
|
||||
|
||||
"{0: 'workpiece'}"
|
||||
|
||||
Ultralytics 是直接 ``str(names)`` 写进去的，所以 ``json.loads`` 会失败。
|
||||
两种都试一遍（JSON 标准、literal_eval 兜底），都不行才交给逗号解析。
|
||||
``ast.literal_eval`` 只解析字面量、不执行代码，是安全的。
|
||||
"""
|
||||
s = text.strip()
|
||||
if not s:
|
||||
return None
|
||||
import ast
|
||||
import json
|
||||
|
||||
for loader in (json.loads, ast.literal_eval):
|
||||
try:
|
||||
obj = loader(s)
|
||||
except Exception: # noqa: BLE001
|
||||
continue
|
||||
if isinstance(obj, (dict, list, tuple)):
|
||||
return parse_names_field(obj)
|
||||
return None
|
||||
|
||||
|
||||
def _parse_freeform_text(text: str) -> Dict[int, str]:
|
||||
"""解析"自由文本"形式的标签内容。
|
||||
|
||||
两种情况要分开，否则会互相吃掉：
|
||||
|
||||
- 一行一个名字（有人把 ``classes.txt`` 直接改名成 ``.labels.yaml``）：
|
||||
``"a\\nb"`` → ``{0:'a', 1:'b'}``
|
||||
- 单行逗号串：``"a, b"`` → ``{0:'a', 1:'b'}``
|
||||
|
||||
判据是"有没有多行"。只有一行时才按逗号切，
|
||||
否则 ``"a, b"`` 这种文件名里带逗号的写法会被切错。
|
||||
"""
|
||||
items = [
|
||||
ln.strip()
|
||||
for ln in text.splitlines()
|
||||
if ln.strip() and not ln.strip().startswith("#")
|
||||
]
|
||||
if len(items) > 1:
|
||||
return {i: n for i, n in enumerate(items)}
|
||||
return parse_label_list(text)
|
||||
|
||||
|
||||
def parse_names_field(value: object) -> Dict[int, str]:
|
||||
"""把 YAML/metadata 里的 ``names`` 字段规范成 ``{int: str}``。
|
||||
|
||||
容忍三种写法，因为它们各自都很常见：
|
||||
|
||||
- 字典（Ultralytics data.yaml 的写法）：``{0: workpiece, 1: hole}``
|
||||
- 列表（本模块写出的规范形式，索引即顺序）：``[workpiece, hole]``
|
||||
- 逗号分隔的字符串：``"workpiece, hole"``
|
||||
|
||||
跳过空项但**保留其索引位置**，这样 ``[a, "", c]`` 不会让 c 变成 1 号。
|
||||
无法解析成整数键的字典项会被忽略（不抛异常 —— 标签读不出来不该让程序崩）。
|
||||
"""
|
||||
if value is None:
|
||||
return {}
|
||||
if isinstance(value, dict):
|
||||
out: Dict[int, str] = {}
|
||||
for k, v in value.items():
|
||||
try:
|
||||
idx = int(k)
|
||||
except (TypeError, ValueError):
|
||||
continue
|
||||
name = _clean(v)
|
||||
if name:
|
||||
out[idx] = name
|
||||
return out
|
||||
if isinstance(value, (list, tuple)):
|
||||
out = {}
|
||||
for i, v in enumerate(value):
|
||||
name = _clean(v)
|
||||
if name:
|
||||
out[i] = name
|
||||
return out
|
||||
if isinstance(value, str):
|
||||
# 字符串可能是三种东西：dict/list 的字面量（ONNX metadata）、
|
||||
# 逗号列表、或者多行文本。先试结构化的，再退到逗号/多行解析。
|
||||
structured = _try_parse_structured(value)
|
||||
if structured is not None:
|
||||
return structured
|
||||
return _parse_freeform_text(value)
|
||||
return {}
|
||||
|
||||
|
||||
def parse_label_list(text: str) -> Dict[int, str]:
|
||||
"""解析逗号（或中文逗号、分号）分隔的类别名。
|
||||
|
||||
``"workpiece, hole"`` → ``{0: 'workpiece', 1: 'hole'}``
|
||||
|
||||
同时兼容 ``"0:workpiece, 1:hole"`` 这种带显式索引的写法，
|
||||
因为用户从别处拷配置过来时经常带着索引。空项被跳过。
|
||||
"""
|
||||
if not text or not text.strip():
|
||||
return {}
|
||||
parts = [p for p in re.split(r"[,，;；\n]", text)]
|
||||
out: Dict[int, str] = {}
|
||||
next_idx = 0
|
||||
for raw in parts:
|
||||
item = raw.strip()
|
||||
if not item:
|
||||
continue
|
||||
m = re.match(r"^\s*(\d+)\s*[:：]\s*(.+)$", item)
|
||||
if m:
|
||||
idx = int(m.group(1))
|
||||
name = m.group(2).strip()
|
||||
if name:
|
||||
out[idx] = name
|
||||
next_idx = max(next_idx, idx + 1)
|
||||
continue
|
||||
out[next_idx] = item
|
||||
next_idx += 1
|
||||
return out
|
||||
|
||||
|
||||
def read_label_text_file(path: Union[str, Path]) -> Dict[int, str]:
|
||||
"""读纯文本标签文件：一行一个类名（YOLO 的 classes.txt 就是这个格式）。
|
||||
|
||||
空行和 ``#`` 开头的注释行被忽略，且**不占索引**。
|
||||
这样用户可以在文件里随意加注释而不破坏索引对应关系。
|
||||
"""
|
||||
p = Path(path).expanduser()
|
||||
out: Dict[int, str] = {}
|
||||
idx = 0
|
||||
with p.open("r", encoding="utf-8") as f:
|
||||
for line in f:
|
||||
s = line.strip()
|
||||
if not s or s.startswith("#"):
|
||||
continue
|
||||
out[idx] = s
|
||||
idx += 1
|
||||
return out
|
||||
|
||||
|
||||
def placeholder_labels(nc: int) -> Dict[int, str]:
|
||||
"""没有标签可用时的占位名。
|
||||
|
||||
刻意用 ``class_0`` 而不是直接回退成字符串形式的数字 ``"0"``：
|
||||
后者和"真的有一个叫 0 的类"无法区分，正是原先难以发现问题所在。
|
||||
``class_`` 前缀一眼就能看出是没配标签。
|
||||
"""
|
||||
n = max(0, int(nc))
|
||||
return {i: f"class_{i}" for i in range(n)}
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 旁挂标签文件读写
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def format_labels_yaml(names: Dict[int, str], model: Optional[str] = None) -> str:
|
||||
"""生成旁挂标签文件的内容（带使用说明的注释头）。
|
||||
|
||||
注释头是刻意保留的：这个文件会被不熟悉的人直接打开编辑，
|
||||
把"第几行 = 第几类"写在文件里比写在文档里有效得多。
|
||||
"""
|
||||
lines: List[str] = []
|
||||
lines.append("# 模型类别标签（class names）")
|
||||
lines.append("#")
|
||||
lines.append("# 一个模型对应一份标签，放在权重旁边、同名前缀：")
|
||||
lines.append("# detect_best_0.engine ← 权重")
|
||||
lines.append("# detect_best_0.labels.yaml ← 本文件")
|
||||
lines.append("#")
|
||||
lines.append("# 写法：names 下面一行一个类名，第 1 行对应类别 0，第 2 行对应类别 1，依次类推。")
|
||||
lines.append("# 改完保存即生效，不需要重新转换模型、也不用重启环境（重跑节点即可）。")
|
||||
lines.append("#")
|
||||
lines.append("# 命令行查看/修改（推荐，不用手改本文件）：")
|
||||
lines.append("# bash ~/ros2/scripts/weights.sh labels <模型名>")
|
||||
lines.append("# bash ~/ros2/scripts/weights.sh labels <模型名> --set \"工件,通止规\"")
|
||||
lines.append("")
|
||||
if model:
|
||||
# 记下所属模型，便于发现"标签文件被拷给了另一个模型"这种错配。
|
||||
lines.append(f"model: {model}")
|
||||
lines.append("names:")
|
||||
if names:
|
||||
for k in sorted(names):
|
||||
lines.append(f" - {names[k]}")
|
||||
else:
|
||||
lines.append(" []")
|
||||
return "\n".join(lines) + "\n"
|
||||
|
||||
|
||||
def write_labels(
|
||||
weights: Union[str, Path],
|
||||
names: Dict[int, str],
|
||||
*,
|
||||
path: Optional[Union[str, Path]] = None,
|
||||
) -> Path:
|
||||
"""把标签写到权重旁边的旁挂文件，返回写入路径。"""
|
||||
dst = Path(path).expanduser() if path else labels_path_for(weights)
|
||||
dst.parent.mkdir(parents=True, exist_ok=True)
|
||||
model = Path(weights).stem
|
||||
dst.write_text(format_labels_yaml(names, model=model), encoding="utf-8")
|
||||
return dst
|
||||
|
||||
|
||||
def read_label_file(path: Union[str, Path]) -> Optional[Dict[int, str]]:
|
||||
"""读旁挂标签文件。文件不存在或内容为空返回 None（不抛异常）。"""
|
||||
p = Path(path).expanduser()
|
||||
if not p.is_file():
|
||||
return None
|
||||
try:
|
||||
raw = p.read_text(encoding="utf-8")
|
||||
except Exception: # noqa: BLE001
|
||||
return None
|
||||
try:
|
||||
import yaml
|
||||
|
||||
data = yaml.safe_load(raw)
|
||||
except Exception: # noqa: BLE001
|
||||
# 文件存在但读不动（语法错、编码错、权限）。返回 None 让调用方按
|
||||
# "没有标签"处理，而不是让整个推理进程崩掉 —— 标签缺失不该是致命的。
|
||||
return None
|
||||
|
||||
if isinstance(data, dict):
|
||||
# 规范形式：names: [...]
|
||||
names = parse_names_field(data.get("names"))
|
||||
if names:
|
||||
return names
|
||||
# 容忍 {0: 'a', 1: 'b'} 这种"顶层直接就是索引字典"的写法（Ultralytics data.yaml）
|
||||
names = {
|
||||
int(k): str(v).strip()
|
||||
for k, v in data.items()
|
||||
if str(k).lstrip("-").isdigit() and str(v).strip()
|
||||
}
|
||||
# 是合法 YAML 字典却读不出类别名 → 就是没有标签。
|
||||
# 这里**不能**再回落到"按行读文件"，否则会把 `names: []` 里的
|
||||
# 'names:' 这几个字当成类名。
|
||||
return names or None
|
||||
|
||||
if isinstance(data, (list, tuple)):
|
||||
return parse_names_field(data) or None
|
||||
|
||||
if isinstance(data, str):
|
||||
# 整个文件就是一个标量 —— 可能是"改名过来的 classes.txt"。
|
||||
# 注意必须用**原始文本**：YAML 会把多行纯标量折成一行
|
||||
# （"a\nb\n" → "a b"），用 data 的话换行已经没了，
|
||||
# "一行一个类名"会被当成一个带空格的长名字。
|
||||
return _parse_freeform_text(raw) or None
|
||||
|
||||
# 空文件 / 只有注释 / 其它标量类型：按行读（能正确处理注释行）。
|
||||
try:
|
||||
return read_label_text_file(p) or None
|
||||
except Exception: # noqa: BLE001
|
||||
return None
|
||||
|
||||
|
||||
def label_file_model(path: Union[str, Path]) -> Optional[str]:
|
||||
"""读旁挂标签文件里记录的 ``model:`` 字段（用于发现标签错配）。"""
|
||||
p = Path(path).expanduser()
|
||||
if not p.is_file():
|
||||
return None
|
||||
try:
|
||||
import yaml
|
||||
|
||||
data = yaml.safe_load(p.read_text(encoding="utf-8"))
|
||||
except Exception: # noqa: BLE001
|
||||
return None
|
||||
if isinstance(data, dict):
|
||||
v = data.get("model")
|
||||
return _clean(v) if v else None
|
||||
return None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 从模型自身读标签
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def onnx_metadata(path: Union[str, Path]) -> Dict[str, str]:
|
||||
"""读 ONNX 的 ``metadata_props``（全是字符串），读不到返回空字典。
|
||||
|
||||
用 ``load_external_data=False``：只读图结构不读权重，对 38MB 的模型也是毫秒级。
|
||||
Ultralytics 导出时会写入 ``names`` / ``imgsz`` / ``nc`` / ``task`` 等。
|
||||
"""
|
||||
p = Path(path).expanduser()
|
||||
if not p.is_file():
|
||||
return {}
|
||||
try:
|
||||
import onnx
|
||||
|
||||
m = onnx.load(str(p), load_external_data=False)
|
||||
except Exception: # noqa: BLE001
|
||||
return {}
|
||||
return {x.key: x.value for x in m.metadata_props}
|
||||
|
||||
|
||||
def labels_from_onnx(path: Union[str, Path]) -> Optional[Tuple[Dict[int, str], int]]:
|
||||
"""从 ONNX 的 ``metadata_props`` 读 ``names`` 与类别数 ``nc``。
|
||||
|
||||
返回 ``(names, nc)``；读不到返回 None。
|
||||
|
||||
注意 ``names`` 在 ONNX 里存的是 **Python 字典的 repr 字符串**（单引号），
|
||||
形如 ``"{0: 'workpiece'}"`` —— 不是 JSON。解析细节见 ``parse_names_field``。
|
||||
"""
|
||||
meta = onnx_metadata(path)
|
||||
if not meta:
|
||||
return None
|
||||
|
||||
names = parse_names_field(meta.get("names"))
|
||||
nc = 0
|
||||
raw_nc = meta.get("nc")
|
||||
if raw_nc is not None:
|
||||
try:
|
||||
nc = int(str(raw_nc).strip())
|
||||
except (TypeError, ValueError):
|
||||
nc = 0
|
||||
if not nc and names:
|
||||
nc = max(names) + 1
|
||||
return (names, nc) if names else None
|
||||
|
||||
|
||||
def labels_from_model(path: Union[str, Path]) -> Optional[Tuple[Dict[int, str], int]]:
|
||||
"""从模型自身 metadata 读标签（走 Ultralytics，主要给 .pt 用）。
|
||||
|
||||
对 .engine / .onnx 也能用（Ultralytics 会自己反序列化），但代价高：
|
||||
要加载整个模型。所以 ``read_labels()`` 只在必要时才调它。
|
||||
|
||||
Ultralytics 的 model 对象除了 ``names`` 还有 ``nc``；拿不到 nc 时用 names 推断。
|
||||
"""
|
||||
p = Path(path).expanduser()
|
||||
if not p.is_file():
|
||||
return None
|
||||
try:
|
||||
from ultralytics import YOLO
|
||||
|
||||
model = YOLO(str(p), task="detect")
|
||||
raw = getattr(model, "names", None) or {}
|
||||
names = parse_names_field(raw)
|
||||
nc = 0
|
||||
for attr in ("nc",):
|
||||
try:
|
||||
nc = int(getattr(model, attr) or 0)
|
||||
except Exception: # noqa: BLE001
|
||||
nc = 0
|
||||
if not nc and names:
|
||||
nc = max(names) + 1
|
||||
return (names, nc) if names else None
|
||||
except Exception: # noqa: BLE001
|
||||
return None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 统一入口
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def read_labels(
|
||||
weights: Union[str, Path],
|
||||
*,
|
||||
allow_model_metadata: bool = True,
|
||||
nc_hint: int = 0,
|
||||
) -> LabelSet:
|
||||
"""按优先级读出一个模型该用的标签。
|
||||
|
||||
优先级：旁挂文件 > ONNX metadata > 模型自身 metadata > 占位符。
|
||||
|
||||
旁挂优先是刻意的：它是用户能直接编辑的那一份，必须覆盖模型内嵌的旧值，
|
||||
否则"我明明改了标签为什么不生效"就会变成一个很难查的问题。
|
||||
|
||||
绝不抛异常。读不到就返回占位符并把原因记在 ``source`` 里，
|
||||
由调用方决定是记一条警告还是直接显示。
|
||||
"""
|
||||
p = Path(weights).expanduser()
|
||||
|
||||
side = labels_path_for(p)
|
||||
names = read_label_file(side)
|
||||
if names:
|
||||
return LabelSet(names=names, source=SOURCE_SIDECAR, path=side)
|
||||
|
||||
suffix = p.suffix.lower()
|
||||
# .pt 的标签只可能来自模型自身；.onnx 先看内嵌 metadata（快）；
|
||||
# .engine / .trt 没有可用 metadata（见模块头说明），直接落到占位符或模型加载。
|
||||
if suffix == ".onnx":
|
||||
got = labels_from_onnx(p)
|
||||
if got:
|
||||
return LabelSet(names=got[0], source=SOURCE_ONNX, path=p)
|
||||
elif suffix in (".pt", ".pth"):
|
||||
got = labels_from_model(p)
|
||||
if got:
|
||||
return LabelSet(names=got[0], source=SOURCE_MODEL, path=p)
|
||||
|
||||
if allow_model_metadata and suffix in (".engine", ".trt"):
|
||||
# engine 一般读不到（当前 TRT 没 metadata API），但也别直接放弃：
|
||||
# 若用户是用 Ultralytics 导出的 engine，metadata 可能在。
|
||||
got = labels_from_model(p)
|
||||
if got:
|
||||
return LabelSet(names=got[0], source=SOURCE_MODEL, path=p)
|
||||
|
||||
return LabelSet(names=placeholder_labels(nc_hint), source=SOURCE_PLACEHOLDER, path=None)
|
||||
|
||||
|
||||
def describe(labels: LabelSet) -> str:
|
||||
"""一句人能看懂的说明，用于日志：``workpiece (来源: 旁挂标签文件)``。"""
|
||||
names = ", ".join(labels.sorted_names()) or "(空)"
|
||||
src = SOURCE_LABELS_ZH.get(labels.source, labels.source)
|
||||
return f"{names} (来源: {src})"
|
||||
|
||||
|
||||
def label_file_status(
|
||||
weights: Union[str, Path],
|
||||
) -> Tuple[bool, Optional[Path], Optional[str]]:
|
||||
"""检查旁挂标签文件状态，供 doctor 用。
|
||||
|
||||
返回 ``(存在, 路径, 提示)``；``提示`` 为 None 表示没有问题。
|
||||
"""
|
||||
p = Path(weights).expanduser()
|
||||
side = labels_path_for(p)
|
||||
if not side.is_file():
|
||||
return False, side, f"缺标签文件：{side.name}"
|
||||
recorded = label_file_model(side)
|
||||
if recorded and Path(weights).stem != recorded:
|
||||
return True, side, (
|
||||
f"标签文件记录的 model 是 '{recorded}'，但当前权重是 "
|
||||
f"'{Path(weights).stem}' —— 可能是拷错了标签文件"
|
||||
)
|
||||
return True, side, None
|
||||
+74
@@ -0,0 +1,74 @@
|
||||
#!/usr/bin/env python3
|
||||
"""``arm_msgs/action/MoveToPose``（笛卡尔目标）的示例客户端。
|
||||
|
||||
等你有了 MoveIt 或自己的 IK 求解 action server 后，把它接上去即可。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import sys
|
||||
|
||||
import rclpy
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.node import Node
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
|
||||
from arm_msgs.action import MoveToPose
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
|
||||
class MoveToPoseClient(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('move_to_pose_client')
|
||||
self._client = ActionClient(self, MoveToPose, 'move_to_pose')
|
||||
|
||||
def send(self, x: float, y: float, z: float) -> None:
|
||||
if not self._client.wait_for_server(timeout_sec=5.0):
|
||||
self.get_logger().error(
|
||||
'move_to_pose action 不可用。'
|
||||
'请启动一个实现了它的笛卡尔规划器（例如 MoveIt）。'
|
||||
)
|
||||
return
|
||||
|
||||
goal = MoveToPose.Goal()
|
||||
goal.target_pose = PoseStamped()
|
||||
goal.target_pose.header.frame_id = 'base_link'
|
||||
goal.target_pose.header.stamp = self.get_clock().now().to_msg()
|
||||
goal.target_pose.pose.position.x = x
|
||||
goal.target_pose.pose.position.y = y
|
||||
goal.target_pose.pose.position.z = z
|
||||
# 单位姿态（不旋转）
|
||||
goal.target_pose.pose.orientation.w = 1.0
|
||||
goal.velocity_scale = 0.3
|
||||
goal.acceleration_scale = 0.3
|
||||
goal.avoid_collisions = True
|
||||
|
||||
self.get_logger().info(f'发送笛卡尔目标：({x:.3f}, {y:.3f}, {z:.3f})')
|
||||
future = self._client.send_goal_async(goal)
|
||||
rclpy.spin_until_future_complete(self, future)
|
||||
handle = future.result()
|
||||
if handle is None or not handle.accepted:
|
||||
self.get_logger().error('目标被拒绝')
|
||||
return
|
||||
|
||||
result_future = handle.get_result_async()
|
||||
rclpy.spin_until_future_complete(self, result_future)
|
||||
result = result_future.result().result
|
||||
self.get_logger().info(f'success={result.success} msg={result.message}')
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = MoveToPoseClient()
|
||||
# 默认演示位姿；可通过命令行覆盖：x y z
|
||||
vals = [0.35, 0.0, 0.35]
|
||||
if len(sys.argv) >= 4:
|
||||
vals = [float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3])]
|
||||
node.send(*vals)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
+152
@@ -0,0 +1,152 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Direct ONNX Runtime inference for Ultralytics end2end detect models.
|
||||
|
||||
为什么不走 ultralytics.YOLO：它对 .onnx 会按发行包名检查 onnxruntime，
|
||||
环境装的是 onnxruntime-gpu 时会触发 pip AutoUpdate（可能卡死/破坏环境）。
|
||||
本模块用 onnxruntime 直接推理，显式 CPU provider，一次性拍照场景足够快。
|
||||
|
||||
模型要求：Ultralytics 导出时 end2end=True（NMS 已融合），
|
||||
输出形状 [1, N, 6] = x1, y1, x2, y2, conf, cls（letterbox 坐标系）。
|
||||
detect_best_1.onnx 满足（metadata: end2end=True, nms=False）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
from pathlib import Path
|
||||
from typing import List, Optional, Union
|
||||
|
||||
import numpy as np
|
||||
|
||||
from arm_application.model_labels import (
|
||||
SOURCE_MODEL,
|
||||
LabelSet,
|
||||
parse_names_field,
|
||||
read_labels,
|
||||
)
|
||||
from arm_application.yolo_detector import Detection
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class OnnxWorkpieceDetector:
|
||||
"""给 detect_holes() 用的最小接口：detect(image, conf, iou, imgsz)。"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
weights: Union[str, Path],
|
||||
*,
|
||||
conf: float = 0.25,
|
||||
imgsz: int = 640,
|
||||
class_names: Optional[dict] = None,
|
||||
) -> None:
|
||||
import onnxruntime as ort
|
||||
|
||||
path = Path(weights).expanduser()
|
||||
if not path.is_file():
|
||||
raise FileNotFoundError(f"ONNX weights not found: {path}")
|
||||
so = ort.SessionOptions()
|
||||
so.log_severity_level = 3 # 关掉 ORT 刷屏告警
|
||||
self._sess = ort.InferenceSession(
|
||||
str(path), sess_options=so, providers=["CPUExecutionProvider"]
|
||||
)
|
||||
inp = self._sess.get_inputs()[0]
|
||||
self._iname = inp.name
|
||||
shape = [d if isinstance(d, int) and d > 0 else imgsz for d in inp.shape]
|
||||
self._in_h, self._in_w = int(shape[-2]), int(shape[-1])
|
||||
# 只认 ultralytics end2end 检测输出 ``[1, N, 6]``，否则**当场报错**。
|
||||
# 为什么必须在这里硬失败：2026-10-08 真机，把一个 ``task=pose`` 的模型
|
||||
# （输出 ``[1, 4+nc+3K, 8400]``）从这条分支解出来，得到的是 12 个垃圾框
|
||||
# ——conf 直接是坐标值（最高 70.87）、类名 ``class_73``、退化框 ``y1=y2=0``。
|
||||
# 而流水线照跑不误，只在二阶段"看不出工件"上体现出来，排查代价极大。
|
||||
# 宁可启动就失败（元规则：静默失效是头号敌人）。
|
||||
outs = self._sess.get_outputs()
|
||||
tail = outs[0].shape[-1] if (outs and outs[0].shape) else None
|
||||
if isinstance(tail, int) and tail != 6:
|
||||
raise RuntimeError(
|
||||
f"{path.name} 的检测输出最后一维是 {tail}，不是 6 —— 它不是"
|
||||
" ultralytics end2end 检测模型（[1,N,6]）。带 k0/k1/k2 关键点的"
|
||||
" pose 模型必须交给 pose_detector / build_detector，否则坐标会被"
|
||||
"当成置信度解出垃圾框，而且不会报错。"
|
||||
)
|
||||
self.conf = float(conf)
|
||||
self.imgsz = int(imgsz)
|
||||
# 类别名：显式传入 > 旁挂标签文件 > ONNX 内嵌 metadata > 占位符。
|
||||
# 原先这里是硬编码 {0: "workpiece"}，等于把类名写死在代码里 ——
|
||||
# 换一个多类模型就会展示错误的名字，而且没人改得动。
|
||||
if class_names:
|
||||
names = parse_names_field(class_names)
|
||||
self._labels = LabelSet(names=names, source=SOURCE_MODEL, path=path)
|
||||
else:
|
||||
self._labels = read_labels(path, allow_model_metadata=False)
|
||||
if self._labels.is_placeholder:
|
||||
logger.warning(
|
||||
"ONNX %s 没有类别标签，将用占位名显示。"
|
||||
"补法: bash ~/ros2/scripts/weights.sh labels %s --from-model",
|
||||
path.name,
|
||||
path.stem,
|
||||
)
|
||||
# 兼容旧属性名，避免外部代码引用时报 AttributeError。
|
||||
self._names = dict(self._labels.names)
|
||||
|
||||
def _letterbox(self, img: np.ndarray):
|
||||
h, w = img.shape[:2]
|
||||
scale = min(self._in_w / w, self._in_h / h)
|
||||
nw, nh = int(round(w * scale)), int(round(h * scale))
|
||||
import cv2
|
||||
|
||||
resized = cv2.resize(img, (nw, nh), interpolation=cv2.INTER_LINEAR)
|
||||
canvas = np.full((self._in_h, self._in_w, 3), 114, dtype=np.uint8)
|
||||
top = (self._in_h - nh) // 2
|
||||
left = (self._in_w - nw) // 2
|
||||
canvas[top : top + nh, left : left + nw] = resized
|
||||
return canvas, scale, left, top
|
||||
|
||||
def detect(
|
||||
self,
|
||||
image_bgr: np.ndarray,
|
||||
conf: Optional[float] = None,
|
||||
iou: Optional[float] = None, # end2end 模型已内置 NMS，忽略
|
||||
imgsz: Optional[int] = None, # 输入尺寸由模型决定，忽略
|
||||
**_kwargs,
|
||||
) -> List[Detection]:
|
||||
del iou, imgsz
|
||||
thr = float(conf if conf is not None else self.conf)
|
||||
canvas, scale, left, top = self._letterbox(image_bgr)
|
||||
blob = canvas[:, :, ::-1].transpose(2, 0, 1)[None].astype(np.float32) / 255.0
|
||||
out = self._sess.run(None, {self._iname: blob})[0] # [1, N, 6]
|
||||
dets: List[Detection] = []
|
||||
if out is None or out.size == 0:
|
||||
return dets
|
||||
arr = np.asarray(out).reshape(-1, out.shape[-1])
|
||||
h, w = image_bgr.shape[:2]
|
||||
for row in arr:
|
||||
if row.shape[0] < 6:
|
||||
continue
|
||||
x1, y1, x2, y2, score, cls = [float(v) for v in row[:6]]
|
||||
if score < thr:
|
||||
continue
|
||||
# letterbox 坐标 → 原图坐标
|
||||
xa = (x1 - left) / scale
|
||||
ya = (y1 - top) / scale
|
||||
xb = (x2 - left) / scale
|
||||
yb = (y2 - top) / scale
|
||||
xyxy = np.array(
|
||||
[
|
||||
float(np.clip(xa, 0, w - 1)),
|
||||
float(np.clip(ya, 0, h - 1)),
|
||||
float(np.clip(xb, 0, w - 1)),
|
||||
float(np.clip(yb, 0, h - 1)),
|
||||
]
|
||||
)
|
||||
cid = int(cls)
|
||||
dets.append(
|
||||
Detection(
|
||||
xyxy=xyxy,
|
||||
conf=float(score),
|
||||
cls_id=cid,
|
||||
cls_name=self._labels.get(cid),
|
||||
)
|
||||
)
|
||||
dets.sort(key=lambda d: d.conf, reverse=True)
|
||||
return dets
|
||||
+1220
File diff suppressed because it is too large
Load Diff
+226
@@ -0,0 +1,226 @@
|
||||
"""工位程序里的相机预览。
|
||||
|
||||
只订彩色和已对齐深度，不订左右红外。画面留在本进程，不另开查看器窗口。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
|
||||
_LOCK = threading.Lock()
|
||||
_STOP = threading.Event()
|
||||
_THREAD: threading.Thread | None = None
|
||||
_JPEG: dict[str, bytes] = {}
|
||||
_STATE = {"on": False, "error": "", "frames": {}, "stamp": 0.0}
|
||||
|
||||
|
||||
def targets() -> list[dict]:
|
||||
"""真机腕部相机和手外相机。仿真相机不进预览。"""
|
||||
from arm_application.operator_console import _arms, _site
|
||||
from arm_application.site_io import overhead_camera
|
||||
|
||||
found: list[dict] = []
|
||||
seen: set[str] = set()
|
||||
for arm in _arms():
|
||||
name = str(arm.get("camera") or "").strip().strip("/")
|
||||
if not name or name.startswith("sim") or name in seen:
|
||||
continue
|
||||
seen.add(name)
|
||||
found.append({"id": name, "label": f"{arm.get('label') or name} 腕部"})
|
||||
cam, _others = overhead_camera(_site(), None)
|
||||
name = str((cam or {}).get("name") or "").strip().strip("/")
|
||||
if name and not name.startswith("sim") and name not in seen:
|
||||
found.append({"id": name, "label": str((cam or {}).get("label") or "手外相机")})
|
||||
return found
|
||||
|
||||
|
||||
def state() -> dict:
|
||||
with _LOCK:
|
||||
return {
|
||||
"on": _STATE["on"],
|
||||
"error": _STATE["error"],
|
||||
"stamp": _STATE["stamp"],
|
||||
"frames": dict(_STATE["frames"]),
|
||||
}
|
||||
|
||||
|
||||
def frame(name: str, kind: str) -> bytes | None:
|
||||
with _LOCK:
|
||||
return _JPEG.get(f"{name}_{kind}")
|
||||
|
||||
|
||||
def _close_external_viewers() -> None:
|
||||
"""关掉独立的四窗格查看器。它会把左右红外挂出来，本程序不再用它。"""
|
||||
subprocess.run(["pkill", "-f", "lib/arm_application/realsense_view"], check=False)
|
||||
subprocess.run(["pkill", "-f", "arm_application.realsense_view"], check=False)
|
||||
|
||||
|
||||
def _jpeg(img: np.ndarray) -> bytes | None:
|
||||
import cv2
|
||||
|
||||
if img.size == 0:
|
||||
return None
|
||||
view = img
|
||||
if view.shape[1] > 640:
|
||||
scale = 640.0 / float(view.shape[1])
|
||||
view = cv2.resize(view, (640, max(1, int(view.shape[0] * scale))))
|
||||
ok, buf = cv2.imencode(".jpg", view, [int(cv2.IMWRITE_JPEG_QUALITY), 70])
|
||||
if not ok:
|
||||
return None
|
||||
return buf.tobytes()
|
||||
|
||||
|
||||
def _depth_bgr(depth: np.ndarray) -> np.ndarray:
|
||||
import cv2
|
||||
|
||||
vis = np.zeros((depth.shape[0], depth.shape[1], 3), np.uint8)
|
||||
valid = depth > 0
|
||||
if not bool(valid.any()):
|
||||
return vis
|
||||
values = depth.astype(np.float32)
|
||||
lo, hi = np.percentile(values[valid], [5, 95])
|
||||
hi = max(float(hi), float(lo) + 1.0)
|
||||
gray = (np.clip((values - lo) / (hi - lo), 0.0, 1.0) * 255.0).astype(np.uint8)
|
||||
gray[~valid] = 0
|
||||
colored = cv2.applyColorMap(gray, cv2.COLORMAP_JET)
|
||||
colored[~valid] = 0
|
||||
return colored
|
||||
|
||||
|
||||
def _color_array(msg) -> np.ndarray | None:
|
||||
if msg.encoding not in ("bgr8", "rgb8") or msg.height <= 0 or msg.width <= 0:
|
||||
return None
|
||||
arr = np.frombuffer(bytes(msg.data), dtype=np.uint8)
|
||||
need = int(msg.height) * int(msg.width) * 3
|
||||
if arr.size < need:
|
||||
return None
|
||||
arr = arr[:need].reshape(int(msg.height), int(msg.width), 3)
|
||||
if msg.encoding == "rgb8":
|
||||
arr = arr[:, :, ::-1]
|
||||
return np.ascontiguousarray(arr)
|
||||
|
||||
|
||||
def _depth_array(msg) -> np.ndarray | None:
|
||||
if msg.encoding not in ("16UC1", "mono16") or msg.height <= 0 or msg.width <= 0:
|
||||
return None
|
||||
arr = np.frombuffer(bytes(msg.data), dtype=np.uint16)
|
||||
need = int(msg.height) * int(msg.width)
|
||||
if arr.size < need:
|
||||
return None
|
||||
return np.ascontiguousarray(arr[:need].reshape(int(msg.height), int(msg.width)))
|
||||
|
||||
|
||||
def _loop(cams: list[dict]) -> None:
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import Image
|
||||
|
||||
latest: dict[str, np.ndarray] = {}
|
||||
lock = threading.Lock()
|
||||
|
||||
class _Node(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("operator_camera_view")
|
||||
for cam in cams:
|
||||
name = cam["id"]
|
||||
self.create_subscription(
|
||||
Image,
|
||||
f"/{name}/color/image_raw",
|
||||
self._color(name),
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
self.create_subscription(
|
||||
Image,
|
||||
f"/{name}/aligned_depth_to_color/image_raw",
|
||||
self._depth(name),
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
|
||||
def _color(self, name: str):
|
||||
def _cb(msg: Image) -> None:
|
||||
arr = _color_array(msg)
|
||||
if arr is None:
|
||||
return
|
||||
with lock:
|
||||
latest[f"{name}_color"] = arr
|
||||
return _cb
|
||||
|
||||
def _depth(self, name: str):
|
||||
def _cb(msg: Image) -> None:
|
||||
arr = _depth_array(msg)
|
||||
if arr is None:
|
||||
return
|
||||
with lock:
|
||||
latest[f"{name}_depth"] = arr
|
||||
return _cb
|
||||
|
||||
rclpy.init()
|
||||
node = _Node()
|
||||
next_encode = 0.0
|
||||
try:
|
||||
while not _STOP.is_set():
|
||||
rclpy.spin_once(node, timeout_sec=0.05)
|
||||
now = time.monotonic()
|
||||
if now < next_encode:
|
||||
continue
|
||||
next_encode = now + 0.12
|
||||
with lock:
|
||||
snap = dict(latest)
|
||||
latest.clear()
|
||||
frames = {}
|
||||
blobs = {}
|
||||
for key, arr in snap.items():
|
||||
name, _, kind = key.rpartition("_")
|
||||
img = _depth_bgr(arr) if kind == "depth" else arr
|
||||
blob = _jpeg(img)
|
||||
if not blob:
|
||||
continue
|
||||
blobs[key] = blob
|
||||
frames[key] = True
|
||||
if blobs:
|
||||
with _LOCK:
|
||||
_JPEG.update(blobs)
|
||||
_STATE["frames"].update(frames)
|
||||
_STATE["stamp"] = time.time()
|
||||
_STATE["error"] = ""
|
||||
except Exception as exc:
|
||||
with _LOCK:
|
||||
_STATE["error"] = str(exc)
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
with _LOCK:
|
||||
_STATE["on"] = False
|
||||
|
||||
|
||||
def start() -> str:
|
||||
global _THREAD
|
||||
cams = targets()
|
||||
if not cams:
|
||||
with _LOCK:
|
||||
_STATE["error"] = "site 里没有可预览的真机相机"
|
||||
return _STATE["error"]
|
||||
_close_external_viewers()
|
||||
with _LOCK:
|
||||
if _STATE["on"]:
|
||||
return "相机预览已经在程序里"
|
||||
_STATE["on"] = True
|
||||
_STATE["error"] = ""
|
||||
_STOP.clear()
|
||||
_THREAD = threading.Thread(target=_loop, args=(cams,), daemon=True, name="cam-view")
|
||||
_THREAD.start()
|
||||
names = "、".join(cam["label"] for cam in cams)
|
||||
return f"预览订了 {names} 的彩色和深度，不含左右红外。"
|
||||
|
||||
|
||||
def stop() -> None:
|
||||
_STOP.set()
|
||||
thread = _THREAD
|
||||
if thread is not None:
|
||||
thread.join(timeout=1.5)
|
||||
+858
@@ -0,0 +1,858 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="zh-CN">
|
||||
<head>
|
||||
<meta charset="utf-8" />
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1" />
|
||||
<title>ORISYS 通止规工位</title>
|
||||
<style>
|
||||
:root {
|
||||
--bg: #070708;
|
||||
--panel: #101216;
|
||||
--panel-2: #171a20;
|
||||
--line: #2a2e36;
|
||||
--text: #f3f4f6;
|
||||
--muted: #8b929d;
|
||||
--blue: #12315d;
|
||||
--blue-2: #1c4f98;
|
||||
--ok: #3f9d6e;
|
||||
--warn: #c8963e;
|
||||
--bad: #c45c5c;
|
||||
}
|
||||
* { box-sizing: border-box; }
|
||||
html, body { margin: 0; height: 100%; background: var(--bg); color: var(--text); }
|
||||
body {
|
||||
font-family: "Noto Sans SC", "Source Han Sans SC", "PingFang SC", "Segoe UI", sans-serif;
|
||||
display: grid;
|
||||
grid-template-columns: 220px 1fr;
|
||||
grid-template-rows: 72px 1fr;
|
||||
height: 100vh;
|
||||
overflow: hidden;
|
||||
}
|
||||
header {
|
||||
grid-column: 1 / -1;
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 16px;
|
||||
padding: 0 18px;
|
||||
border-bottom: 1px solid var(--line);
|
||||
background: #0b0c0e;
|
||||
}
|
||||
header img { height: 46px; width: auto; }
|
||||
header .name { display: flex; flex-direction: column; gap: 2px; }
|
||||
header .name b { font-size: 15px; font-weight: 560; letter-spacing: 0.08em; }
|
||||
header .name span { color: var(--muted); font-size: 12px; }
|
||||
header .grow { flex: 1; }
|
||||
.chips { display: flex; gap: 6px; flex-wrap: wrap; }
|
||||
.arms { display: flex; gap: 6px; }
|
||||
.arms button, .btn {
|
||||
border-radius: 8px;
|
||||
border: 1px solid var(--line);
|
||||
background: var(--panel-2);
|
||||
color: var(--text);
|
||||
padding: 8px 12px;
|
||||
cursor: pointer;
|
||||
font-size: 13px;
|
||||
}
|
||||
.arms button.on { background: var(--blue); border-color: #1a4580; }
|
||||
.btn.primary { background: var(--blue-2); border-color: #2a62b0; }
|
||||
.btn.on { background: var(--blue); border-color: #1a4580; }
|
||||
.btn.danger { background: #6e2a2a; border-color: #8a3a3a; }
|
||||
.clock { color: var(--muted); font-variant-numeric: tabular-nums; font-size: 13px; min-width: 72px; text-align: right; }
|
||||
aside {
|
||||
border-right: 1px solid var(--line);
|
||||
background: #0b0c0e;
|
||||
padding: 16px 12px;
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 6px;
|
||||
}
|
||||
nav { display: flex; flex-direction: column; gap: 6px; }
|
||||
nav button {
|
||||
display: block;
|
||||
width: 100%;
|
||||
text-align: left;
|
||||
background: transparent;
|
||||
color: var(--text);
|
||||
border: 1px solid transparent;
|
||||
border-radius: 8px;
|
||||
padding: 11px 12px;
|
||||
font-size: 14px;
|
||||
cursor: pointer;
|
||||
}
|
||||
nav button small { display: block; color: var(--muted); font-size: 11px; margin-top: 3px; }
|
||||
nav button.active { background: var(--blue); border-color: #1a4580; }
|
||||
nav button.active small { color: #c5d4ea; }
|
||||
nav button:hover { border-color: var(--line); }
|
||||
.side-note { margin-top: auto; color: var(--muted); font-size: 12px; line-height: 1.55; padding: 0 4px; }
|
||||
main { min-width: 0; min-height: 0; overflow: auto; }
|
||||
section { display: none; height: 100%; padding: 16px 18px; }
|
||||
section.on { display: flex; flex-direction: column; gap: 12px; }
|
||||
h2 { margin: 0; font-size: 18px; font-weight: 560; }
|
||||
.lead { margin: 0; color: var(--muted); font-size: 13px; line-height: 1.55; }
|
||||
.dash {
|
||||
flex: 1;
|
||||
min-height: 0;
|
||||
display: grid;
|
||||
grid-template-columns: 1.35fr 0.95fr;
|
||||
grid-template-rows: minmax(0, 1fr) 168px;
|
||||
gap: 12px;
|
||||
}
|
||||
.devices, .mini-forces { display: grid; gap: 12px; min-height: 0; }
|
||||
.devices { grid-template-columns: 1fr 1fr; grid-template-rows: 1fr 1fr; }
|
||||
.mini-forces { grid-template-rows: 1fr 1fr; }
|
||||
.flow { grid-column: 1 / -1; display: grid; grid-template-columns: 1fr 1fr 1fr; gap: 12px; }
|
||||
.card {
|
||||
background: var(--panel);
|
||||
border: 1px solid var(--line);
|
||||
border-radius: 12px;
|
||||
padding: 14px;
|
||||
min-height: 0;
|
||||
}
|
||||
.card h3 {
|
||||
margin: 0 0 8px;
|
||||
font-size: 12px;
|
||||
color: #9aa3b2;
|
||||
font-weight: 500;
|
||||
letter-spacing: 0.12em;
|
||||
}
|
||||
.stat { font-size: 32px; font-variant-numeric: tabular-nums; letter-spacing: 0.02em; }
|
||||
.sub { color: var(--muted); font-size: 12px; margin-top: 6px; line-height: 1.5; white-space: pre-line; }
|
||||
.pill {
|
||||
display: inline-block;
|
||||
border-radius: 999px;
|
||||
padding: 2px 8px;
|
||||
font-size: 12px;
|
||||
border: 1px solid var(--line);
|
||||
margin-right: 6px;
|
||||
}
|
||||
.pill.ok { color: var(--ok); border-color: #245c40; }
|
||||
.pill.warn { color: var(--warn); border-color: #6d5424; }
|
||||
.pill.bad { color: var(--bad); border-color: #6d3030; }
|
||||
.split { display: grid; grid-template-columns: 1fr 1fr; gap: 12px; flex: 1; min-height: 0; }
|
||||
.split.three { grid-template-columns: 1fr 1fr 1fr; }
|
||||
.instrument { display: flex; flex-direction: column; gap: 10px; height: 100%; }
|
||||
.readout { display: flex; gap: 18px; }
|
||||
.readout div { min-width: 88px; }
|
||||
.readout em { display: block; color: var(--muted); font-style: normal; font-size: 12px; }
|
||||
.readout strong { font-size: 28px; font-weight: 520; font-variant-numeric: tabular-nums; }
|
||||
.meter { display: grid; grid-template-columns: 52px 1fr 72px; align-items: center; gap: 8px; font-size: 12px; color: var(--muted); }
|
||||
.track { height: 8px; background: #1c212b; border-radius: 99px; overflow: hidden; }
|
||||
.track i { display: block; height: 100%; width: 0; background: linear-gradient(90deg, #12315d, #4c86d4); }
|
||||
.finder {
|
||||
flex: 1;
|
||||
min-height: 180px;
|
||||
border: 1px solid #243044;
|
||||
border-radius: 10px;
|
||||
background:
|
||||
linear-gradient(#4c86d4, #4c86d4) left top / 18px 2px no-repeat,
|
||||
linear-gradient(#4c86d4, #4c86d4) left top / 2px 18px no-repeat,
|
||||
linear-gradient(#4c86d4, #4c86d4) right top / 18px 2px no-repeat,
|
||||
linear-gradient(#4c86d4, #4c86d4) right top / 2px 18px no-repeat,
|
||||
linear-gradient(#4c86d4, #4c86d4) left bottom / 18px 2px no-repeat,
|
||||
linear-gradient(#4c86d4, #4c86d4) left bottom / 2px 18px no-repeat,
|
||||
linear-gradient(#4c86d4, #4c86d4) right bottom / 18px 2px no-repeat,
|
||||
linear-gradient(#4c86d4, #4c86d4) right bottom / 2px 18px no-repeat,
|
||||
#0c0e12;
|
||||
display: flex;
|
||||
align-items: flex-end;
|
||||
padding: 14px;
|
||||
color: var(--muted);
|
||||
font-size: 13px;
|
||||
}
|
||||
.rail { display: grid; grid-template-columns: repeat(6, minmax(0, 1fr)); gap: 8px; }
|
||||
.step { background: var(--panel); border: 1px solid var(--line); border-radius: 10px; padding: 12px; min-height: 78px; }
|
||||
.step b { display: block; color: #7f93b5; font-size: 12px; letter-spacing: 0.08em; }
|
||||
.step span { display: block; margin-top: 8px; font-size: 14px; }
|
||||
.ops { display: grid; grid-template-columns: repeat(3, minmax(0, 1fr)); gap: 12px; }
|
||||
.row { display: flex; gap: 8px; flex-wrap: wrap; }
|
||||
.pair { display: grid; grid-template-columns: 1fr 1fr; gap: 8px; }
|
||||
.pair img:not([src]) { visibility: hidden; }
|
||||
.pair img {
|
||||
width: 100%;
|
||||
height: 200px;
|
||||
object-fit: contain;
|
||||
background: #0c0e12;
|
||||
border-radius: 8px;
|
||||
border: 1px solid #243044;
|
||||
}
|
||||
.charts { display: grid; grid-template-columns: 1fr 1fr; gap: 12px; }
|
||||
canvas.chart { width: 100%; height: 210px; background: #0c0e12; border-radius: 8px; display: block; }
|
||||
.legend { display: flex; gap: 14px; font-size: 12px; color: var(--muted); margin-bottom: 6px; }
|
||||
.legend i { display: inline-block; width: 18px; height: 3px; margin-right: 6px; vertical-align: middle; }
|
||||
pre.out {
|
||||
margin: 0;
|
||||
min-height: 140px;
|
||||
max-height: 260px;
|
||||
overflow: auto;
|
||||
background: #0c0e12;
|
||||
border-radius: 8px;
|
||||
padding: 10px;
|
||||
color: #c5d4ea;
|
||||
font-size: 12px;
|
||||
white-space: pre-wrap;
|
||||
}
|
||||
dialog {
|
||||
border: 1px solid var(--line);
|
||||
background: #12141a;
|
||||
color: var(--text);
|
||||
border-radius: 12px;
|
||||
padding: 18px;
|
||||
width: min(480px, calc(100vw - 32px));
|
||||
}
|
||||
dialog::backdrop { background: rgba(0, 0, 0, 0.55); }
|
||||
dialog h3 { margin: 0; }
|
||||
dialog code {
|
||||
display: block;
|
||||
margin: 12px 0;
|
||||
padding: 10px;
|
||||
background: #0b0c0e;
|
||||
border-radius: 8px;
|
||||
color: #c5d4ea;
|
||||
font-size: 12px;
|
||||
white-space: pre-wrap;
|
||||
}
|
||||
.toast {
|
||||
position: fixed; right: 18px; bottom: 18px;
|
||||
background: #151922; border: 1px solid var(--line);
|
||||
border-radius: 10px; padding: 12px 14px; max-width: 420px;
|
||||
display: none;
|
||||
}
|
||||
@media (max-width: 1100px) {
|
||||
body { grid-template-columns: 1fr; grid-template-rows: auto auto 1fr; height: auto; overflow: auto; }
|
||||
header { flex-wrap: wrap; height: auto; padding: 12px; }
|
||||
.dash, .split, .split.three, .ops, .rail, .flow { grid-template-columns: 1fr 1fr; }
|
||||
}
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<header>
|
||||
<img src="/logo.png" alt="ORISYS 叠动科技" />
|
||||
<div class="name">
|
||||
<b>通止规工位</b>
|
||||
<span id="arm-line">左臂 89</span>
|
||||
</div>
|
||||
<div class="grow"></div>
|
||||
<div class="chips" id="chips"></div>
|
||||
<div class="arms" id="arms"></div>
|
||||
<div class="clock" id="clock">--:--:--</div>
|
||||
</header>
|
||||
<aside>
|
||||
<nav>
|
||||
<button class="active" data-page="overview">总览<small>设备与流程</small></button>
|
||||
<button data-page="tactile">视触觉<small>切向、热成像、曲线</small></button>
|
||||
<button data-page="handeye">手眼<small>眼在手上 / 手外</small></button>
|
||||
<button data-page="camera">相机<small>D405 与 D435</small></button>
|
||||
<button data-page="gripper">夹爪<small>平行爪与旋转爪</small></button>
|
||||
<button data-page="assembly">装配<small>S00 到 S11</small></button>
|
||||
<button data-page="operate">操作<small>启动与确认</small></button>
|
||||
</nav>
|
||||
<div class="side-note">读的是本机 USB、进程和话题。动臂、张合夹爪都要再确认一次。</div>
|
||||
</aside>
|
||||
<main>
|
||||
<section id="overview" class="on">
|
||||
<div>
|
||||
<h2>现场</h2>
|
||||
<p class="lead" id="arm-lead"></p>
|
||||
</div>
|
||||
<div class="dash">
|
||||
<div class="devices" id="usb-cards"></div>
|
||||
<div class="mini-forces" id="mini-forces"></div>
|
||||
<div class="flow" id="flow"></div>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section id="tactile">
|
||||
<div>
|
||||
<h2>视触觉</h2>
|
||||
<p class="lead">切向力是 SDK 的剪切场箭头，热成像是接触散度的伪彩。下面两条曲线是法向、切向 X、切向 Y，同一时间轴。单位是形变读数，不是牛顿。</p>
|
||||
</div>
|
||||
<div class="row">
|
||||
<button class="btn primary" id="view-on" type="button">开始成像</button>
|
||||
<button class="btn" id="view-off" type="button">停止成像</button>
|
||||
<button class="btn" data-act="tactile_up">启动三力节点</button>
|
||||
<button class="btn" data-act="tactile_down">停止触觉节点</button>
|
||||
</div>
|
||||
<p class="sub" id="view-note"></p>
|
||||
<div class="split" id="views"></div>
|
||||
<div class="charts">
|
||||
<div class="card">
|
||||
<h3>左指</h3>
|
||||
<div class="legend"><span><i style="background:#e8e8e8"></i>法向</span><span><i style="background:#4c86d4"></i>切向 X</span><span><i style="background:#d7a15a"></i>切向 Y</span></div>
|
||||
<canvas class="chart" id="chart-left"></canvas>
|
||||
</div>
|
||||
<div class="card">
|
||||
<h3>右指</h3>
|
||||
<div class="legend"><span><i style="background:#e8e8e8"></i>法向</span><span><i style="background:#4c86d4"></i>切向 X</span><span><i style="background:#d7a15a"></i>切向 Y</span></div>
|
||||
<canvas class="chart" id="chart-right"></canvas>
|
||||
</div>
|
||||
</div>
|
||||
<p class="sub" id="tac-note"></p>
|
||||
</section>
|
||||
|
||||
<section id="handeye">
|
||||
<div>
|
||||
<h2>手眼标定</h2>
|
||||
<p class="lead">眼在手上用腕部 D405 和 A5 板，结果是相机到法兰。眼在手外用顶置 D435 和 A4 板，结果是相机到基座。两份文件不能混用。示教和求解会动臂。</p>
|
||||
</div>
|
||||
<div class="row" id="he-modes">
|
||||
<button class="btn on" type="button" data-mode="eye_in_hand">眼在手上</button>
|
||||
<button class="btn" type="button" data-mode="eye_to_hand">眼在手外</button>
|
||||
</div>
|
||||
<div class="card" id="he-card"></div>
|
||||
<div class="row">
|
||||
<button class="btn" data-act="handeye_show">查看配置</button>
|
||||
<button class="btn danger" data-act="handeye_teach">示教采点</button>
|
||||
<button class="btn" data-act="handeye_merge">合并示教点</button>
|
||||
<button class="btn danger" data-act="handeye_calibrate">求解手眼</button>
|
||||
</div>
|
||||
<pre class="out" id="he-out">查看配置后，脚本输出显示在这里。</pre>
|
||||
</section>
|
||||
|
||||
<section id="camera">
|
||||
<div>
|
||||
<h2>相机</h2>
|
||||
<p class="lead">腕部 D405 是 848×480 @ 30，手外 D435 是 1280×720 @ 30。预览开在桌面窗口，这一页只报链路，不造画面。</p>
|
||||
</div>
|
||||
<div class="split three" id="cams"></div>
|
||||
<div class="row">
|
||||
<button class="btn primary" data-act="camera_d405">本臂 D405 预览</button>
|
||||
<button class="btn" data-act="camera_d435">D435 预览</button>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section id="gripper">
|
||||
<div>
|
||||
<h2>夹爪</h2>
|
||||
<p class="lead" id="grip-lead"></p>
|
||||
</div>
|
||||
<div class="split" id="grip-body"></div>
|
||||
</section>
|
||||
|
||||
<section id="assembly">
|
||||
<div>
|
||||
<h2>通止规装配</h2>
|
||||
<p class="lead">顺序与状态机一致。干跑只打印计划。真跑会动臂、会拧，而且这台机器上还没跑过完整真机装配。</p>
|
||||
</div>
|
||||
<div class="rail" id="rail"></div>
|
||||
<div class="row">
|
||||
<button class="btn" data-act="gauge_dry">干跑</button>
|
||||
<button class="btn danger" data-act="gauge_run">真跑</button>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section id="operate">
|
||||
<div>
|
||||
<h2>操作</h2>
|
||||
<p class="lead" id="op-lead"></p>
|
||||
</div>
|
||||
<div class="ops" id="ops"></div>
|
||||
</section>
|
||||
</main>
|
||||
|
||||
<dialog id="ask">
|
||||
<h3 id="ask-title">确认</h3>
|
||||
<p class="sub" id="ask-text"></p>
|
||||
<code id="ask-cmd"></code>
|
||||
<div class="row">
|
||||
<button class="btn" id="ask-no">取消</button>
|
||||
<button class="btn primary" id="ask-yes">确认执行</button>
|
||||
</div>
|
||||
</dialog>
|
||||
<div class="toast" id="toast"></div>
|
||||
|
||||
<script>
|
||||
const titles = {overview:"总览", tactile:"视触觉", camera:"相机", assembly:"装配", operate:"操作", gripper:"夹爪", handeye:"手眼"};
|
||||
const boot = /*BOOT*/null;
|
||||
let arm = "89";
|
||||
let catalog = (boot && boot.catalog) || {actions:[], stages:[]};
|
||||
let status = (boot && boot.status) || null;
|
||||
let pending = null;
|
||||
let heMode = "eye_in_hand";
|
||||
|
||||
function $(id){ return document.getElementById(id); }
|
||||
function pill(ok, word, warn){
|
||||
const s = document.createElement("span");
|
||||
s.className = "pill " + (ok ? "ok" : warn ? "warn" : "bad");
|
||||
s.textContent = word;
|
||||
return s;
|
||||
}
|
||||
function toast(text){
|
||||
const el = $("toast");
|
||||
el.textContent = text;
|
||||
el.style.display = "block";
|
||||
clearTimeout(toast._t);
|
||||
toast._t = setTimeout(() => { el.style.display = "none"; }, 4200);
|
||||
}
|
||||
async function api(path, body){
|
||||
const res = await fetch(path, body ? {
|
||||
method:"POST", headers:{"Content-Type":"application/json"}, body: JSON.stringify(body)
|
||||
} : undefined);
|
||||
return res.json();
|
||||
}
|
||||
function cmdOf(action){
|
||||
const spec = catalog.actions.find(a => a.id === action);
|
||||
if (!spec) return action;
|
||||
const args = (spec.args || []).map(a => a.replace("{arm}", arm).replace("{mode}", heMode)).join(" ");
|
||||
return "bash scripts/" + ({
|
||||
real_up:"run_real.sh", camera_d405:"run_camera_d405.sh", camera_d435:"run_camera_d435.sh",
|
||||
tactile_up:"run_tactile.sh", gripper_up:"run_gripper.sh", gripper_probe:"run_gripper.sh",
|
||||
gripper_open:"run_gripper.sh", gripper_close:"run_gripper.sh", gripper_rotate:"run_gripper.sh",
|
||||
pipeline:"run_all.sh", gauge_dry:"run_gauge_assembly.sh", gauge_run:"run_gauge_assembly.sh",
|
||||
handeye_show:"run_handeye.sh", handeye_teach:"run_handeye.sh",
|
||||
handeye_merge:"run_handeye.sh", handeye_calibrate:"run_handeye.sh"
|
||||
}[action] || "scripts") + (args ? " " + args : "");
|
||||
}
|
||||
async function fire(action, confirm){
|
||||
const data = await api("/api/action", {action, arm, confirm: !!confirm, mode: heMode});
|
||||
toast(data.message || (data.ok ? "已发出" : "失败"));
|
||||
if (String(action).startsWith("handeye") && $("he-out")) {
|
||||
$("he-out").textContent = data.detail || data.message || "";
|
||||
}
|
||||
}
|
||||
function ask(action){
|
||||
const spec = catalog.actions.find(a => a.id === action);
|
||||
if (action === "gripper_rotate" && arm !== "91"){
|
||||
toast("旋转只对右臂 91");
|
||||
return;
|
||||
}
|
||||
if (spec && spec.moves){
|
||||
pending = action;
|
||||
$("ask-title").textContent = spec.title;
|
||||
$("ask-text").textContent = spec.blurb + " 当前臂 " + arm + "。";
|
||||
$("ask-cmd").textContent = cmdOf(action);
|
||||
$("ask").showModal();
|
||||
return;
|
||||
}
|
||||
fire(action, false);
|
||||
}
|
||||
function svc(id){ return (status?.services || []).find(s => s.id === id); }
|
||||
function meters(card, data){
|
||||
const keys = [["法向", data.fnormal], ["切向 X", data.fshearx], ["切向 Y", data.fsheary]];
|
||||
const peak = Math.max(1, ...keys.map(([,v]) => Math.abs(typeof v === "number" ? v : 0)));
|
||||
keys.forEach(([name, value]) => {
|
||||
const row = document.createElement("div");
|
||||
row.className = "meter";
|
||||
const lab = document.createElement("span");
|
||||
lab.textContent = name;
|
||||
const track = document.createElement("div");
|
||||
track.className = "track";
|
||||
const bar = document.createElement("i");
|
||||
const n = typeof value === "number" ? value : null;
|
||||
bar.style.width = n == null ? "0%" : (Math.abs(n) / peak * 100).toFixed(1) + "%";
|
||||
track.appendChild(bar);
|
||||
const num = document.createElement("span");
|
||||
num.textContent = n == null ? "—" : n.toFixed(1);
|
||||
row.append(lab, track, num);
|
||||
card.appendChild(row);
|
||||
});
|
||||
}
|
||||
function forceCard(title, data, tall){
|
||||
const card = document.createElement("div");
|
||||
card.className = "card instrument";
|
||||
const h = document.createElement("h3");
|
||||
h.textContent = title;
|
||||
card.appendChild(h);
|
||||
const has = data && (data.connected !== undefined || data.fnormal !== undefined);
|
||||
if (tall) {
|
||||
const row = document.createElement("div");
|
||||
row.className = "readout";
|
||||
[["法向", data.fnormal], ["切向 X", data.fshearx], ["切向 Y", data.fsheary], ["fps", data.fps]].forEach(([k,v]) => {
|
||||
const d = document.createElement("div");
|
||||
const em = document.createElement("em");
|
||||
em.textContent = k;
|
||||
const st = document.createElement("strong");
|
||||
st.textContent = typeof v === "number" ? (k === "fps" ? v.toFixed(1) : v.toFixed(1)) : "—";
|
||||
d.append(em, st);
|
||||
row.appendChild(d);
|
||||
});
|
||||
card.appendChild(row);
|
||||
}
|
||||
meters(card, data || {});
|
||||
const sub = document.createElement("div");
|
||||
sub.className = "sub";
|
||||
sub.textContent = has
|
||||
? ((data.contact ? "有接触" : "无接触") + (typeof data.fps === "number" ? " · " + data.fps.toFixed(1) + " fps" : "") + (data.connected === false ? " · 未连接" : ""))
|
||||
: "话题上还没有这一侧";
|
||||
card.appendChild(sub);
|
||||
return card;
|
||||
}
|
||||
function renderHeader(){
|
||||
const box = $("arms");
|
||||
box.replaceChildren();
|
||||
(status?.arms || [{id:"89", label:"左臂89"}, {id:"91", label:"右臂91"}]).forEach(a => {
|
||||
const b = document.createElement("button");
|
||||
b.textContent = a.label;
|
||||
b.className = a.id === arm ? "on" : "";
|
||||
b.onclick = () => { arm = a.id; render(); };
|
||||
box.appendChild(b);
|
||||
});
|
||||
const chips = $("chips");
|
||||
chips.replaceChildren();
|
||||
(status?.services || []).forEach(s => {
|
||||
chips.appendChild(pill(s.up, s.label + (s.up ? " 在跑" : " 未开")));
|
||||
});
|
||||
const cur = (status?.arms || []).find(a => a.id === arm);
|
||||
$("arm-line").textContent = cur ? cur.label : ("臂 " + arm);
|
||||
$("arm-lead").textContent = cur
|
||||
? cur.label + " · " + (cur.sn || "") + " · 相机 " + (cur.camera || "—") + " · 夹爪 " + (cur.gripper || "—") + (cur.tactile ? " · 本臂有视触觉" : " · 本臂无视触觉")
|
||||
: "";
|
||||
$("op-lead").textContent = "当前臂 " + arm + "。红色按钮会动设备，先弹出命令再执行。";
|
||||
$("grip-lead").textContent = arm === "91"
|
||||
? "右臂 91 是旋转爪 RGI。张开、闭合、转动都要确认。探测只读状态。"
|
||||
: "左臂 89 是平行爪 PGEA，串口以现场为准。张开和闭合要确认。探测只读状态。";
|
||||
}
|
||||
function renderUsb(){
|
||||
const box = $("usb-cards");
|
||||
box.replaceChildren();
|
||||
const ports = status?.grippers || [];
|
||||
const rows = [...(status?.usb || []), {id:"tty", label:"夹爪串口", expect:1, items: ports.map(p => ({port:p, speed:""}))}];
|
||||
rows.forEach(dev => {
|
||||
const card = document.createElement("div");
|
||||
card.className = "card";
|
||||
const h = document.createElement("h3");
|
||||
h.textContent = dev.label;
|
||||
const n = document.createElement("div");
|
||||
n.className = "stat";
|
||||
n.textContent = dev.id === "tty" ? String(dev.items.length) : (dev.items.length + " / " + dev.expect);
|
||||
const sub = document.createElement("div");
|
||||
sub.className = "sub";
|
||||
sub.textContent = dev.items.length
|
||||
? dev.items.map(i => i.speed ? (i.port + " · " + i.speed + " Mbps") : i.port).join("\n")
|
||||
: "未检测到";
|
||||
const ok = dev.id === "tty" ? dev.items.length > 0 : dev.items.length >= dev.expect;
|
||||
const partial = !ok && dev.items.length > 0;
|
||||
card.append(h, n, pill(ok, ok ? "齐" : partial ? "缺一" : "缺失", partial), sub);
|
||||
box.appendChild(card);
|
||||
});
|
||||
}
|
||||
function renderMini(){
|
||||
const box = $("mini-forces");
|
||||
const t = status?.tactile || {};
|
||||
box.replaceChildren(forceCard("左指", t.left, false), forceCard("右指", t.right, false));
|
||||
}
|
||||
function renderFlow(){
|
||||
const box = $("flow");
|
||||
box.replaceChildren();
|
||||
const pipe = svc("pipeline");
|
||||
[["跟踪", "D405 看住工件，锁住悬停"], ["对孔", "拍照找孔，移到孔上方"], ["搜孔轻压", "力闭环入孔，再轻压"]].forEach(([name, desc]) => {
|
||||
const card = document.createElement("div");
|
||||
card.className = "card";
|
||||
const h = document.createElement("h3");
|
||||
h.textContent = name;
|
||||
const sub = document.createElement("div");
|
||||
sub.className = "sub";
|
||||
sub.textContent = desc + (pipe && pipe.up ? "\n进程在跑，具体阶段看终端日志。" : "\n流程未启动。");
|
||||
card.append(h, pill(false, pipe && pipe.up ? "进行中" : "待命", pipe && pipe.up), sub);
|
||||
box.appendChild(card);
|
||||
});
|
||||
}
|
||||
function renderFingers(){
|
||||
const t = status?.tactile || {};
|
||||
const node = svc("tactile");
|
||||
const live = ["left", "right"].some(side => (t[side] || {}).fnormal !== undefined);
|
||||
const note = $("tac-note");
|
||||
if (note) note.textContent = node && !node.up && live && !status?.view?.on
|
||||
? "正式触觉节点没开。曲线若在走，是话题上另一个发布者。"
|
||||
: "";
|
||||
}
|
||||
function camCard(title, desc, dev){
|
||||
const card = document.createElement("div");
|
||||
card.className = "card instrument";
|
||||
const h = document.createElement("h3");
|
||||
h.textContent = title;
|
||||
const frame = document.createElement("div");
|
||||
frame.className = "finder";
|
||||
frame.textContent = dev && dev.items.length ? "链路在线 · 预览用下面的按钮挂到桌面" : "未插入";
|
||||
const sub = document.createElement("div");
|
||||
sub.className = "sub";
|
||||
const where = (dev?.items || []).map(i => i.port + " · " + i.speed + " Mbps").join("\n");
|
||||
sub.textContent = desc + (where ? "\n" + where : "");
|
||||
card.append(h, frame, pill(!!(dev && dev.items.length), dev && dev.items.length ? dev.items.length + " 台" : "缺失"), sub);
|
||||
return card;
|
||||
}
|
||||
function renderCams(){
|
||||
const usb = Object.fromEntries((status?.usb || []).map(d => [d.id, d]));
|
||||
const d405 = usb.d405 || {items:[]};
|
||||
const d435 = usb.d435 || {items:[]};
|
||||
const other = d405.items.length >= 2 ? {items: d405.items.slice(1)} : {items: []};
|
||||
$("cams").replaceChildren(
|
||||
camCard(arm === "91" ? "右腕 D405" : "左腕 D405", "848×480 @ 30 · 彩色 + 深度", {items: d405.items.slice(0, 1)}),
|
||||
camCard("另一只腕部", "现场期望两台。缺的那台插上后这里会亮。", other),
|
||||
camCard("手外 D435", "1280×720 @ 30 · 顶视", d435),
|
||||
);
|
||||
}
|
||||
function renderGripper(){
|
||||
const ports = status?.grippers || [];
|
||||
const up = svc("gripper");
|
||||
const box = $("grip-body");
|
||||
box.replaceChildren();
|
||||
[
|
||||
["89", "左臂平行爪", "PGEA-100-20-W", ["gripper_probe", "gripper_up", "gripper_open", "gripper_close"]],
|
||||
["91", "右臂旋转爪", "RGI-100-30-O-S-M1", ["gripper_probe", "gripper_up", "gripper_open", "gripper_close", "gripper_rotate"]],
|
||||
].forEach(([id, title, model, acts]) => {
|
||||
const card = document.createElement("div");
|
||||
card.className = "card";
|
||||
const h = document.createElement("h3");
|
||||
h.textContent = title;
|
||||
const sub = document.createElement("div");
|
||||
sub.className = "sub";
|
||||
sub.textContent = model + "\n串口 " + (ports.join(" ") || "没有 ttyUSB") + (up && up.up ? "\n驱动在跑" : "\n驱动未开");
|
||||
card.append(h, pill(id === arm, id === arm ? "当前臂" : "切换顶栏", id !== arm), sub);
|
||||
const row = document.createElement("div");
|
||||
row.className = "row";
|
||||
acts.forEach(act => {
|
||||
const spec = catalog.actions.find(a => a.id === act);
|
||||
const b = document.createElement("button");
|
||||
b.className = "btn " + (spec && spec.moves ? "danger" : "primary");
|
||||
b.textContent = spec ? spec.title : act;
|
||||
b.onclick = () => { if (arm !== id) arm = id; ask(act); render(); };
|
||||
row.appendChild(b);
|
||||
});
|
||||
card.appendChild(row);
|
||||
box.appendChild(card);
|
||||
});
|
||||
}
|
||||
function renderRail(){
|
||||
const box = $("rail");
|
||||
if (box.childElementCount) return;
|
||||
catalog.stages.forEach(([id, name]) => {
|
||||
const d = document.createElement("div");
|
||||
d.className = "step";
|
||||
const b = document.createElement("b");
|
||||
b.textContent = id;
|
||||
const s = document.createElement("span");
|
||||
s.textContent = name;
|
||||
d.append(b, s);
|
||||
box.appendChild(d);
|
||||
});
|
||||
}
|
||||
function renderOps(){
|
||||
const box = $("ops");
|
||||
box.replaceChildren();
|
||||
const skip = new Set(["gripper_open", "gripper_close", "gripper_rotate", "gripper_probe", "handeye_show", "handeye_teach", "handeye_merge", "handeye_calibrate"]);
|
||||
catalog.actions.filter(spec => !skip.has(spec.id)).forEach(spec => {
|
||||
const card = document.createElement("div");
|
||||
card.className = "card";
|
||||
const h = document.createElement("h3");
|
||||
h.textContent = spec.title;
|
||||
const sub = document.createElement("div");
|
||||
sub.className = "sub";
|
||||
sub.textContent = spec.blurb;
|
||||
const b = document.createElement("button");
|
||||
b.className = "btn " + (spec.moves ? "danger" : "primary");
|
||||
b.textContent = spec.moves ? "确认后执行" : "执行";
|
||||
b.style.marginTop = "10px";
|
||||
b.onclick = () => ask(spec.id);
|
||||
card.append(h, sub, b);
|
||||
box.appendChild(card);
|
||||
});
|
||||
const stop = document.createElement("div");
|
||||
stop.className = "card";
|
||||
const h = document.createElement("h3");
|
||||
h.textContent = "停止视触觉";
|
||||
const sub = document.createElement("div");
|
||||
sub.className = "sub";
|
||||
sub.textContent = "关掉触觉节点和占着相机的 worker。";
|
||||
const b = document.createElement("button");
|
||||
b.className = "btn";
|
||||
b.textContent = "停止";
|
||||
b.style.marginTop = "10px";
|
||||
b.onclick = () => ask("tactile_down");
|
||||
stop.append(h, sub, b);
|
||||
box.appendChild(stop);
|
||||
}
|
||||
function shot(side, kind, title){
|
||||
const wrap = document.createElement("div");
|
||||
const h = document.createElement("div");
|
||||
h.className = "sub";
|
||||
h.textContent = title;
|
||||
const img = document.createElement("img");
|
||||
img.alt = "";
|
||||
img.dataset.side = side;
|
||||
img.dataset.kind = kind;
|
||||
const ready = status?.view?.frames?.[side + "_" + kind];
|
||||
if (status?.view?.on && ready) {
|
||||
img.src = "/api/tactile/frame/" + side + "/" + kind + ".jpg?t=" + Math.floor((status.view.stamp || 0) * 5);
|
||||
}
|
||||
wrap.append(h, img);
|
||||
return wrap;
|
||||
}
|
||||
function renderViews(){
|
||||
const box = $("views");
|
||||
if (!box.childElementCount) {
|
||||
[["left", "左指"], ["right", "右指"]].forEach(([side, name]) => {
|
||||
const card = document.createElement("div");
|
||||
card.className = "card";
|
||||
const h = document.createElement("h3");
|
||||
h.textContent = name;
|
||||
const pair = document.createElement("div");
|
||||
pair.className = "pair";
|
||||
pair.append(shot(side, "shear", "切向力"), shot(side, "thermal", "热成像"));
|
||||
card.append(h, pair);
|
||||
box.appendChild(card);
|
||||
});
|
||||
}
|
||||
const view = status?.view || {};
|
||||
$("view-note").textContent = view.error || view.note || (view.on ? "成像已开，等第一帧。" : "成像没开。点开始成像后这里出切向力和热成像。");
|
||||
}
|
||||
function drawChart(canvas, samples){
|
||||
if (!canvas) return;
|
||||
const dpr = window.devicePixelRatio || 1;
|
||||
const w = canvas.clientWidth || 360;
|
||||
const h = canvas.clientHeight || 210;
|
||||
canvas.width = Math.floor(w * dpr);
|
||||
canvas.height = Math.floor(h * dpr);
|
||||
const ctx = canvas.getContext("2d");
|
||||
ctx.setTransform(dpr, 0, 0, dpr, 0, 0);
|
||||
ctx.clearRect(0, 0, w, h);
|
||||
ctx.fillStyle = "#0c0e12";
|
||||
ctx.fillRect(0, 0, w, h);
|
||||
const rows = (samples || []).filter(p => ["fnormal", "fshearx", "fsheary"].some(k => typeof p[k] === "number"));
|
||||
if (rows.length < 2) {
|
||||
ctx.fillStyle = "#8b929d";
|
||||
ctx.font = "13px sans-serif";
|
||||
ctx.fillText("还没有足够的采样", 16, 28);
|
||||
return;
|
||||
}
|
||||
const keys = [["fnormal", "#e8e8e8"], ["fshearx", "#4c86d4"], ["fsheary", "#d7a15a"]];
|
||||
let lo = Infinity, hi = -Infinity;
|
||||
rows.forEach(p => keys.forEach(([k]) => {
|
||||
if (typeof p[k] === "number") { lo = Math.min(lo, p[k]); hi = Math.max(hi, p[k]); }
|
||||
}));
|
||||
if (!(hi > lo)) { hi = lo + 1; }
|
||||
const pad = (hi - lo) * 0.08;
|
||||
lo -= pad; hi += pad;
|
||||
const t0 = rows[0].t, t1 = rows[rows.length - 1].t;
|
||||
const span = Math.max(t1 - t0, 1e-3);
|
||||
const left = 46, right = 10, top = 12, bottom = 22;
|
||||
ctx.strokeStyle = "#2a2e36";
|
||||
ctx.beginPath();
|
||||
const y0 = top + (hi - 0) / (hi - lo) * (h - top - bottom);
|
||||
if (y0 > top && y0 < h - bottom) {
|
||||
ctx.moveTo(left, y0);
|
||||
ctx.lineTo(w - right, y0);
|
||||
ctx.stroke();
|
||||
}
|
||||
keys.forEach(([k, color]) => {
|
||||
ctx.beginPath();
|
||||
ctx.strokeStyle = color;
|
||||
ctx.lineWidth = 1.6;
|
||||
let started = false;
|
||||
rows.forEach(p => {
|
||||
if (typeof p[k] !== "number") return;
|
||||
const x = left + (p.t - t0) / span * (w - left - right);
|
||||
const y = top + (hi - p[k]) / (hi - lo) * (h - top - bottom);
|
||||
if (!started) { ctx.moveTo(x, y); started = true; }
|
||||
else ctx.lineTo(x, y);
|
||||
});
|
||||
ctx.stroke();
|
||||
});
|
||||
ctx.fillStyle = "#8b929d";
|
||||
ctx.font = "11px sans-serif";
|
||||
ctx.fillText(hi.toFixed(0), 4, top + 10);
|
||||
ctx.fillText(lo.toFixed(0), 4, h - bottom);
|
||||
ctx.fillText((span).toFixed(0) + " 秒", w - 48, h - 6);
|
||||
}
|
||||
function mm(v){
|
||||
if (!Array.isArray(v) || v.length < 3) return "—";
|
||||
return v.slice(0, 3).map(x => (Number(x) * 1000).toFixed(1) + " mm").join(" ");
|
||||
}
|
||||
function renderHandeye(){
|
||||
document.querySelectorAll("#he-modes button").forEach(b => b.classList.toggle("on", b.dataset.mode === heMode));
|
||||
const card = (status?.handeye || {})[arm]?.[heMode];
|
||||
const box = $("he-card");
|
||||
if (!box) return;
|
||||
box.replaceChildren();
|
||||
if (!card) {
|
||||
box.textContent = "没有这一臂的标定记录";
|
||||
return;
|
||||
}
|
||||
const h = document.createElement("h3");
|
||||
h.textContent = heMode === "eye_in_hand" ? "眼在手上" : "眼在手外";
|
||||
const sub = document.createElement("div");
|
||||
sub.className = "sub";
|
||||
const board = card.board || {};
|
||||
sub.textContent = [
|
||||
card.exists ? "已有结果" : "还没有结果文件",
|
||||
card.meaning || "",
|
||||
"文件 " + card.file,
|
||||
"视角 " + (card.views || "—") + " 时间 " + (card.utc || "—"),
|
||||
"相机 " + (card.camera || "—") + " 工具 " + (card.tool || "—"),
|
||||
"平移 X / Y / Z " + mm(card.translation_m),
|
||||
"示教点 " + (card.poses == null ? "没有位姿文件" : card.poses + " 个"),
|
||||
"板 " + (board.cols || "—") + "×" + (board.rows || "—") + " 方格 " + (board.square_m == null ? "—" : board.square_m + " m"),
|
||||
].filter(Boolean).join("\n");
|
||||
box.append(h, pill(!!card.exists, card.exists ? "有结果" : "无结果"), sub);
|
||||
}
|
||||
function render(){
|
||||
renderHeader();
|
||||
renderUsb();
|
||||
renderMini();
|
||||
renderFlow();
|
||||
renderFingers();
|
||||
renderViews();
|
||||
drawChart($("chart-left"), status?.history?.left);
|
||||
drawChart($("chart-right"), status?.history?.right);
|
||||
renderCams();
|
||||
renderGripper();
|
||||
renderHandeye();
|
||||
renderRail();
|
||||
renderOps();
|
||||
}
|
||||
function show(page){
|
||||
if (!titles[page]) page = "overview";
|
||||
document.querySelectorAll("nav button").forEach(b => b.classList.toggle("active", b.dataset.page === page));
|
||||
document.querySelectorAll("main section").forEach(s => s.classList.remove("on"));
|
||||
$(page).classList.add("on");
|
||||
if (location.hash !== "#" + page) history.replaceState(null, "", "#" + page);
|
||||
}
|
||||
document.querySelectorAll("nav button").forEach(btn => { btn.onclick = () => show(btn.dataset.page); });
|
||||
window.addEventListener("hashchange", () => show(location.hash.slice(1)));
|
||||
document.body.addEventListener("click", ev => {
|
||||
const act = ev.target.closest("[data-act]");
|
||||
if (act) ask(act.dataset.act);
|
||||
});
|
||||
document.querySelectorAll("#he-modes button").forEach(btn => {
|
||||
btn.onclick = () => { heMode = btn.dataset.mode; renderHandeye(); };
|
||||
});
|
||||
$("view-on").onclick = async () => {
|
||||
const data = await api("/api/tactile/view", {on: true});
|
||||
toast(data.message || "已请求");
|
||||
};
|
||||
$("view-off").onclick = async () => {
|
||||
const data = await api("/api/tactile/view", {on: false});
|
||||
toast(data.message || "已停止");
|
||||
};
|
||||
setInterval(() => {
|
||||
if (!$("tactile").classList.contains("on") || !status?.view?.on) return;
|
||||
document.querySelectorAll("#views img").forEach(img => {
|
||||
const side = img.dataset.side, kind = img.dataset.kind;
|
||||
if (!status.view.frames?.[side + "_" + kind]) return;
|
||||
img.src = "/api/tactile/frame/" + side + "/" + kind + ".jpg?t=" + Date.now();
|
||||
});
|
||||
}, 200);
|
||||
$("ask-no").onclick = () => $("ask").close();
|
||||
$("ask-yes").onclick = () => { const action = pending; $("ask").close(); if (action) fire(action, true); };
|
||||
async function tick(){
|
||||
try { status = await api("/api/status"); render(); }
|
||||
catch (err) { toast("状态刷新失败"); }
|
||||
paintClock();
|
||||
}
|
||||
function paintClock(){
|
||||
$("clock").textContent = new Date().toLocaleTimeString("zh-CN", {hour12:false});
|
||||
}
|
||||
async function start(){
|
||||
if (!boot) {
|
||||
catalog = await api("/api/catalog");
|
||||
await tick();
|
||||
} else {
|
||||
render();
|
||||
paintClock();
|
||||
}
|
||||
show(location.hash.slice(1) || "overview");
|
||||
setInterval(tick, 2000);
|
||||
}
|
||||
start();
|
||||
</script>
|
||||
</body>
|
||||
</html>
|
||||
+725
@@ -0,0 +1,725 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Orisys 工位界面。
|
||||
|
||||
只做两件事：把本机真实状态摊开，以及按白名单启动仓库里已有的脚本。
|
||||
会动臂的动作必须带 ``confirm=true``，避免页面上误点一下就上电。
|
||||
|
||||
bash ~/ros2/scripts/run_console.sh
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import os
|
||||
import re
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
from collections import deque
|
||||
from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
|
||||
from pathlib import Path
|
||||
from urllib.parse import urlparse
|
||||
|
||||
import yaml
|
||||
|
||||
from arm_application import operator_tactile_view as tactile_view
|
||||
from arm_application.site_io import eye_to_hand_result_name, overhead_camera
|
||||
|
||||
HOST = "127.0.0.1"
|
||||
PORT = 8766
|
||||
_ROOT = Path(__file__).resolve()
|
||||
for _p in _ROOT.parents:
|
||||
if (_p / "scripts" / "run_real.sh").is_file():
|
||||
ROOT = _p
|
||||
break
|
||||
else:
|
||||
ROOT = Path.home() / "ros2"
|
||||
|
||||
LOG_DIR = Path("/tmp/orisys_console")
|
||||
ASSETS = Path(__file__).resolve().parent
|
||||
_LOCK = threading.Lock()
|
||||
_TACTILE = {"left": {}, "right": {}, "stamp": 0.0}
|
||||
_HIST = {"left": deque(maxlen=360), "right": deque(maxlen=360)}
|
||||
_PROCS: dict[str, int] = {}
|
||||
|
||||
|
||||
def _push_force(side: str, sample: dict) -> None:
|
||||
if side not in _HIST:
|
||||
return
|
||||
point = {
|
||||
"t": float(sample.get("t") or time.time()),
|
||||
"fnormal": sample.get("fnormal"),
|
||||
"fshearx": sample.get("fshearx"),
|
||||
"fsheary": sample.get("fsheary"),
|
||||
}
|
||||
if not any(isinstance(point[k], (int, float)) for k in ("fnormal", "fshearx", "fsheary")):
|
||||
return
|
||||
with _LOCK:
|
||||
_HIST[side].append(point)
|
||||
_TACTILE[side] = {
|
||||
"fnormal": point["fnormal"],
|
||||
"fshearx": point["fshearx"],
|
||||
"fsheary": point["fsheary"],
|
||||
"connected": True,
|
||||
}
|
||||
|
||||
|
||||
tactile_view.set_sample_hook(_push_force)
|
||||
|
||||
|
||||
def _site() -> dict:
|
||||
path = ROOT / "config" / "site.yaml"
|
||||
if not path.is_file():
|
||||
return {}
|
||||
data = yaml.safe_load(path.read_text()) or {}
|
||||
return data if isinstance(data, dict) else {}
|
||||
|
||||
|
||||
def _arms() -> list[dict]:
|
||||
robots = (_site().get("robot") or {}).get("robots") or []
|
||||
out = []
|
||||
for robot in robots:
|
||||
if not isinstance(robot, dict):
|
||||
continue
|
||||
sn = str(robot.get("sn") or "")
|
||||
arm_no = "89" if sn.endswith("063989") else "91" if sn.endswith("063991") else ""
|
||||
if not arm_no:
|
||||
continue
|
||||
out.append({
|
||||
"id": arm_no,
|
||||
"label": str(robot.get("label") or f"臂{arm_no}"),
|
||||
"sn": sn,
|
||||
"tactile": bool(robot.get("tactile")),
|
||||
"camera": str((robot.get("camera") or {}).get("name") or ""),
|
||||
"gripper": str((robot.get("gripper") or {}).get("model") or ""),
|
||||
})
|
||||
return out or [
|
||||
{"id": "89", "label": "左臂89", "sn": "", "tactile": True, "camera": "d405_left", "gripper": ""},
|
||||
{"id": "91", "label": "右臂91", "sn": "", "tactile": False, "camera": "d405_right", "gripper": ""},
|
||||
]
|
||||
|
||||
|
||||
def _usb() -> list[dict]:
|
||||
found = {"d405": [], "d435": [], "orisys": []}
|
||||
want = {
|
||||
("8086", "0b5b"): "d405",
|
||||
("8086", "0b07"): "d435",
|
||||
("1bcf", "2281"): "orisys",
|
||||
}
|
||||
base = Path("/sys/bus/usb/devices")
|
||||
if not base.is_dir():
|
||||
return []
|
||||
for dev in base.iterdir():
|
||||
vid = dev / "idVendor"
|
||||
pid = dev / "idProduct"
|
||||
if not vid.is_file() or not pid.is_file():
|
||||
continue
|
||||
key = want.get((vid.read_text().strip(), pid.read_text().strip()))
|
||||
if not key:
|
||||
continue
|
||||
speed = (dev / "speed").read_text().strip() if (dev / "speed").is_file() else "?"
|
||||
found[key].append({"port": dev.name, "speed": speed})
|
||||
return [
|
||||
{"id": "d405", "label": "腕部 D405", "expect": 2, "items": found["d405"]},
|
||||
{"id": "d435", "label": "手外 D435", "expect": 1, "items": found["d435"]},
|
||||
{"id": "orisys", "label": "视触觉", "expect": 2, "items": found["orisys"]},
|
||||
]
|
||||
|
||||
|
||||
def _listeners(pattern: str) -> bool:
|
||||
try:
|
||||
out = subprocess.run(
|
||||
["pgrep", "-af", pattern],
|
||||
capture_output=True, text=True, timeout=2,
|
||||
).stdout
|
||||
except Exception:
|
||||
return False
|
||||
for line in out.splitlines():
|
||||
if "pgrep" in line or "operator_console" in line:
|
||||
continue
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
def _services() -> list[dict]:
|
||||
rows = [
|
||||
("real", "控制栈", "ros2_control_node"),
|
||||
("tactile", "视触觉节点", "lib/arm_application/tactile"),
|
||||
("camera", "相机", "realsense_color_pub"),
|
||||
("gripper", "夹爪", "dh_pgea|dual_gripper_driver"),
|
||||
("pipeline", "三段流程", "assembly_pipeline"),
|
||||
("gauge", "通止规装配", "gauge_assembly"),
|
||||
]
|
||||
out = []
|
||||
for sid, label, pattern in rows:
|
||||
if "|" in pattern:
|
||||
up = any(_listeners(part) for part in pattern.split("|"))
|
||||
else:
|
||||
up = _listeners(pattern)
|
||||
out.append({"id": sid, "label": label, "up": up})
|
||||
return out
|
||||
|
||||
|
||||
def _grippers() -> list[str]:
|
||||
return sorted(p.name for p in Path("/dev").glob("ttyUSB*"))
|
||||
|
||||
|
||||
def _parse_tactile(text: str) -> dict:
|
||||
got: dict[str, object] = {}
|
||||
for line in text.splitlines():
|
||||
if ":" not in line:
|
||||
continue
|
||||
key, _, raw = line.partition(":")
|
||||
key = key.strip()
|
||||
raw = raw.strip()
|
||||
if key in ("fnormal", "fshearx", "fsheary", "fps"):
|
||||
try:
|
||||
got[key] = float(raw)
|
||||
except ValueError:
|
||||
continue
|
||||
elif key in ("connected", "contact"):
|
||||
got[key] = raw.lower() == "true"
|
||||
return got
|
||||
|
||||
|
||||
def _poll_tactile() -> None:
|
||||
while True:
|
||||
snap = {}
|
||||
for side in ("left", "right"):
|
||||
try:
|
||||
proc = subprocess.run(
|
||||
["timeout", "1.2", "ros2", "topic", "echo", f"/tactile/{side}/status", "--once"],
|
||||
capture_output=True, text=True, timeout=2.5,
|
||||
)
|
||||
snap[side] = _parse_tactile(proc.stdout) if proc.returncode == 0 else {}
|
||||
except Exception:
|
||||
snap[side] = {}
|
||||
view_on = bool(tactile_view.state().get("on"))
|
||||
with _LOCK:
|
||||
_TACTILE["stamp"] = time.time()
|
||||
if not view_on:
|
||||
_TACTILE["left"] = snap.get("left") or {}
|
||||
_TACTILE["right"] = snap.get("right") or {}
|
||||
now = time.time()
|
||||
for side in ("left", "right"):
|
||||
row = snap.get(side) or {}
|
||||
if row:
|
||||
_HIST[side].append({
|
||||
"t": now,
|
||||
"fnormal": row.get("fnormal"),
|
||||
"fshearx": row.get("fshearx"),
|
||||
"fsheary": row.get("fsheary"),
|
||||
})
|
||||
time.sleep(0.35)
|
||||
|
||||
|
||||
def _robot(arm: str) -> dict:
|
||||
robots = (_site().get("robot") or {}).get("robots") or []
|
||||
for robot in robots:
|
||||
if not isinstance(robot, dict):
|
||||
continue
|
||||
sn = str(robot.get("sn") or "")
|
||||
short = "89" if sn.endswith("063989") else "91" if sn.endswith("063991") else ""
|
||||
if short == arm:
|
||||
return robot
|
||||
return {}
|
||||
|
||||
|
||||
def _comment(text: str, key: str) -> str:
|
||||
match = re.search(rf"^#\s*{re.escape(key)}:\s*(.+)$", text, re.M)
|
||||
return match.group(1).strip() if match else ""
|
||||
|
||||
|
||||
def _pose_count(path: Path) -> int | None:
|
||||
if not path.is_file():
|
||||
return None
|
||||
match = re.search(r"num_poses:\s*(\d+)", path.read_text(errors="replace"))
|
||||
return int(match.group(1)) if match else None
|
||||
|
||||
|
||||
def _result_card(path: Path, pose_path: Path, board: dict) -> dict:
|
||||
text = path.read_text(errors="replace") if path.is_file() else ""
|
||||
trans = None
|
||||
tool = ""
|
||||
if text:
|
||||
try:
|
||||
data = yaml.safe_load(text) or {}
|
||||
except yaml.YAMLError:
|
||||
data = {}
|
||||
if isinstance(data, dict):
|
||||
raw = data.get("translation_m")
|
||||
if isinstance(raw, list) and len(raw) >= 3:
|
||||
trans = [float(raw[0]), float(raw[1]), float(raw[2])]
|
||||
tool = str(data.get("gripper_tool") or "")
|
||||
meaning = ""
|
||||
for line in text.splitlines():
|
||||
if line.startswith("# T_4x4"):
|
||||
meaning = line[2:].strip()
|
||||
break
|
||||
return {
|
||||
"file": str(path),
|
||||
"exists": path.is_file(),
|
||||
"views": _comment(text, "views"),
|
||||
"utc": _comment(text, "utc"),
|
||||
"board_note": _comment(text, "board"),
|
||||
"camera": _comment(text, "camera_source"),
|
||||
"tool": tool,
|
||||
"meaning": meaning,
|
||||
"translation_m": trans,
|
||||
"poses": _pose_count(pose_path),
|
||||
"pose_file": str(pose_path),
|
||||
"board": {
|
||||
"cols": board.get("cols"),
|
||||
"rows": board.get("rows"),
|
||||
"square_m": board.get("square_size_m"),
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def _handeye(arm: str) -> dict:
|
||||
site = _site()
|
||||
robot = _robot(arm)
|
||||
paths = robot.get("paths") if isinstance(robot.get("paths"), dict) else {}
|
||||
shared = site.get("paths") if isinstance(site.get("paths"), dict) else {}
|
||||
boards = ((site.get("handeye") or {}).get("boards") or {})
|
||||
wrist_file = Path(str(
|
||||
paths.get("handeye_file")
|
||||
or shared.get("handeye_file")
|
||||
or (ROOT / "handeye" / "results" / f"{arm}_eye_in_hand.yaml")
|
||||
))
|
||||
wrist_pose = Path(str(
|
||||
paths.get("handeye_waypoints")
|
||||
or shared.get("handeye_waypoints")
|
||||
or (ROOT / "handeye" / "poses" / f"{arm}_eye_in_hand.yaml")
|
||||
))
|
||||
cam, _others = overhead_camera(site, robot or None)
|
||||
cam_name = str((cam or {}).get("name") or "")
|
||||
e2h_name = eye_to_hand_result_name(arm, cam_name)
|
||||
e2h_file = Path(str(
|
||||
paths.get("handeye_file_eye_to_hand")
|
||||
or shared.get("handeye_file_eye_to_hand")
|
||||
or (ROOT / "handeye" / "results" / e2h_name)
|
||||
))
|
||||
e2h_pose = Path(str(
|
||||
paths.get("handeye_waypoints_eye_to_hand")
|
||||
or shared.get("handeye_waypoints_eye_to_hand")
|
||||
or (ROOT / "handeye" / "poses" / f"{arm}_eye_to_hand.yaml")
|
||||
))
|
||||
return {
|
||||
"eye_in_hand": _result_card(wrist_file, wrist_pose, boards.get("eye_in_hand") or {}),
|
||||
"eye_to_hand": _result_card(e2h_file, e2h_pose, boards.get("eye_to_hand") or {}),
|
||||
}
|
||||
|
||||
|
||||
def status() -> dict:
|
||||
with _LOCK:
|
||||
tactile = {
|
||||
"left": dict(_TACTILE["left"]),
|
||||
"right": dict(_TACTILE["right"]),
|
||||
"stamp": _TACTILE["stamp"],
|
||||
}
|
||||
history = {
|
||||
"left": list(_HIST["left"]),
|
||||
"right": list(_HIST["right"]),
|
||||
}
|
||||
return {
|
||||
"arms": _arms(),
|
||||
"usb": _usb(),
|
||||
"services": _services(),
|
||||
"grippers": _grippers(),
|
||||
"tactile": tactile,
|
||||
"history": history,
|
||||
"view": tactile_view.state(),
|
||||
"handeye": {"89": _handeye("89"), "91": _handeye("91")},
|
||||
"now": time.time(),
|
||||
}
|
||||
|
||||
|
||||
# 会动真机的动作必须 confirm。argv 里的 {arm} 只接受 89 或 91。
|
||||
_ACTIONS = {
|
||||
"real_up": {
|
||||
"title": "上电控制栈",
|
||||
"script": "run_real.sh",
|
||||
"args": ["--arm", "{arm}"],
|
||||
"moves": True,
|
||||
"blurb": "连接机械臂并拉起 ros2_control。",
|
||||
},
|
||||
"camera_d405": {
|
||||
"title": "本臂腕部相机",
|
||||
"script": "run_camera_d405.sh",
|
||||
"args": ["--arm", "{arm}", "--no-preview"],
|
||||
"moves": False,
|
||||
"blurb": "只拉起本臂腕部相机。彩色和深度显示在程序里，不看左右红外，不另开窗口。",
|
||||
},
|
||||
"camera_d435": {
|
||||
"title": "手外 D435",
|
||||
"script": "run_camera_d435.sh",
|
||||
"args": ["--no-preview"],
|
||||
"moves": False,
|
||||
"blurb": "只拉起顶视 D435。彩色和深度显示在程序里，不另开窗口。",
|
||||
},
|
||||
"tactile_up": {
|
||||
"title": "视触觉三力",
|
||||
"script": "run_tactile.sh",
|
||||
"args": [],
|
||||
"moves": False,
|
||||
"blurb": "发布左右指法向力与两路切向力。",
|
||||
},
|
||||
"gripper_up": {
|
||||
"title": "夹爪节点",
|
||||
"script": "run_gripper.sh",
|
||||
"args": ["--arm", "{arm}"],
|
||||
"moves": False,
|
||||
"blurb": "只拉起夹爪驱动，不主动张合。",
|
||||
},
|
||||
"gripper_probe": {
|
||||
"title": "夹爪探测",
|
||||
"script": "run_gripper.sh",
|
||||
"args": ["--arm", "{arm}", "--cli", "probe"],
|
||||
"moves": False,
|
||||
"blurb": "读夹爪是否在线，不动手指。",
|
||||
},
|
||||
"gripper_open": {
|
||||
"title": "夹爪张开",
|
||||
"script": "run_gripper.sh",
|
||||
"args": ["--arm", "{arm}", "--cli", "open"],
|
||||
"moves": True,
|
||||
"blurb": "张开当前臂夹爪。",
|
||||
},
|
||||
"gripper_close": {
|
||||
"title": "夹爪闭合",
|
||||
"script": "run_gripper.sh",
|
||||
"args": ["--arm", "{arm}", "--cli", "close"],
|
||||
"moves": True,
|
||||
"blurb": "闭合当前臂夹爪。",
|
||||
},
|
||||
"gripper_rotate": {
|
||||
"title": "右爪转 90°",
|
||||
"script": "run_gripper.sh",
|
||||
"args": ["--arm", "{arm}", "--cli", "rotate", "90"],
|
||||
"moves": True,
|
||||
"blurb": "右臂旋转爪转到 90°。左臂平行爪没有这一轴。",
|
||||
},
|
||||
"pipeline": {
|
||||
"title": "整体三段",
|
||||
"script": "run_all.sh",
|
||||
"args": ["--arm", "{arm}"],
|
||||
"moves": True,
|
||||
"blurb": "跟踪、对孔、搜孔轻压一次跑完。会动臂。",
|
||||
},
|
||||
"stage_track": {
|
||||
"title": "只测跟踪",
|
||||
"script": "run_visual_servo_rt.sh",
|
||||
"args": ["--arm", "{arm}", "--control"],
|
||||
"moves": True,
|
||||
"blurb": "只跑视觉跟踪。从当前位姿开始，会动臂。要先有控制栈和相机。",
|
||||
},
|
||||
"stage_locate": {
|
||||
"title": "只测对孔",
|
||||
"script": "run_stage2_locate.sh",
|
||||
"args": ["--arm", "{arm}"],
|
||||
"moves": True,
|
||||
"blurb": "只跑二阶段：朝下、悬停、拍照、对准孔。会动臂。",
|
||||
},
|
||||
"stage_search": {
|
||||
"title": "只测搜孔",
|
||||
"script": "run_stage3_search.sh",
|
||||
"args": ["--arm", "{arm}"],
|
||||
"moves": True,
|
||||
"blurb": "只跑三阶段：触面、搜孔、轻压。会动臂。臂应已停在孔上方。",
|
||||
},
|
||||
"reset_home": {
|
||||
"title": "复位到 home",
|
||||
"script": "go_home.sh",
|
||||
"args": ["--arm", "{arm}", "--pose", "home"],
|
||||
"moves": True,
|
||||
"sync": True,
|
||||
"timeout": 120,
|
||||
"blurb": "先停跟踪，再回到命名位姿 home。要控制栈已经起来。会动臂。",
|
||||
},
|
||||
"gauge_dry": {
|
||||
"title": "装配干跑",
|
||||
"script": "run_gauge_assembly.sh",
|
||||
"args": ["--arm", "{arm}", "--dry-run"],
|
||||
"moves": False,
|
||||
"blurb": "S00 到 S11 只打印计划，不发运动指令。",
|
||||
},
|
||||
"gauge_run": {
|
||||
"title": "装配真跑",
|
||||
"script": "run_gauge_assembly.sh",
|
||||
"args": ["--arm", "{arm}"],
|
||||
"moves": True,
|
||||
"blurb": "通止规状态机。会动臂、会拧。",
|
||||
},
|
||||
"handeye_show": {
|
||||
"title": "查看标定配置",
|
||||
"script": "run_handeye.sh",
|
||||
"args": ["--arm", "{arm}", "show", "--mode", "{mode}"],
|
||||
"moves": False,
|
||||
"sync": True,
|
||||
"blurb": "打印这块板、结果文件和已有点数，不动臂。",
|
||||
},
|
||||
"handeye_teach": {
|
||||
"title": "示教采点",
|
||||
"script": "run_handeye.sh",
|
||||
"args": ["--arm", "{arm}", "teach", "--mode", "{mode}", "--points", "12"],
|
||||
"moves": True,
|
||||
"blurb": "软拖拽示教，目标 12 个姿态。要先停控制栈，会动臂。",
|
||||
},
|
||||
"handeye_merge": {
|
||||
"title": "合并示教点",
|
||||
"script": "run_handeye.sh",
|
||||
"args": ["--arm", "{arm}", "merge", "--mode", "{mode}"],
|
||||
"moves": False,
|
||||
"sync": True,
|
||||
"blurb": "把刚采的点写入该模式的位姿文件，不动臂。",
|
||||
},
|
||||
"handeye_calibrate": {
|
||||
"title": "求解手眼",
|
||||
"script": "run_handeye.sh",
|
||||
"args": ["--arm", "{arm}", "calibrate", "--mode", "{mode}"],
|
||||
"moves": True,
|
||||
"blurb": "按点位走一遍并写结果。需要控制栈和对应相机，会动臂。",
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def catalog() -> dict:
|
||||
return {
|
||||
"actions": [
|
||||
{"id": key, **{k: v for k, v in spec.items() if k not in ("script", "sync")}}
|
||||
for key, spec in _ACTIONS.items()
|
||||
],
|
||||
"stages": [
|
||||
("S00", "自检"),
|
||||
("S01", "回零预绕"),
|
||||
("S02", "夹持确认"),
|
||||
("S03", "手外粗定位"),
|
||||
("S04", "视觉跟踪"),
|
||||
("S05", "精拍"),
|
||||
("S06", "正悬停"),
|
||||
("S07", "柔顺接触"),
|
||||
("S08", "棘轮拧入"),
|
||||
("S09", "反向拧出"),
|
||||
("S10", "换孔"),
|
||||
("S11", "回零结束"),
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
def _stop_tactile() -> str:
|
||||
subprocess.run(["pkill", "-f", "lib/arm_application/tactile"], check=False)
|
||||
subprocess.run(["pkill", "-f", "arm_application/tactile_node.py"], check=False)
|
||||
subprocess.run(["pkill", "orisys_tac_w"], check=False)
|
||||
return "已发送停止视触觉节点"
|
||||
|
||||
|
||||
def run_action(action: str, arm: str, confirm: bool, mode: str = "eye_in_hand") -> dict:
|
||||
if action == "tactile_down":
|
||||
tactile_view.stop()
|
||||
return {"ok": True, "message": _stop_tactile()}
|
||||
if action == "tactile_view_on":
|
||||
with _LOCK:
|
||||
_HIST["left"].clear()
|
||||
_HIST["right"].clear()
|
||||
return {"ok": True, "message": tactile_view.start(_site())}
|
||||
if action == "tactile_view_off":
|
||||
return {"ok": True, "message": tactile_view.stop()}
|
||||
spec = _ACTIONS.get(action)
|
||||
if spec is None:
|
||||
return {"ok": False, "message": "没有这个动作"}
|
||||
if arm not in ("89", "91"):
|
||||
return {"ok": False, "message": "臂号只接受 89 或 91"}
|
||||
if mode not in ("eye_in_hand", "eye_to_hand"):
|
||||
return {"ok": False, "message": "手眼模式只接受 eye_in_hand 或 eye_to_hand"}
|
||||
if spec["moves"] and not confirm:
|
||||
return {"ok": False, "message": "这个动作会动真机，需要确认"}
|
||||
script = ROOT / "scripts" / spec["script"]
|
||||
if not script.is_file():
|
||||
return {"ok": False, "message": f"找不到 {script}"}
|
||||
args = [a.replace("{arm}", arm).replace("{mode}", mode) for a in spec["args"]]
|
||||
LOG_DIR.mkdir(parents=True, exist_ok=True)
|
||||
log_path = LOG_DIR / f"{action}_{arm}.log"
|
||||
shown = " ".join(["bash", f"scripts/{spec['script']}", *args])
|
||||
if spec.get("sync"):
|
||||
try:
|
||||
proc = subprocess.run(
|
||||
["bash", str(script), *args],
|
||||
cwd=str(ROOT),
|
||||
capture_output=True,
|
||||
text=True,
|
||||
timeout=float(spec.get("timeout") or 40),
|
||||
)
|
||||
except subprocess.TimeoutExpired:
|
||||
return {"ok": False, "message": "命令超时还没返回", "detail": shown}
|
||||
detail = ((proc.stdout or "") + (proc.stderr or "")).strip()
|
||||
log_path.write_text(detail)
|
||||
ok = proc.returncode == 0
|
||||
return {
|
||||
"ok": ok,
|
||||
"message": shown if ok else f"退出码 {proc.returncode}",
|
||||
"detail": detail[-4000:],
|
||||
"log": str(log_path),
|
||||
}
|
||||
log_f = log_path.open("ab")
|
||||
proc = subprocess.Popen(
|
||||
["bash", str(script), *args],
|
||||
cwd=str(ROOT),
|
||||
stdout=log_f,
|
||||
stderr=subprocess.STDOUT,
|
||||
start_new_session=True,
|
||||
)
|
||||
_PROCS[action] = proc.pid
|
||||
return {"ok": True, "message": shown, "pid": proc.pid, "log": str(log_path)}
|
||||
|
||||
|
||||
def _bounded(name: str, raw, low: float, high: float) -> float:
|
||||
try:
|
||||
value = float(raw)
|
||||
except (TypeError, ValueError):
|
||||
raise ValueError(f"{name} 不是数字") from None
|
||||
if value != value or value < low or value > high:
|
||||
raise ValueError(f"{name} 只接受 {low:g}–{high:g}")
|
||||
return value
|
||||
|
||||
|
||||
def run_gripper_tune(arm: str, kind: str, numbers: dict) -> dict:
|
||||
"""按界面上的位置、力度、速度下发夹爪。右臂另有角度、圈数。"""
|
||||
if arm not in ("89", "91"):
|
||||
return {"ok": False, "message": "臂号只接受 89 或 91"}
|
||||
if kind not in ("pos", "rotate", "rotate_by", "revolutions"):
|
||||
return {"ok": False, "message": "没有这种夹爪调节"}
|
||||
if kind != "pos" and arm != "91":
|
||||
return {"ok": False, "message": "左臂平行爪没有旋转轴"}
|
||||
try:
|
||||
if kind == "pos":
|
||||
pos = int(round(_bounded("位置", numbers.get("pos"), 0, 1000)))
|
||||
force = int(round(_bounded("力度", numbers.get("force"), 20, 100)))
|
||||
speed = int(round(_bounded("速度", numbers.get("speed"), 1, 100)))
|
||||
cli = ["pos", str(pos)]
|
||||
summary = f"位置 {pos}（0 合，1000 开） 力度 {force}% 速度 {speed}%"
|
||||
else:
|
||||
force = int(round(_bounded("力矩", numbers.get("torque"), 20, 100)))
|
||||
speed = int(round(_bounded("旋转速度", numbers.get("rot_speed"), 1, 100)))
|
||||
if kind == "rotate":
|
||||
angle = _bounded("绝对角", numbers.get("angle"), -32767, 32767)
|
||||
cli = ["rotate", f"{angle:.3f}"]
|
||||
summary = f"绝对角 {angle:.3f}° 力矩 {force}% 速度 {speed}%"
|
||||
elif kind == "rotate_by":
|
||||
angle = _bounded("相对角", numbers.get("delta"), -32767, 32767)
|
||||
cli = ["rotate_by", f"{angle:.3f}"]
|
||||
summary = f"相对转 {angle:.3f}° 力矩 {force}% 速度 {speed}%"
|
||||
else:
|
||||
turns = _bounded("圈数", numbers.get("revolutions"), -50, 50)
|
||||
cli = ["revolutions", f"{turns:.3f}"]
|
||||
summary = f"转 {turns:.3f} 圈 力矩 {force}% 速度 {speed}%"
|
||||
except ValueError as exc:
|
||||
return {"ok": False, "message": str(exc)}
|
||||
script = ROOT / "scripts" / "run_gripper.sh"
|
||||
argv = [
|
||||
"bash", str(script),
|
||||
"--arm", arm,
|
||||
"--force", str(force),
|
||||
"--speed", str(speed),
|
||||
"--cli", *cli,
|
||||
]
|
||||
shown = " ".join(argv)
|
||||
try:
|
||||
proc = subprocess.run(argv, cwd=str(ROOT), capture_output=True, text=True, timeout=90)
|
||||
except subprocess.TimeoutExpired:
|
||||
return {"ok": False, "message": "夹爪 90 秒还没到位", "detail": shown}
|
||||
detail = ((proc.stdout or "") + (proc.stderr or "")).strip()
|
||||
ok = proc.returncode == 0
|
||||
return {
|
||||
"ok": ok,
|
||||
"message": summary if ok else f"夹爪退出码 {proc.returncode}",
|
||||
"detail": (summary + "\n" + shown + "\n" + detail)[-4000:],
|
||||
}
|
||||
|
||||
|
||||
class Handler(BaseHTTPRequestHandler):
|
||||
def log_message(self, fmt: str, *args) -> None:
|
||||
return
|
||||
|
||||
def _send(self, code: int, body: bytes, content_type: str) -> None:
|
||||
self.send_response(code)
|
||||
self.send_header("Content-Type", content_type)
|
||||
self.send_header("Content-Length", str(len(body)))
|
||||
self.end_headers()
|
||||
self.wfile.write(body)
|
||||
|
||||
def _json(self, code: int, payload: dict) -> None:
|
||||
self._send(code, json.dumps(payload, ensure_ascii=False).encode(), "application/json; charset=utf-8")
|
||||
|
||||
def do_GET(self) -> None:
|
||||
path = urlparse(self.path).path
|
||||
if path in ("/", "/index.html"):
|
||||
page = (ASSETS / "operator_console.html").read_text(encoding="utf-8")
|
||||
payload = json.dumps(
|
||||
{"status": status(), "catalog": catalog()},
|
||||
ensure_ascii=False,
|
||||
).replace("<", "\\u003c")
|
||||
page = page.replace("/*BOOT*/null", payload, 1)
|
||||
self._send(200, page.encode(), "text/html; charset=utf-8")
|
||||
return
|
||||
if path == "/logo.png":
|
||||
self._send(200, (ASSETS / "operator_console_logo.png").read_bytes(), "image/png")
|
||||
return
|
||||
if path == "/api/status":
|
||||
self._json(200, status())
|
||||
return
|
||||
if path == "/api/catalog":
|
||||
self._json(200, catalog())
|
||||
return
|
||||
parts = path.strip("/").split("/")
|
||||
if len(parts) == 5 and parts[:3] == ["api", "tactile", "frame"] and parts[4].endswith(".jpg"):
|
||||
side, kind = parts[3], parts[4][:-4]
|
||||
blob = tactile_view.frame(side, kind) if side in ("left", "right") and kind in ("shear", "thermal") else None
|
||||
if not blob:
|
||||
self._json(404, {"ok": False, "message": "还没有这一帧"})
|
||||
return
|
||||
self.send_response(200)
|
||||
self.send_header("Content-Type", "image/jpeg")
|
||||
self.send_header("Cache-Control", "no-store")
|
||||
self.send_header("Content-Length", str(len(blob)))
|
||||
self.end_headers()
|
||||
self.wfile.write(blob)
|
||||
return
|
||||
self._json(404, {"ok": False, "message": "没有这个路径"})
|
||||
|
||||
def do_POST(self) -> None:
|
||||
path = urlparse(self.path).path
|
||||
if path not in ("/api/action", "/api/tactile/view"):
|
||||
self._json(404, {"ok": False, "message": "没有这个路径"})
|
||||
return
|
||||
length = int(self.headers.get("Content-Length") or 0)
|
||||
try:
|
||||
body = json.loads(self.rfile.read(length) or b"{}")
|
||||
except json.JSONDecodeError:
|
||||
self._json(400, {"ok": False, "message": "请求不是 JSON"})
|
||||
return
|
||||
if path == "/api/tactile/view":
|
||||
action = "tactile_view_on" if body.get("on") else "tactile_view_off"
|
||||
result = run_action(action, "89", False)
|
||||
self._json(200 if result.get("ok") else 400, result)
|
||||
return
|
||||
result = run_action(
|
||||
str(body.get("action") or ""),
|
||||
str(body.get("arm") or "89"),
|
||||
bool(body.get("confirm")),
|
||||
str(body.get("mode") or "eye_in_hand"),
|
||||
)
|
||||
self._json(200 if result.get("ok") else 400, result)
|
||||
|
||||
|
||||
def main() -> None:
|
||||
import atexit
|
||||
atexit.register(tactile_view.stop)
|
||||
threading.Thread(target=_poll_tactile, daemon=True).start()
|
||||
server = ThreadingHTTPServer((HOST, PORT), Handler)
|
||||
print(f"Orisys 工位界面 http://{HOST}:{PORT}", flush=True)
|
||||
server.serve_forever()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
BIN
Binary file not shown.
|
After Width: | Height: | Size: 36 KiB |
+353
@@ -0,0 +1,353 @@
|
||||
"""工位界面用的视触觉成像。
|
||||
|
||||
切向力是 SDK 的 ``VSHEAR`` 箭头，热成像是 ``DEPTH`` 的 JET 伪彩。
|
||||
相机独占，所以只有页面点了「开始成像」才打开，失败把原因交回去，不造图。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import re
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
_LOCK = threading.Lock()
|
||||
_STOP = threading.Event()
|
||||
_THREADS: list[threading.Thread] = []
|
||||
_ON_SAMPLE = None
|
||||
_STATE = {
|
||||
"on": False,
|
||||
"error": "",
|
||||
"stamp": 0.0,
|
||||
"note": "",
|
||||
"frames": {},
|
||||
}
|
||||
_JPEG: dict[str, bytes] = {}
|
||||
|
||||
|
||||
def set_sample_hook(fn) -> None:
|
||||
global _ON_SAMPLE
|
||||
_ON_SAMPLE = fn
|
||||
|
||||
|
||||
def state() -> dict:
|
||||
with _LOCK:
|
||||
return {
|
||||
"on": _STATE["on"],
|
||||
"error": _STATE["error"],
|
||||
"stamp": _STATE["stamp"],
|
||||
"note": _STATE["note"],
|
||||
"frames": dict(_STATE["frames"]),
|
||||
}
|
||||
|
||||
|
||||
def frame(side: str, kind: str) -> bytes | None:
|
||||
with _LOCK:
|
||||
return _JPEG.get(f"{side}_{kind}")
|
||||
|
||||
|
||||
def _videos() -> list[str]:
|
||||
found = []
|
||||
by_path = Path("/dev/v4l/by-path")
|
||||
if not by_path.is_dir():
|
||||
return found
|
||||
for link in sorted(by_path.iterdir()):
|
||||
if not link.name.endswith("-video-index0"):
|
||||
continue
|
||||
real = os.path.realpath(link)
|
||||
base = os.path.basename(real)
|
||||
sysdev = os.path.realpath(f"/sys/class/video4linux/{base}/device")
|
||||
dev = ""
|
||||
for part in reversed(sysdev.split("/")):
|
||||
if re.fullmatch(r"\d+-\d+(\.\d+)*", part):
|
||||
dev = part
|
||||
break
|
||||
if not dev:
|
||||
continue
|
||||
vid = Path(f"/sys/bus/usb/devices/{dev}/idVendor")
|
||||
pid = Path(f"/sys/bus/usb/devices/{dev}/idProduct")
|
||||
if (
|
||||
vid.is_file()
|
||||
and pid.is_file()
|
||||
and vid.read_text().strip() == "1bcf"
|
||||
and pid.read_text().strip() == "2281"
|
||||
):
|
||||
found.append(str(link))
|
||||
return found
|
||||
|
||||
|
||||
def _resolve_cfg(site: dict) -> dict:
|
||||
tactile = site.get("tactile") if isinstance(site.get("tactile"), dict) else {}
|
||||
left = str(tactile.get("left_video") or "")
|
||||
right = str(tactile.get("right_video") or "")
|
||||
if not Path(left).exists() or not Path(right).exists():
|
||||
found = _videos()
|
||||
if len(found) >= 2:
|
||||
left, right = found[0], found[1]
|
||||
elif len(found) == 1:
|
||||
left = right = found[0]
|
||||
return {
|
||||
"cfg": str(tactile.get("config_file") or ""),
|
||||
"backend": str(tactile.get("infer_backend") or "cpu"),
|
||||
"cuda": bool(tactile.get("cuda", False)),
|
||||
"isstitch": bool(tactile.get("isstitch", True)),
|
||||
"baseline": float(tactile.get("baseline_seconds") or 1.5),
|
||||
"left": left,
|
||||
"right": right,
|
||||
}
|
||||
|
||||
|
||||
def _as_bgr(img):
|
||||
if not isinstance(img, np.ndarray) or img.size == 0:
|
||||
return None
|
||||
import cv2
|
||||
|
||||
if img.ndim == 2:
|
||||
return cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
|
||||
if img.ndim == 3 and img.shape[2] >= 3:
|
||||
return np.ascontiguousarray(img[:, :, :3])
|
||||
return None
|
||||
|
||||
|
||||
def _flat(arr) -> bool:
|
||||
if not isinstance(arr, np.ndarray) or arr.size == 0 or arr.ndim < 2:
|
||||
return True
|
||||
a = arr.reshape(-1, arr.shape[-1]) if arr.ndim == 3 else arr.reshape(-1)
|
||||
finite = np.isfinite(a)
|
||||
if not bool(finite.any()):
|
||||
return True
|
||||
vals = a[finite]
|
||||
return float(np.max(vals) - np.min(vals)) < 1e-6
|
||||
|
||||
|
||||
def _thermal(depth):
|
||||
import cv2
|
||||
|
||||
if depth is None:
|
||||
return None
|
||||
a = np.asarray(depth)
|
||||
if a.size == 0:
|
||||
return None
|
||||
if a.ndim == 3 and a.shape[2] == 2:
|
||||
a = np.linalg.norm(a.astype(np.float32), axis=2)
|
||||
if a.ndim != 2:
|
||||
return None
|
||||
finite = np.isfinite(a)
|
||||
if not finite.any():
|
||||
return None
|
||||
vals = a[finite]
|
||||
lo, hi = float(vals.min()), float(vals.max())
|
||||
if hi <= lo:
|
||||
norm = np.zeros(a.shape, np.uint8)
|
||||
else:
|
||||
norm = np.clip((a - lo) / (hi - lo) * 255.0, 0, 255).astype(np.uint8)
|
||||
return cv2.applyColorMap(norm, cv2.COLORMAP_JET)
|
||||
|
||||
|
||||
def _views(sensor, gray):
|
||||
import cv2
|
||||
import orisys
|
||||
|
||||
bgr = _as_bgr(gray)
|
||||
h, w = (bgr.shape[0], bgr.shape[1]) if bgr is not None else (450, 420)
|
||||
try:
|
||||
sensor.compute_deformation(decompose=True, check_motion=False)
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
sensor.compute_contact()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
vshear, vnormal, depth = sensor.read_info(
|
||||
sensor.info.VSHEAR, sensor.info.VNORMAL, sensor.info.DEPTH
|
||||
)
|
||||
except Exception:
|
||||
vshear, vnormal, depth = None, None, None
|
||||
canvas = np.zeros((h, w, 3), np.uint8)
|
||||
shear = canvas
|
||||
if isinstance(vshear, np.ndarray) and vshear.ndim == 3 and vshear.shape[2] >= 2 and vshear.size:
|
||||
field = np.ascontiguousarray(vshear[:, :, :2], dtype=np.float32)
|
||||
if field.shape[0] != h or field.shape[1] != w:
|
||||
field = cv2.resize(field, (w, h))
|
||||
mag = np.linalg.norm(field, axis=2)
|
||||
peak = float(np.percentile(mag, 99)) if mag.size else 0.0
|
||||
arrow_scale = (2.0 * peak / 12.0) if peak > 1e-6 else 1.0
|
||||
try:
|
||||
shear = orisys.util.draw_arrows(
|
||||
canvas,
|
||||
field,
|
||||
threshold=peak * 0.15 if peak > 1e-6 else 1.0,
|
||||
grid_spacing=16,
|
||||
arrow_scale=max(arrow_scale, 1e-6),
|
||||
below_threshold_color=(40, 40, 40),
|
||||
)
|
||||
except Exception:
|
||||
shear = canvas
|
||||
heat = None if _flat(depth) else _thermal(depth)
|
||||
if heat is None:
|
||||
heat = _thermal(vnormal)
|
||||
if not isinstance(heat, np.ndarray) or heat.ndim != 3:
|
||||
heat = np.zeros((h, w, 3), np.uint8)
|
||||
elif heat.shape[0] != h or heat.shape[1] != w:
|
||||
heat = cv2.resize(heat, (w, h))
|
||||
return shear, heat
|
||||
|
||||
|
||||
def _jpeg(img) -> bytes | None:
|
||||
import cv2
|
||||
|
||||
if not isinstance(img, np.ndarray) or img.size == 0:
|
||||
return None
|
||||
ok, buf = cv2.imencode(".jpg", img, [int(cv2.IMWRITE_JPEG_QUALITY), 70])
|
||||
if not ok:
|
||||
return None
|
||||
return buf.tobytes()
|
||||
|
||||
|
||||
def _store(side: str, shear, heat, sample: dict | None) -> None:
|
||||
frames = {}
|
||||
blobs = {}
|
||||
for kind, img in (("shear", shear), ("thermal", heat)):
|
||||
blob = _jpeg(img)
|
||||
if blob:
|
||||
blobs[f"{side}_{kind}"] = blob
|
||||
frames[f"{side}_{kind}"] = True
|
||||
with _LOCK:
|
||||
_JPEG.update(blobs)
|
||||
_STATE["frames"].update(frames)
|
||||
_STATE["stamp"] = time.time()
|
||||
if _STATE["error"].startswith(side):
|
||||
_STATE["error"] = ""
|
||||
if sample and _ON_SAMPLE is not None:
|
||||
_ON_SAMPLE(side, sample)
|
||||
|
||||
|
||||
def _open_sensor(cfg: dict, side: str):
|
||||
import orisys
|
||||
|
||||
return orisys.Sensor(
|
||||
cfg[side],
|
||||
cuda=cfg["cuda"],
|
||||
isstitch=cfg["isstitch"],
|
||||
verbose=False,
|
||||
config_name=cfg["cfg"],
|
||||
backend=cfg["backend"],
|
||||
)
|
||||
|
||||
|
||||
def _worker(side: str, cfg: dict) -> None:
|
||||
if not cfg.get(side):
|
||||
with _LOCK:
|
||||
_STATE["error"] = f"{side} 没有视频节点"
|
||||
return
|
||||
sensor = None
|
||||
try:
|
||||
sensor = _open_sensor(cfg, side)
|
||||
except Exception as exc:
|
||||
with _LOCK:
|
||||
prev = _STATE["error"]
|
||||
_STATE["error"] = (prev + "；" if prev else "") + f"{side} 打开失败：{exc}"
|
||||
return
|
||||
buckets = {"n": [], "x": [], "y": []}
|
||||
deadline = time.perf_counter() + float(cfg["baseline"])
|
||||
while time.perf_counter() < deadline and not _STOP.is_set():
|
||||
try:
|
||||
img = sensor.get_img()
|
||||
sensor.compute_deformation(check_motion=False)
|
||||
n, x, y = sensor.read_info(
|
||||
sensor.info.FNORMAL, sensor.info.FSHEARX, sensor.info.FSHEARY
|
||||
)
|
||||
buckets["n"].append(float(n))
|
||||
buckets["x"].append(float(x))
|
||||
buckets["y"].append(float(y))
|
||||
except Exception:
|
||||
pass
|
||||
time.sleep(0.02)
|
||||
base = {
|
||||
"n": float(np.mean(buckets["n"])) if buckets["n"] else 0.0,
|
||||
"x": float(np.mean(buckets["x"])) if buckets["x"] else 0.0,
|
||||
"y": float(np.mean(buckets["y"])) if buckets["y"] else 0.0,
|
||||
}
|
||||
with _LOCK:
|
||||
base_note = "曲线已减去打开后短暂基线，单位仍是形变读数。"
|
||||
if "右指" in str(_STATE["note"]):
|
||||
_STATE["note"] = _STATE["note"] + " " + base_note
|
||||
else:
|
||||
_STATE["note"] = base_note
|
||||
last_sample = 0.0
|
||||
while not _STOP.is_set():
|
||||
try:
|
||||
gray = sensor.get_img()
|
||||
except Exception:
|
||||
gray = None
|
||||
if gray is None:
|
||||
time.sleep(0.05)
|
||||
continue
|
||||
shear, heat = _views(sensor, gray)
|
||||
sample = None
|
||||
now = time.time()
|
||||
if now - last_sample >= 0.1:
|
||||
try:
|
||||
n, x, y = sensor.read_info(
|
||||
sensor.info.FNORMAL, sensor.info.FSHEARX, sensor.info.FSHEARY
|
||||
)
|
||||
sample = {
|
||||
"t": now,
|
||||
"fnormal": float(n) - base["n"],
|
||||
"fshearx": float(x) - base["x"],
|
||||
"fsheary": float(y) - base["y"],
|
||||
}
|
||||
last_sample = now
|
||||
except Exception:
|
||||
sample = None
|
||||
_store(side, shear, heat, sample)
|
||||
time.sleep(0.04)
|
||||
try:
|
||||
sensor.disconnect()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
def start(site: dict) -> str:
|
||||
global _THREADS
|
||||
with _LOCK:
|
||||
if _STATE["on"]:
|
||||
return "成像已经开着"
|
||||
_STATE["on"] = True
|
||||
_STATE["error"] = ""
|
||||
_STATE["frames"] = {}
|
||||
_STOP.clear()
|
||||
cfg = _resolve_cfg(site)
|
||||
if not cfg["left"] and not cfg["right"]:
|
||||
with _LOCK:
|
||||
_STATE["on"] = False
|
||||
_STATE["error"] = "没有找到 Orisys 相机"
|
||||
return _STATE["error"]
|
||||
sides = ["left"]
|
||||
if cfg.get("right") and cfg.get("right") != cfg.get("left"):
|
||||
sides.append("right")
|
||||
elif not cfg.get("right") or cfg.get("right") == cfg.get("left"):
|
||||
with _LOCK:
|
||||
_STATE["note"] = "只找到一路 Orisys，右指没有画面。插上后重新开始成像。"
|
||||
_THREADS = [
|
||||
threading.Thread(target=_worker, args=(side, cfg), daemon=True, name=f"tac-{side}")
|
||||
for side in sides
|
||||
]
|
||||
for thread in _THREADS:
|
||||
thread.start()
|
||||
return "正在打开两只视触觉。切向力和热成像出来之前会先采一小段基线。"
|
||||
|
||||
|
||||
def stop() -> str:
|
||||
_STOP.set()
|
||||
for thread in list(_THREADS):
|
||||
thread.join(timeout=2.0)
|
||||
with _LOCK:
|
||||
_STATE["on"] = False
|
||||
_STATE["frames"] = {}
|
||||
_JPEG.clear()
|
||||
return "已停止视触觉成像"
|
||||
+203
@@ -0,0 +1,203 @@
|
||||
"""软拖拽示教：开拖拽、记下当前关节角、再回到该点。
|
||||
|
||||
点位写在 ``handeye/tmp/<臂号>/operator_teach.yaml``。关节角单位是弧度，
|
||||
直接存桥接返回的 7 个数，不另做姿态换算。
|
||||
|
||||
软拖拽占用 RDK，和 ``run_real.sh`` 不能同时连同一台臂。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
from datetime import datetime, timezone
|
||||
from pathlib import Path
|
||||
|
||||
import yaml
|
||||
|
||||
_LOCK = threading.Lock()
|
||||
_BRIDGE = None
|
||||
_SN = ""
|
||||
|
||||
|
||||
def _file(arm: str) -> Path:
|
||||
from arm_application.operator_console import ROOT
|
||||
|
||||
if arm not in ("89", "91"):
|
||||
raise ValueError("臂号只接受 89 或 91")
|
||||
return ROOT / "handeye" / "tmp" / arm / "operator_teach.yaml"
|
||||
|
||||
|
||||
def _bridge_for(sn: str):
|
||||
global _BRIDGE, _SN
|
||||
sn = str(sn or "").strip()
|
||||
if not sn:
|
||||
raise RuntimeError("site.yaml 里这台臂没有序列号，拒绝连接")
|
||||
if _BRIDGE is not None and _SN == sn and _BRIDGE._proc.poll() is None:
|
||||
return _BRIDGE
|
||||
close()
|
||||
from arm_application.handeye_teach import RdkBridge
|
||||
|
||||
_BRIDGE = RdkBridge(sn)
|
||||
_SN = sn
|
||||
return _BRIDGE
|
||||
|
||||
|
||||
def close() -> None:
|
||||
global _BRIDGE, _SN
|
||||
bridge = _BRIDGE
|
||||
_BRIDGE = None
|
||||
_SN = ""
|
||||
if bridge is None:
|
||||
return
|
||||
try:
|
||||
bridge.close()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
def drag_on(sn: str) -> dict:
|
||||
with _LOCK:
|
||||
try:
|
||||
bridge = _bridge_for(sn)
|
||||
result = bridge.drag_on(mode="soft")
|
||||
except Exception as exc:
|
||||
return {"ok": False, "message": str(exc)}
|
||||
if not result.get("ok"):
|
||||
return {"ok": False, "message": str(result.get("error") or result)}
|
||||
return {
|
||||
"ok": True,
|
||||
"message": "软拖拽已开启。轻推机械臂，再记点。这一步要先停掉控制栈。",
|
||||
}
|
||||
|
||||
|
||||
def drag_off(sn: str) -> dict:
|
||||
with _LOCK:
|
||||
try:
|
||||
bridge = _bridge_for(sn)
|
||||
result = bridge.drag_off()
|
||||
except Exception as exc:
|
||||
return {"ok": False, "message": str(exc)}
|
||||
if not result.get("ok"):
|
||||
return {"ok": False, "message": str(result.get("error") or result)}
|
||||
return {"ok": True, "message": "软拖拽已关闭，臂保持当前位置。"}
|
||||
|
||||
|
||||
def list_points(arm: str) -> dict:
|
||||
try:
|
||||
path = _file(arm)
|
||||
except ValueError as exc:
|
||||
return {"ok": False, "message": str(exc), "points": []}
|
||||
if not path.is_file():
|
||||
return {"ok": True, "points": [], "file": str(path)}
|
||||
try:
|
||||
data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
except Exception as exc:
|
||||
return {"ok": False, "message": f"示教文件读不了：{exc}", "points": [], "file": str(path)}
|
||||
points = []
|
||||
for row in data.get("points") or []:
|
||||
if not isinstance(row, dict):
|
||||
continue
|
||||
joints = row.get("joints")
|
||||
if not isinstance(joints, list) or len(joints) < 7:
|
||||
continue
|
||||
try:
|
||||
values = [float(v) for v in joints[:7]]
|
||||
except (TypeError, ValueError):
|
||||
continue
|
||||
if any(v != v for v in values):
|
||||
continue
|
||||
points.append({
|
||||
"name": str(row.get("name") or "").strip(),
|
||||
"utc": str(row.get("utc") or ""),
|
||||
"joints": values,
|
||||
})
|
||||
points = [row for row in points if row["name"]]
|
||||
return {"ok": True, "points": points, "file": str(path)}
|
||||
|
||||
|
||||
def _write(arm: str, points: list[dict]) -> Path:
|
||||
path = _file(arm)
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
path.write_text(
|
||||
yaml.safe_dump({"points": points}, allow_unicode=True, sort_keys=False),
|
||||
encoding="utf-8",
|
||||
)
|
||||
return path
|
||||
|
||||
|
||||
def record(sn: str, arm: str, name: str) -> dict:
|
||||
title = str(name or "").strip()
|
||||
if not title or "/" in title or "\\" in title or title in (".", ".."):
|
||||
return {"ok": False, "message": "点名不能为空，也不能带路径"}
|
||||
with _LOCK:
|
||||
try:
|
||||
bridge = _bridge_for(sn)
|
||||
result = bridge._ask({"cmd": "joints"}, timeout=2.0)
|
||||
except Exception as exc:
|
||||
return {"ok": False, "message": str(exc)}
|
||||
if not result.get("ok"):
|
||||
return {"ok": False, "message": str(result.get("error") or "没有读到关节，点位没写下去")}
|
||||
raw = result.get("q") or []
|
||||
if len(raw) < 7:
|
||||
return {"ok": False, "message": "关节数不够 7 个，点位没写下去"}
|
||||
try:
|
||||
values = [round(float(v), 6) for v in raw[:7]]
|
||||
except (TypeError, ValueError):
|
||||
return {"ok": False, "message": "关节角不是数字，点位没写下去"}
|
||||
if any(v != v for v in values):
|
||||
return {"ok": False, "message": "关节角不是有限值，点位没写下去"}
|
||||
listed = list_points(arm)
|
||||
if not listed.get("ok"):
|
||||
return listed
|
||||
points = [row for row in listed["points"] if row["name"] != title]
|
||||
points.append({
|
||||
"name": title,
|
||||
"utc": datetime.now(timezone.utc).strftime("%Y-%m-%dT%H:%M:%SZ"),
|
||||
"joints": values,
|
||||
})
|
||||
path = _write(arm, points)
|
||||
text = " ".join(f"{v:.4f}" for v in values)
|
||||
return {"ok": True, "message": f"已记录 {title}", "detail": f"{path}\n关节 rad：{text}", "points": points}
|
||||
|
||||
|
||||
def delete_point(arm: str, name: str) -> dict:
|
||||
title = str(name or "").strip()
|
||||
listed = list_points(arm)
|
||||
if not listed.get("ok"):
|
||||
return listed
|
||||
kept = [row for row in listed["points"] if row["name"] != title]
|
||||
if len(kept) == len(listed["points"]):
|
||||
return {"ok": False, "message": f"没有名为 {title} 的点"}
|
||||
path = _write(arm, kept)
|
||||
return {"ok": True, "message": f"已删除 {title}", "detail": str(path), "points": kept}
|
||||
|
||||
|
||||
def goto(sn: str, arm: str, name: str, speed: float) -> dict:
|
||||
title = str(name or "").strip()
|
||||
listed = list_points(arm)
|
||||
if not listed.get("ok"):
|
||||
return listed
|
||||
hit = next((row for row in listed["points"] if row["name"] == title), None)
|
||||
if hit is None:
|
||||
return {"ok": False, "message": f"没有名为 {title} 的点"}
|
||||
try:
|
||||
speed_f = float(speed)
|
||||
except (TypeError, ValueError):
|
||||
return {"ok": False, "message": "关节速度不是数字"}
|
||||
if not 0.05 <= speed_f <= 0.35:
|
||||
return {"ok": False, "message": "关节速度只接受 0.05–0.35 rad/s"}
|
||||
with _LOCK:
|
||||
try:
|
||||
bridge = _bridge_for(sn)
|
||||
bridge.drag_off()
|
||||
result = bridge.move_joints(hit["joints"], speed_f)
|
||||
except Exception as exc:
|
||||
return {"ok": False, "message": str(exc)}
|
||||
if not result.get("ok"):
|
||||
return {"ok": False, "message": str(result.get("error") or result)}
|
||||
text = " ".join(f"{float(v):.4f}" for v in hit["joints"])
|
||||
return {
|
||||
"ok": True,
|
||||
"message": f"已回到 {title}",
|
||||
"detail": f"速度 {speed_f:.2f} rad/s\n关节 rad：{text}",
|
||||
}
|
||||
+587
@@ -0,0 +1,587 @@
|
||||
"""Orisys 视触觉适配器。SDK 通过 pip 安装；设备 id 存放在 ``site.yaml`` 里。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any, Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
from arm_application.site_io import ros2_root
|
||||
from arm_application.tactile_interface import TactileCommand, TactileInterface, TactileSample
|
||||
|
||||
|
||||
@dataclass
|
||||
class OrisysSensorConfig:
|
||||
vid_src: int | str = 0
|
||||
config_name: str = "dd03"
|
||||
rotation_config_path: str = ""
|
||||
cuda: bool = True
|
||||
isstitch: bool = True
|
||||
backend: str = "auto"
|
||||
motion_threshold: float = 0.0
|
||||
target_fps: float = 30.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class OrisysDualConfig:
|
||||
left: OrisysSensorConfig
|
||||
right: OrisysSensorConfig
|
||||
baseline_seconds: float = 2.0
|
||||
filter_alpha: float = 0.25
|
||||
backend: str = "orisys" # orisys | mock
|
||||
image_hz: float = 0.0
|
||||
"""视触觉图像（深度/形变图）的发布频率。0 = 不发布图像。
|
||||
|
||||
为什么默认关：图像每帧几百 KB，跨进程序列化 + ROS 发布的开销远大于三力。
|
||||
三力路径必须跑在 30fps（接触判据靠它），图像只是给人看和给算法备用，
|
||||
默认不开以免动到已经调好的三力时序。装配现场按需在 site.yaml 打开。"""
|
||||
|
||||
|
||||
def resolve_orisys_config(config_name: str, config_file: str = "") -> str:
|
||||
"""把型号名解析成实际的 config.json 路径。
|
||||
|
||||
查找顺序（先命中先用）：
|
||||
|
||||
1. 显式传入的 ``config_file``
|
||||
2. 参数本身就是存在的文件路径
|
||||
3. ``devices/sensors/orisys/models/<型号>/config.json`` —— 型号配置随 SDK 放，
|
||||
这是现在唯一推荐的位置
|
||||
4. 老位置 ``src/arm_application/config/orisys/<型号>.json``（兼容保留：
|
||||
站点自己放的定制配置可能还在这里）
|
||||
5. Orisys 包自带的 ``configs/<型号>.json``
|
||||
6. 都没找到就把原样返回，交给 SDK 自己解析（它支持内置型号名）
|
||||
"""
|
||||
if config_file:
|
||||
p = Path(config_file).expanduser()
|
||||
if p.is_file():
|
||||
return str(p)
|
||||
raw = (config_name or "dd03").strip()
|
||||
p = Path(raw).expanduser()
|
||||
if p.is_file():
|
||||
return str(p)
|
||||
|
||||
root = ros2_root()
|
||||
for cand in (
|
||||
root / "devices" / "sensors" / "orisys" / "models" / raw / "config.json",
|
||||
root / "src" / "arm_application" / "config" / "orisys" / f"{raw}.json",
|
||||
):
|
||||
if cand.is_file():
|
||||
return str(cand)
|
||||
try:
|
||||
import orisys
|
||||
|
||||
pkg_root = Path(orisys.__file__).resolve().parent.parent
|
||||
for cand in (pkg_root / "configs" / f"{raw}.json", pkg_root / "config" / f"{raw}.json"):
|
||||
if cand.is_file():
|
||||
return str(cand)
|
||||
except Exception:
|
||||
pass
|
||||
return raw
|
||||
|
||||
|
||||
def load_orisys_from_site(data: dict) -> OrisysDualConfig:
|
||||
t = data.get("tactile") or {}
|
||||
left_raw = t.get("left") if isinstance(t.get("left"), dict) else {}
|
||||
right_raw = t.get("right") if isinstance(t.get("right"), dict) else {}
|
||||
cfg_file = str(t.get("config_file") or "")
|
||||
# model 是设备契约层选型号用的键（对应 models/<型号>/ 目录名）；
|
||||
# config 是它的旧名，仅为兼容老站点文件保留，站点里应统一用 model。
|
||||
cfg_name = str(t.get("model") or t.get("config") or "dd03")
|
||||
resolved = resolve_orisys_config(cfg_name, cfg_file)
|
||||
common = dict(
|
||||
config_name=resolved,
|
||||
rotation_config_path=str(t.get("rotation_config") or ""),
|
||||
cuda=bool(t.get("cuda", True)),
|
||||
isstitch=bool(t.get("isstitch", True)),
|
||||
backend=str(t.get("infer_backend") or t.get("backend_infer") or "auto"),
|
||||
motion_threshold=float(t.get("motion_threshold") or 0.0),
|
||||
target_fps=float(t.get("target_fps") or 30.0),
|
||||
)
|
||||
left_vid = left_raw.get("video", t.get("left_video", 0))
|
||||
right_vid = right_raw.get("video", t.get("right_video", 2))
|
||||
left = OrisysSensorConfig(vid_src=_parse_video(left_vid), **common)
|
||||
right = OrisysSensorConfig(vid_src=_parse_video(right_vid), **common)
|
||||
if left_raw.get("config") or left_raw.get("config_file"):
|
||||
left.config_name = resolve_orisys_config(
|
||||
str(left_raw.get("config") or cfg_name),
|
||||
str(left_raw.get("config_file") or ""),
|
||||
)
|
||||
if right_raw.get("config") or right_raw.get("config_file"):
|
||||
right.config_name = resolve_orisys_config(
|
||||
str(right_raw.get("config") or cfg_name),
|
||||
str(right_raw.get("config_file") or ""),
|
||||
)
|
||||
left.rotation_config_path = str(
|
||||
left_raw.get("rotation_config") or t.get("left_rotation") or t.get("rotation_config") or ""
|
||||
)
|
||||
right.rotation_config_path = str(
|
||||
right_raw.get("rotation_config") or t.get("right_rotation") or t.get("rotation_config") or ""
|
||||
)
|
||||
return OrisysDualConfig(
|
||||
left=left,
|
||||
right=right,
|
||||
baseline_seconds=float(t.get("baseline_seconds") or 2.0),
|
||||
filter_alpha=float(t.get("filter_alpha") or 0.25),
|
||||
backend=str(t.get("backend") or "orisys"),
|
||||
image_hz=float(t.get("image_hz") or 0.0),
|
||||
)
|
||||
|
||||
|
||||
def read_vector_map(sensor: Any) -> Optional[np.ndarray]:
|
||||
"""尝试从传感器读出力矢量场（每像素的切向力/位移矢量）。
|
||||
|
||||
为什么要"尝试"
|
||||
--------------
|
||||
Orisys 的 Python 绑定是编译过的，公开接口里没有一个明确叫
|
||||
"force vector map" 的方法。按语义最接近的是 ``compute_flow()``
|
||||
（形变光流 ≈ 切向位移场 ≈ 力矢量场），其次是 ``compute_hhd()``。
|
||||
不同 SDK 版本这两个方法的返回值也不一样 —— 有的是 ``(H,W,2)`` 数组，
|
||||
有的返回 ``bool``（只给"形变是否超阈值"）或可视化图。
|
||||
|
||||
所以这里逐个试，只接受真正是二维以上数组的结果；都拿不到就返回 ``None``，
|
||||
上层跳过该话题而不是发一张假图。哪一路成功会打到日志里，便于现场核对。
|
||||
"""
|
||||
for name in ("compute_flow", "compute_hhd", "compute_deformation"):
|
||||
fn = getattr(sensor, name, None)
|
||||
if fn is None:
|
||||
continue
|
||||
try:
|
||||
result = fn()
|
||||
except Exception:
|
||||
continue
|
||||
if isinstance(result, np.ndarray) and result.ndim >= 2 and result.size:
|
||||
return result
|
||||
return None
|
||||
|
||||
|
||||
def _parse_video(value) -> int | str:
|
||||
if isinstance(value, int):
|
||||
return value
|
||||
text = str(value).strip()
|
||||
if text.isdigit():
|
||||
return int(text)
|
||||
return text
|
||||
|
||||
|
||||
class OrisysTactile(TactileInterface):
|
||||
name = "orisys_tactile"
|
||||
|
||||
def __init__(self, config: OrisysDualConfig | None = None) -> None:
|
||||
self.cfg = config or OrisysDualConfig(
|
||||
left=OrisysSensorConfig(vid_src=0),
|
||||
right=OrisysSensorConfig(vid_src=2),
|
||||
)
|
||||
self._sensors: list[dict[str, Any]] = []
|
||||
self._baseline: dict[str, float] = {}
|
||||
self._connected = False
|
||||
|
||||
def connect(self, sides=("left", "right")) -> None:
|
||||
"""连接指定侧的传感器。注意：SDK 单进程双开会严重掉帧（30→0.8fps），
|
||||
双指请拆两个进程各连一侧（tactile.launch.py 已是双进程）。"""
|
||||
try:
|
||||
import orisys
|
||||
except ImportError as exc:
|
||||
raise RuntimeError(
|
||||
"缺少 orisys SDK。请执行 pip install -e /path/to/orisys-sdk "
|
||||
"（git.orisys-tech.cn/orisys/test）。配置依旧从 ``site.yaml`` 的 ``tactile`` 段读取。"
|
||||
) from exc
|
||||
|
||||
self._sensors = []
|
||||
for side, cfg in (("left", self.cfg.left), ("right", self.cfg.right)):
|
||||
if side not in sides:
|
||||
continue
|
||||
kwargs = dict(
|
||||
cuda=cfg.cuda,
|
||||
isstitch=cfg.isstitch,
|
||||
verbose=False,
|
||||
config_name=cfg.config_name,
|
||||
backend=cfg.backend,
|
||||
)
|
||||
if cfg.rotation_config_path:
|
||||
kwargs["rotation_config_path"] = cfg.rotation_config_path
|
||||
sensor = orisys.Sensor(cfg.vid_src, **kwargs)
|
||||
self._sensors.append({"side": side, "sensor": sensor, "cfg": cfg})
|
||||
self._calibrate_baseline()
|
||||
self._connected = True
|
||||
|
||||
def read_side(self, side: str, want_vector: bool = False) -> dict[str, Any]:
|
||||
"""读指定侧（单侧进程模式）；未连接/未知侧返回全零。
|
||||
|
||||
``want_vector=True`` 时额外计算力矢量场（逐像素运算，较贵）——
|
||||
只应在需要发布矢量图的那些帧上打开。
|
||||
"""
|
||||
if not self._connected:
|
||||
return self._empty_sample(side)
|
||||
for item in self._sensors:
|
||||
if item["side"] == side:
|
||||
return self._read_single(item, want_vector=want_vector)
|
||||
return self._empty_sample(side)
|
||||
|
||||
def disconnect(self) -> None:
|
||||
for item in self._sensors:
|
||||
try:
|
||||
item["sensor"].disconnect()
|
||||
except Exception:
|
||||
pass
|
||||
self._sensors = []
|
||||
self._connected = False
|
||||
|
||||
def reset(self, hole_id: int) -> None:
|
||||
del hole_id
|
||||
|
||||
def step(self, sample: TactileSample) -> TactileCommand:
|
||||
del sample
|
||||
return TactileCommand(
|
||||
delta_xyz=np.zeros(3),
|
||||
delta_rpy=np.zeros(3),
|
||||
done=False,
|
||||
ok=True,
|
||||
message="orisys step",
|
||||
)
|
||||
|
||||
def read_sample(self) -> TactileSample:
|
||||
if not self._connected or len(self._sensors) != 2:
|
||||
return TactileSample(
|
||||
timestamp=time.time(),
|
||||
force_xyz=np.zeros(3),
|
||||
torque_xyz=np.zeros(3),
|
||||
contact=False,
|
||||
)
|
||||
left = self._read_single(self._sensors[0])
|
||||
right = self._read_single(self._sensors[1])
|
||||
force_xyz = np.array(
|
||||
[
|
||||
(left["fnormal"] + right["fnormal"]) * 0.5,
|
||||
(left["fshearx"] + right["fshearx"]) * 0.5,
|
||||
(left["fsheary"] + right["fsheary"]) * 0.5,
|
||||
]
|
||||
)
|
||||
torque_xyz = np.array(
|
||||
[
|
||||
right["fnormal"] - left["fnormal"],
|
||||
right["fshearx"] - left["fshearx"],
|
||||
right["fsheary"] - left["fsheary"],
|
||||
]
|
||||
)
|
||||
return TactileSample(
|
||||
timestamp=time.time(),
|
||||
force_xyz=force_xyz,
|
||||
torque_xyz=torque_xyz,
|
||||
contact=bool(left["is_contact"] or right["is_contact"]),
|
||||
embedding=np.array(
|
||||
[
|
||||
left["fnormal"],
|
||||
right["fnormal"],
|
||||
left["fshearx"],
|
||||
right["fshearx"],
|
||||
left["fsheary"],
|
||||
right["fsheary"],
|
||||
]
|
||||
),
|
||||
raw={"left": left, "right": right, "centroid": self._estimate_centroid(left, right)},
|
||||
)
|
||||
|
||||
def _read_single(self, item: dict, want_vector: bool = False) -> dict[str, Any]:
|
||||
sensor = item["sensor"]
|
||||
cfg = item["cfg"]
|
||||
img = sensor.get_img()
|
||||
if img is None:
|
||||
return self._empty_sample(item["side"])
|
||||
is_contact = sensor.compute_deformation(check_motion=True, threshold=cfg.motion_threshold)
|
||||
fps, fnormal, fshearx, fsheary = sensor.read_info(
|
||||
sensor.info.FPS,
|
||||
sensor.info.FNORMAL,
|
||||
sensor.info.FSHEARX,
|
||||
sensor.info.FSHEARY,
|
||||
)
|
||||
side = item["side"]
|
||||
fnormal = max(0.0, float(fnormal) - self._baseline.get(f"{side}_normal", 0.0))
|
||||
fshearx = float(fshearx) - self._baseline.get(f"{side}_shearx", 0.0)
|
||||
fsheary = float(fsheary) - self._baseline.get(f"{side}_sheary", 0.0)
|
||||
centroid = (0.0, 0.0)
|
||||
centroid_found = False
|
||||
try:
|
||||
sensor.compute_contact()
|
||||
c, found = sensor.read_info(sensor.info.CENTROID)
|
||||
if found:
|
||||
centroid = c
|
||||
centroid_found = True
|
||||
except Exception:
|
||||
pass
|
||||
return {
|
||||
"side": side,
|
||||
"fnormal": fnormal,
|
||||
"fshearx": fshearx,
|
||||
"fsheary": fsheary,
|
||||
"fps": float(fps),
|
||||
"is_contact": bool(is_contact),
|
||||
"centroid": centroid,
|
||||
"centroid_found": centroid_found,
|
||||
"img": img,
|
||||
# 矢量场只在被要求时才算：compute_flow 是逐像素运算，每帧都做会
|
||||
# 明显增加 worker 耗时，可能拖累三力的 30fps。
|
||||
"vector": read_vector_map(sensor) if want_vector else None,
|
||||
}
|
||||
|
||||
def _empty_sample(self, side: str = "unknown") -> dict[str, Any]:
|
||||
return {
|
||||
"side": side,
|
||||
"fnormal": 0.0,
|
||||
"fshearx": 0.0,
|
||||
"fsheary": 0.0,
|
||||
"fps": 0.0,
|
||||
"is_contact": False,
|
||||
"centroid": (0.0, 0.0),
|
||||
"centroid_found": False,
|
||||
"img": None,
|
||||
"vector": None,
|
||||
}
|
||||
|
||||
def _calibrate_baseline(self) -> None:
|
||||
seconds = self.cfg.baseline_seconds
|
||||
if seconds <= 0:
|
||||
return
|
||||
buckets: dict[str, list[float]] = {
|
||||
"left_normal": [],
|
||||
"right_normal": [],
|
||||
"left_shearx": [],
|
||||
"right_shearx": [],
|
||||
"left_sheary": [],
|
||||
"right_sheary": [],
|
||||
}
|
||||
deadline = time.perf_counter() + seconds
|
||||
while time.perf_counter() < deadline:
|
||||
for item in self._sensors:
|
||||
sensor = item["sensor"]
|
||||
side = item["side"]
|
||||
img = sensor.get_img()
|
||||
if img is None:
|
||||
continue
|
||||
sensor.compute_deformation(check_motion=False)
|
||||
fnormal, fshearx, fsheary = sensor.read_info(
|
||||
sensor.info.FNORMAL,
|
||||
sensor.info.FSHEARX,
|
||||
sensor.info.FSHEARY,
|
||||
)
|
||||
buckets[f"{side}_normal"].append(float(fnormal))
|
||||
buckets[f"{side}_shearx"].append(float(fshearx))
|
||||
buckets[f"{side}_sheary"].append(float(fsheary))
|
||||
time.sleep(0.01)
|
||||
self._baseline = {k: float(np.mean(v)) if v else 0.0 for k, v in buckets.items()}
|
||||
|
||||
def _estimate_centroid(self, left: dict, right: dict) -> tuple[float, float]:
|
||||
if left["centroid_found"] and right["centroid_found"]:
|
||||
return (
|
||||
(left["centroid"][0] + right["centroid"][0]) * 0.5,
|
||||
(left["centroid"][1] + right["centroid"][1]) * 0.5,
|
||||
)
|
||||
if left["centroid_found"]:
|
||||
return left["centroid"]
|
||||
if right["centroid_found"]:
|
||||
return right["centroid"]
|
||||
return (0.0, 0.0)
|
||||
|
||||
|
||||
class MockOrisysTactile(OrisysTactile):
|
||||
name = "mock_orisys"
|
||||
|
||||
def __init__(self, config: OrisysDualConfig | None = None) -> None:
|
||||
super().__init__(config)
|
||||
self._n = 0
|
||||
|
||||
def connect(self) -> None:
|
||||
self._connected = True
|
||||
self._baseline = {}
|
||||
self._n = 0
|
||||
self._sensors = [
|
||||
{"side": "left", "sensor": None, "cfg": self.cfg.left},
|
||||
{"side": "right", "sensor": None, "cfg": self.cfg.right},
|
||||
]
|
||||
|
||||
def read_sample(self) -> TactileSample:
|
||||
self._n += 1
|
||||
contact = self._n >= 8
|
||||
force = 0.2 if contact else 0.0
|
||||
return TactileSample(
|
||||
timestamp=time.time(),
|
||||
force_xyz=np.array([force, 0.01, 0.005]),
|
||||
torque_xyz=np.zeros(3),
|
||||
contact=contact,
|
||||
raw={
|
||||
"left": {"fnormal": force, "fshearx": 0.0, "fsheary": 0.0, "is_contact": contact, "fps": 30.0, "centroid": (0, 0), "centroid_found": False},
|
||||
"right": {"fnormal": force, "fshearx": 0.0, "fsheary": 0.0, "is_contact": contact, "fps": 30.0, "centroid": (0, 0), "centroid_found": False},
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def make_tactile(data: dict) -> TactileInterface:
|
||||
cfg = load_orisys_from_site(data)
|
||||
backend = (cfg.backend or "orisys").strip().lower()
|
||||
if backend in ("mock", "mock_orisys"):
|
||||
return MockOrisysTactile(cfg)
|
||||
return OrisysTactile(cfg)
|
||||
|
||||
|
||||
# ==================== 多进程工作器（厂家推荐架构，每进程独立 CUDA 上下文） ====================
|
||||
# 实测：单进程双开两路传感器会互相拖垮（30→0.8fps）；两个独立进程各开一路则稳定 30fps。
|
||||
|
||||
def _usb_devpath_for_video(vid_src) -> Optional[str]:
|
||||
"""/dev/v4l/by-path/... 或 /dev/videoN → sysfs USB 设备号（如 1-3.4.1）。"""
|
||||
import re
|
||||
|
||||
try:
|
||||
real = os.path.realpath(str(vid_src))
|
||||
base = os.path.basename(real)
|
||||
if not base.startswith("video"):
|
||||
return None
|
||||
sysdev = os.path.realpath(f"/sys/class/video4linux/{base}/device")
|
||||
for part in reversed(sysdev.split("/")):
|
||||
if re.fullmatch(r"\d+-\d+(\.\d+)*", part):
|
||||
return part
|
||||
except Exception:
|
||||
pass
|
||||
return None
|
||||
|
||||
|
||||
def _usb_self_heal(vid_src, side: str) -> None:
|
||||
"""内核级卡死（select 超时循环）只有 unbind/bind 能救。orisys-usb-reset
|
||||
是 root 拥有的白名单脚本（sudoers NOPASSWD），只认 Orisys 的 vid/pid。"""
|
||||
import subprocess
|
||||
|
||||
dev = _usb_devpath_for_video(vid_src)
|
||||
if not dev:
|
||||
return f"[{side}] USB 自愈：解析不到设备号（{vid_src}）"
|
||||
try:
|
||||
r = subprocess.run(
|
||||
["sudo", "-n", "/usr/local/bin/orisys-usb-reset", dev],
|
||||
capture_output=True, text=True, timeout=15.0,
|
||||
)
|
||||
note = (r.stdout or r.stderr).strip().splitlines()
|
||||
return f"[{side}] USB 自愈复位 {dev}: {note[-1] if note else 'ok'}"
|
||||
except Exception as exc:
|
||||
return f"[{side}] USB 自愈失败 {dev}: {exc}"
|
||||
|
||||
|
||||
def tactile_worker(side: str, cfg: OrisysDualConfig, q, stop_event, image_q=None) -> None:
|
||||
"""独立进程：开一侧传感器，把样本推进队列。
|
||||
|
||||
q: mp.Queue（调用方给 maxsize≈2，满了丢旧留新）—— 三力等轻量数据，每帧都推。
|
||||
image_q: mp.Queue 或 None —— 视触觉图像与力矢量场。
|
||||
|
||||
图像默认**不过队列**（``image_q=None``）：每帧几百 KB，跨进程序列化 +
|
||||
ROS 发布的开销会挤占三力的 30fps 时序。需要图像时调用方传一个
|
||||
``maxsize=1`` 的队列，并按 ``cfg.image_hz`` 节流推送 —— 队列满时丢旧留新，
|
||||
主进程来不及消费也只会丢图像，不会影响三力。
|
||||
|
||||
stop_event: mp.Event，置位后干净退出（先 disconnect 释放 V4L2 设备）
|
||||
|
||||
自动重连：摄像头被强占/USB 抖动后会出现 get_img 持续 select 超时（空读，
|
||||
单次最长阻塞 ~10s）。连续 2 次空读或单次读取 >2s 即断开重连，直到恢复。
|
||||
"""
|
||||
try: # 给进程起个可识别的 comm 名，便于 pkill 清理孤儿（否则占死 /dev/video*）
|
||||
import ctypes
|
||||
ctypes.CDLL("libc.so.6").prctl(15, b"orisys_tac_w", 0, 0, 0) # PR_SET_NAME
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def _put(item: dict) -> None:
|
||||
try:
|
||||
if q.full():
|
||||
try:
|
||||
q.get_nowait()
|
||||
except Exception:
|
||||
pass
|
||||
q.put(item)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def _put_image(item: dict) -> None:
|
||||
"""图像入队：只保留最新一帧。队列满说明主进程没在消费，丢掉旧的即可。"""
|
||||
try:
|
||||
if image_q is None:
|
||||
return
|
||||
if image_q.full():
|
||||
try:
|
||||
image_q.get_nowait()
|
||||
except Exception:
|
||||
pass
|
||||
image_q.put(item)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
period = 1.0 / max(1.0, float(getattr(cfg.left if side == "left" else cfg.right, "target_fps", 30.0)))
|
||||
img_period = (1.0 / cfg.image_hz) if (image_q is not None and cfg.image_hz > 0) else 0.0
|
||||
last_img_t = 0.0
|
||||
vid_src = getattr(cfg.left if side == "left" else cfg.right, "vid_src", None)
|
||||
tac: Optional[OrisysTactile] = None
|
||||
bad = 0
|
||||
dead_cycles = 0 # 连续“重连了但读不到数据”的轮数 → 内核级卡死 → USB 自愈
|
||||
backoff = 0.5
|
||||
while not stop_event.is_set():
|
||||
if tac is None:
|
||||
try:
|
||||
tac = OrisysTactile(cfg)
|
||||
tac.connect(sides=(side,))
|
||||
_put({"side": side, "error": f"[{side}] (re)connected"})
|
||||
backoff = 0.5
|
||||
except Exception as exc:
|
||||
_put({"side": side, "error": f"connect failed: {exc}"})
|
||||
tac = None
|
||||
dead_cycles += 1
|
||||
if dead_cycles >= 2:
|
||||
_put({"side": side, "error": _usb_self_heal(vid_src, side)})
|
||||
dead_cycles = 0
|
||||
time.sleep(min(backoff, 3.0))
|
||||
backoff *= 2.0
|
||||
continue
|
||||
# 只有图像帧才算矢量场：compute_flow 是逐像素运算，每帧都算会拖累三力。
|
||||
want_img = image_q is not None and (time.perf_counter() - last_img_t) >= img_period
|
||||
t0 = time.perf_counter()
|
||||
try:
|
||||
d = tac.read_side(side, want_vector=want_img)
|
||||
empty = d.get("img") is None
|
||||
except Exception:
|
||||
d, empty = None, True
|
||||
dt = time.perf_counter() - t0
|
||||
if empty or dt > 2.0:
|
||||
bad += 1
|
||||
if bad >= 2:
|
||||
_put({"side": side, "error": f"[{side}] 相机断流，自动重连中…"})
|
||||
try:
|
||||
tac.disconnect()
|
||||
except Exception:
|
||||
pass
|
||||
tac = None
|
||||
bad = 0
|
||||
dead_cycles += 1
|
||||
if dead_cycles >= 2:
|
||||
_put({"side": side, "error": _usb_self_heal(vid_src, side)})
|
||||
dead_cycles = 0
|
||||
continue
|
||||
else:
|
||||
bad = 0
|
||||
dead_cycles = 0
|
||||
if d is not None and not empty:
|
||||
if want_img:
|
||||
last_img_t = time.perf_counter()
|
||||
_put_image({
|
||||
"side": side,
|
||||
"img": d.get("img"),
|
||||
"vector": d.get("vector"),
|
||||
"stamp": time.time(),
|
||||
})
|
||||
# 图像与矢量场已单独走 image_q；三力这条轻量队列不背它们。
|
||||
d = {k: v for k, v in d.items() if k not in ("img", "vector")}
|
||||
d["error"] = ""
|
||||
_put(d)
|
||||
rest = period - (time.perf_counter() - t0)
|
||||
if rest > 0:
|
||||
time.sleep(rest)
|
||||
try:
|
||||
if tac is not None:
|
||||
tac.disconnect()
|
||||
except Exception:
|
||||
pass
|
||||
+820
@@ -0,0 +1,820 @@
|
||||
"""D405 主跟踪丢了之后，用顶置 D435 **拍一张** 把工件 / k0 定到基座系。
|
||||
|
||||
为什么单独成模块
|
||||
----------------
|
||||
一阶段平时走腕部 D405（眼在手上）。画面里没有 conf≥门槛的目标时，
|
||||
不能让伺服继续追 0.12 的假框，也不能把顶视外参再乘一遍法兰
|
||||
（那会让一次性定位差几十厘米还看起来合理）。
|
||||
|
||||
判定、取 k0、反投影、臂是否静止 —— 全是纯函数，测试不启 ROS。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Iterable, Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
from arm_application.handeye_lib import cam_point_to_base
|
||||
from arm_application.path_profile import ( # 伺服和测试从本模块再导出，不另抄一份
|
||||
D435_FULL_ACC_M,
|
||||
D435_FULL_CRUISE_MPS,
|
||||
D435_FULL_DEC_M,
|
||||
D435_MID_HOP_MAX_M,
|
||||
D435_MID_LEFTOVER_M,
|
||||
D435_MOVE_ACC_MPS2,
|
||||
D435_MOVE_SPEED_SCALE,
|
||||
D435_NEAR_CRUISE_MPS,
|
||||
D435_NEAR_HOP_MAX_M,
|
||||
D435_NEAR_LEFTOVER_M,
|
||||
D435_VENDOR_MOVEL_ACC_MPS2,
|
||||
D435_VENDOR_MOVEL_VEL_MPS,
|
||||
D435_VERT_TILT_RAD,
|
||||
D435_SETTLE_S,
|
||||
FROZEN_J_HOP_MAX_M,
|
||||
build_arc_xy_move,
|
||||
flange_down_w,
|
||||
flange_tilt_rad,
|
||||
hybrid_cruise_vxy,
|
||||
oneshot_hop_limits,
|
||||
smooth_joint_schedule,
|
||||
trap_acc_for_hop,
|
||||
translate_hold_ori_waypoints,
|
||||
)
|
||||
|
||||
# 上一跳实际少走这么多，就认定冻结 J 在当前姿态走不动，不许按同一剩差再刷。
|
||||
D435_MIN_PROGRESS_FRAC = 0.40
|
||||
|
||||
|
||||
def best_det_conf(dets: Optional[Iterable]) -> Optional[float]:
|
||||
"""画面内最高检测置信度。空列表 / None → ``None``（不是 0：0 是"看到了但很差"）。"""
|
||||
if not dets:
|
||||
return None
|
||||
vals = [float(getattr(d, "conf", float("nan"))) for d in dets]
|
||||
vals = [c for c in vals if np.isfinite(c)]
|
||||
if not vals:
|
||||
return None
|
||||
return max(vals)
|
||||
|
||||
|
||||
def z_locked_qos():
|
||||
"""``visual_servo/z_locked`` 的 QoS。发布端和订阅端必须用这一份。
|
||||
|
||||
感知比伺服晚启动。默认 volatile 会丢掉伺服启动时已经发出的「已到锁高」，
|
||||
感知就停在初值 False，D435 永远不拍。
|
||||
"""
|
||||
from rclpy.qos import ( # noqa: PLC0415 — 纯函数测试不需要在导入时拉起 ROS
|
||||
DurabilityPolicy,
|
||||
HistoryPolicy,
|
||||
QoSProfile,
|
||||
ReliabilityPolicy,
|
||||
)
|
||||
|
||||
return QoSProfile(
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=1,
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
||||
)
|
||||
|
||||
|
||||
def at_lock_height(z_m: float, lock_z_m: float, gate_m: float) -> bool:
|
||||
"""法兰是否在锁高带内。
|
||||
|
||||
低于锁高**不算**到位。上一轮观测下降停在 0.26 m 时，若把「不高于锁高」
|
||||
当成到位，XY 会在低位开跑，D435 又因高度不对拒绝拍照，两边都不再动。
|
||||
``lock_z_m <= 0`` 表示没启用锁高。
|
||||
"""
|
||||
lock = float(lock_z_m)
|
||||
if lock <= 0.0:
|
||||
return True
|
||||
gate = max(0.02, float(gate_m))
|
||||
return abs(float(z_m) - lock) <= gate
|
||||
|
||||
|
||||
def lock_z_velocity(z_m: float, lock_z_m: float, *, kp: float, v_lim: float) -> float:
|
||||
"""回到锁高的竖直速度 [m/s]，基座 +Z 朝上。
|
||||
|
||||
低于锁高为正（上抬），高于为负（下降）。4 mm 内视为到位。
|
||||
不许在低于锁高时把速度写成 0：那会和「没到锁高不拍 D435」卡死。
|
||||
"""
|
||||
lock = float(lock_z_m)
|
||||
if lock <= 0.0:
|
||||
return 0.0
|
||||
ez = lock - float(z_m)
|
||||
if abs(ez) < 0.004:
|
||||
return 0.0
|
||||
lim = abs(float(v_lim))
|
||||
return float(np.clip(float(kp) * ez, -lim, lim))
|
||||
|
||||
|
||||
def hold_translation_vz(z_m: float, z_hold_m: float, *, kp: float, v_lim: float) -> float:
|
||||
"""平移时把法兰钉在**这一段开始时的高度**。
|
||||
|
||||
只写 v_z=0 不够：大范围平移加姿态校正时，阻尼求逆会把竖直速度吃掉，
|
||||
2026-10-09 真机从 0.327 m 漂到 0.307 m 又抬到 0.337 m。这里用开始那一
|
||||
帧的高度当设定，4 mm 内不动，避免来回抖。不追 site 里的锁高数字，
|
||||
也不因此停掉 XY。下臂仍只在二阶段。
|
||||
"""
|
||||
ez = float(z_hold_m) - float(z_m)
|
||||
if abs(ez) < 0.004:
|
||||
return 0.0
|
||||
lim = abs(float(v_lim))
|
||||
return float(np.clip(float(kp) * ez, -lim, lim))
|
||||
|
||||
|
||||
# D405 连续这么久没有高置信度目标，才让待命的 D435 拍一张。
|
||||
# 单帧漏检就拍，会在平移半路把目标换掉。
|
||||
D405_MISS_BEFORE_D435_S = 0.5
|
||||
# 拍了但伺服没 ``d435_commanding``（工作盒拒收等）：超过这时限就允许再拍。
|
||||
# 不清的话 ``_oneshot_shot`` 会永远真，臂停着、感知还以为在赶路。
|
||||
ONESHOT_ACCEPT_TIMEOUT_S = 0.8
|
||||
|
||||
|
||||
def oneshot_shot_timed_out(
|
||||
*,
|
||||
shot: bool,
|
||||
commanding: bool,
|
||||
age_s: float,
|
||||
timeout_s: float = ONESHOT_ACCEPT_TIMEOUT_S,
|
||||
) -> bool:
|
||||
"""拍过一张、伺服却没接住 → True。已经在赶路不许清。"""
|
||||
if not bool(shot) or bool(commanding):
|
||||
return False
|
||||
return float(age_s) >= float(timeout_s)
|
||||
|
||||
|
||||
def d405_commands_arm(
|
||||
*,
|
||||
using_d435: bool,
|
||||
arrived: bool,
|
||||
settled: bool,
|
||||
move_only: bool = False,
|
||||
) -> bool:
|
||||
"""D405 这一帧的点能不能下发给左臂。
|
||||
|
||||
推理一直开着。正在用 D435 那一个点时，进程内 MoveL 结束并且停稳之前不下发。
|
||||
停稳用 ``d435_handback_sec`` 和 ``lock_still_move_m``。赶路时 XY 已经进 15 mm
|
||||
也不算到位。不许在 MoveL 期间下发 D405（两套控制互抢会晃）。
|
||||
``move_only``：只测 D435 位移，D405 点一律不下发（到位之后也不交）。
|
||||
"""
|
||||
if move_only:
|
||||
return False
|
||||
if not using_d435:
|
||||
return True
|
||||
return bool(arrived) and bool(settled)
|
||||
|
||||
|
||||
def oneshot_needs_verticalize(
|
||||
tilt_rad: float,
|
||||
leftover_m: float,
|
||||
arrive_m: float,
|
||||
*,
|
||||
tilt_lim: float = D435_VERT_TILT_RAD,
|
||||
already_done: bool = False,
|
||||
) -> bool:
|
||||
"""XY 已经进到位门槛、倾角还大，才单独收垂直。不许和平移捆在一起。"""
|
||||
if already_done:
|
||||
return False
|
||||
if float(leftover_m) > float(arrive_m):
|
||||
return False
|
||||
return float(tilt_rad) > float(tilt_lim)
|
||||
|
||||
|
||||
def oneshot_next_hop_m(
|
||||
dist_m: float, hop_max_m: float = FROZEN_J_HOP_MAX_M
|
||||
) -> float:
|
||||
"""这一跳最多走 ``hop_max_m``。冻结 J 一次规划 400 mm 会少走一大截。"""
|
||||
return min(max(0.0, float(dist_m)), float(hop_max_m))
|
||||
|
||||
|
||||
def oneshot_cartesian_dp(
|
||||
p_ee,
|
||||
goal_xy,
|
||||
lock_z: float,
|
||||
*,
|
||||
gate_m: float,
|
||||
hop_max_m: Optional[float] = None,
|
||||
leftover_xy_m: Optional[float] = None,
|
||||
):
|
||||
"""这一跳的笛卡尔位移。没到锁高只收 Z；远处 XY 带锁高；近处只走 XY。
|
||||
|
||||
工件上方最后一跳若再把 7 mm 的 Z 和 60 mm 的 XY 捆在一起，冻结 J 会把腕部带偏。
|
||||
远处仍带 ``ee_z_m``，避免长跳把高度抬出锁高带。
|
||||
"""
|
||||
p = np.asarray(p_ee, dtype=float).reshape(3)
|
||||
z_lock = float(lock_z)
|
||||
leftover = (
|
||||
float(leftover_xy_m)
|
||||
if leftover_xy_m is not None
|
||||
else float(
|
||||
np.hypot(float(goal_xy[0]) - float(p[0]), float(goal_xy[1]) - float(p[1]))
|
||||
)
|
||||
)
|
||||
hop_cap = (
|
||||
float(hop_max_m)
|
||||
if hop_max_m is not None
|
||||
else oneshot_hop_limits(leftover)[0]
|
||||
)
|
||||
if not at_lock_height(float(p[2]), z_lock, float(gate_m)):
|
||||
dz = z_lock - float(p[2])
|
||||
mag = oneshot_next_hop_m(abs(dz), hop_cap)
|
||||
if mag < 1e-6:
|
||||
return np.zeros(3, dtype=float)
|
||||
return np.array([0.0, 0.0, float(np.sign(dz)) * mag], dtype=float)
|
||||
dz = 0.0 if leftover <= D435_NEAR_LEFTOVER_M else (z_lock - float(p[2]))
|
||||
dp = np.array(
|
||||
[
|
||||
float(goal_xy[0]) - float(p[0]),
|
||||
float(goal_xy[1]) - float(p[1]),
|
||||
dz,
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
n = float(np.linalg.norm(dp))
|
||||
hop = oneshot_next_hop_m(n, hop_cap)
|
||||
if n > hop > 1e-9:
|
||||
dp = dp * (hop / n)
|
||||
return dp
|
||||
|
||||
|
||||
def oneshot_full_segments(
|
||||
p_ee,
|
||||
goal_xy,
|
||||
lock_z: float,
|
||||
*,
|
||||
gate_m: float,
|
||||
):
|
||||
"""整段一次走完的笛卡尔分段。先收齐锁高，再走全部 XY，不切 80/15 mm。
|
||||
|
||||
近处（剩差 ≤80 mm）仍然只走 XY，不混 Z。收垂直不在这里。
|
||||
返回 ``[(dp, tag), ...]``，``tag`` 是 ``lift_z`` 或 ``xy``。
|
||||
"""
|
||||
p = np.asarray(p_ee, dtype=float).reshape(3).copy()
|
||||
z_lock = float(lock_z)
|
||||
goal = np.asarray(goal_xy, dtype=float).reshape(2)
|
||||
out = []
|
||||
if not at_lock_height(float(p[2]), z_lock, float(gate_m)):
|
||||
dz = z_lock - float(p[2])
|
||||
if abs(dz) >= 1e-6:
|
||||
out.append((np.array([0.0, 0.0, dz], dtype=float), "lift_z"))
|
||||
p[2] = z_lock
|
||||
leftover = float(np.hypot(float(goal[0]) - float(p[0]), float(goal[1]) - float(p[1])))
|
||||
dz = 0.0 if leftover <= D435_NEAR_LEFTOVER_M else (z_lock - float(p[2]))
|
||||
dp = np.array(
|
||||
[float(goal[0]) - float(p[0]), float(goal[1]) - float(p[1]), dz],
|
||||
dtype=float,
|
||||
)
|
||||
if float(np.linalg.norm(dp)) >= 1e-6:
|
||||
out.append((dp, "xy"))
|
||||
return out
|
||||
|
||||
|
||||
def oneshot_concat_hold_ori(q0, J, segments, *, R=None):
|
||||
"""把分段 ``dp`` 用同一份冻结 J 串成一条关节折线。中途不回零速。"""
|
||||
q = np.asarray(q0, dtype=float).copy()
|
||||
wps = [q.copy()]
|
||||
planned = 0.0
|
||||
asked = 0.0
|
||||
tags = []
|
||||
for dp, tag in segments:
|
||||
hop = float(np.linalg.norm(np.asarray(dp, dtype=float).reshape(3)))
|
||||
if hop < 1e-6:
|
||||
continue
|
||||
asked += hop
|
||||
part, remain = translate_hold_ori_waypoints(
|
||||
q, J, dp, R=R, verticalize=False
|
||||
)
|
||||
if len(part) < 2:
|
||||
break
|
||||
wps.extend(part[1:])
|
||||
q = wps[-1]
|
||||
planned += hop - float(remain)
|
||||
tags.append(str(tag))
|
||||
tag = "full"
|
||||
if tags == ["lift_z"]:
|
||||
tag = "lift_z"
|
||||
elif "lift_z" in tags and "xy" in tags:
|
||||
tag = "full"
|
||||
elif tags == ["xy"]:
|
||||
tag = "full"
|
||||
return wps, planned, asked, tag
|
||||
|
||||
|
||||
def oneshot_full_cruise_mps(leftover_m: float, far_cruise_mps: float) -> float:
|
||||
"""整段巡航：工件上方 45 mm/s；远处封顶 ``D435_FULL_CRUISE_MPS``，不许再弹到远处 hop 上限。"""
|
||||
far = max(1e-4, float(far_cruise_mps))
|
||||
if float(leftover_m) <= D435_NEAR_LEFTOVER_M:
|
||||
return min(D435_NEAR_CRUISE_MPS, far)
|
||||
return min(D435_FULL_CRUISE_MPS, far)
|
||||
|
||||
|
||||
def oneshot_may_send_another(
|
||||
leftover_m: float,
|
||||
last_dist_m: Optional[float],
|
||||
*,
|
||||
arrive_m: float,
|
||||
hop_max_m: float = FROZEN_J_HOP_MAX_M,
|
||||
min_progress_frac: float = D435_MIN_PROGRESS_FRAC,
|
||||
) -> bool:
|
||||
"""上一跳走完后还要不要再发一条。
|
||||
|
||||
剩差已经进到位门槛 → 钉住。
|
||||
上一跳几乎没缩短剩差 → 不许按同一距离再刷（真机 28 mm 死循环）。
|
||||
"""
|
||||
if float(leftover_m) <= float(arrive_m):
|
||||
return False
|
||||
if last_dist_m is None:
|
||||
return True
|
||||
progressed = float(last_dist_m) - float(leftover_m)
|
||||
planned = min(float(last_dist_m), float(hop_max_m))
|
||||
if progressed < float(min_progress_frac) * max(planned, 1e-6):
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def oneshot_should_submit(
|
||||
*,
|
||||
handoff: bool,
|
||||
low_for_s: float,
|
||||
already_shot: bool,
|
||||
wait_s: float = D405_MISS_BEFORE_D435_S,
|
||||
) -> bool:
|
||||
"""腕部连续 ``wait_s`` 秒没有高置信度目标、这一轮还没拍成，才让 D435 拍一张。
|
||||
|
||||
高度不在这里再判一次。进一阶段之前已经抬到锁高；再等伺服报「已到锁高」
|
||||
会和「没到锁高就只收 Z、不平移」卡死。拍成一张之后不许连拍，顶视不是第二套跟踪。
|
||||
"""
|
||||
return (
|
||||
bool(handoff)
|
||||
and (not already_shot)
|
||||
and float(low_for_s) >= float(wait_s)
|
||||
)
|
||||
|
||||
|
||||
def select_oneshot_det(dets: Optional[Iterable], min_conf: float):
|
||||
"""顶视一次性：只收 conf≥门槛的框，取最高分。没有就 None，不许降级。"""
|
||||
if not dets:
|
||||
return None
|
||||
ok = [
|
||||
d
|
||||
for d in dets
|
||||
if np.isfinite(float(getattr(d, "conf", float("nan"))))
|
||||
and float(d.conf) >= float(min_conf)
|
||||
]
|
||||
if not ok:
|
||||
return None
|
||||
return max(ok, key=lambda d: float(d.conf))
|
||||
|
||||
|
||||
def depth_raw_to_m(raw: Optional[np.ndarray], encoding: str) -> Optional[np.ndarray]:
|
||||
"""深度图 → 米。16UC1 常见是毫米；用中位数判量纲，不许默认当米。"""
|
||||
if raw is None:
|
||||
return None
|
||||
enc = (encoding or "").lower()
|
||||
arr = np.asarray(raw)
|
||||
if arr.size == 0:
|
||||
return None
|
||||
if enc in ("16uc1", "mono16"):
|
||||
pos = arr[arr > 0]
|
||||
if pos.size < 8:
|
||||
return None
|
||||
med = float(np.median(pos))
|
||||
scale = 0.001 if 20.0 <= med <= 8000.0 else (1.0 if med < 20 else 0.0001)
|
||||
out = arr.astype(float) * scale
|
||||
out[arr <= 0] = np.nan
|
||||
return out
|
||||
if enc == "32fc1":
|
||||
return arr.astype(float)
|
||||
return None
|
||||
|
||||
|
||||
def map_uv_to_depth(
|
||||
u: float, v: float, color_wh: tuple[int, int], depth_wh: tuple[int, int]
|
||||
) -> tuple[float, float]:
|
||||
"""彩色图像素 → 对齐深度图像素。分辨率相同则原样返回。"""
|
||||
cw, ch = int(color_wh[0]), int(color_wh[1])
|
||||
dw, dh = int(depth_wh[0]), int(depth_wh[1])
|
||||
if cw <= 0 or ch <= 0 or (cw == dw and ch == dh):
|
||||
return float(u), float(v)
|
||||
return float(u) * (dw / float(cw)), float(v) * (dh / float(ch))
|
||||
|
||||
|
||||
def wrist_needs_handoff(best_conf: Optional[float], threshold: float) -> bool:
|
||||
"""腕部这一帧有没有 conf≥门槛的目标。没有 → 该切 D435。
|
||||
|
||||
``None`` 当没有检测。门槛必须显式传入，不许在这里写死再抄一份到 site.yaml。
|
||||
"""
|
||||
if best_conf is None:
|
||||
return True
|
||||
try:
|
||||
c = float(best_conf)
|
||||
except (TypeError, ValueError):
|
||||
return True
|
||||
if not np.isfinite(c):
|
||||
return True
|
||||
return c < float(threshold)
|
||||
|
||||
|
||||
def k0_uv_from_det(
|
||||
det,
|
||||
*,
|
||||
kpt_index: int = 0,
|
||||
kpt_min_conf: float = 0.30,
|
||||
) -> Optional[tuple[float, float]]:
|
||||
"""从 pose 检测取 k0 像素。k0 不可信就返回 None（不许拿框心冒充 k0）。"""
|
||||
kpts = getattr(det, "kpts", None)
|
||||
if kpts is None:
|
||||
return None
|
||||
arr = np.asarray(kpts, dtype=float)
|
||||
if arr.ndim != 2 or arr.shape[0] <= int(kpt_index) or arr.shape[1] < 3:
|
||||
return None
|
||||
u, v, kc = (float(x) for x in arr[int(kpt_index)][:3])
|
||||
if not (np.isfinite(u) and np.isfinite(v) and np.isfinite(kc)):
|
||||
return None
|
||||
if kc < float(kpt_min_conf):
|
||||
return None
|
||||
return u, v
|
||||
|
||||
|
||||
def workpiece_uv_from_det(
|
||||
det,
|
||||
*,
|
||||
kpt_index: int = 0,
|
||||
kpt_min_conf: float = 0.30,
|
||||
) -> tuple[float, float, bool]:
|
||||
"""优先 k0；没有可信 k0 时退回框心，并标明 ``is_k0=False``。
|
||||
|
||||
顶视经常看不到腕部视角训出来的 k0。退回框心仍能把臂迁到工件上方，
|
||||
让 D405 重新看见 k0 —— 比假装"已经是 k0"好。调用方必须看第三个返回值。
|
||||
"""
|
||||
uv = k0_uv_from_det(det, kpt_index=kpt_index, kpt_min_conf=kpt_min_conf)
|
||||
if uv is not None:
|
||||
return uv[0], uv[1], True
|
||||
return float(det.cx), float(det.cy), False
|
||||
|
||||
|
||||
def depth_at_m(
|
||||
depth_m: np.ndarray,
|
||||
u: float,
|
||||
v: float,
|
||||
*,
|
||||
win: int = 3,
|
||||
) -> Optional[float]:
|
||||
"""窗口中位数深度 [m]。无效则 None，不许拿 0 去反投影。"""
|
||||
if depth_m is None or depth_m.size == 0:
|
||||
return None
|
||||
h, w = int(depth_m.shape[0]), int(depth_m.shape[1])
|
||||
ui, vi = int(round(u)), int(round(v))
|
||||
r = max(1, int(win))
|
||||
x0, x1 = max(0, ui - r), min(w, ui + r + 1)
|
||||
y0, y1 = max(0, vi - r), min(h, vi + r + 1)
|
||||
if x1 <= x0 or y1 <= y0:
|
||||
return None
|
||||
patch = np.asarray(depth_m[y0:y1, x0:x1], dtype=float).reshape(-1)
|
||||
patch = patch[np.isfinite(patch) & (patch > 1e-4)]
|
||||
if patch.size == 0:
|
||||
return None
|
||||
return float(np.median(patch))
|
||||
|
||||
|
||||
def pixel_on_base_plane(
|
||||
u: float,
|
||||
v: float,
|
||||
fx: float,
|
||||
fy: float,
|
||||
cx: float,
|
||||
cy: float,
|
||||
T_cam_to_base: np.ndarray,
|
||||
plane_z_m: float,
|
||||
) -> Optional[np.ndarray]:
|
||||
"""彩色像素射线和基座水平面 ``z=plane_z_m`` 的交点 [m]。不用深度图。
|
||||
|
||||
相机系方向是 ``((u-cx)/fx, (v-cy)/fy, 1)``，乘外参旋转到基座。
|
||||
交点在相机后面，或射线几乎贴着平面，返回 None。
|
||||
"""
|
||||
T = np.asarray(T_cam_to_base, dtype=float).reshape(4, 4)
|
||||
direction = np.array(
|
||||
[(float(u) - float(cx)) / float(fx), (float(v) - float(cy)) / float(fy), 1.0],
|
||||
dtype=float,
|
||||
)
|
||||
d_base = T[:3, :3] @ direction
|
||||
if abs(float(d_base[2])) < 1e-9:
|
||||
return None
|
||||
scale = (float(plane_z_m) - float(T[2, 3])) / float(d_base[2])
|
||||
if not np.isfinite(scale) or scale <= 0.0:
|
||||
return None
|
||||
point = T[:3, 3] + scale * d_base
|
||||
if not np.all(np.isfinite(point)):
|
||||
return None
|
||||
return np.asarray(point, dtype=float).reshape(3)
|
||||
|
||||
|
||||
def cam_xy_bias_axes(T_cam_to_base: np.ndarray):
|
||||
"""基座水平面上两个单位向量：``(视线前方, 画面右)``。
|
||||
|
||||
视线 = 相机 +Z 投到基座 XY（前倾 30° 时的交平面敏感轴）；
|
||||
画面右 = 相机 +X 投到基座 XY（立体基线 / 图像 u 方向）。
|
||||
水平投影退化时返回 None，不许拿 (0,0) 当轴。
|
||||
"""
|
||||
T = np.asarray(T_cam_to_base, dtype=float).reshape(4, 4)
|
||||
look = T[:3, :3] @ np.array([0.0, 0.0, 1.0], dtype=float)
|
||||
right = T[:3, :3] @ np.array([1.0, 0.0, 0.0], dtype=float)
|
||||
look_xy = np.asarray(look[:2], dtype=float)
|
||||
right_xy = np.asarray(right[:2], dtype=float)
|
||||
n_l = float(np.linalg.norm(look_xy))
|
||||
n_r = float(np.linalg.norm(right_xy))
|
||||
if n_l < 1e-6 or n_r < 1e-6:
|
||||
return None
|
||||
return look_xy / n_l, right_xy / n_r
|
||||
|
||||
|
||||
# 单次精拍这一步必须能走完「框心 → k1」。两孔间距约 60mm，半距 30mm。
|
||||
# 多走多拍的 12mm 封顶在这里会把一半孔距留下（2026-10-13 真机：指令 29mm 只走 12mm）。
|
||||
ONESHOT_ALIGN_CAP_M = 0.040
|
||||
# 下降后补 XY：一跳上限。2026-10-13 两跳合计 4.3mm 把近距检测带崩。
|
||||
ONCE_TRIM_MAX_STEP_M = 0.003
|
||||
ONCE_TRIM_REFUSE_M = 0.008
|
||||
HOLE_PAIR_TRIM_MIN_M = 0.050
|
||||
HOLE_PAIR_TRIM_MAX_M = 0.070
|
||||
# 悬停 → 观察高（约 15mm）单步上限。左臂间隙约 58mm，20mm × 3 步走完。
|
||||
# 观察高 → 贴面不再走这段，改斜线一步（XY 补偿 + 剩余 Z）。
|
||||
ONCE_APPROACH_STEP_M = 0.020
|
||||
# 悬停已对上时 ΔXY 必须为 0。site ``once_ff_look_per_m`` 同源。
|
||||
# 2026-10-14 下午：框心改平面交点后两条链路下臂都仍 +1.3mm X 过补插不进。
|
||||
# k=0.018 是深度框心时代的对冲，不是下降几何。笛卡尔 MoveL 只降 Z。
|
||||
ONCE_FF_LOOK_PER_M = 0.0
|
||||
ONCE_FF_MOVEL_VEL_MPS = 0.05
|
||||
|
||||
|
||||
def once_look_ff_xy(look_xy, descent_m: float, k: float):
|
||||
"""视线前馈 ΔXY [m]。k≤0 是笛卡尔锁 XY：返回 (0,0)，不是 None。
|
||||
|
||||
k>0 时沿 -look 走 k×下降。下降≤0 / 视线退化 → None。
|
||||
"""
|
||||
if float(descent_m) <= 1e-9:
|
||||
return None
|
||||
if float(k) <= 1e-9:
|
||||
return np.array([0.0, 0.0], dtype=float)
|
||||
look = np.asarray(look_xy, dtype=float).reshape(2)
|
||||
n = float(np.linalg.norm(look))
|
||||
if n < 1e-6 or not np.all(np.isfinite(look)):
|
||||
return None
|
||||
look = look / n
|
||||
dxy = -look * float(k) * float(descent_m)
|
||||
if not np.all(np.isfinite(dxy)):
|
||||
return None
|
||||
return dxy
|
||||
|
||||
|
||||
def once_ff_tcp_xyz(
|
||||
flange_xy,
|
||||
tip_off_xy,
|
||||
look_xy,
|
||||
descent_m: float,
|
||||
k: float,
|
||||
surface_z_m: float,
|
||||
gap_m: float,
|
||||
):
|
||||
"""规尖（厂家 MoveL 的 TCP）目标 [m]。Z 是面 + 间隙，不是法兰。"""
|
||||
dxy = once_look_ff_xy(look_xy, descent_m, k)
|
||||
if dxy is None:
|
||||
return None
|
||||
off = np.asarray(tip_off_xy, dtype=float).reshape(2)
|
||||
fl = np.asarray(flange_xy, dtype=float).reshape(2)
|
||||
if not np.all(np.isfinite(off)) or not np.all(np.isfinite(fl)):
|
||||
return None
|
||||
tcp = np.array(
|
||||
[
|
||||
float(fl[0]) + float(off[0]) + float(dxy[0]),
|
||||
float(fl[1]) + float(off[1]) + float(dxy[1]),
|
||||
float(surface_z_m) + float(gap_m),
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
if not np.all(np.isfinite(tcp)):
|
||||
return None
|
||||
return tcp, dxy
|
||||
|
||||
|
||||
def d435_movel_from_tcp(tcp_xyz, flange_xy, k0_xy):
|
||||
"""D435 粗定位 MoveL 目标 [m]：当前厂家 TCP + (K0−法兰) 的 ROS XY。
|
||||
|
||||
keep_z 由硬件用当前 TCP 的 Z 覆盖。ROS 绝对 K0 不能当 WORLD 目标
|
||||
（2026-10-14 贴面 MoveL 已踩过）。缺数 / 非有限 → None。
|
||||
"""
|
||||
try:
|
||||
t = np.asarray(tcp_xyz, dtype=float).reshape(3)
|
||||
fl = np.asarray(flange_xy, dtype=float).reshape(2)
|
||||
k0 = np.asarray(k0_xy, dtype=float).reshape(2)
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
if t.size != 3 or fl.size != 2 or k0.size != 2:
|
||||
return None
|
||||
if not np.all(np.isfinite(t)) or not np.all(np.isfinite(fl)) or not np.all(np.isfinite(k0)):
|
||||
return None
|
||||
out = np.array(
|
||||
[float(t[0]) + float(k0[0]) - float(fl[0]),
|
||||
float(t[1]) + float(k0[1]) - float(fl[1]),
|
||||
float(t[2])],
|
||||
dtype=float,
|
||||
)
|
||||
if not np.all(np.isfinite(out)):
|
||||
return None
|
||||
return out
|
||||
|
||||
|
||||
def once_ff_movel_from_tcp(tcp_xyz, dxy, descent_m: float):
|
||||
"""厂家 MoveL 目标 = 当前 Studio TCP + (ΔXY, −下降) [m]。
|
||||
|
||||
2026-10-14：把 ROS 规尖绝对坐标发给 MoveL，k=0.018 与 0.035 都落到
|
||||
WORLD (0.595, −0.468, 0.024)，法兰实走约 (+9, −8)mm。WORLD 原点与
|
||||
ROS 基座不是同一点，绝对坐标不能用。相对位移才跟 k 有关。
|
||||
"""
|
||||
if float(descent_m) <= 1e-9:
|
||||
return None
|
||||
t = np.asarray(tcp_xyz, dtype=float).reshape(3)
|
||||
d = np.asarray(dxy, dtype=float).reshape(2)
|
||||
if t.size != 3 or d.size != 2:
|
||||
return None
|
||||
if not np.all(np.isfinite(t)) or not np.all(np.isfinite(d)):
|
||||
return None
|
||||
out = np.array(
|
||||
[float(t[0]) + float(d[0]), float(t[1]) + float(d[1]), float(t[2]) - float(descent_m)],
|
||||
dtype=float,
|
||||
)
|
||||
if not np.all(np.isfinite(out)):
|
||||
return None
|
||||
return out
|
||||
|
||||
|
||||
def oneshot_align_step_m(err_m: float) -> float:
|
||||
"""单次精拍允许的步长 [m]。不超过半孔距余量；超过 cap 才截断。"""
|
||||
e = abs(float(err_m))
|
||||
cap = float(ONESHOT_ALIGN_CAP_M)
|
||||
if e <= cap:
|
||||
return e
|
||||
return cap
|
||||
|
||||
|
||||
def hole_pair_span_m(p0, p1) -> float:
|
||||
"""两孔基座 XY 间距 [m]。"""
|
||||
a = np.asarray(p0, dtype=float).reshape(-1)
|
||||
b = np.asarray(p1, dtype=float).reshape(-1)
|
||||
return float(np.hypot(float(a[0]) - float(b[0]), float(a[1]) - float(b[1])))
|
||||
|
||||
|
||||
def hole_pair_span_ok(span_m: float) -> bool:
|
||||
"""下降后补孔用的间距窗。真机 k1/k2 约 60mm；45mm 是近距检测漂了。"""
|
||||
s = float(span_m)
|
||||
return HOLE_PAIR_TRIM_MIN_M <= s <= HOLE_PAIR_TRIM_MAX_M
|
||||
|
||||
|
||||
def flange_z_for_gap_m(tip_len_m: float, surface_z_m: float, gap_m: float) -> float:
|
||||
"""规尖离面 ``gap_m`` 时的法兰 Z [m]。与轻降几何同一条式子。"""
|
||||
return float(tip_len_m) + float(surface_z_m) + float(gap_m)
|
||||
|
||||
|
||||
def final_gap_refusal(
|
||||
gap_m: float,
|
||||
tool_len_m: float,
|
||||
surface_z_m: float,
|
||||
hover_z_m: float,
|
||||
z_min_m: float,
|
||||
z_max_m: float,
|
||||
) -> str:
|
||||
"""笛卡尔锁 XY 的贴面间隙能不能用。空串表示可以；非空就是拒绝原因。
|
||||
|
||||
参数是规尖离面 [m]，不是法兰 Z：法兰 = 工具长 + 工件面 + 间隙，只在这里算一次。
|
||||
"""
|
||||
gap = float(gap_m)
|
||||
if not np.isfinite(gap):
|
||||
return "贴面间隙不是有限数，拒绝开跑"
|
||||
if gap <= 1e-9:
|
||||
return (
|
||||
f"贴面间隙 {gap * 1000:.1f}mm ≤ 0，规尖会进工件/贴面接触，拒绝开跑"
|
||||
)
|
||||
tool = float(tool_len_m)
|
||||
surf = float(surface_z_m)
|
||||
if tool <= 0.0 or surf <= 0.0:
|
||||
return "写了贴面间隙，但工具长度或工件面未知，无法确认尖端在面之上，拒绝开跑"
|
||||
hover_clear = float(hover_z_m) - tool - surf
|
||||
if gap > hover_clear + 1e-6:
|
||||
return (
|
||||
f"贴面间隙 {gap * 1000:.1f}mm 比悬停离面 {hover_clear * 1000:.1f}mm 还高，"
|
||||
"锁 XY 只会抬臂，拒绝开跑"
|
||||
)
|
||||
flange = flange_z_for_gap_m(tool, surf, gap)
|
||||
z_lo = float(z_min_m)
|
||||
z_hi = float(z_max_m)
|
||||
if flange < z_lo - 1e-6 or flange > z_hi + 1e-6:
|
||||
return (
|
||||
f"贴面法兰 Z {flange:.3f}m（规尖离面 {gap * 1000:.1f}mm）"
|
||||
f"超出工作盒 Z [{z_lo:.3f}, {z_hi:.3f}]，拒绝开跑"
|
||||
)
|
||||
return ""
|
||||
|
||||
|
||||
def decompose_base_xy(delta_xy, T_cam_to_base: np.ndarray):
|
||||
"""把基座 XY 残差拆成沿视线 / 沿画面右 [m]。投影退化返回 None。"""
|
||||
axes = cam_xy_bias_axes(T_cam_to_base)
|
||||
if axes is None:
|
||||
return None
|
||||
look_xy, right_xy = axes
|
||||
d = np.asarray(delta_xy, dtype=float).reshape(2)
|
||||
return {
|
||||
"along_look_m": float(np.dot(d, look_xy)),
|
||||
"along_right_m": float(np.dot(d, right_xy)),
|
||||
"look_xy": [float(look_xy[0]), float(look_xy[1])],
|
||||
"right_xy": [float(right_xy[0]), float(right_xy[1])],
|
||||
}
|
||||
|
||||
|
||||
def px_to_cam(
|
||||
u: float, v: float, z: float, fx: float, fy: float, cx: float, cy: float
|
||||
) -> np.ndarray:
|
||||
return np.array(
|
||||
[(u - cx) * z / fx, (v - cy) * z / fy, z], dtype=float
|
||||
)
|
||||
|
||||
|
||||
def oneshot_k0_base(
|
||||
det,
|
||||
depth_m: np.ndarray,
|
||||
*,
|
||||
fx: float,
|
||||
fy: float,
|
||||
cx: float,
|
||||
cy: float,
|
||||
T_cam_to_base: np.ndarray,
|
||||
min_z_m: float,
|
||||
max_z_m: float,
|
||||
kpt_index: int = 0,
|
||||
kpt_min_conf: float = 0.30,
|
||||
min_det_conf: float = 0.5,
|
||||
color_wh: Optional[tuple[int, int]] = None,
|
||||
depth_wh: Optional[tuple[int, int]] = None,
|
||||
) -> Optional[tuple[np.ndarray, bool]]:
|
||||
"""D435 一张图 → 基座系 XYZ。失败返回 None。
|
||||
|
||||
外参必须是 **cam→基座**（eye_to_hand）。传入腕部 T 会得到错位置且不报错，
|
||||
所以调用方必须自己保证文件对；本函数不再猜。
|
||||
反投影用**彩色图**像素 + 彩色图内参；深度只在对齐图上取样。
|
||||
"""
|
||||
if det is None or float(getattr(det, "conf", 0.0)) < float(min_det_conf):
|
||||
return None
|
||||
u, v, is_k0 = workpiece_uv_from_det(
|
||||
det, kpt_index=kpt_index, kpt_min_conf=kpt_min_conf
|
||||
)
|
||||
u_d, v_d = u, v
|
||||
if color_wh is not None and depth_wh is not None:
|
||||
u_d, v_d = map_uv_to_depth(u, v, color_wh, depth_wh)
|
||||
z = depth_at_m(depth_m, u_d, v_d)
|
||||
if z is None or z < float(min_z_m) or z > float(max_z_m):
|
||||
return None
|
||||
p_cam = px_to_cam(u, v, z, fx, fy, cx, cy)
|
||||
T_ee = np.eye(4) # eye_to_hand 不用法兰，占位
|
||||
p_base = cam_point_to_base(p_cam, T_cam_to_base, T_ee, "eye_to_hand")
|
||||
if not np.all(np.isfinite(p_base)):
|
||||
return None
|
||||
return np.asarray(p_base, dtype=float).reshape(3), bool(is_k0)
|
||||
|
||||
|
||||
@dataclass
|
||||
class StillLock:
|
||||
"""D405 跟住时，法兰 XY 连续 ``still_sec`` 秒位移不超过 ``still_move_m`` → 锁定。
|
||||
|
||||
顶视一次性迁过去的那段路**不算**锁定：``tracking_ok`` 必须是腕部有效目标。
|
||||
大幅一动就重计时，避免"晃一下刚好满 3 秒"误进二阶段。
|
||||
"""
|
||||
|
||||
still_sec: float
|
||||
still_move_m: float
|
||||
_t0: Optional[float] = None
|
||||
_anchor_xy: Optional[np.ndarray] = None
|
||||
|
||||
def reset(self) -> None:
|
||||
self._t0 = None
|
||||
self._anchor_xy = None
|
||||
|
||||
def update(
|
||||
self,
|
||||
p_ee_xy: Optional[np.ndarray],
|
||||
now: float,
|
||||
*,
|
||||
tracking_ok: bool,
|
||||
) -> bool:
|
||||
if (not tracking_ok) or p_ee_xy is None:
|
||||
self.reset()
|
||||
return False
|
||||
xy = np.asarray(p_ee_xy, dtype=float).reshape(2)
|
||||
if self._anchor_xy is None or self._t0 is None:
|
||||
self._anchor_xy = xy.copy()
|
||||
self._t0 = float(now)
|
||||
return False
|
||||
if float(np.linalg.norm(xy - self._anchor_xy)) > float(self.still_move_m):
|
||||
self._anchor_xy = xy.copy()
|
||||
self._t0 = float(now)
|
||||
return False
|
||||
return (float(now) - float(self._t0)) >= float(self.still_sec)
|
||||
@@ -0,0 +1,556 @@
|
||||
"""路径弧长上的梯形速度，以及 XY 圆弧。
|
||||
|
||||
二阶段下臂和一阶段 D435 迁臂共用这一份。不许再抄一套加减速。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
# 整段同一巡航：site ``v_slow_xy`` × 本系数。1.5 → 120 mm/s（远处 hop 上限）。
|
||||
D435_MOVE_SPEED_SCALE = 1.5
|
||||
# D435 厂家 MoveL：SDK ``vel`` [m/s]、``acc`` [m/s²]。
|
||||
# 厂家示例自由空间 0.6 / 近接触 0.2；本档 0.3 和 1.5，短行程也能爬到巡航。
|
||||
# 只改 vel、不给 acc 时，短 hop 仍被默认加速度卡住，肉眼看不出。
|
||||
D435_VENDOR_MOVEL_VEL_MPS = 0.3
|
||||
D435_VENDOR_MOVEL_ACC_MPS2 = 1.5
|
||||
# 缺路程时的兜底加速度。真机 hop 用 ``trap_acc_for_hop``，和二阶段同一条刹车。
|
||||
D435_MOVE_ACC_MPS2 = 0.1875
|
||||
# action 走完后再停这么久，才重测 TF。二阶段 ``_goto`` 的 settle_s。
|
||||
D435_SETTLE_S = 0.25
|
||||
# 冻结雅可比一次只信这么远。二阶段 ``_plan_and_send`` 默认 ``max_seg`` 同源。
|
||||
# 400 mm 一次规划，真机剩 117 mm，随后 28 mm 死循环刷轨迹。
|
||||
FROZEN_J_HOP_MAX_M = 0.08
|
||||
# 剩差进入这一带 = 工件上方。真机最后一跳 37–75 mm @ 80 mm/s 会晃并偏转。
|
||||
D435_NEAR_LEFTOVER_M = 0.08
|
||||
D435_NEAR_HOP_MAX_M = 0.015
|
||||
D435_NEAR_CRUISE_MPS = 0.045
|
||||
# 法兰倾角超过这个才单独收垂直。和 XY 捆在同一跳会把腕部漏成 Y 过冲。
|
||||
D435_VERT_TILT_RAD = 0.015
|
||||
D435_MID_LEFTOVER_M = 0.15
|
||||
D435_MID_HOP_MAX_M = 0.040
|
||||
# 弦长的 12% 当矢高，封顶 50 mm，避免大圆弧甩出工作盒。
|
||||
D435_ARC_SAGITTA_FRAC = 0.12
|
||||
D435_ARC_SAGITTA_MAX_M = 0.05
|
||||
# 整段一次走完。起步必须够长：40 mm 就爬到 80 mm/s 是真机那下弹射
|
||||
# （头尾速度本来就是 0，不是初速度；平移也不下发角速度）。
|
||||
# 峰值加速度 a_max = π v² / (4 d_acc)；150 mm @ 90 mm/s → 约 0.042 m/s²。
|
||||
# 巡航相对原 60 mm/s 加 50%；加减速路程同比加长，起步时间差不多。
|
||||
D435_FULL_ACC_M = 0.150
|
||||
D435_FULL_DEC_M = 0.120
|
||||
D435_FULL_CRUISE_MPS = 0.090
|
||||
|
||||
|
||||
def hybrid_cruise_vxy(
|
||||
v_slow_xy: float, scale: float = D435_MOVE_SPEED_SCALE
|
||||
) -> float:
|
||||
"""远处巡航。近处由 ``oneshot_hop_limits`` 另给一档，不许再抄 15 mm/s。"""
|
||||
return max(1e-4, float(v_slow_xy) * float(scale))
|
||||
|
||||
|
||||
def oneshot_hop_limits(
|
||||
leftover_m: float, far_cruise_mps: float = 0.08
|
||||
) -> Tuple[float, float]:
|
||||
"""这一跳的 ``(hop_max_m, cruise_mps)``。近处必须比 80 mm / 80 mm/s 短且慢。"""
|
||||
d = max(0.0, float(leftover_m))
|
||||
far = max(1e-4, float(far_cruise_mps))
|
||||
if d <= D435_NEAR_LEFTOVER_M:
|
||||
return D435_NEAR_HOP_MAX_M, min(D435_NEAR_CRUISE_MPS, far)
|
||||
if d <= D435_MID_LEFTOVER_M:
|
||||
return D435_MID_HOP_MAX_M, far
|
||||
return FROZEN_J_HOP_MAX_M, far
|
||||
|
||||
|
||||
def flange_down_w(
|
||||
R,
|
||||
*,
|
||||
dead_rad: float = 0.006,
|
||||
gain: float = 2.0,
|
||||
mag_lim: float = 0.5,
|
||||
) -> Tuple[np.ndarray, float]:
|
||||
"""让法兰 +Z 指向世界 −Z 的角速度 + 当前倾角。
|
||||
|
||||
和二阶段 ``_w_down`` 同一份。死区 0.006 rad（约 0.34°）以下不再修。
|
||||
"""
|
||||
z_now = np.asarray(R, dtype=float).reshape(3, 3)[:, 2]
|
||||
z_des = np.array([0.0, 0.0, -1.0], dtype=float)
|
||||
c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0))
|
||||
ang = float(math.acos(c))
|
||||
if ang < float(dead_rad):
|
||||
return np.zeros(3, dtype=float), ang
|
||||
axis = np.cross(z_now, z_des)
|
||||
n = float(np.linalg.norm(axis))
|
||||
axis = axis / n if n > 1e-9 else np.array([1.0, 0.0, 0.0], dtype=float)
|
||||
mag = float(np.clip(float(gain) * ang, -float(mag_lim), float(mag_lim)))
|
||||
return axis * mag, ang
|
||||
|
||||
|
||||
def flange_tilt_rad(R) -> float:
|
||||
"""法兰 +Z 相对世界 −Z 的倾角 [rad]。只读角度，不带死区。"""
|
||||
z_now = np.asarray(R, dtype=float).reshape(3, 3)[:, 2]
|
||||
z_des = np.array([0.0, 0.0, -1.0], dtype=float)
|
||||
c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0))
|
||||
return float(math.acos(c))
|
||||
|
||||
|
||||
def trap_acc_for_hop(dist: float, v: float) -> float:
|
||||
"""二阶段 ``_plan_and_send`` 同一条刹车：路程越长刹车越远，不许另写一套。"""
|
||||
v_cmd = max(min(float(v), 0.08), 1e-3)
|
||||
d_brake = min(0.030, max(0.006, 0.32 * max(float(dist), 1e-6)))
|
||||
return max(v_cmd * v_cmd / (2.0 * d_brake), 0.030)
|
||||
|
||||
|
||||
def trap_params(dist: float, v: float, a: float) -> Tuple[float, float, float, float, float]:
|
||||
"""Rest-to-rest 梯形：返回 (T, v_peak, t_acc, t_cruise, a)。巡航速度保持 v。"""
|
||||
dist = max(float(dist), 1e-6)
|
||||
v = max(float(v), 1e-4)
|
||||
a = max(float(a), 1e-3)
|
||||
d_acc = v * v / (2.0 * a)
|
||||
if 2.0 * d_acc >= dist:
|
||||
v_peak = math.sqrt(max(a * dist, 1e-12))
|
||||
t_acc = v_peak / a
|
||||
return 2.0 * t_acc, v_peak, t_acc, 0.0, a
|
||||
t_acc = v / a
|
||||
t_cruise = (dist - 2.0 * d_acc) / v
|
||||
return 2.0 * t_acc + t_cruise, v, t_acc, t_cruise, a
|
||||
|
||||
|
||||
def trap_sv(
|
||||
t: float, dist: float, v_peak: float, t_acc: float, t_cruise: float, a: float,
|
||||
) -> Tuple[float, float]:
|
||||
"""时刻 t 的路径弧长 s 与切向速度。"""
|
||||
t = max(0.0, float(t))
|
||||
t_dec0 = t_acc + t_cruise
|
||||
t_end = t_dec0 + t_acc
|
||||
if t >= t_end:
|
||||
return dist, 0.0
|
||||
if t < t_acc:
|
||||
return 0.5 * a * t * t, a * t
|
||||
if t < t_dec0:
|
||||
return 0.5 * a * t_acc * t_acc + v_peak * (t - t_acc), v_peak
|
||||
td = t - t_dec0
|
||||
s0 = 0.5 * a * t_acc * t_acc + v_peak * t_cruise
|
||||
return s0 + v_peak * td - 0.5 * a * td * td, max(0.0, v_peak - a * td)
|
||||
|
||||
|
||||
def smooth_path_params(
|
||||
dist: float,
|
||||
v: float,
|
||||
*,
|
||||
d_acc: float = D435_FULL_ACC_M,
|
||||
d_dec: float = D435_FULL_DEC_M,
|
||||
) -> Tuple[float, float, float, float, float, float]:
|
||||
"""整段一次走完的余弦加减速：(T, v_peak, t_acc, t_cruise, d_acc, d_dec)。
|
||||
|
||||
加速/减速段速度按半个余弦走，两端加速度为 0，没有梯形那种加速度台阶。
|
||||
路程不够巡航时整段一条余弦，峰值不超过 ``v``。
|
||||
"""
|
||||
dist = max(float(dist), 1e-6)
|
||||
v = max(float(v), 1e-4)
|
||||
acc = min(max(float(d_acc), 1e-4), 0.45 * dist)
|
||||
dec = min(max(float(d_dec), 1e-4), 0.45 * dist)
|
||||
if acc + dec >= dist - 1e-6:
|
||||
# 整段一条正弦速度：v = v_peak sin(π t / T)，s(T)=2 v_peak T / π。
|
||||
T = math.pi * dist / (2.0 * v)
|
||||
return T, v, 0.0, 0.0, 0.0, dist
|
||||
t_acc = 2.0 * acc / v
|
||||
t_dec = 2.0 * dec / v
|
||||
t_cruise = (dist - acc - dec) / v
|
||||
T = t_acc + t_cruise + t_dec
|
||||
return T, v, t_acc, t_cruise, acc, dec
|
||||
|
||||
|
||||
def cosine_peak_acc(v: float, d_acc: float) -> float:
|
||||
"""余弦加速段的峰值切向加速度 [m/s²]。``d_acc`` 越长，起步越不弹。"""
|
||||
d = max(float(d_acc), 1e-6)
|
||||
return math.pi * float(v) * float(v) / (4.0 * d)
|
||||
|
||||
|
||||
def smooth_path_sv(
|
||||
t: float,
|
||||
dist: float,
|
||||
v_peak: float,
|
||||
t_acc: float,
|
||||
t_cruise: float,
|
||||
d_acc: float,
|
||||
d_dec: float,
|
||||
) -> Tuple[float, float]:
|
||||
"""时刻 t 的弧长与切向速度。头尾速度为 0。"""
|
||||
t = max(0.0, float(t))
|
||||
dist = max(float(dist), 1e-6)
|
||||
v_peak = max(float(v_peak), 1e-4)
|
||||
if t_acc <= 1e-9 and t_cruise <= 1e-9:
|
||||
T = math.pi * dist / (2.0 * v_peak)
|
||||
if t >= T:
|
||||
return dist, 0.0
|
||||
u = math.pi * t / T
|
||||
s = (v_peak * T / math.pi) * (1.0 - math.cos(u))
|
||||
vs = v_peak * math.sin(u)
|
||||
return min(s, dist), max(0.0, vs)
|
||||
t_dec = 2.0 * float(d_dec) / v_peak if v_peak > 1e-9 else 0.0
|
||||
t_end = float(t_acc) + float(t_cruise) + t_dec
|
||||
if t >= t_end:
|
||||
return dist, 0.0
|
||||
if t < t_acc:
|
||||
u = math.pi * t / t_acc
|
||||
s = 0.5 * v_peak * (t - t_acc / math.pi * math.sin(u))
|
||||
vs = 0.5 * v_peak * (1.0 - math.cos(u))
|
||||
return s, vs
|
||||
if t < t_acc + t_cruise:
|
||||
return float(d_acc) + v_peak * (t - t_acc), v_peak
|
||||
td = t - t_acc - t_cruise
|
||||
u = math.pi * td / t_dec if t_dec > 1e-9 else math.pi
|
||||
s0 = float(d_acc) + v_peak * float(t_cruise)
|
||||
s = s0 + 0.5 * v_peak * (td + t_dec / math.pi * math.sin(u))
|
||||
vs = 0.5 * v_peak * (1.0 + math.cos(u))
|
||||
return min(s, dist), max(0.0, vs)
|
||||
|
||||
|
||||
def _circle_3(
|
||||
a: np.ndarray, b: np.ndarray, c: np.ndarray
|
||||
) -> Optional[Tuple[np.ndarray, float]]:
|
||||
ax, ay = float(a[0]), float(a[1])
|
||||
bx, by = float(b[0]), float(b[1])
|
||||
cx, cy = float(c[0]), float(c[1])
|
||||
d = 2.0 * (ax * (by - cy) + bx * (cy - ay) + cx * (ay - by))
|
||||
if abs(d) < 1e-9:
|
||||
return None
|
||||
a2 = ax * ax + ay * ay
|
||||
b2 = bx * bx + by * by
|
||||
c2 = cx * cx + cy * cy
|
||||
ux = (a2 * (by - cy) + b2 * (cy - ay) + c2 * (ay - by)) / d
|
||||
uy = (a2 * (cx - bx) + b2 * (ax - cx) + c2 * (bx - ax)) / d
|
||||
center = np.array([ux, uy], dtype=float)
|
||||
r = float(np.hypot(ax - ux, ay - uy))
|
||||
if r < 1e-4:
|
||||
return None
|
||||
return center, r
|
||||
|
||||
|
||||
def _ccw(a0: float, a1: float) -> float:
|
||||
return (a1 - a0) % (2.0 * math.pi)
|
||||
|
||||
|
||||
def _xy_in_box(xy: np.ndarray, lo: np.ndarray, hi: np.ndarray) -> bool:
|
||||
return bool(
|
||||
float(lo[0]) <= float(xy[0]) <= float(hi[0])
|
||||
and float(lo[1]) <= float(xy[1]) <= float(hi[1])
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class ArcXyMove:
|
||||
"""从 p0 走到 p1 的 XY 圆弧（或退化成直线），弧长上跑梯形速度。"""
|
||||
|
||||
p0: np.ndarray
|
||||
p1: np.ndarray
|
||||
length: float
|
||||
v_cruise: float
|
||||
T: float
|
||||
v_peak: float
|
||||
t_acc: float
|
||||
t_cruise: float
|
||||
a: float
|
||||
t0: float
|
||||
linear: bool
|
||||
center: Optional[np.ndarray] = None
|
||||
radius: float = 0.0
|
||||
ang0: float = 0.0
|
||||
sweep: float = 0.0
|
||||
s_last: float = 0.0
|
||||
_dir: np.ndarray = field(default_factory=lambda: np.array([1.0, 0.0]))
|
||||
|
||||
def sample(self, now: float) -> Tuple[np.ndarray, np.ndarray, bool]:
|
||||
"""返回 (计划 XY, 切向速度, 是否走完)。"""
|
||||
t = max(0.0, float(now) - float(self.t0))
|
||||
s, sdot = trap_sv(
|
||||
t, self.length, self.v_peak, self.t_acc, self.t_cruise, self.a
|
||||
)
|
||||
s = min(float(s), float(self.length))
|
||||
self.s_last = s
|
||||
done = t >= float(self.T) - 1e-4 or s >= float(self.length) - 1e-5
|
||||
if self.linear or self.radius <= 1e-6 or self.center is None:
|
||||
L = max(float(self.length), 1e-6)
|
||||
u = s / L
|
||||
xy = (1.0 - u) * self.p0 + u * self.p1
|
||||
vxy = self._dir * float(sdot)
|
||||
return xy, vxy, done
|
||||
frac = 0.0 if abs(self.sweep) < 1e-9 else s / (self.radius * abs(self.sweep))
|
||||
frac = min(1.0, max(0.0, frac))
|
||||
ang = self.ang0 + self.sweep * frac
|
||||
xy = self.center + self.radius * np.array(
|
||||
[math.cos(ang), math.sin(ang)], dtype=float
|
||||
)
|
||||
tang = np.array([-math.sin(ang), math.cos(ang)], dtype=float)
|
||||
if self.sweep < 0.0:
|
||||
tang = -tang
|
||||
return xy, tang * float(sdot), done
|
||||
|
||||
|
||||
def build_arc_xy_move(
|
||||
p0,
|
||||
p1,
|
||||
*,
|
||||
v: float,
|
||||
a: float,
|
||||
now: float,
|
||||
ws_min=None,
|
||||
ws_max=None,
|
||||
sag_frac: float = D435_ARC_SAGITTA_FRAC,
|
||||
sag_max: float = D435_ARC_SAGITTA_MAX_M,
|
||||
) -> ArcXyMove:
|
||||
"""p0→p1 的圆弧。工作盒装不下就换侧，再不行走直线。速度整段同一档。"""
|
||||
p0 = np.asarray(p0, dtype=float).reshape(2)
|
||||
p1 = np.asarray(p1, dtype=float).reshape(2)
|
||||
chord = p1 - p0
|
||||
L = float(np.hypot(chord[0], chord[1]))
|
||||
direction = (
|
||||
chord / L if L > 1e-6 else np.array([1.0, 0.0], dtype=float)
|
||||
)
|
||||
cruise = max(float(v), 1e-4)
|
||||
acc = max(float(a), 1e-3)
|
||||
|
||||
def _linear() -> ArcXyMove:
|
||||
T, v_peak, t_acc, t_cruise, a_use = trap_params(max(L, 1e-6), cruise, acc)
|
||||
return ArcXyMove(
|
||||
p0=p0.copy(),
|
||||
p1=p1.copy(),
|
||||
length=max(L, 1e-6),
|
||||
v_cruise=cruise,
|
||||
T=T,
|
||||
v_peak=v_peak,
|
||||
t_acc=t_acc,
|
||||
t_cruise=t_cruise,
|
||||
a=a_use,
|
||||
t0=float(now),
|
||||
linear=True,
|
||||
_dir=direction,
|
||||
)
|
||||
|
||||
if L < 0.003:
|
||||
return _linear()
|
||||
|
||||
n = np.array([-direction[1], direction[0]], dtype=float)
|
||||
h = min(float(sag_max), float(sag_frac) * L)
|
||||
mid = 0.5 * (p0 + p1)
|
||||
lo = None if ws_min is None else np.asarray(ws_min, dtype=float).reshape(-1)[:2]
|
||||
hi = None if ws_max is None else np.asarray(ws_max, dtype=float).reshape(-1)[:2]
|
||||
|
||||
def _try(sign: float) -> Optional[ArcXyMove]:
|
||||
apex = mid + n * (h * sign)
|
||||
cir = _circle_3(p0, apex, p1)
|
||||
if cir is None:
|
||||
return None
|
||||
center, r = cir
|
||||
a0 = math.atan2(p0[1] - center[1], p0[0] - center[0])
|
||||
a1 = math.atan2(p1[1] - center[1], p1[0] - center[0])
|
||||
a_via = math.atan2(apex[1] - center[1], apex[0] - center[0])
|
||||
ccw_end = _ccw(a0, a1)
|
||||
ccw_via = _ccw(a0, a_via)
|
||||
if 1e-6 < ccw_via < ccw_end - 1e-6:
|
||||
sweep = ccw_end
|
||||
else:
|
||||
sweep = ccw_end - 2.0 * math.pi
|
||||
if abs(sweep) < 1e-6:
|
||||
return None
|
||||
length = r * abs(sweep)
|
||||
if lo is not None and hi is not None:
|
||||
for k in range(9):
|
||||
frac = k / 8.0
|
||||
ang = a0 + sweep * frac
|
||||
xy = center + r * np.array([math.cos(ang), math.sin(ang)])
|
||||
if not _xy_in_box(xy, lo, hi):
|
||||
return None
|
||||
T, v_peak, t_acc, t_cruise, a_use = trap_params(length, cruise, acc)
|
||||
return ArcXyMove(
|
||||
p0=p0.copy(),
|
||||
p1=p1.copy(),
|
||||
length=length,
|
||||
v_cruise=cruise,
|
||||
T=T,
|
||||
v_peak=v_peak,
|
||||
t_acc=t_acc,
|
||||
t_cruise=t_cruise,
|
||||
a=a_use,
|
||||
t0=float(now),
|
||||
linear=False,
|
||||
center=center,
|
||||
radius=r,
|
||||
ang0=a0,
|
||||
sweep=sweep,
|
||||
)
|
||||
|
||||
for sign in (1.0, -1.0):
|
||||
got = _try(sign)
|
||||
if got is not None:
|
||||
return got
|
||||
return _linear()
|
||||
|
||||
|
||||
def path_interp(waypoints, u: float):
|
||||
"""沿关节路点折线插值。u∈[0,1] → (q, dq/du)。"""
|
||||
nseg = len(waypoints) - 1
|
||||
if nseg <= 0:
|
||||
z = np.zeros_like(waypoints[0])
|
||||
return waypoints[0].copy(), z
|
||||
u = float(min(1.0, max(0.0, u)))
|
||||
x = u * nseg
|
||||
i = min(nseg - 1, int(math.floor(x)))
|
||||
a = x - i
|
||||
q = (1.0 - a) * waypoints[i] + a * waypoints[i + 1]
|
||||
dq_du = (waypoints[i + 1] - waypoints[i]) * nseg
|
||||
return q, dq_du
|
||||
|
||||
|
||||
def translate_hold_ori_waypoints(
|
||||
q0,
|
||||
J,
|
||||
dp,
|
||||
*,
|
||||
damping: float = 0.08,
|
||||
max_dq: float = 0.06,
|
||||
R=None,
|
||||
verticalize: bool = False,
|
||||
):
|
||||
"""冻结当前雅可比，把 ``dp`` 拆成关节路点。
|
||||
|
||||
和二阶段 ``_plan_and_send`` 同一份逆解。``verticalize`` 时顺带收法兰朝下，
|
||||
不许另抄一份 ``_w_down``。返回 ``(路点, 还剩多少米)``。
|
||||
"""
|
||||
from arm_application.geom_ik import dls_translate_hold_ori, dls_twist
|
||||
|
||||
q = np.asarray(q0, dtype=float).copy()
|
||||
remain = np.asarray(dp, dtype=float).reshape(3).copy()
|
||||
dist = float(np.linalg.norm(remain))
|
||||
wps = [q.copy()]
|
||||
inner_tol = min(0.00012, max(0.00005, 0.05 * max(dist, 1e-6)))
|
||||
n_max = int(math.ceil(dist / 0.008)) + 16
|
||||
Jj = np.asarray(J, dtype=float)
|
||||
w_step = np.zeros(3, dtype=float)
|
||||
ang = 0.0
|
||||
if verticalize:
|
||||
if R is None:
|
||||
raise ValueError("verticalize 必须传入当前法兰 R，不许假装已经朝下")
|
||||
w_step, ang = flange_down_w(R)
|
||||
for _ in range(n_max):
|
||||
if float(np.linalg.norm(remain)) < inner_tol and (
|
||||
(not verticalize) or ang < 0.006
|
||||
):
|
||||
break
|
||||
nseg = max(1, int(math.ceil(float(np.linalg.norm(remain)) / 0.012)))
|
||||
step = remain / nseg
|
||||
if verticalize and ang > 0.008:
|
||||
qdot = dls_twist(Jj, step / 0.12, w_base=w_step, damping=damping, max_qdot=0.45)
|
||||
dq = np.clip(qdot * 0.12, -float(max_dq), float(max_dq))
|
||||
else:
|
||||
dq = dls_translate_hold_ori(Jj, step, damping=damping, max_dq=max_dq)
|
||||
if float(np.max(np.abs(dq))) < 1e-5:
|
||||
break
|
||||
q = q + dq
|
||||
wps.append(q.copy())
|
||||
remain = remain - (Jj[:3, :] @ dq)
|
||||
ang = max(0.0, ang - float(np.linalg.norm(w_step)) * 0.12)
|
||||
return wps, float(np.linalg.norm(remain))
|
||||
|
||||
|
||||
def trap_joint_schedule(
|
||||
waypoints,
|
||||
dist: float,
|
||||
v: float,
|
||||
a: float,
|
||||
*,
|
||||
t0: float = 0.06,
|
||||
dt: float = 0.04,
|
||||
t_hold: float = 0.25,
|
||||
):
|
||||
"""路点 + 梯形速度 → ``[(q, qdot, t), ...]``。头尾速度为 0，时间严格递增。
|
||||
|
||||
一次下发整段。中途再发下一条会把这条掐掉，臂就重新加速 —— 那就是抖。
|
||||
"""
|
||||
dist = max(float(dist), 1e-6)
|
||||
T_move, v_peak, t_acc, t_cruise, a_use = trap_params(dist, v, a)
|
||||
n_samp = max(8, int(math.ceil(T_move / max(float(dt), 1e-3))))
|
||||
zeros = np.zeros_like(waypoints[0])
|
||||
out = []
|
||||
last_t = -1.0
|
||||
for k in range(n_samp + 1):
|
||||
t_local = T_move * k / n_samp
|
||||
s, vs = trap_sv(t_local, dist, v_peak, t_acc, t_cruise, a_use)
|
||||
u = s / dist
|
||||
qi, dq_du = path_interp(waypoints, u)
|
||||
qdot = dq_du * (vs / dist) if k < n_samp else zeros
|
||||
if k >= n_samp:
|
||||
qi = waypoints[-1]
|
||||
qdot = zeros
|
||||
t = float(t0) + t_local
|
||||
if t <= last_t:
|
||||
t = last_t + 0.02
|
||||
out.append((np.asarray(qi, dtype=float).copy(), np.asarray(qdot, dtype=float).copy(), t))
|
||||
last_t = t
|
||||
hold = max(0.12, float(t_hold))
|
||||
for frac in (0.40, 0.70, 1.00):
|
||||
t = float(t0) + T_move + hold * frac
|
||||
if t < last_t + 0.05:
|
||||
t = last_t + 0.05
|
||||
out.append((np.asarray(waypoints[-1], dtype=float).copy(), zeros.copy(), t))
|
||||
last_t = t
|
||||
return out, T_move, v_peak
|
||||
|
||||
|
||||
def smooth_joint_schedule(
|
||||
waypoints,
|
||||
dist: float,
|
||||
v: float,
|
||||
*,
|
||||
t0: float = 0.06,
|
||||
dt: float = 0.04,
|
||||
t_hold: float = 0.25,
|
||||
d_acc: float = D435_FULL_ACC_M,
|
||||
d_dec: float = D435_FULL_DEC_M,
|
||||
):
|
||||
"""路点 + 余弦加减速 → ``[(q, qdot, t), ...]``。头尾速度为 0。
|
||||
|
||||
D435 整段一次下发用这一份。二阶段短跳仍走 ``trap_joint_schedule``。
|
||||
"""
|
||||
dist = max(float(dist), 1e-6)
|
||||
T_move, v_peak, t_acc, t_cruise, acc_m, dec_m = smooth_path_params(
|
||||
dist, v, d_acc=d_acc, d_dec=d_dec
|
||||
)
|
||||
n_samp = max(12, int(math.ceil(T_move / max(float(dt), 1e-3))))
|
||||
zeros = np.zeros_like(waypoints[0])
|
||||
out = []
|
||||
last_t = -1.0
|
||||
for k in range(n_samp + 1):
|
||||
t_local = T_move * k / n_samp
|
||||
s, vs = smooth_path_sv(
|
||||
t_local, dist, v_peak, t_acc, t_cruise, acc_m, dec_m
|
||||
)
|
||||
u = min(1.0, s / dist)
|
||||
qi, dq_du = path_interp(waypoints, u)
|
||||
qdot = dq_du * (vs / dist) if k < n_samp else zeros
|
||||
if k >= n_samp:
|
||||
qi = waypoints[-1]
|
||||
qdot = zeros
|
||||
t = float(t0) + t_local
|
||||
if t <= last_t:
|
||||
t = last_t + 0.02
|
||||
out.append(
|
||||
(
|
||||
np.asarray(qi, dtype=float).copy(),
|
||||
np.asarray(qdot, dtype=float).copy(),
|
||||
t,
|
||||
)
|
||||
)
|
||||
last_t = t
|
||||
hold = max(0.12, float(t_hold))
|
||||
for frac in (0.40, 0.70, 1.00):
|
||||
t = float(t0) + T_move + hold * frac
|
||||
if t < last_t + 0.05:
|
||||
t = last_t + 0.05
|
||||
out.append((np.asarray(waypoints[-1], dtype=float).copy(), zeros.copy(), t))
|
||||
last_t = t
|
||||
return out, T_move, v_peak
|
||||
+1674
File diff suppressed because it is too large
Load Diff
+64
@@ -0,0 +1,64 @@
|
||||
#!/usr/bin/env python3
|
||||
"""PID 控制器，带积分限幅、微分作用于测量值（derivative-on-measurement）以及积分分离。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Optional
|
||||
|
||||
|
||||
class PidController:
|
||||
"""``u = Kp e + Ki ∫e + Kd (−dmeas/dt)``，然后再做饱和限幅。
|
||||
|
||||
积分分离：当 ``|error| > i_sep_thresh`` 时，积分被强制置 0 且不再累积
|
||||
（只用 P+D，以便快速追赶 / 减少超调）。
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
kp: float,
|
||||
ki: float,
|
||||
kd: float,
|
||||
*,
|
||||
i_limit: float = 0.05,
|
||||
out_limit: float = 0.20,
|
||||
i_sep_thresh: float = 0.005,
|
||||
) -> None:
|
||||
self.kp = float(kp)
|
||||
self.ki = float(ki)
|
||||
self.kd = float(kd)
|
||||
self.i_limit = abs(float(i_limit))
|
||||
self.out_limit = abs(float(out_limit))
|
||||
self.i_sep_thresh = abs(float(i_sep_thresh))
|
||||
self._i = 0.0
|
||||
self._meas_prev: Optional[float] = None
|
||||
|
||||
def reset(self) -> None:
|
||||
self._i = 0.0
|
||||
self._meas_prev = None
|
||||
|
||||
@property
|
||||
def integral(self) -> float:
|
||||
return float(self._i)
|
||||
|
||||
def update(self, error: float, meas: float, dt: float) -> float:
|
||||
dt = max(float(dt), 1e-4)
|
||||
err = float(error)
|
||||
x = float(meas)
|
||||
if self._meas_prev is None:
|
||||
dterm = 0.0
|
||||
else:
|
||||
dterm = -(x - self._meas_prev) / dt
|
||||
self._meas_prev = x
|
||||
|
||||
# 积分分离 —— 误差较大：清空 I，不再累积
|
||||
if self.i_sep_thresh > 0.0 and abs(err) > self.i_sep_thresh:
|
||||
self._i = 0.0
|
||||
else:
|
||||
self._i = max(-self.i_limit, min(self.i_limit, self._i + err * dt))
|
||||
|
||||
u = self.kp * err + self.ki * self._i + self.kd * dterm
|
||||
if u > self.out_limit:
|
||||
u = self.out_limit
|
||||
elif u < -self.out_limit:
|
||||
u = -self.out_limit
|
||||
return float(u)
|
||||
+466
@@ -0,0 +1,466 @@
|
||||
#!/usr/bin/env python3
|
||||
"""三阶段串联：视觉跟踪锁定 → 孔定位 → 搜孔并轻轻按压（默认不拧入）。
|
||||
|
||||
流畅性（相对 bash 杀进程再起节点）：
|
||||
- 相机 / 触觉 / 手眼 TF 全程只起一次
|
||||
- 一阶段锁定后 SIGTERM 跟踪节点，立刻 pin 当前关节，不回零、不抬臂
|
||||
- 二阶段已垂直 / 已在悬停高则跳过对应动作；对准完立刻下压搜孔
|
||||
- 找到孔位后力闭环轻压保持；拧入是后续阶段（--depth 才插入）
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import os
|
||||
import signal
|
||||
import subprocess
|
||||
import time
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from geometry_msgs.msg import PointStamped
|
||||
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
|
||||
from std_msgs.msg import Bool, String
|
||||
|
||||
from arm_application.stage3_search_node import Stage3Search
|
||||
|
||||
|
||||
#: 悬停时规尖离面不够，跟踪高再抬的量 [m]。
|
||||
#:
|
||||
#: 只在「悬停高上规尖离工件面 < :data:`TRACK_LOCK_GAP_M`」时加上。仿真悬停按
|
||||
#: 工具几何只留约 2mm，不抬就会在跟踪时刮到工件。真机 89 悬停 0.320 时规尖
|
||||
#: 已经在面上方约 75mm，再抬 50mm 会让对孔前先改一次高度（0.37→0.32）。
|
||||
#:
|
||||
#: ⚠️ 抽成常量是因为它不只被流程用：``scripts/check_sim_scene.py`` 要用它核对
|
||||
#: "跟踪高有没有超出工作盒上界"（仿真里悬停 0.632 + 0.05 = 0.682 > 真机那套
|
||||
#: 0.65 上界，就是被这条查出来的）。两处各写一个字面量迟早漂移。
|
||||
TRACK_CLEARANCE_M = 0.05
|
||||
#: 悬停时规尖离面已经不少于这个值 [m]，一阶段就锁在悬停高，对孔完成后再下降。
|
||||
TRACK_LOCK_GAP_M = 0.040
|
||||
|
||||
|
||||
def init_z_refusal(
|
||||
z_m: float,
|
||||
tool_len_m: float,
|
||||
surface_z_m: float,
|
||||
gap_m: float,
|
||||
z_min: float,
|
||||
z_max: float,
|
||||
) -> str:
|
||||
"""初始法兰高度能不能用。空串表示可以；非空就是拒绝原因，不许悄悄抬高。"""
|
||||
z = float(z_m)
|
||||
if z < float(z_min) - 1e-6 or z > float(z_max) + 1e-6:
|
||||
return (
|
||||
f"初始法兰 Z {z:.3f}m 超出工作盒 [{float(z_min):.3f}, {float(z_max):.3f}]，"
|
||||
"拒绝开跑"
|
||||
)
|
||||
tool = float(tool_len_m)
|
||||
surf = float(surface_z_m)
|
||||
if tool <= 0.0 or surf <= 0.0:
|
||||
return "写了初始高度，但工具长度或工件面未知，无法确认尖端在面之上，拒绝开跑"
|
||||
clear = z - tool - surf
|
||||
if clear < float(gap_m) - 0.001:
|
||||
return (
|
||||
f"初始法兰 Z {z:.3f}m 会让工具尖进入工件："
|
||||
f"尖端 {z - tool:.3f}，工件面 {surf:.3f}，"
|
||||
f"间隙 {clear * 1000:.1f}mm，要求 ≥ {float(gap_m) * 1000:.1f}mm。"
|
||||
"拒绝开跑，不会悄悄抬高。"
|
||||
)
|
||||
return ""
|
||||
|
||||
|
||||
def track_lock_z(hover_z: float, tool_len_m: float, surface_z_m: float) -> float:
|
||||
"""一阶段锁高。
|
||||
|
||||
悬停时规尖离面已经 ≥ ``TRACK_LOCK_GAP_M``：锁在悬停高，跟踪和对孔都不改 Z，
|
||||
对孔完成后再下降。离面不够（仿真悬停只留 2mm）：再抬 ``TRACK_CLEARANCE_M``，
|
||||
否则跟踪会刮到工件。工具长或工件面缺失时抬升 —— 缺数时锁在悬停高等于没检查。
|
||||
"""
|
||||
tool = float(tool_len_m)
|
||||
surf = float(surface_z_m)
|
||||
hover = float(hover_z)
|
||||
if tool > 0.0 and surf > 0.0 and (hover - tool - surf) >= TRACK_LOCK_GAP_M:
|
||||
return hover
|
||||
return hover + TRACK_CLEARANCE_M
|
||||
|
||||
|
||||
def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) -> str:
|
||||
"""一阶段超时了，到底是"感知没给点"还是"给了点但没收敛"。
|
||||
|
||||
这两种情况的排查方向完全相反，而现象长得一模一样（都是"没锁定"）：
|
||||
|
||||
* **一个点都没收到** → 感知侧的事：yolo 后端多半缺 ultralytics/torch、
|
||||
truth 后端真值节点没起来、external 后端没人发。跟标定、跟工件在不在
|
||||
视野里**都没关系**。
|
||||
* **收到了点但没收敛** → 控制/几何侧的事：工件不在视野、手眼不对、
|
||||
目标超出工作空间、增益太小。
|
||||
|
||||
以前两种情况共用一句"没锁定（视野里没工件？）"，于是"perception_node
|
||||
因为缺依赖早退了"被读成"算法不收敛"，白查半天。
|
||||
"""
|
||||
if points_seen <= 0:
|
||||
return (
|
||||
f"一阶段 {timeout_s:.0f}s 没锁定：**一个视觉目标点都没收到**"
|
||||
"（visual_servo/filtered_point 上 0 条）—— 这是感知没起来，"
|
||||
"不是标定/视野问题。按 perception.source 对号入座：\n"
|
||||
" yolo → 多半缺推理依赖（ultralytics/torch），"
|
||||
f"看感知进程退出前几行：{log_path}\n"
|
||||
" truth → 仿真真值节点没起来，或场景/物体名读不出来"
|
||||
"（它退出前会打印原因）\n"
|
||||
" external → 外部节点没按这个话题/这个类型发"
|
||||
)
|
||||
return (
|
||||
f"一阶段 {timeout_s:.0f}s 没锁定（收到 {points_seen} 个目标点但没收敛）："
|
||||
f"工件不在视野 / 手眼不对 / 目标超出工作空间。看 {log_path}"
|
||||
)
|
||||
|
||||
|
||||
class AssemblyPipeline(Stage3Search):
|
||||
def __init__(self, node_name: str = "assembly_pipeline") -> None:
|
||||
# node_name 可覆盖：通止规装配（gauge_assembly_node）继承本类以复用
|
||||
# 一阶段跟踪的拉起/拆分与二阶段定位链，但它是**另一条流程**，
|
||||
# 在图上必须是另一个节点名（否则 ros2 node list 里两个同名的节点，
|
||||
# 出问题时连"是哪条流程在发指令"都分不清）。
|
||||
super().__init__(node_name=node_name)
|
||||
self.declare_parameter("skip_stage1", False)
|
||||
self.declare_parameter("skip_stage2", False)
|
||||
self.declare_parameter("skip_stage3", False) # 默认搜孔+轻压；拧入靠 --depth
|
||||
self.declare_parameter("d435_move_only", False)
|
||||
self.declare_parameter("init_z_m", -1.0) # >0：启动后先到这个法兰高度
|
||||
self.declare_parameter("stage1_timeout_sec", 150.0)
|
||||
self.linger3 = 1.2 # 串联结束短保持；单独跑 stage3 仍用 site linger_sec
|
||||
|
||||
self._s1: Optional[subprocess.Popen] = None
|
||||
self._locked = False
|
||||
self.create_subscription(Bool, "/visual_servo/locked", self._on_lock, 10)
|
||||
# 诊断用：一阶段超时时靠它区分"感知没给点"和"给了点没收敛"。
|
||||
# 只计数，不参与控制 —— 它绝不能影响流程本身的判断。
|
||||
#
|
||||
# ⚠️ QoS 必须与感知发布端**逐项一致**（BEST_EFFORT + KEEP_LAST(5)，
|
||||
# 见 perception_node / sim_truth_perception）。用默认的 RELIABLE 订阅
|
||||
# BEST_EFFORT 的话题：不报错、话题列表里也在，但**一条都收不到** ——
|
||||
# 于是超时归因会一口咬定"感知没起来"，把人往完全错误的方向带
|
||||
#（真实原因可能只是这里 QoS 不匹配）。这类"诊断自己坏了"最难查，
|
||||
# 所以宁可多写这几行。
|
||||
self.declare_parameter("diag_point_topic", "visual_servo/filtered_point")
|
||||
self._diag_point_topic = str(self.get_parameter("diag_point_topic").value)
|
||||
self._point_msgs = 0
|
||||
self.create_subscription(
|
||||
PointStamped,
|
||||
self._diag_point_topic,
|
||||
self._on_point_seen,
|
||||
QoSProfile(
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=5,
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
),
|
||||
)
|
||||
self._pipe_pub = self.create_publisher(String, "/pipeline/state", 10)
|
||||
self._pipe_result = self.create_publisher(String, "/pipeline/result", 10)
|
||||
|
||||
skip1 = bool(self.get_parameter("skip_stage1").value)
|
||||
skip2 = bool(self.get_parameter("skip_stage2").value)
|
||||
skip3 = bool(self.get_parameter("skip_stage3").value)
|
||||
self.d435_move_only = bool(self.get_parameter("d435_move_only").value)
|
||||
self.get_logger().info(
|
||||
f"pipeline arm_ee={self.ee_frame} hole={self.target_hole} "
|
||||
f"skip1={skip1} skip2={skip2} skip3={skip3} "
|
||||
f"d435_only={self.d435_move_only} insert={self.insert_depth_mm:.1f}mm"
|
||||
)
|
||||
|
||||
def _state(self, s: str) -> None:
|
||||
self.get_logger().info(f"[pipeline] === {s} ===")
|
||||
msg = String()
|
||||
msg.data = s
|
||||
self._state_pub.publish(msg)
|
||||
self._pipe_pub.publish(msg)
|
||||
|
||||
def _on_lock(self, msg: Bool) -> None:
|
||||
if bool(msg.data):
|
||||
self._locked = True
|
||||
|
||||
def _on_point_seen(self, _msg: PointStamped) -> None:
|
||||
"""只记"收到过目标点"，用于超时后的归因（见 stage1_timeout_reason）。"""
|
||||
self._point_msgs += 1
|
||||
|
||||
def _kill_old_stage1(self) -> None:
|
||||
for pat in (
|
||||
"visual_servo.launch",
|
||||
"lib/arm_application/servo_node",
|
||||
"lib/arm_application/perception_node",
|
||||
):
|
||||
subprocess.call(["pkill", "-f", pat], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
|
||||
time.sleep(0.25)
|
||||
|
||||
def _stop_stage1(self) -> None:
|
||||
proc = self._s1
|
||||
self._s1 = None
|
||||
if proc is None:
|
||||
self._kill_old_stage1()
|
||||
return
|
||||
if proc.poll() is None:
|
||||
try:
|
||||
os.killpg(proc.pid, signal.SIGTERM)
|
||||
except Exception:
|
||||
proc.terminate()
|
||||
try:
|
||||
proc.wait(timeout=4.0)
|
||||
except Exception:
|
||||
try:
|
||||
os.killpg(proc.pid, signal.SIGKILL)
|
||||
except Exception:
|
||||
proc.kill()
|
||||
self._kill_old_stage1()
|
||||
|
||||
def _track_z(self) -> float:
|
||||
"""一阶段锁高。写了 ``init_z_m`` 就用它，否则见 :func:`track_lock_z`。"""
|
||||
z = float(self.get_parameter("init_z_m").value)
|
||||
if z > 0.0:
|
||||
return z
|
||||
return track_lock_z(
|
||||
float(self.hover_z),
|
||||
float(getattr(self, "tool_tip_below_flange_m", 0.0) or 0.0),
|
||||
float(getattr(self, "surface_z_m", 0.0) or 0.0),
|
||||
)
|
||||
|
||||
def _ensure_clearance(self) -> bool:
|
||||
"""没写初始高度：只在低于锁高时抬上去。写了：升或降到那个法兰 Z，不安全就拒绝。"""
|
||||
user = float(self.get_parameter("init_z_m").value) > 0.0
|
||||
q, Tee = self._wait_state(4.0)
|
||||
if q is None or Tee is None:
|
||||
if user:
|
||||
self.get_logger().error("设了 --init-z，但拿不到当前位姿，拒绝开跑")
|
||||
return False
|
||||
return True
|
||||
tz = self._track_z()
|
||||
if user:
|
||||
why = init_z_refusal(
|
||||
tz,
|
||||
float(getattr(self, "tool_tip_below_flange_m", 0.0) or 0.0),
|
||||
float(getattr(self, "surface_z_m", 0.0) or 0.0),
|
||||
float(self.final_gap_m),
|
||||
float(self.ws_min[2]),
|
||||
float(self.ws_max[2]),
|
||||
)
|
||||
if why:
|
||||
self.get_logger().error(why)
|
||||
return False
|
||||
z = float(Tee[2, 3])
|
||||
if user:
|
||||
if abs(z - tz) <= 0.008:
|
||||
self.get_logger().info(f"已在初始高度 Z={z:.3f}m（目标 {tz:.3f}）")
|
||||
return True
|
||||
elif z >= tz - 0.008:
|
||||
return True
|
||||
self._state("pre_lift")
|
||||
self.get_logger().info(f"先到初始高度：Z {z:.3f} → {tz:.3f}m")
|
||||
ok, q, Tee = self._goto(
|
||||
q, Tee, np.array([float(Tee[0, 3]), float(Tee[1, 3]), tz]),
|
||||
verticalize=True, tol_m=0.003, tol_tilt=self.vert_tol_rad * 2.0,
|
||||
tag="pre_lift", v=self.move_speed, max_hops=8, hold_xy=True, settle_s=0.25,
|
||||
)
|
||||
q2 = self._q()
|
||||
self._pin(q2 if q2 is not None else q)
|
||||
z2 = float(Tee[2, 3])
|
||||
if user and (not ok or abs(z2 - tz) > 0.008):
|
||||
self.get_logger().error(f"初始高度没到位：Z={z2:.3f}，目标 {tz:.3f}")
|
||||
return False
|
||||
return True
|
||||
|
||||
def _run_stage1(self) -> int:
|
||||
"""拉起 perception+servo，等到 /visual_servo/locked 后冻结并拆跟踪。"""
|
||||
if not self._ensure_clearance():
|
||||
return 2
|
||||
self._state("stage1_track")
|
||||
self._kill_old_stage1()
|
||||
self._locked = False
|
||||
log_path = "/tmp/pipeline_stage1.log"
|
||||
logf = open(log_path, "w", encoding="utf-8")
|
||||
lock_z = self._track_z()
|
||||
# use_sim_time 必须**原样传下去**：本节点在仿真里是 true，子 launch 若是
|
||||
# false，perception/servo 就跑在系统时间上，TF 全是仿真时刻的戳 →
|
||||
# "看得见图、算得出点，就是查不到变换"，一阶段永远不收敛。
|
||||
use_sim = bool(self.get_parameter("use_sim_time").value)
|
||||
cmd = [
|
||||
"ros2", "launch", "arm_application", "visual_servo.launch.py",
|
||||
"enable_control:=true",
|
||||
"publish_handeye_tf:=false",
|
||||
f"ee_z_m:={lock_z:.3f}",
|
||||
f"use_sim_time:={str(use_sim).lower()}",
|
||||
f"d435_move_only:={str(self.d435_move_only).lower()}",
|
||||
]
|
||||
if self.d435_move_only:
|
||||
self.get_logger().info(
|
||||
f"启动 D435 位移测试（锁高 {lock_z:.3f}m，到位后不交 D405、不进二阶段）"
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
f"启动一阶段跟踪（锁高 {lock_z:.3f}m，锁定后自动交二阶段）"
|
||||
)
|
||||
self._s1 = subprocess.Popen(
|
||||
cmd, stdout=logf, stderr=subprocess.STDOUT, start_new_session=True,
|
||||
)
|
||||
timeout = float(self.get_parameter("stage1_timeout_sec").value)
|
||||
self._point_msgs = 0
|
||||
t0 = time.monotonic()
|
||||
while time.monotonic() - t0 < timeout:
|
||||
self._spin(0.1)
|
||||
if self._locked:
|
||||
break
|
||||
if self._s1.poll() is not None:
|
||||
self.get_logger().error(f"一阶段进程提前退出，看 {log_path}")
|
||||
return 1
|
||||
else:
|
||||
self._stop_stage1()
|
||||
self.get_logger().error(
|
||||
stage1_timeout_reason(self._point_msgs, timeout, log_path)
|
||||
)
|
||||
return 1
|
||||
if self.d435_move_only:
|
||||
self.get_logger().info(
|
||||
"D435 位移测试结束 — 停跟踪、钉住当前关节，跳过二阶段"
|
||||
)
|
||||
else:
|
||||
self.get_logger().info("一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段")
|
||||
self._stop_stage1()
|
||||
time.sleep(0.35)
|
||||
q = self._q()
|
||||
if q is not None:
|
||||
self._pin(q)
|
||||
return 0
|
||||
|
||||
def run(self) -> int:
|
||||
skip1 = bool(self.get_parameter("skip_stage1").value)
|
||||
skip2 = bool(self.get_parameter("skip_stage2").value)
|
||||
skip3 = bool(self.get_parameter("skip_stage3").value)
|
||||
if self.d435_move_only:
|
||||
skip2 = True
|
||||
skip3 = True
|
||||
if not self._cli.wait_for_server(timeout_sec=5.0):
|
||||
self.get_logger().error("JTC action 不在 —— 先开 T1: run_real.sh --arm 89")
|
||||
return 2
|
||||
|
||||
try:
|
||||
if not skip1:
|
||||
code = self._run_stage1()
|
||||
if code != 0:
|
||||
return code
|
||||
else:
|
||||
if not self._ensure_clearance():
|
||||
return 2
|
||||
self._state("skip_stage1")
|
||||
|
||||
q, Tee = self._wait_state(10.0)
|
||||
if q is None or Tee is None:
|
||||
self.get_logger().error("拿不到 joint_states/TF")
|
||||
return 2
|
||||
|
||||
final: dict = {}
|
||||
if not skip2:
|
||||
self._state("stage2_locate")
|
||||
code, q, Tee, final = self._run_locate(q, Tee)
|
||||
if code != 0:
|
||||
self.get_logger().error(f"二阶段失败 exit={code}，停在当前位姿")
|
||||
return code
|
||||
self.get_logger().info(
|
||||
f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm"
|
||||
+ (
|
||||
" — 跳过搜孔，停在观测高度（看规尖是否对中；进程退出后臂保持）"
|
||||
if skip3
|
||||
else " — 立刻下压搜孔并轻压（不拧入）"
|
||||
)
|
||||
)
|
||||
else:
|
||||
self._state("skip_stage2")
|
||||
|
||||
if skip3:
|
||||
self._state("skip_stage3")
|
||||
if self.d435_move_only:
|
||||
self.get_logger().info(
|
||||
"D435 位移测试结束 —— 臂停在当前高度，不交 D405、不对孔；"
|
||||
"15s 后本进程退出，臂保持不动"
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
"跳过搜孔 —— 臂停在当前高度。请看规尖与孔是否对中；"
|
||||
"15s 后本进程退出，臂保持不动"
|
||||
)
|
||||
if final:
|
||||
self._publish_result(final)
|
||||
self._linger_hold(final if final else {"ok": True}, seconds=15.0)
|
||||
payload = {"ok": True, "stage": "pipeline", "exit": 0, "skipped": "stage3"}
|
||||
try:
|
||||
payload["stage2"] = json.loads(
|
||||
open("/tmp/stage2_result.json", encoding="utf-8").read()
|
||||
)
|
||||
except Exception:
|
||||
pass
|
||||
msg = String()
|
||||
msg.data = json.dumps(payload, ensure_ascii=False)
|
||||
self._pipe_result.publish(msg)
|
||||
return 0
|
||||
|
||||
self._state("stage3_search")
|
||||
code = self._run_search(q, Tee, linger=False, stage2_final=final)
|
||||
payload = {
|
||||
"ok": code == 0,
|
||||
"stage": "pipeline",
|
||||
"exit": code,
|
||||
}
|
||||
try:
|
||||
payload["stage3"] = json.loads(
|
||||
open("/tmp/stage3_result.json", encoding="utf-8").read()
|
||||
)
|
||||
except Exception:
|
||||
pass
|
||||
msg = String()
|
||||
msg.data = json.dumps(payload, ensure_ascii=False)
|
||||
self._pipe_result.publish(msg)
|
||||
return code
|
||||
finally:
|
||||
self._stop_stage1()
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = AssemblyPipeline()
|
||||
code = 2
|
||||
try:
|
||||
code = node.run()
|
||||
except KeyboardInterrupt:
|
||||
node.get_logger().info("用户中断")
|
||||
code = 130
|
||||
except Exception as exc: # noqa: BLE001
|
||||
node.get_logger().error(f"pipeline 异常终止: {exc}")
|
||||
code = 2
|
||||
finally:
|
||||
try:
|
||||
node._stop_stage1()
|
||||
except Exception:
|
||||
pass
|
||||
if code != 0:
|
||||
try:
|
||||
q = node._q()
|
||||
if q is not None:
|
||||
node._pin(q)
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
node._shutdown_tf()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
node.destroy_node()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
except Exception:
|
||||
pass
|
||||
os._exit(code)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
+300
@@ -0,0 +1,300 @@
|
||||
#!/usr/bin/env python3
|
||||
"""在 YOLO 检测框中心处求立体平面的 Z。
|
||||
|
||||
悬停目标是「检测框中心射线」与「拟合出的近平面」的交点。这样既能留在检测结果上，
|
||||
又带有真实的米制 Z（30° 相机下，Z 错了会把 base 系的 XY 也带偏）。不使用点云均值
|
||||
质心 —— 孔 / 桌面的内点会把它从绿框上拉走。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass
|
||||
class PlaneFeature:
|
||||
uv: np.ndarray # (2,) 三维质心对应的彩色像素坐标
|
||||
z: float # 三维质心在相机坐标系下的 Z
|
||||
xyz: np.ndarray # (3,) 相机坐标系下的立体点云质心
|
||||
n_inliers: int
|
||||
note: str
|
||||
# 这个点的 Z 是不是**借**来的（该像素处读不到自己的深度，从检测框里借了一个）。
|
||||
# 为什么要显式带出来、而不是让下游去认 ``note`` 里的字：借深度意味着该像素的
|
||||
# 深度不可读 ⇒ 绝对位置不足以用来**重定位**（2026-10-08 真机，假 k0 带着借来的
|
||||
# 深度、z 看着很合理 0.27~0.39m，于是"物理量程"守卫也失效）。
|
||||
borrowed: bool = False
|
||||
|
||||
|
||||
def _depth_meters(buf: np.ndarray, encoding: str) -> Tuple[np.ndarray, float]:
|
||||
enc = (encoding or "").lower()
|
||||
raw = np.asarray(buf)
|
||||
if enc in ("16uc1", "mono16") or raw.dtype == np.uint16:
|
||||
pos = raw.reshape(-1).astype(np.float32)
|
||||
pos = pos[pos > 0]
|
||||
if pos.size >= 8:
|
||||
med = float(np.median(pos))
|
||||
scale = 0.001 if 20.0 <= med <= 8000.0 else (1.0 if med < 20.0 else 0.0001)
|
||||
else:
|
||||
scale = 0.001
|
||||
return raw.astype(np.float32) * scale, scale
|
||||
return raw.astype(np.float32), 1.0
|
||||
|
||||
|
||||
def _near_band_mask(
|
||||
meters: np.ndarray,
|
||||
valid: np.ndarray,
|
||||
min_depth: float,
|
||||
work_z_max: float,
|
||||
band_m: float,
|
||||
) -> np.ndarray:
|
||||
vals = meters[valid]
|
||||
if vals.size < 8:
|
||||
return valid
|
||||
vals = np.sort(vals)
|
||||
n_near = max(8, int(0.40 * vals.size))
|
||||
near = vals[:n_near]
|
||||
z_p10 = float(np.percentile(near, 10))
|
||||
hi = min(float(work_z_max), z_p10 + float(band_m))
|
||||
return valid & (meters <= hi) & (meters >= float(min_depth))
|
||||
|
||||
|
||||
def _ransac_plane(
|
||||
pts: np.ndarray,
|
||||
thresh: float,
|
||||
n_iter: int = 24,
|
||||
rng: Optional[np.random.Generator] = None,
|
||||
) -> Optional[Tuple[np.ndarray, np.ndarray]]:
|
||||
n = int(pts.shape[0])
|
||||
if n < 12:
|
||||
return None
|
||||
rng = rng or np.random.default_rng()
|
||||
best_inl: Optional[np.ndarray] = None
|
||||
best_count = 0
|
||||
for _ in range(n_iter):
|
||||
i = rng.choice(n, 3, replace=False)
|
||||
p0, p1, p2 = pts[i]
|
||||
nvec = np.cross(p1 - p0, p2 - p0)
|
||||
nn = float(np.linalg.norm(nvec))
|
||||
if nn < 1e-9:
|
||||
continue
|
||||
nvec = nvec / nn
|
||||
dist = np.abs((pts - p0) @ nvec)
|
||||
inl = dist < thresh
|
||||
c = int(inl.sum())
|
||||
if c > best_count:
|
||||
best_count = c
|
||||
best_inl = inl
|
||||
if best_inl is None or best_count < max(12, int(0.15 * n)):
|
||||
return None
|
||||
subset = pts[best_inl]
|
||||
p0 = subset.mean(axis=0)
|
||||
_, _, vh = np.linalg.svd(subset - p0, full_matrices=False)
|
||||
nvec = vh[-1]
|
||||
nvec = nvec / (float(np.linalg.norm(nvec)) + 1e-12)
|
||||
if float(nvec @ p0) > 0.0:
|
||||
nvec = -nvec
|
||||
return nvec, p0
|
||||
|
||||
|
||||
def _plane_z(
|
||||
nvec: np.ndarray,
|
||||
p0: np.ndarray,
|
||||
u: float,
|
||||
v: float,
|
||||
fx: float,
|
||||
fy: float,
|
||||
cx: float,
|
||||
cy: float,
|
||||
) -> Optional[float]:
|
||||
direc = np.array([(u - cx) / max(fx, 1.0), (v - cy) / max(fy, 1.0), 1.0], dtype=float)
|
||||
denom = float(nvec @ direc)
|
||||
if abs(denom) < 1e-8:
|
||||
return None
|
||||
z = float(nvec @ p0) / denom
|
||||
return z if np.isfinite(z) else None
|
||||
|
||||
|
||||
def workpiece_xyz_in_box(
|
||||
depth_buf: np.ndarray,
|
||||
encoding: str,
|
||||
xyxy,
|
||||
fx: float,
|
||||
fy: float,
|
||||
cx: float,
|
||||
cy: float,
|
||||
img_wh: Tuple[int, int],
|
||||
min_depth: float,
|
||||
max_depth: float,
|
||||
work_z_max: float,
|
||||
inner: float = 0.40,
|
||||
band_m: float = 0.06,
|
||||
) -> Optional[PlaneFeature]:
|
||||
"""工件上的实体点：内层检测框里近表面深度的三维中位数。
|
||||
|
||||
不是单目射线（孔会穿透到桌面），也不是整个 ROI 的点云均值
|
||||
（桌面 / 单侧深度会把它从工件上拉走）。
|
||||
"""
|
||||
meters, _ = _depth_meters(depth_buf, encoding)
|
||||
if meters.ndim != 2:
|
||||
return None
|
||||
img_w, img_h = int(img_wh[0]), int(img_wh[1])
|
||||
dh, dw = int(meters.shape[0]), int(meters.shape[1])
|
||||
x1, y1, x2, y2 = [float(v) for v in xyxy]
|
||||
u_c = 0.5 * (x1 + x2)
|
||||
v_c = 0.5 * (y1 + y2)
|
||||
if dw != img_w or dh != img_h:
|
||||
sx, sy = dw / max(float(img_w), 1.0), dh / max(float(img_h), 1.0)
|
||||
x1, y1, x2, y2 = x1 * sx, y1 * sy, x2 * sx, y2 * sy
|
||||
scale_u, scale_v = 1.0 / sx, 1.0 / sy
|
||||
else:
|
||||
scale_u = scale_v = 1.0
|
||||
bw = max(8.0, x2 - x1)
|
||||
bh = max(8.0, y2 - y1)
|
||||
frac = 0.5 * float(np.clip(inner, 0.2, 0.8))
|
||||
ix0 = int(np.clip(min(x1, x2) + (0.5 - frac) * bw, 0, dw - 1))
|
||||
iy0 = int(np.clip(min(y1, y2) + (0.5 - frac) * bh, 0, dh - 1))
|
||||
ix1 = int(np.clip(max(x1, x2) - (0.5 - frac) * bw, ix0 + 2, dw))
|
||||
iy1 = int(np.clip(max(y1, y2) - (0.5 - frac) * bh, iy0 + 2, dh))
|
||||
roi = meters[iy0:iy1, ix0:ix1]
|
||||
finite = np.isfinite(roi) & (roi > float(min_depth)) & (roi < float(max_depth))
|
||||
if int(finite.sum()) < 12:
|
||||
return None
|
||||
near = _near_band_mask(roi, finite, min_depth, work_z_max, band_m)
|
||||
if int(near.sum()) < 8:
|
||||
near = finite
|
||||
yy, xx = np.nonzero(near)
|
||||
if yy.size < 8:
|
||||
return None
|
||||
zs = roi[yy, xx]
|
||||
us = (xx + ix0) * scale_u
|
||||
vs = (yy + iy0) * scale_v
|
||||
pts = np.stack(
|
||||
[
|
||||
(us - cx) * zs / max(fx, 1.0),
|
||||
(vs - cy) * zs / max(fy, 1.0),
|
||||
zs,
|
||||
],
|
||||
axis=1,
|
||||
)
|
||||
p_xyz = np.median(pts, axis=0)
|
||||
if not np.all(np.isfinite(p_xyz)):
|
||||
return None
|
||||
z = float(p_xyz[2])
|
||||
if not (min_depth <= z <= work_z_max):
|
||||
return None
|
||||
uv = np.array(
|
||||
[
|
||||
fx * float(p_xyz[0]) / max(z, 1e-6) + cx,
|
||||
fy * float(p_xyz[1]) / max(z, 1e-6) + cy,
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
return PlaneFeature(
|
||||
uv=uv,
|
||||
z=z,
|
||||
xyz=p_xyz.astype(float),
|
||||
n_inliers=int(yy.size),
|
||||
note=f"inner3d n={int(yy.size)} z={z:.3f}",
|
||||
)
|
||||
|
||||
|
||||
def geometric_center_on_plane(
|
||||
depth_buf: np.ndarray,
|
||||
encoding: str,
|
||||
xyxy,
|
||||
fx: float,
|
||||
fy: float,
|
||||
cx: float,
|
||||
cy: float,
|
||||
img_wh: Tuple[int, int],
|
||||
min_depth: float,
|
||||
max_depth: float,
|
||||
work_z_max: float,
|
||||
inlier_m: float = 0.008,
|
||||
band_m: float = 0.06,
|
||||
) -> Optional[PlaneFeature]:
|
||||
"""拟合出的近平面上、位于检测框中心的点，拿不到则返回 None。"""
|
||||
meters, _ = _depth_meters(depth_buf, encoding)
|
||||
if meters.ndim != 2:
|
||||
return None
|
||||
img_w, img_h = int(img_wh[0]), int(img_wh[1])
|
||||
dh, dw = int(meters.shape[0]), int(meters.shape[1])
|
||||
x1, y1, x2, y2 = [float(v) for v in xyxy]
|
||||
u_c = 0.5 * (x1 + x2)
|
||||
v_c = 0.5 * (y1 + y2)
|
||||
if dw != img_w or dh != img_h:
|
||||
sx, sy = dw / max(float(img_w), 1.0), dh / max(float(img_h), 1.0)
|
||||
x1, y1, x2, y2 = x1 * sx, y1 * sy, x2 * sx, y2 * sy
|
||||
# uv 保持用彩色像素坐标：先在深度像素下计算，再缩放回来。
|
||||
scale_u, scale_v = 1.0 / sx, 1.0 / sy
|
||||
else:
|
||||
scale_u = scale_v = 1.0
|
||||
|
||||
x0 = int(np.clip(min(x1, x2), 0, dw - 1))
|
||||
y0 = int(np.clip(min(y1, y2), 0, dh - 1))
|
||||
x1i = int(np.clip(max(x1, x2), x0 + 2, dw))
|
||||
y1i = int(np.clip(max(y1, y2), y0 + 2, dh))
|
||||
roi = meters[y0:y1i, x0:x1i]
|
||||
if roi.size < 16:
|
||||
return None
|
||||
|
||||
finite = np.isfinite(roi) & (roi > float(min_depth)) & (roi < float(max_depth))
|
||||
if int(finite.sum()) < 16:
|
||||
return None
|
||||
near = _near_band_mask(roi, finite, min_depth, work_z_max, band_m)
|
||||
if int(near.sum()) < 12:
|
||||
near = finite
|
||||
|
||||
yy, xx = np.nonzero(near)
|
||||
zs = roi[yy, xx]
|
||||
# 反投影用的彩色等效像素（对齐后的深度使用的是彩色内参 K）。
|
||||
us = (xx + x0) * scale_u
|
||||
vs = (yy + y0) * scale_v
|
||||
pts = np.stack(
|
||||
[
|
||||
(us - cx) * zs / max(fx, 1.0),
|
||||
(vs - cy) * zs / max(fy, 1.0),
|
||||
zs,
|
||||
],
|
||||
axis=1,
|
||||
)
|
||||
if pts.shape[0] > 800:
|
||||
rng = np.random.default_rng()
|
||||
sel = rng.choice(pts.shape[0], 800, replace=False)
|
||||
fit_pts = pts[sel]
|
||||
else:
|
||||
fit_pts = pts
|
||||
|
||||
fitted = _ransac_plane(fit_pts, thresh=float(inlier_m))
|
||||
if fitted is None:
|
||||
return None
|
||||
nvec, p0 = fitted
|
||||
dist = np.abs((pts - p0) @ nvec)
|
||||
n_inl = int((dist < float(inlier_m)).sum())
|
||||
if n_inl < 12:
|
||||
return None
|
||||
z = _plane_z(nvec, p0, u_c, v_c, fx, fy, cx, cy)
|
||||
if z is None or not (min_depth <= float(z) <= work_z_max):
|
||||
zs_inl = pts[dist < float(inlier_m), 2]
|
||||
z = float(np.median(zs_inl)) if zs_inl.size else None
|
||||
if z is None or not (min_depth <= z <= work_z_max):
|
||||
return None
|
||||
z = float(z)
|
||||
p_xyz = np.array(
|
||||
[
|
||||
(u_c - cx) * z / max(fx, 1.0),
|
||||
(v_c - cy) * z / max(fy, 1.0),
|
||||
z,
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
return PlaneFeature(
|
||||
uv=np.array([u_c, v_c], dtype=float),
|
||||
z=z,
|
||||
xyz=p_xyz,
|
||||
n_inliers=n_inl,
|
||||
note=f"box_on_plane n={n_inl} z={z:.3f}",
|
||||
)
|
||||
+479
@@ -0,0 +1,479 @@
|
||||
#!/usr/bin/env python3
|
||||
"""YOLO **pose** 模型的推理封装（自解码，不经过 ultralytics 的 pose 后处理）。
|
||||
|
||||
背景：为什么会多出这一个模块
|
||||
--------------------------------
|
||||
`gongjian0929` 这类权重是 ``task=pose``：它输出**一个工件框 + K 个关键点**。
|
||||
本项目 K=3，就是现场说的 k0 / k1 / k2（k0 跟踪、k1/k2 对孔）。
|
||||
|
||||
ultralytics 走不通，两条路都实测过（2026-09-29）：
|
||||
|
||||
1. ``YOLO(engine, task="detect")`` —— 按检测去解 14 通道输出，结果吐出 300 个
|
||||
垃圾框，``conf`` 直接是坐标值（600+），全被算成 ``workpiece`` 类。
|
||||
跟踪会锁到空气上 —— 真机上这比不跑更危险。
|
||||
2. ``YOLO(engine, task="pose")`` —— 直接崩：``negative dimension -983``。
|
||||
根因是 ``scripts/weights.py`` 用**裸 TensorRT API** 构建 engine，engine 不带
|
||||
metadata；ultralytics 拿不到 ``kpt_shape``，后处理就错位了。
|
||||
（TensorRT 10.13 的 Python API 确实没有任何 metadata 接口 —— 见
|
||||
``models/README.md`` 第 3 节。）
|
||||
|
||||
所以这里自己解码。输出布局是 YOLO pose 的固定约定::
|
||||
|
||||
output0 : (1, 4 + nc + 3*K, A) # A = anchor 数（640 输入时是 8400）
|
||||
ch 0..3 : cx, cy, w, h # 输入尺度下的像素
|
||||
ch 4.. : 每类置信度（nc 个）
|
||||
之后 : K 组 (x, y, conf)
|
||||
|
||||
关键点是**同一个工件框的属性**，不是独立目标 —— 所以它们不参与 NMS 的竞争，
|
||||
只跟着自己那个框一起被选出/淘汰。这点和"每个孔一个类"的检测模型不一样。
|
||||
|
||||
后端
|
||||
----
|
||||
- ``.engine``：TensorRT 10 的 ``set_tensor_address`` + ``execute_async_v3``。
|
||||
用 torch 张量当设备缓冲、torch 的当前流当执行流 —— 不需要 pycuda。
|
||||
实测 ~1.5 ms/帧（640 输入，FP16 engine）。
|
||||
- ``.onnx``：onnxruntime。**本环境没有 CUDA provider**（只有 CPU），
|
||||
所以 ONNX 只是兜底，真机请用 engine。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import ast
|
||||
import logging
|
||||
from pathlib import Path
|
||||
from typing import Any, List, Optional, Sequence, Tuple, Union
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
from .yolo_detector import Detection, resolve_class_labels
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
# ultralytics 的默认填充值。改了它并不会错 —— 只是输入分布和训练时不一致，
|
||||
# 精度会掉一点，所以别改。
|
||||
LETTERBOX_FILL = 114
|
||||
|
||||
# 关键点语义（现场约定）：k0 是跟踪孔，k1/k2 是对孔。
|
||||
# 默认值写在这里，site.yaml 可以覆盖（见 arm_application/pose_detector 的调用方）。
|
||||
TRACK_KPT_INDEX = 0
|
||||
HOLE_KPT_INDICES: Tuple[int, int] = (1, 2)
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 模型信息：engine 没有 metadata，就从旁边同名的 .onnx 读
|
||||
# --------------------------------------------------------------------------- #
|
||||
def _parse_python_literal(raw: Any, default: Any = None) -> Any:
|
||||
"""ONNX metadata 里存的是 python repr（单引号、嵌套 list/dict），要 literal_eval。"""
|
||||
if raw is None:
|
||||
return default
|
||||
if isinstance(raw, (dict, list, tuple, int)):
|
||||
return raw
|
||||
try:
|
||||
return ast.literal_eval(str(raw))
|
||||
except (ValueError, SyntaxError):
|
||||
return default
|
||||
|
||||
|
||||
def read_pose_info(weights: Union[str, Path]) -> Optional[dict]:
|
||||
"""读同名 ``.onnx`` 的 metadata，判断这是不是一个 pose 模型。
|
||||
|
||||
engine 本身读不到任何东西（TensorRT 10 的 Python API 没有 metadata 接口），
|
||||
但 ``.onnx`` 一定在它旁边（``resolve_weights`` 保证同目录同主干），
|
||||
所以统一以 ONNX 为准。
|
||||
|
||||
返回 ``None`` = 不是 pose 模型（调用方应回落到普通检测器）。
|
||||
返回 dict 时至少含 ``kpt_count`` / ``nc`` / ``names``。
|
||||
"""
|
||||
p = Path(weights).expanduser()
|
||||
onnx_path = p.with_suffix(".onnx")
|
||||
if not onnx_path.is_file():
|
||||
return None
|
||||
try:
|
||||
import onnx
|
||||
except Exception: # noqa: BLE001
|
||||
logger.warning("没装 onnx，无法判断 %s 是不是 pose 模型", p.name)
|
||||
return None
|
||||
try:
|
||||
meta = {m.key: m.value for m in onnx.load(str(onnx_path)).metadata_props}
|
||||
except Exception as exc: # noqa: BLE001
|
||||
logger.warning("读 %s 的 metadata 失败：%s", onnx_path.name, exc)
|
||||
return None
|
||||
|
||||
task = str(meta.get("task", "")).strip().lower()
|
||||
kpt_shape = _parse_python_literal(meta.get("kpt_shape"))
|
||||
if task != "pose" and not kpt_shape:
|
||||
return None
|
||||
|
||||
kpt_count = 0
|
||||
if isinstance(kpt_shape, (list, tuple)) and len(kpt_shape) >= 1:
|
||||
try:
|
||||
kpt_count = int(kpt_shape[0])
|
||||
except (TypeError, ValueError):
|
||||
kpt_count = 0
|
||||
if kpt_count <= 0:
|
||||
# metadata 缺 kpt_shape 也不能猜 —— 猜错就是"静默错位"，比报错难查得多。
|
||||
logger.warning("%s 标记为 pose 但读不到 kpt_shape，按非 pose 处理", onnx_path.name)
|
||||
return None
|
||||
|
||||
names = _parse_python_literal(meta.get("names"), default={}) or {}
|
||||
kpt_names = _parse_python_literal(meta.get("kpt_names"), default={}) or {}
|
||||
|
||||
nc = 0
|
||||
if isinstance(names, dict) and names:
|
||||
nc = max(int(k) for k in names) + 1 if all(
|
||||
str(k).lstrip("-").isdigit() for k in names
|
||||
) else len(names)
|
||||
if nc <= 0 and isinstance(names, (list, tuple)):
|
||||
nc = len(names)
|
||||
|
||||
return {
|
||||
"task": "pose",
|
||||
"kpt_count": kpt_count,
|
||||
"kpt_names": kpt_names,
|
||||
"nc": nc,
|
||||
"names": names,
|
||||
"model": str(meta.get("description") or p.name),
|
||||
}
|
||||
|
||||
|
||||
def is_pose_model(weights: Union[str, Path]) -> bool:
|
||||
return read_pose_info(weights) is not None
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 预处理 / 后处理（纯 numpy，可离线单测）
|
||||
# --------------------------------------------------------------------------- #
|
||||
def letterbox(
|
||||
img: np.ndarray,
|
||||
new_shape: int = 640,
|
||||
color: Tuple[int, int, int] = (LETTERBOX_FILL,) * 3,
|
||||
) -> Tuple[np.ndarray, float, Tuple[float, float]]:
|
||||
"""等比缩放 + 居中填充到 ``new_shape``（和 ultralytics 推理时一致）。
|
||||
|
||||
返回 ``(padded, gain, (dw, dh))``：``dw/dh`` 是**填充量**，还原坐标时减掉它。
|
||||
"""
|
||||
h, w = img.shape[:2]
|
||||
gain = min(new_shape / h, new_shape / w)
|
||||
new_unpad = (max(1, int(round(w * gain))), max(1, int(round(h * gain))))
|
||||
if (w, h) != new_unpad:
|
||||
img = cv2.resize(img, new_unpad, interpolation=cv2.INTER_LINEAR)
|
||||
dw = (new_shape - new_unpad[0]) / 2.0
|
||||
dh = (new_shape - new_unpad[1]) / 2.0
|
||||
top, bottom = int(round(dh - 0.1)), int(round(dh + 0.1))
|
||||
left, right = int(round(dw - 0.1)), int(round(dw + 0.1))
|
||||
if top or bottom or left or right:
|
||||
img = cv2.copyMakeBorder(
|
||||
img, top, bottom, left, right, cv2.BORDER_CONSTANT, value=color
|
||||
)
|
||||
return img, float(gain), (float(dw), float(dh))
|
||||
|
||||
|
||||
def nms_xyxy(
|
||||
boxes: np.ndarray,
|
||||
scores: np.ndarray,
|
||||
iou_thr: float,
|
||||
max_det: int = 20,
|
||||
) -> np.ndarray:
|
||||
"""纯 numpy 的 NMS，返回保留下来的下标（按分数从高到低）。"""
|
||||
if boxes.size == 0:
|
||||
return np.zeros((0,), dtype=int)
|
||||
x1, y1, x2, y2 = boxes[:, 0], boxes[:, 1], boxes[:, 2], boxes[:, 3]
|
||||
areas = np.maximum(0.0, x2 - x1) * np.maximum(0.0, y2 - y1)
|
||||
order = scores.argsort()[::-1]
|
||||
keep: List[int] = []
|
||||
while order.size > 0 and len(keep) < int(max_det):
|
||||
i = int(order[0])
|
||||
keep.append(i)
|
||||
if order.size == 1:
|
||||
break
|
||||
rest = order[1:]
|
||||
ix1 = np.maximum(x1[i], x1[rest])
|
||||
iy1 = np.maximum(y1[i], y1[rest])
|
||||
ix2 = np.minimum(x2[i], x2[rest])
|
||||
iy2 = np.minimum(y2[i], y2[rest])
|
||||
inter = np.maximum(0.0, ix2 - ix1) * np.maximum(0.0, iy2 - iy1)
|
||||
union = areas[i] + areas[rest] - inter
|
||||
iou = np.where(union > 1e-9, inter / np.maximum(union, 1e-9), 0.0)
|
||||
order = rest[iou <= float(iou_thr)]
|
||||
return np.asarray(keep, dtype=int)
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 后端：TensorRT engine / ONNX Runtime
|
||||
# --------------------------------------------------------------------------- #
|
||||
class _TrtRunner:
|
||||
"""TensorRT 10 执行器：torch 张量当设备缓冲，torch 当前流当执行流。
|
||||
|
||||
不用 pycuda —— 环境里只有 torch，而 ``set_tensor_address`` +
|
||||
``execute_async_v3`` 正好够用。
|
||||
"""
|
||||
|
||||
def __init__(self, engine_path: Path, device: str = "0") -> None:
|
||||
import tensorrt as trt
|
||||
import torch
|
||||
|
||||
self._torch = torch
|
||||
idx = 0
|
||||
dev = str(device or "").strip().lower()
|
||||
if dev and dev not in ("auto", "cuda", "gpu"):
|
||||
tail = dev.split(":")[-1]
|
||||
if tail.isdigit():
|
||||
idx = int(tail)
|
||||
if not torch.cuda.is_available():
|
||||
raise RuntimeError("backend=tensorrt 但 torch 看不到 CUDA")
|
||||
self._device = torch.device(f"cuda:{idx}")
|
||||
|
||||
trt_logger = trt.Logger(trt.Logger.ERROR)
|
||||
with open(engine_path, "rb") as f:
|
||||
engine = trt.Runtime(trt_logger).deserialize_cuda_engine(f.read())
|
||||
if engine is None:
|
||||
raise RuntimeError(
|
||||
f"engine 反序列化失败：{engine_path}。"
|
||||
"engine 和 TensorRT 版本绑死，重装环境后要重新导出"
|
||||
"（bash scripts/weights.sh doctor --check-engines 可查）"
|
||||
)
|
||||
self._engine = engine
|
||||
self._ctx = engine.create_execution_context()
|
||||
|
||||
names = [engine.get_tensor_name(i) for i in range(engine.num_io_tensors)]
|
||||
ins = [n for n in names if engine.get_tensor_mode(n) == trt.TensorIOMode.INPUT]
|
||||
outs = [n for n in names if engine.get_tensor_mode(n) == trt.TensorIOMode.OUTPUT]
|
||||
if len(ins) != 1 or len(outs) != 1:
|
||||
raise RuntimeError(f"pose engine 期望 1 进 1 出，实际 in={ins} out={outs}")
|
||||
self.in_name, self.out_name = ins[0], outs[0]
|
||||
|
||||
with torch.cuda.device(self._device):
|
||||
self._x = torch.zeros(
|
||||
tuple(engine.get_tensor_shape(self.in_name)),
|
||||
dtype=torch.float32,
|
||||
device=self._device,
|
||||
)
|
||||
self._y = torch.zeros(
|
||||
tuple(engine.get_tensor_shape(self.out_name)),
|
||||
dtype=torch.float32,
|
||||
device=self._device,
|
||||
)
|
||||
self._ctx.set_tensor_address(self.in_name, self._x.data_ptr())
|
||||
self._ctx.set_tensor_address(self.out_name, self._y.data_ptr())
|
||||
self.out_shape = tuple(engine.get_tensor_shape(self.out_name))
|
||||
|
||||
def run(self, chw: np.ndarray) -> np.ndarray:
|
||||
torch = self._torch
|
||||
with torch.cuda.device(self._device):
|
||||
self._x.copy_(torch.from_numpy(np.ascontiguousarray(chw)).to(self._device))
|
||||
self._ctx.execute_async_v3(torch.cuda.current_stream().cuda_stream)
|
||||
torch.cuda.current_stream().synchronize()
|
||||
return self._y.cpu().numpy()
|
||||
|
||||
|
||||
class _OnnxRunner:
|
||||
"""onnxruntime 兜底。本环境只有 CPU provider —— 真机请用 .engine。"""
|
||||
|
||||
def __init__(self, onnx_path: Path) -> None:
|
||||
import onnxruntime as ort
|
||||
|
||||
providers = ort.get_available_providers()
|
||||
if "CUDAExecutionProvider" in providers:
|
||||
chosen = ["CUDAExecutionProvider", "CPUExecutionProvider"]
|
||||
else:
|
||||
logger.warning(
|
||||
"onnxruntime 没有 CUDA provider，%s 会跑在 CPU 上（慢）。"
|
||||
"真机请用同名的 .engine",
|
||||
onnx_path.name,
|
||||
)
|
||||
chosen = ["CPUExecutionProvider"]
|
||||
self._sess = ort.InferenceSession(str(onnx_path), providers=chosen)
|
||||
self.in_name = self._sess.get_inputs()[0].name
|
||||
self.out_shape = tuple(self._sess.get_outputs()[0].shape)
|
||||
|
||||
def run(self, chw: np.ndarray) -> np.ndarray:
|
||||
return self._sess.run(None, {self.in_name: chw[None].astype(np.float32)})[0]
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 检测器
|
||||
# --------------------------------------------------------------------------- #
|
||||
class PoseDetector:
|
||||
"""把 pose 模型包装成和 ``YoloDetector`` 一样的 ``detect()/infer()`` 接口。
|
||||
|
||||
多出来的一件事：每个 ``Detection`` 带上 ``kpts``（``(K, 3)`` 的
|
||||
``x, y, conf``，原图像素坐标）。
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
weights: Union[str, Path],
|
||||
*,
|
||||
conf: float = 0.25,
|
||||
iou: float = 0.45,
|
||||
imgsz: int = 640,
|
||||
device: str = "0",
|
||||
class_ids: Optional[Sequence[int]] = None,
|
||||
class_names: Optional[Sequence[str]] = None,
|
||||
max_det: int = 20,
|
||||
) -> None:
|
||||
path = Path(weights).expanduser()
|
||||
if not path.is_file():
|
||||
raise FileNotFoundError(f"找不到 pose 权重：{path}")
|
||||
|
||||
info = read_pose_info(path) or {}
|
||||
self.kpt_count = int(info.get("kpt_count") or 0)
|
||||
if self.kpt_count <= 0:
|
||||
raise RuntimeError(f"{path.name} 不是可识别的 pose 模型（缺 kpt_shape）")
|
||||
self.weights_path = str(path)
|
||||
self.conf = float(conf)
|
||||
self.iou = float(iou)
|
||||
self.imgsz = int(imgsz)
|
||||
self.max_det = int(max_det)
|
||||
|
||||
if path.suffix.lower() in {".engine", ".trt"}:
|
||||
self._runner = _TrtRunner(path, device)
|
||||
self.backend = "tensorrt"
|
||||
else:
|
||||
self._runner = _OnnxRunner(path)
|
||||
self.backend = "onnx"
|
||||
|
||||
# 类别标签仍然走统一入口（旁挂 .labels.yaml 优先）。
|
||||
# engine 没有 metadata，所以把 ONNX 里的 names 当"模型内嵌"传进去。
|
||||
names = info.get("names") or {}
|
||||
|
||||
class _Meta: # 只为 resolve_class_labels 能读到 ONNX 内嵌标签
|
||||
pass
|
||||
|
||||
meta = _Meta()
|
||||
meta.names = names
|
||||
meta.nc = int(info.get("nc") or 0)
|
||||
self._labels = resolve_class_labels(path, meta)
|
||||
|
||||
self.class_ids = set(int(v) for v in class_ids) if class_ids else None
|
||||
self.class_names = (
|
||||
{str(n).strip().lower() for n in class_names if str(n).strip()}
|
||||
if class_names
|
||||
else None
|
||||
)
|
||||
self._warmed = False
|
||||
|
||||
# -- 对外 ---------------------------------------------------------------
|
||||
def warmup(self, *, half: bool = False) -> None:
|
||||
del half # engine 已是 FP16；ONNX 这边没有 half 的开关
|
||||
if self._warmed:
|
||||
return
|
||||
# 先置位：infer() 开头会检查 _warmed，不置位就会反过来再调 warmup。
|
||||
self._warmed = True
|
||||
try:
|
||||
self.infer(np.zeros((self.imgsz, self.imgsz, 3), dtype=np.uint8))
|
||||
except Exception:
|
||||
self._warmed = False
|
||||
raise
|
||||
|
||||
def detect(
|
||||
self,
|
||||
image_bgr: np.ndarray,
|
||||
conf: float | None = None,
|
||||
iou: float | None = None,
|
||||
imgsz: int | None = None,
|
||||
**_kwargs,
|
||||
) -> List[Detection]:
|
||||
"""``detect_holes`` 用的别名接口（xyxy + conf + kpts）。"""
|
||||
if conf is not None:
|
||||
self.conf = float(conf)
|
||||
if iou is not None:
|
||||
self.iou = float(iou)
|
||||
# imgsz 在这里**不能**改：engine 的输入尺寸是编译期定死的。
|
||||
if imgsz is not None and int(imgsz) != self.imgsz:
|
||||
logger.warning(
|
||||
"pose engine 输入固定 %d，忽略调用方的 imgsz=%s", self.imgsz, imgsz
|
||||
)
|
||||
return self.infer(image_bgr)
|
||||
|
||||
def infer(self, image_bgr: np.ndarray, *, half: bool = False, seq: int = 0) -> List[Detection]:
|
||||
del half, seq
|
||||
if image_bgr is None or getattr(image_bgr, "size", 0) == 0:
|
||||
return []
|
||||
if not self._warmed:
|
||||
self.warmup()
|
||||
padded, gain, (dw, dh) = letterbox(image_bgr, self.imgsz)
|
||||
chw = np.ascontiguousarray(
|
||||
padded[:, :, ::-1].transpose(2, 0, 1), dtype=np.float32
|
||||
) / 255.0
|
||||
out = self._runner.run(chw)
|
||||
return self._decode(out, gain, dw, dh)
|
||||
|
||||
# -- 解码 ---------------------------------------------------------------
|
||||
def _decode(
|
||||
self, raw: np.ndarray, gain: float, dw: float, dh: float
|
||||
) -> List[Detection]:
|
||||
o = np.asarray(raw, dtype=np.float32)[0] # (C, A)
|
||||
ch, n_anchor = o.shape
|
||||
k = self.kpt_count
|
||||
nc = int(ch - 4 - 3 * k)
|
||||
if nc < 1:
|
||||
raise RuntimeError(
|
||||
f"pose 输出通道数不对：C={ch}，按 kpt={k} 反推 nc={nc}。"
|
||||
"引擎/onnx 与 kpt_shape 对不上"
|
||||
)
|
||||
|
||||
scores = o[4 : 4 + nc] # (nc, A)
|
||||
cls_ids = scores.argmax(axis=0) # (A,)
|
||||
confs = scores[cls_ids, np.arange(n_anchor)]
|
||||
|
||||
keep = confs >= self.conf
|
||||
if not np.any(keep):
|
||||
return []
|
||||
xywh = o[0:4, keep].T # (N, 4)
|
||||
confs = confs[keep]
|
||||
cls_ids = cls_ids[keep]
|
||||
kpts = o[4 + nc :, keep].reshape(k, 3, -1).transpose(2, 0, 1) # (N, k, 3)
|
||||
|
||||
xyxy = np.empty_like(xywh)
|
||||
xyxy[:, 0] = xywh[:, 0] - 0.5 * xywh[:, 2]
|
||||
xyxy[:, 1] = xywh[:, 1] - 0.5 * xywh[:, 3]
|
||||
xyxy[:, 2] = xywh[:, 0] + 0.5 * xywh[:, 2]
|
||||
xyxy[:, 3] = xywh[:, 1] + 0.5 * xywh[:, 3]
|
||||
|
||||
keep_i = nms_xyxy(xyxy, confs, self.iou, self.max_det)
|
||||
if keep_i.size == 0:
|
||||
return []
|
||||
xyxy, confs, cls_ids, kpts = xyxy[keep_i], confs[keep_i], cls_ids[keep_i], kpts[keep_i]
|
||||
|
||||
# 去 letterbox 的填充，再还原到原图尺度
|
||||
xyxy = xyxy.astype(float)
|
||||
xyxy[:, [0, 2]] = (xyxy[:, [0, 2]] - dw) / gain
|
||||
xyxy[:, [1, 3]] = (xyxy[:, [1, 3]] - dh) / gain
|
||||
kpts = kpts.astype(float)
|
||||
kpts[:, :, 0] = (kpts[:, :, 0] - dw) / gain
|
||||
kpts[:, :, 1] = (kpts[:, :, 1] - dh) / gain
|
||||
|
||||
out: List[Detection] = []
|
||||
for i in range(len(xyxy)):
|
||||
cid = int(cls_ids[i])
|
||||
name = self._labels.get(cid)
|
||||
if self.class_ids is not None and cid not in self.class_ids:
|
||||
continue
|
||||
if self.class_names is not None and name.lower() not in self.class_names:
|
||||
continue
|
||||
out.append(
|
||||
Detection(
|
||||
xyxy=xyxy[i],
|
||||
conf=float(confs[i]),
|
||||
cls_id=cid,
|
||||
cls_name=name,
|
||||
kpts=kpts[i].copy(),
|
||||
)
|
||||
)
|
||||
out.sort(key=lambda d: d.conf, reverse=True)
|
||||
return out
|
||||
|
||||
def close(self) -> None:
|
||||
return None
|
||||
|
||||
|
||||
def keypoint_roles(kpt_count: int) -> Tuple[int, Tuple[int, ...]]:
|
||||
"""返回 ``(跟踪点下标, 孔下标元组)``，供调用方统一口径。
|
||||
|
||||
现场约定：k0 跟踪，k1/k2 对孔。模型关键点少于 3 个时只返回存在的部分。
|
||||
"""
|
||||
track = TRACK_KPT_INDEX if kpt_count > TRACK_KPT_INDEX else -1
|
||||
holes = tuple(i for i in HOLE_KPT_INDICES if i < kpt_count)
|
||||
return track, holes
|
||||
+919
@@ -0,0 +1,919 @@
|
||||
#!/usr/bin/env python3
|
||||
"""把 RealSense 彩色流发布到 ROS 话题（不依赖 realsense2_camera 的 apt 包）。
|
||||
|
||||
当你没装 ``ros-humble-realsense2-camera``、但 ``pyrealsense2`` 能正常工作时用它。
|
||||
|
||||
默认话题（camera_name=d405）：
|
||||
/d405/color/image_raw
|
||||
/d405/color/camera_info
|
||||
/d405/aligned_depth_to_color/image_raw (16UC1 mm，需开启 enable_depth)
|
||||
|
||||
性能说明
|
||||
--------
|
||||
* 在采集线程里直接发布（没有定时器带来的延迟）。
|
||||
* 使用 SensorDataQoS（BEST_EFFORT，depth=1），这样慢的订阅者会主动丢旧帧。
|
||||
* 可选的 SDK 预览：``-p show_preview:=true``（沿用 Intel 示例的链路：pipeline → numpy → 窗口）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import array
|
||||
import os
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rcl_interfaces.msg import ParameterDescriptor
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import CameraInfo, Image
|
||||
from std_msgs.msg import Float32MultiArray
|
||||
|
||||
from arm_application.ros_node import TolerantNode, param_as_str
|
||||
|
||||
# RealSense 掉到 480Mbps（或整台消失）时的排查口径。写在这里是为了"只有一处"：
|
||||
# 相机节点的报错、preflight 脚本、文档都引用同一套说法，不会各说各的。
|
||||
# 为什么 480Mbps 值得专门解释一次（2026-09-28 现场）：
|
||||
# D435 原本协商到 USB 3.2、1280x720@30 跑得好好的，中途
|
||||
# `xioctl(UVCIOC_CTRL_QUERY) ... No such file or directory`（= 设备从总线上消失）
|
||||
# 之后重新枚举成了 **USB 2.1**。USB3 线缆/座子里 SuperSpeed 那一对针脚一旦
|
||||
# 接触不良，USB2 的针脚还能用 —— 于是设备不会"掉线"，只是**悄悄掉速**，
|
||||
# 表现成"相机在，但帧率/分辨率不对"。这时查软件是查不出来的。
|
||||
USB_RESEAT_HINT = (
|
||||
'拔插一次、换一根 USB3 线，或换到机箱后面板直连的蓝色口 —— '
|
||||
'只协商到 480Mbps 通常是 SuperSpeed 针脚没接上，不是相机坏了。'
|
||||
'看当前协商速率：lsusb -t（设备后面写 480M 还是 5000M）。'
|
||||
)
|
||||
|
||||
|
||||
def _fill_camera_info(
|
||||
msg: CameraInfo, width: int, height: int, fx, fy, cx, cy, frame_id: str, dist
|
||||
) -> None:
|
||||
msg.width = width
|
||||
msg.height = height
|
||||
msg.distortion_model = 'plumb_bob'
|
||||
msg.d = [float(v) for v in list(dist)[:5]]
|
||||
while len(msg.d) < 5:
|
||||
msg.d.append(0.0)
|
||||
msg.k = [
|
||||
float(fx), 0.0, float(cx),
|
||||
0.0, float(fy), float(cy),
|
||||
0.0, 0.0, 1.0,
|
||||
]
|
||||
msg.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
|
||||
msg.p = [
|
||||
float(fx), 0.0, float(cx), 0.0,
|
||||
0.0, float(fy), float(cy), 0.0,
|
||||
0.0, 0.0, 1.0, 0.0,
|
||||
]
|
||||
msg.header.frame_id = frame_id
|
||||
|
||||
|
||||
def _site_yaml_path() -> Path:
|
||||
from arm_application.site_io import site_path
|
||||
|
||||
return site_path()
|
||||
|
||||
|
||||
class RealsenseColorPub(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('realsense_color_pub')
|
||||
self.declare_parameter('camera_name', 'd405') # 话题命名空间
|
||||
self._name = str(self.get_parameter('camera_name').value).strip().strip('/')
|
||||
self._serial = param_as_str(self, 'camera_serial', '')
|
||||
self.declare_parameter('width', 1280)
|
||||
self.declare_parameter('height', 720)
|
||||
self.declare_parameter('fps', 15)
|
||||
self.declare_parameter('show_preview', False)
|
||||
self.declare_parameter('overlay_detection', True)
|
||||
self.declare_parameter('overlay_hold_sec', 1.5)
|
||||
self.declare_parameter('enable_depth', True)
|
||||
self.declare_parameter('enable_infra', True)
|
||||
self.declare_parameter('preview_stereo', True)
|
||||
# Qt 预览每 N 帧才渲染一次 —— 发布始终保持满帧率；
|
||||
# CPU 上色 + 四窗格绘制绝不能拖慢数据流。
|
||||
self.declare_parameter('preview_decimation', 2)
|
||||
self.declare_parameter('auto_exposure', True)
|
||||
self.declare_parameter('ae_limit_us', 3000.0)
|
||||
self.declare_parameter('emitter_enabled', False)
|
||||
self.declare_parameter('laser_power', 0.0)
|
||||
self.declare_parameter('exposure_us', 0.0)
|
||||
self.declare_parameter('gain', 0.0)
|
||||
|
||||
self._width = int(self.get_parameter('width').value)
|
||||
self._height = int(self.get_parameter('height').value)
|
||||
self._fps = int(self.get_parameter('fps').value)
|
||||
self._show_preview = bool(self.get_parameter('show_preview').value)
|
||||
self._overlay_detection = bool(self.get_parameter('overlay_detection').value)
|
||||
self._overlay_hold_sec = float(self.get_parameter('overlay_hold_sec').value)
|
||||
self._enable_depth_param = bool(self.get_parameter('enable_depth').value)
|
||||
self._enable_infra_param = bool(self.get_parameter('enable_infra').value)
|
||||
self._preview_stereo = bool(self.get_parameter('preview_stereo').value)
|
||||
self._preview_every = max(
|
||||
1, int(self.get_parameter('preview_decimation').value)
|
||||
)
|
||||
self._auto_exposure = bool(self.get_parameter('auto_exposure').value)
|
||||
self._ae_limit_us = float(self.get_parameter('ae_limit_us').value)
|
||||
self._emitter_enabled = bool(self.get_parameter('emitter_enabled').value)
|
||||
self._laser_power = float(self.get_parameter('laser_power').value)
|
||||
self._exposure_us = float(self.get_parameter('exposure_us').value)
|
||||
self._gain = float(self.get_parameter('gain').value)
|
||||
# FPS 表：硬件帧号对比实际发布帧数（循环速率）
|
||||
self._fps_t0 = 0.0
|
||||
self._fps_pub_color = 0
|
||||
self._fps_pub_depth = 0
|
||||
self._fps_fn_color = -1
|
||||
self._fps_fn_depth = -1
|
||||
self._t_wait = 0.0
|
||||
self._t_align = 0.0
|
||||
self._t_pub = 0.0
|
||||
|
||||
qos = qos_profile_sensor_data # BEST_EFFORT，只保留最后 1 帧
|
||||
base = f'/{self._name}/color'
|
||||
self._img_pub = self.create_publisher(
|
||||
Image, f'{base}/image_raw', qos
|
||||
)
|
||||
self._info_pub = self.create_publisher(
|
||||
CameraInfo, f'{base}/camera_info', qos
|
||||
)
|
||||
self._depth_pub = self.create_publisher(
|
||||
Image, f'/{self._name}/aligned_depth_to_color/image_raw', qos
|
||||
)
|
||||
# IR 流也会一起发布，这样预览可以放在独立的查看器进程里
|
||||
# （Qt 不能和采集循环共享 GIL）。
|
||||
self._ir1_pub = None
|
||||
self._ir2_pub = None
|
||||
if self._enable_infra_param and self._preview_stereo:
|
||||
self._ir1_pub = self.create_publisher(
|
||||
Image, f'/{self._name}/infra1/image_rect_raw', qos
|
||||
)
|
||||
self._ir2_pub = self.create_publisher(
|
||||
Image, f'/{self._name}/infra2/image_rect_raw', qos
|
||||
)
|
||||
|
||||
self._pipe = None
|
||||
self._profile = None
|
||||
self._align = None
|
||||
self._have_depth = False
|
||||
self._have_infra = False
|
||||
self._infra_mode = 'none'
|
||||
self._colorizer = None
|
||||
self._logged_depth = False
|
||||
# 设备深度尺度 → 用原始 uint16 乘上它才是真实的 mm。
|
||||
# D405 = 0.1 mm/单位，D435 = 1.0 mm/单位。启动时从传感器读取。
|
||||
self._depth_to_mm = 1.0
|
||||
self._running = True
|
||||
self._thread: Optional[threading.Thread] = None
|
||||
self._fx = self._fy = self._cx = self._cy = 0.0
|
||||
self._dist = [0.0] * 5
|
||||
self._frame_id = f'{self._name}_color_optical_frame'
|
||||
self._info_every = 1
|
||||
self._frame_i = 0
|
||||
self._preview = None
|
||||
self._overlay: Optional[list] = None
|
||||
self._overlay_stamp = 0.0
|
||||
self._capture_started = False
|
||||
self._usb_desc = ''
|
||||
self._last_ok = None
|
||||
self._site_yaml = _site_yaml_path()
|
||||
self._site_mtime = 0.0
|
||||
self._site_wh_warned = False
|
||||
if self._show_preview:
|
||||
from arm_application.realsense_sdk_view import SdkPreviewWindow, ensure_app
|
||||
|
||||
self._qt_app = ensure_app()
|
||||
self._preview = SdkPreviewWindow(
|
||||
title=f'RealSense {self._name} COLOR|DEPTH|IR-L|IR-R (pyrealsense2 SDK)'
|
||||
)
|
||||
self._preview.show()
|
||||
if self._overlay_detection:
|
||||
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
|
||||
|
||||
det_qos = QoSProfile(
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=5,
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
)
|
||||
self.create_subscription(
|
||||
Float32MultiArray,
|
||||
'workpiece/detection',
|
||||
self._on_det,
|
||||
det_qos,
|
||||
)
|
||||
self.get_logger().info(
|
||||
'SDK 预览已开启 —— 本进程内显示 COLOR|DEPTH|IR-L|IR-R '
|
||||
f'（保留 {self._overlay_hold_sec:.1f}s）。关窗口 ≠ 停相机。'
|
||||
)
|
||||
self._start_pipeline()
|
||||
|
||||
def _pick_device(self, devices, rs):
|
||||
"""优先用配置的序列号，其次按名字选 D405（这台机器上还接了 D435）。"""
|
||||
hint = (self._name or 'd405').lower()
|
||||
if self._serial:
|
||||
for d in devices:
|
||||
if d.get_info(rs.camera_info.serial_number) == self._serial:
|
||||
return d
|
||||
avail = ', '.join(
|
||||
f"{d.get_info(rs.camera_info.name)}:{d.get_info(rs.camera_info.serial_number)}"
|
||||
for d in devices
|
||||
)
|
||||
raise RuntimeError(
|
||||
f'找不到序列号 {self._serial!r}。当前已连接：{avail}'
|
||||
)
|
||||
for d in devices:
|
||||
name = str(d.get_info(rs.camera_info.name) or '').lower()
|
||||
if hint in name or (hint == 'd405' and '405' in name):
|
||||
return d
|
||||
return devices[0]
|
||||
|
||||
def _reload_site_preview(self, stream_w: int = 0, stream_h: int = 0) -> None:
|
||||
"""不重启就能读到新的 overlay_hold_sec（宽高变了则给出警告）。"""
|
||||
path = self._site_yaml
|
||||
if not path.is_file():
|
||||
return
|
||||
try:
|
||||
mtime = path.stat().st_mtime
|
||||
except OSError:
|
||||
return
|
||||
if mtime <= self._site_mtime:
|
||||
return
|
||||
self._site_mtime = mtime
|
||||
try:
|
||||
import yaml
|
||||
|
||||
data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
cam = data.get("camera") or {}
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(f"site.yaml 重新加载失败：{exc}")
|
||||
return
|
||||
if "overlay_hold_sec" in cam:
|
||||
hold = max(0.0, float(cam.get("overlay_hold_sec") or 0.0))
|
||||
if abs(hold - self._overlay_hold_sec) > 1e-6:
|
||||
self.get_logger().info(
|
||||
f"overlay_hold_sec {self._overlay_hold_sec:.2f}s → {hold:.2f}s (site.yaml)"
|
||||
)
|
||||
self._overlay_hold_sec = hold
|
||||
want_w = int(cam.get("width") or self._width)
|
||||
want_h = int(cam.get("height") or self._height)
|
||||
live_w = stream_w or self._width
|
||||
live_h = stream_h or self._height
|
||||
if (want_w, want_h) != (live_w, live_h) and not self._site_wh_warned:
|
||||
self._site_wh_warned = True
|
||||
self.get_logger().warn(
|
||||
f"预览流是 {live_w}x{live_h}，而 site.yaml 里的 camera 是 "
|
||||
f"{want_w}x{want_h} —— 请重启：bash ~/ros2/scripts/run_camera_d405.sh --preview"
|
||||
)
|
||||
elif (want_w, want_h) == (live_w, live_h):
|
||||
self._site_wh_warned = False
|
||||
|
||||
def _on_det(self, msg: Float32MultiArray) -> None:
|
||||
# 空负载 = 感知漏检 → 立刻清掉绿框
|
||||
if not msg.data or len(msg.data) < 8:
|
||||
self._overlay = None
|
||||
self._overlay_stamp = 0.0
|
||||
if self._preview is not None:
|
||||
self._preview.set_overlay(None)
|
||||
return
|
||||
self._overlay = list(msg.data)
|
||||
self._overlay_stamp = time.monotonic()
|
||||
if self._preview is not None:
|
||||
self._preview.set_overlay(self._overlay)
|
||||
|
||||
def _publish_frame(self, bgr: np.ndarray, depth_u16: Optional[np.ndarray] = None) -> None:
|
||||
h, w = bgr.shape[0], bgr.shape[1]
|
||||
stamp = self.get_clock().now().to_msg()
|
||||
|
||||
img = Image()
|
||||
img.header.stamp = stamp
|
||||
img.header.frame_id = self._frame_id
|
||||
img.height = h
|
||||
img.width = w
|
||||
img.encoding = 'bgr8'
|
||||
img.is_bigendian = 0
|
||||
img.step = w * 3
|
||||
# array.array('B', ...) 会走 setter 的快路径 —— 直接传 bytes 会触发
|
||||
# 逐元素的 debug 检查，在这里每帧要多花约 40 ms。
|
||||
img.data = array.array('B', np.ascontiguousarray(bgr).tobytes())
|
||||
self._img_pub.publish(img)
|
||||
|
||||
info = CameraInfo()
|
||||
info.header.stamp = stamp
|
||||
info.header.frame_id = self._frame_id
|
||||
_fill_camera_info(
|
||||
info, w, h, self._fx, self._fy, self._cx, self._cy,
|
||||
self._frame_id, self._dist,
|
||||
)
|
||||
self._info_pub.publish(info)
|
||||
|
||||
if depth_u16 is not None:
|
||||
dh, dw = int(depth_u16.shape[0]), int(depth_u16.shape[1])
|
||||
dmsg = Image()
|
||||
dmsg.header.stamp = stamp
|
||||
dmsg.header.frame_id = self._frame_id
|
||||
dmsg.height = dh
|
||||
dmsg.width = dw
|
||||
dmsg.encoding = '16UC1'
|
||||
dmsg.is_bigendian = 0
|
||||
dmsg.step = dw * 2
|
||||
dmsg.data = array.array('B', np.ascontiguousarray(depth_u16).tobytes())
|
||||
self._depth_pub.publish(dmsg)
|
||||
if not self._logged_depth:
|
||||
self._logged_depth = True
|
||||
self.get_logger().info(
|
||||
f'首个对齐深度 {dw}x{dh} → '
|
||||
f'/{self._name}/aligned_depth_to_color/image_raw'
|
||||
)
|
||||
|
||||
def _start_pipeline(self, reconnect: bool = False) -> None:
|
||||
try:
|
||||
import pyrealsense2 as rs
|
||||
except Exception as exc: # noqa: BLE001
|
||||
raise RuntimeError(
|
||||
'pyrealsense2 不可用。请执行 conda activate ros_flexiv'
|
||||
) from exc
|
||||
|
||||
# 杀掉上一个发布进程后，USB 需要 1–3s 才能重新枚举。
|
||||
ctx = rs.context()
|
||||
devices = []
|
||||
for attempt in range(8):
|
||||
devices = list(ctx.devices)
|
||||
if devices:
|
||||
break
|
||||
time.sleep(0.4)
|
||||
ctx = rs.context()
|
||||
if not devices:
|
||||
raise RuntimeError(
|
||||
'没有连接 RealSense（多次重试后 USB 仍为空）。'
|
||||
'拔插一次；确认没有 realsense-viewer / 其它进程占着这台相机；'
|
||||
+ USB_RESEAT_HINT
|
||||
)
|
||||
|
||||
found = self._pick_device(devices, rs)
|
||||
try:
|
||||
self._serial = str(found.get_info(rs.camera_info.serial_number))
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
usb_desc = ''
|
||||
try:
|
||||
usb_desc = str(found.get_info(rs.camera_info.usb_type_descriptor))
|
||||
except Exception: # noqa: BLE001
|
||||
usb_desc = '?'
|
||||
self._usb_desc = usb_desc
|
||||
usb2 = usb_desc.startswith('2')
|
||||
cam_name = ''
|
||||
try:
|
||||
cam_name = str(found.get_info(rs.camera_info.name))
|
||||
except Exception: # noqa: BLE001
|
||||
cam_name = self._name
|
||||
self.get_logger().info(
|
||||
f'{cam_name} serial={self._serial or "?"} USB={usb_desc} '
|
||||
f'requested {self._width}x{self._height}@{self._fps}Hz'
|
||||
)
|
||||
if usb2:
|
||||
# 只在 480Mbps 口上才降。实测（2026-09-28）640x480@15 彩色+深度
|
||||
# 可以稳定跑满 15fps；但 1280x720@30 在 USB2 上物理不可能
|
||||
# （SuperSpeed 没协商上）。
|
||||
self.get_logger().warn(
|
||||
f'{cam_name} 只协商到 USB {usb_desc}（480Mbps）—— 降到 '
|
||||
f'640x480@15Hz。' + USB_RESEAT_HINT
|
||||
)
|
||||
self._width, self._height, self._fps = 640, 480, 15
|
||||
|
||||
want = (self._width, self._height, self._fps)
|
||||
last = self._last_ok
|
||||
if reconnect:
|
||||
candidates = []
|
||||
if last:
|
||||
candidates.append(last)
|
||||
if want not in candidates:
|
||||
candidates.append(want)
|
||||
if (640, 480, 15) not in candidates:
|
||||
candidates.append((640, 480, 15))
|
||||
else:
|
||||
candidates = [
|
||||
want,
|
||||
(1280, 720, 15),
|
||||
(640, 480, 15),
|
||||
(640, 480, 5),
|
||||
]
|
||||
if usb2:
|
||||
candidates = [(640, 480, 15), (640, 480, 5), (424, 240, 15)]
|
||||
# 丢弃固件里的垃圾模式（848@90 之类），它们会 ioctl 失败并卡死 USB。
|
||||
seen = set()
|
||||
trimmed = []
|
||||
for item in candidates:
|
||||
if item in seen:
|
||||
continue
|
||||
w, h, fps = item
|
||||
if fps not in (5, 15, 30):
|
||||
continue
|
||||
if w * h > 1280 * 720:
|
||||
continue
|
||||
seen.add(item)
|
||||
trimmed.append(item)
|
||||
candidates = trimmed or [(640, 480, 15)]
|
||||
|
||||
pipe = rs.pipeline()
|
||||
profile = None
|
||||
last_err = None
|
||||
want_depth = bool(self._enable_depth_param)
|
||||
want_infra = (
|
||||
bool(self._enable_infra_param)
|
||||
and bool(self._preview_stereo)
|
||||
)
|
||||
have_depth = False
|
||||
have_infra = False
|
||||
|
||||
# USB3：先尝试真正的 IR-L+IR-R（共享立体自动曝光）。彩色+IR-L 在 D405 上
|
||||
# 经常失败 —— 那就退回 IR-R + 把彩色当左目（观感不同，且不共享自动曝光）。
|
||||
def _modes():
|
||||
if want_depth and want_infra and not usb2:
|
||||
yield True, 'both'
|
||||
if want_depth and want_infra:
|
||||
yield True, 'right'
|
||||
if want_depth:
|
||||
yield True, 'none'
|
||||
yield False, 'none'
|
||||
|
||||
for w, h, fps in candidates:
|
||||
for with_depth, infra_mode in _modes():
|
||||
try:
|
||||
trial = rs.config()
|
||||
if self._serial:
|
||||
trial.enable_device(self._serial)
|
||||
trial.enable_stream(rs.stream.color, w, h, rs.format.bgr8, fps)
|
||||
if with_depth:
|
||||
trial.enable_stream(
|
||||
rs.stream.depth, w, h, rs.format.z16, fps
|
||||
)
|
||||
if infra_mode in ('both', 'left'):
|
||||
trial.enable_stream(
|
||||
rs.stream.infrared, 1, w, h, rs.format.y8, fps
|
||||
)
|
||||
if infra_mode in ('both', 'right'):
|
||||
trial.enable_stream(
|
||||
rs.stream.infrared, 2, w, h, rs.format.y8, fps
|
||||
)
|
||||
profile = pipe.start(trial)
|
||||
have_depth = with_depth
|
||||
have_infra = infra_mode != 'none'
|
||||
self._infra_mode = infra_mode
|
||||
if (w, h, fps) != want:
|
||||
self.get_logger().warn(
|
||||
f'请求 {want[0]}x{want[1]}@{want[2]}Hz '
|
||||
f'→ 改用 {w}x{h}@{fps}Hz'
|
||||
)
|
||||
if want_depth and not with_depth:
|
||||
self.get_logger().warn(
|
||||
f'{w}x{h}@{fps}Hz 彩色+深度失败 —— 只用彩色'
|
||||
)
|
||||
elif want_infra and infra_mode == 'none':
|
||||
self.get_logger().warn(
|
||||
f'{w}x{h}@{fps}Hz 立体 IR 失败 —— 改用彩色+深度预览'
|
||||
)
|
||||
elif infra_mode == 'both':
|
||||
self.get_logger().info(
|
||||
f'{w}x{h}@{fps}Hz 立体 IR-L+IR-R（共享自动曝光）+ 深度'
|
||||
)
|
||||
elif infra_mode == 'right':
|
||||
self.get_logger().info(
|
||||
f'{w}x{h}@{fps}Hz IR-L=彩色灰度，IR-R=原始 IR '
|
||||
'（明暗不同 —— 这不是两路自动曝光）'
|
||||
)
|
||||
self._width, self._height, self._fps = w, h, fps
|
||||
self._last_ok = (w, h, fps)
|
||||
break
|
||||
except Exception as exc: # noqa: BLE001
|
||||
last_err = exc
|
||||
kind = f'color+depth+IR-{infra_mode}' if with_depth else 'color'
|
||||
self.get_logger().warn(f'{w}x{h}@{fps}Hz {kind} 失败：{exc}')
|
||||
profile = None
|
||||
have_depth = False
|
||||
have_infra = False
|
||||
try:
|
||||
pipe.stop()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
time.sleep(0.8)
|
||||
pipe = rs.pipeline()
|
||||
if profile is not None:
|
||||
break
|
||||
if profile is None:
|
||||
raise RuntimeError(
|
||||
f'{self._name} 没有可用的彩色配置。最后的错误：{last_err}'
|
||||
)
|
||||
|
||||
self._pipe = pipe
|
||||
self._profile = profile
|
||||
self._have_depth = have_depth
|
||||
self._have_infra = have_infra
|
||||
self._logged_depth = False
|
||||
# 深度按**真实毫米**发布。D405 的原始单位是 0.1 mm（D435 是 1 mm）；
|
||||
# 下游（感知/伺服）都按 mm 处理，所以要按设备尺度归一化。
|
||||
self._depth_to_mm = 1.0
|
||||
if have_depth:
|
||||
try:
|
||||
ds = float(profile.get_device().first_depth_sensor().get_depth_scale())
|
||||
self._depth_to_mm = ds * 1000.0 # m/单位 → mm/单位
|
||||
self.get_logger().info(
|
||||
f'深度尺度 {ds * 1000.0:.4f} mm/单位 → 按真实 mm 发布'
|
||||
)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(f'读取深度尺度失败，按 mm 处理：{exc}')
|
||||
self._depth_to_mm = 1.0
|
||||
self._align = rs.align(rs.stream.color) if have_depth else None
|
||||
self._colorizer = None
|
||||
if have_depth:
|
||||
try:
|
||||
self._colorizer = rs.colorizer()
|
||||
self._colorizer.set_option(rs.option.color_scheme, 0)
|
||||
except Exception: # noqa: BLE001
|
||||
self._colorizer = None
|
||||
try:
|
||||
from arm_application.handeye_realsense import apply_realsense_exposure
|
||||
import time as _time
|
||||
|
||||
exp_us = self._exposure_us if self._exposure_us > 0 else None
|
||||
gain = self._gain if self._gain > 0 else None
|
||||
kwargs = dict(
|
||||
auto_exposure=self._auto_exposure,
|
||||
ae_limit_us=self._ae_limit_us,
|
||||
emitter_enabled=self._emitter_enabled,
|
||||
laser_power=self._laser_power,
|
||||
exposure_us=exp_us,
|
||||
gain=gain,
|
||||
log=self.get_logger().info,
|
||||
)
|
||||
apply_realsense_exposure(profile.get_device(), **kwargs)
|
||||
# 固件经常会在最初几帧重置自动曝光；所以再设置一次。
|
||||
_time.sleep(0.25)
|
||||
apply_realsense_exposure(profile.get_device(), **kwargs)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(f'应用曝光参数失败：{exc}')
|
||||
if self._preview is not None:
|
||||
if self._infra_mode == 'both':
|
||||
self._preview.set_pane_titles(
|
||||
'COLOR', 'DEPTH', 'IR-L (左红外，与右目同一曝光)', 'IR-R (右红外)'
|
||||
)
|
||||
elif self._infra_mode == 'right':
|
||||
self._preview.set_pane_titles(
|
||||
'COLOR', 'DEPTH', '左目≈彩图灰度（不是红外曝光）', 'IR-R (右红外)'
|
||||
)
|
||||
else:
|
||||
self._preview.set_pane_titles(
|
||||
'COLOR', 'DEPTH', 'IR-L (无)', 'IR-R (无)'
|
||||
)
|
||||
|
||||
vs = profile.get_stream(rs.stream.color).as_video_stream_profile()
|
||||
try:
|
||||
intr = vs.get_intrinsics()
|
||||
self._fx = float(intr.fx)
|
||||
self._fy = float(intr.fy)
|
||||
self._cx = float(intr.ppx)
|
||||
self._cy = float(intr.ppy)
|
||||
dist = list(intr.coeffs[:5]) if intr.coeffs else [0.0] * 5
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(
|
||||
f'get_intrinsics 失败（{exc}）；改用 D405 的默认内参，'
|
||||
f'分辨率 {self._width}x{self._height}'
|
||||
)
|
||||
w, h = float(self._width), float(self._height)
|
||||
if w >= 1200:
|
||||
self._fx, self._fy = 654.8, 654.0
|
||||
else:
|
||||
self._fx = 393.9 * (w / 640.0)
|
||||
self._fy = 393.4 * (h / 480.0)
|
||||
self._cx, self._cy = w * 0.5, h * 0.5
|
||||
dist = [0.0] * 5
|
||||
while len(dist) < 5:
|
||||
dist.append(0.0)
|
||||
self._dist = dist
|
||||
|
||||
sn = profile.get_device().get_info(rs.camera_info.serial_number)
|
||||
model = profile.get_device().get_info(rs.camera_info.name)
|
||||
depth_txt = (
|
||||
f' + aligned depth /{self._name}/aligned_depth_to_color/image_raw'
|
||||
if have_depth
|
||||
else ' （仅彩色）'
|
||||
)
|
||||
if have_infra:
|
||||
depth_txt += ' + IR-L/IR-R 预览'
|
||||
self.get_logger().info(
|
||||
f'{model} serial={sn} USB={usb_desc} → /{self._name}/color/image_raw '
|
||||
f'{self._width}x{self._height}@{self._fps}Hz '
|
||||
f'fx={self._fx:.1f} fy={self._fy:.1f}{depth_txt} (SensorDataQoS)'
|
||||
)
|
||||
|
||||
if not getattr(self, '_capture_started', False):
|
||||
self._capture_started = True
|
||||
self._thread = threading.Thread(target=self._capture_loop, daemon=True)
|
||||
self._thread.start()
|
||||
|
||||
def _enable_color_ae_fast(self, profile, rs) -> None:
|
||||
"""彩色自动曝光，优先用短曝光（减少运动模糊）。"""
|
||||
try:
|
||||
sensor = profile.get_device().first_color_sensor()
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(f'彩色 AE：没有彩色传感器（{exc}）')
|
||||
return
|
||||
try:
|
||||
if sensor.supports(rs.option.enable_auto_exposure):
|
||||
sensor.set_option(rs.option.enable_auto_exposure, 1.0)
|
||||
if sensor.supports(rs.option.auto_exposure_priority):
|
||||
# 0 = 保住帧率，场景够亮时缩短曝光时间
|
||||
sensor.set_option(rs.option.auto_exposure_priority, 0.0)
|
||||
limit = getattr(rs.option, "auto_exposure_limit", None)
|
||||
if limit is not None and sensor.supports(limit):
|
||||
try:
|
||||
sensor.set_option(limit, 5000.0) # µs；30 Hz 下减少运动模糊
|
||||
except Exception:
|
||||
pass
|
||||
self.get_logger().info('彩色自动曝光已开启（优先帧率 / 减少模糊）')
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(f'彩色 AE：{exc}')
|
||||
|
||||
def _enable_stereo_ae(self, profile, rs) -> None:
|
||||
"""两路 IR 成像器共用一路自动曝光（立体模块）。"""
|
||||
try:
|
||||
sensor = profile.get_device().first_depth_sensor()
|
||||
if sensor.supports(rs.option.enable_auto_exposure):
|
||||
sensor.set_option(rs.option.enable_auto_exposure, 1.0)
|
||||
limit = getattr(rs.option, "auto_exposure_limit", None)
|
||||
if limit is not None and sensor.supports(limit):
|
||||
try:
|
||||
# 把 IR 曝光上限压到远低于 33 ms 的帧周期，这样光线暗的场景
|
||||
# 也不会悄悄把数据流掉到 30 Hz 以下。
|
||||
sensor.set_option(limit, 20000.0)
|
||||
self.get_logger().info('立体 IR 曝光上限 20ms')
|
||||
except Exception:
|
||||
pass
|
||||
self.get_logger().info('立体 IR 自动曝光已开启（左右共享）')
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(f'立体 AE：{exc}')
|
||||
|
||||
def _colorize_depth(self, depth_frame, depth_u16: np.ndarray) -> np.ndarray:
|
||||
if self._colorizer is not None:
|
||||
try:
|
||||
rgb = np.asanyarray(self._colorizer.colorize(depth_frame).get_data())
|
||||
if rgb.ndim == 3 and rgb.shape[2] >= 3:
|
||||
return rgb[:, :, ::-1].copy()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
d = np.asarray(depth_u16)
|
||||
vis = np.zeros((d.shape[0], d.shape[1], 3), dtype=np.uint8)
|
||||
valid = d > 0
|
||||
if not valid.any():
|
||||
return vis
|
||||
x = d.astype(np.float32)
|
||||
lo, hi = np.percentile(x[valid], [5, 95])
|
||||
hi = max(float(hi), float(lo) + 1.0)
|
||||
gray = np.clip((x - lo) / (hi - lo), 0.0, 1.0)
|
||||
gray = (gray * 255.0).astype(np.uint8)
|
||||
gray[~valid] = 0
|
||||
try:
|
||||
import cv2
|
||||
|
||||
vis = cv2.applyColorMap(gray, cv2.COLORMAP_JET)
|
||||
vis[~valid] = 0
|
||||
except Exception: # noqa: BLE001
|
||||
vis[:, :, 0] = gray
|
||||
vis[:, :, 1] = gray
|
||||
vis[:, :, 2] = gray
|
||||
return vis
|
||||
|
||||
def _reconnect(self) -> bool:
|
||||
try:
|
||||
if self._pipe is not None:
|
||||
self._pipe.stop()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
self._pipe = None
|
||||
time.sleep(2.5)
|
||||
try:
|
||||
self._start_pipeline(reconnect=True)
|
||||
return self._pipe is not None
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().error(
|
||||
f'重连失败：{exc}。请拔插 USB3，并给线缆留点余量。',
|
||||
throttle_duration_sec=5.0,
|
||||
)
|
||||
return False
|
||||
|
||||
def _capture_loop(self) -> None:
|
||||
got = False
|
||||
misses = 0
|
||||
while self._running and rclpy.ok():
|
||||
if self._pipe is None:
|
||||
if not self._reconnect():
|
||||
time.sleep(2.0)
|
||||
continue
|
||||
try:
|
||||
t_wait0 = time.monotonic()
|
||||
frames = self._pipe.wait_for_frames(timeout_ms=2000)
|
||||
self._t_wait = time.monotonic() - t_wait0
|
||||
ir_l = (
|
||||
frames.get_infrared_frame(1)
|
||||
if self._have_infra and self._infra_mode in ('both', 'left')
|
||||
else None
|
||||
)
|
||||
ir_r = (
|
||||
frames.get_infrared_frame(2)
|
||||
if self._have_infra and self._infra_mode in ('both', 'right')
|
||||
else None
|
||||
)
|
||||
if self._align is not None:
|
||||
t_al = time.monotonic()
|
||||
frames = self._align.process(frames)
|
||||
self._t_align = time.monotonic() - t_al
|
||||
color = frames.get_color_frame()
|
||||
except Exception as exc: # noqa: BLE001
|
||||
misses += 1
|
||||
text = str(exc)
|
||||
dropped = (
|
||||
'disconnect' in text.lower()
|
||||
or 'ioctl' in text.lower()
|
||||
or 'errno=5' in text.lower()
|
||||
)
|
||||
if dropped or misses >= 4:
|
||||
usb = getattr(self, '_usb_desc', '?')
|
||||
self.get_logger().error(
|
||||
f'相机数据流中断（USB={usb}）：{text[:120]}。'
|
||||
'正在重连 —— 请给 USB 线缆留点余量，别让机械臂把它拽掉。'
|
||||
)
|
||||
got = False
|
||||
misses = 0
|
||||
self._reconnect()
|
||||
continue
|
||||
if misses == 1:
|
||||
self.get_logger().warn(f'wait_for_frames：{exc}')
|
||||
continue
|
||||
if not color:
|
||||
misses += 1
|
||||
continue
|
||||
misses = 0
|
||||
bgr = np.asanyarray(color.get_data())
|
||||
depth = frames.get_depth_frame()
|
||||
depth_u16 = np.asanyarray(depth.get_data()) if depth else None
|
||||
if depth_u16 is not None and self._depth_to_mm != 1.0:
|
||||
# 原始设备单位 → 真实 mm（D405 原始为 0.1 mm/单位）
|
||||
depth_u16 = np.clip(
|
||||
depth_u16.astype(np.float32) * self._depth_to_mm, 0, 65535
|
||||
).astype(np.uint16)
|
||||
self._frame_i += 1
|
||||
self._fps_meter(color, depth, depth_u16)
|
||||
# 预览相关的工作（CPU 上色、IR 拷贝、四窗格绘制）每 N 帧才做一次
|
||||
# —— 它绝不能拖慢发布。
|
||||
do_push = (
|
||||
self._preview is not None
|
||||
and (self._frame_i % self._preview_every) == 0
|
||||
)
|
||||
depth_bgr = None
|
||||
ir_l_img = None
|
||||
ir_r_img = None
|
||||
if do_push:
|
||||
if depth and depth_u16 is not None and depth_u16.size:
|
||||
depth_bgr = self._colorize_depth(depth, depth_u16)
|
||||
ir_l_img = np.asanyarray(ir_l.get_data()).copy() if ir_l else None
|
||||
ir_r_img = np.asanyarray(ir_r.get_data()).copy() if ir_r else None
|
||||
# 只有在拿不到原始左目 IR 流时，才用彩色灰度来顶替。
|
||||
if ir_l_img is None and self._infra_mode != 'both':
|
||||
ir_l_img = np.mean(bgr, axis=2).astype(np.uint8)
|
||||
if not got:
|
||||
got = True
|
||||
sw, sh = int(bgr.shape[1]), int(bgr.shape[0])
|
||||
self.get_logger().info(f'首帧 {sw}x{sh}')
|
||||
if self._preview is not None:
|
||||
self._preview.set_pane_titles(
|
||||
f"COLOR {sw}x{sh}",
|
||||
"DEPTH",
|
||||
"IR-L (左红外)",
|
||||
"IR-R (右红外)",
|
||||
)
|
||||
try:
|
||||
t_pub = time.monotonic()
|
||||
self._publish_frame(bgr, depth_u16)
|
||||
if self._ir1_pub is not None and ir_l is not None:
|
||||
self._publish_ir(ir_l, self._ir1_pub)
|
||||
if self._ir2_pub is not None and ir_r is not None:
|
||||
self._publish_ir(ir_r, self._ir2_pub)
|
||||
self._t_pub = time.monotonic() - t_pub
|
||||
if do_push:
|
||||
self._reload_site_preview(int(bgr.shape[1]), int(bgr.shape[0]))
|
||||
hold = max(0.0, self._overlay_hold_sec)
|
||||
age = time.monotonic() - self._overlay_stamp
|
||||
if self._overlay_detection and self._overlay is not None and age > hold:
|
||||
self._overlay = None
|
||||
self._preview.set_overlay(None)
|
||||
if self._overlay_detection and (
|
||||
self._overlay is None or age > hold
|
||||
):
|
||||
self._preview.set_status(
|
||||
'没有框 —— 检查 perception_node / conf'
|
||||
)
|
||||
else:
|
||||
self._preview.set_status('')
|
||||
self._preview.push_views(
|
||||
bgr.copy(), depth_bgr, ir_l_img, ir_r_img
|
||||
)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
# 上下文可能正在关闭
|
||||
if not self._running:
|
||||
break
|
||||
self.get_logger().warn(
|
||||
f'发布失败：{exc}', throttle_duration_sec=2.0
|
||||
)
|
||||
|
||||
def _publish_ir(self, frame, pub) -> None:
|
||||
"""把原始 IR 成像器以 mono8 发布，供外部查看器进程使用。"""
|
||||
arr = np.asanyarray(frame.get_data())
|
||||
h, w = int(arr.shape[0]), int(arr.shape[1])
|
||||
msg = Image()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = self._frame_id
|
||||
msg.height = h
|
||||
msg.width = w
|
||||
msg.encoding = 'mono8'
|
||||
msg.is_bigendian = 0
|
||||
msg.step = w
|
||||
msg.data = array.array('B', np.ascontiguousarray(arr).tobytes())
|
||||
pub.publish(msg)
|
||||
|
||||
def _fps_meter(self, color, depth, depth_u16) -> None:
|
||||
"""记录发布帧率与硬件帧率 —— 用来区分循环太慢
|
||||
（预览/CPU）和 SDK 侧掉帧（自动曝光长曝光 / USB）。"""
|
||||
now = time.monotonic()
|
||||
try:
|
||||
fn_c = int(color.get_frame_number())
|
||||
except Exception: # noqa: BLE001
|
||||
fn_c = -1
|
||||
try:
|
||||
fn_d = int(depth.get_frame_number()) if depth else -1
|
||||
except Exception: # noqa: BLE001
|
||||
fn_d = -1
|
||||
if self._fps_t0 <= 0.0:
|
||||
self._fps_t0 = now
|
||||
self._fps_pub_color = 0
|
||||
self._fps_pub_depth = 0
|
||||
self._fps_fn_color = fn_c
|
||||
self._fps_fn_depth = fn_d
|
||||
return
|
||||
self._fps_pub_color += 1
|
||||
if depth_u16 is not None:
|
||||
self._fps_pub_depth += 1
|
||||
dt = now - self._fps_t0
|
||||
if dt < 5.0:
|
||||
return
|
||||
hw_c = (fn_c - self._fps_fn_color) / dt if self._fps_fn_color >= 0 and fn_c >= 0 else -1.0
|
||||
hw_d = (fn_d - self._fps_fn_depth) / dt if self._fps_fn_depth >= 0 and fn_d >= 0 else -1.0
|
||||
self.get_logger().info(
|
||||
f'stream fps color={self._fps_pub_color / dt:.1f} (hw {hw_c:.1f}) '
|
||||
f'depth={self._fps_pub_depth / dt:.1f} (hw {hw_d:.1f}) '
|
||||
f'wait={self._t_wait * 1e3:.0f} align={getattr(self, "_t_align", 0.0) * 1e3:.0f} '
|
||||
f'pub={getattr(self, "_t_pub", 0.0) * 1e3:.0f}ms'
|
||||
)
|
||||
self._fps_t0 = now
|
||||
self._fps_pub_color = 0
|
||||
self._fps_pub_depth = 0
|
||||
self._fps_fn_color = fn_c
|
||||
self._fps_fn_depth = fn_d
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = RealsenseColorPub()
|
||||
try:
|
||||
preview = getattr(node, '_preview', None)
|
||||
if preview is not None:
|
||||
app = node._qt_app
|
||||
# 关闭窗口绝不能停掉相机 —— 跟踪需要这条数据流。
|
||||
while rclpy.ok() and node._running:
|
||||
rclpy.spin_once(node, timeout_sec=0.01)
|
||||
app.processEvents()
|
||||
if node._preview is not None and not node._preview.isVisible():
|
||||
node.get_logger().warn(
|
||||
'预览已关闭 —— 相机继续发布'
|
||||
)
|
||||
node._preview = None
|
||||
else:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node._running = False
|
||||
if node._pipe is not None:
|
||||
try:
|
||||
node._pipe.stop()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
if node._thread is not None:
|
||||
node._thread.join(timeout=1.0)
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
+236
@@ -0,0 +1,236 @@
|
||||
#!/usr/bin/env python3
|
||||
"""RealSense SDK 预览：固定的 2x2 Qt 窗格（彩色 / 深度 / IR-L / IR-R）。
|
||||
|
||||
把四路画面拼成一张 pixmap 时，一旦缺了深度 / IR 看起来就像单相机视图。
|
||||
现在每路画面都有自己带标题的独立窗格。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
from typing import Optional, Sequence, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
try:
|
||||
from PySide6.QtCore import Qt, QTimer
|
||||
from PySide6.QtGui import QImage, QPixmap
|
||||
from PySide6.QtWidgets import (
|
||||
QApplication,
|
||||
QGridLayout,
|
||||
QLabel,
|
||||
QMainWindow,
|
||||
QSizePolicy,
|
||||
QVBoxLayout,
|
||||
QWidget,
|
||||
)
|
||||
except Exception: # noqa: BLE001
|
||||
QApplication = None # type: ignore
|
||||
|
||||
|
||||
def _to_bgr(img: Optional[np.ndarray]) -> Optional[np.ndarray]:
|
||||
if img is None:
|
||||
return None
|
||||
vis = img
|
||||
if vis.ndim == 2:
|
||||
vis = np.stack([vis, vis, vis], axis=-1)
|
||||
elif vis.ndim == 3 and vis.shape[2] == 4:
|
||||
vis = vis[:, :, :3]
|
||||
if vis.dtype != np.uint8:
|
||||
vis = np.clip(vis, 0, 255).astype(np.uint8)
|
||||
return np.ascontiguousarray(vis)
|
||||
|
||||
|
||||
def _bgr_to_pixmap(bgr: np.ndarray) -> "QPixmap":
|
||||
rgb = np.ascontiguousarray(bgr[:, :, ::-1])
|
||||
h, w, _ = rgb.shape
|
||||
qimg = QImage(rgb.data, w, h, w * 3, QImage.Format_RGB888).copy()
|
||||
return QPixmap.fromImage(qimg)
|
||||
|
||||
|
||||
def _placeholder(text: str, w: int = 424, h: int = 240) -> np.ndarray:
|
||||
vis = np.zeros((h, w, 3), dtype=np.uint8)
|
||||
vis[:] = (28, 28, 28)
|
||||
try:
|
||||
import cv2
|
||||
|
||||
cv2.putText(
|
||||
vis, text, (16, h // 2), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (180, 180, 180), 2
|
||||
)
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
return vis
|
||||
|
||||
|
||||
class SdkPreviewWindow(QMainWindow):
|
||||
def __init__(self, title: str = "RealSense") -> None:
|
||||
super().__init__()
|
||||
self.setWindowTitle(title)
|
||||
self._lock = threading.Lock()
|
||||
self._color: Optional[np.ndarray] = None
|
||||
self._depth: Optional[np.ndarray] = None
|
||||
self._ir_l: Optional[np.ndarray] = None
|
||||
self._ir_r: Optional[np.ndarray] = None
|
||||
self._overlay: Optional[Sequence[float]] = None
|
||||
self._status: str = ""
|
||||
self._hud: str = ""
|
||||
|
||||
central = QWidget()
|
||||
grid = QGridLayout(central)
|
||||
grid.setContentsMargins(6, 6, 6, 6)
|
||||
grid.setSpacing(6)
|
||||
self._img_labels: Tuple[QLabel, QLabel, QLabel, QLabel]
|
||||
self._cap_labels: Tuple[QLabel, QLabel, QLabel, QLabel]
|
||||
panes = []
|
||||
caps = []
|
||||
for row, col, name in (
|
||||
(0, 0, "COLOR"),
|
||||
(0, 1, "DEPTH"),
|
||||
(1, 0, "IR-L (左红外)"),
|
||||
(1, 1, "IR-R (右红外)"),
|
||||
):
|
||||
box, lab, cap = self._make_pane(name)
|
||||
grid.addWidget(box, row, col)
|
||||
panes.append(lab)
|
||||
caps.append(cap)
|
||||
self._img_labels = tuple(panes) # type: ignore[assignment]
|
||||
self._cap_labels = tuple(caps) # type: ignore[assignment]
|
||||
self.setCentralWidget(central)
|
||||
self.resize(1360, 820)
|
||||
self.setMinimumSize(880, 520)
|
||||
|
||||
self._timer = QTimer(self)
|
||||
self._timer.timeout.connect(self._redraw)
|
||||
self._timer.start(66)
|
||||
|
||||
@staticmethod
|
||||
def _make_pane(title: str) -> Tuple[QWidget, QLabel, QLabel]:
|
||||
box = QWidget()
|
||||
lay = QVBoxLayout(box)
|
||||
lay.setContentsMargins(0, 0, 0, 0)
|
||||
lay.setSpacing(2)
|
||||
cap = QLabel(title)
|
||||
cap.setAlignment(Qt.AlignCenter)
|
||||
cap.setStyleSheet("color:#e8e8e8; font-weight:600; font-size:13px; background:#222;")
|
||||
img = QLabel("等待画面 …")
|
||||
img.setAlignment(Qt.AlignCenter)
|
||||
img.setMinimumSize(400, 220)
|
||||
img.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
|
||||
img.setStyleSheet("background:#111; color:#777;")
|
||||
img.setScaledContents(False)
|
||||
lay.addWidget(cap)
|
||||
lay.addWidget(img, 1)
|
||||
return box, img, cap
|
||||
|
||||
def set_pane_titles(self, color: str, depth: str, ir_l: str, ir_r: str) -> None:
|
||||
for lab, text in zip(self._cap_labels, (color, depth, ir_l, ir_r)):
|
||||
lab.setText(text)
|
||||
|
||||
def push_bgr(self, bgr: np.ndarray) -> None:
|
||||
self.push_views(bgr)
|
||||
|
||||
def push_views(
|
||||
self,
|
||||
color: np.ndarray,
|
||||
depth_bgr: Optional[np.ndarray] = None,
|
||||
ir_left: Optional[np.ndarray] = None,
|
||||
ir_right: Optional[np.ndarray] = None,
|
||||
) -> None:
|
||||
with self._lock:
|
||||
self._color = color
|
||||
self._depth = depth_bgr
|
||||
self._ir_l = ir_left
|
||||
self._ir_r = ir_right
|
||||
|
||||
def set_overlay(self, data: Optional[Sequence[float]]) -> None:
|
||||
with self._lock:
|
||||
self._overlay = data
|
||||
|
||||
def set_status(self, text: str) -> None:
|
||||
with self._lock:
|
||||
self._status = text or ""
|
||||
|
||||
def _draw_overlay(self, bgr: np.ndarray, ov, status: str) -> np.ndarray:
|
||||
vis = bgr
|
||||
need = (ov is not None and len(ov) >= 8) or bool(status)
|
||||
if not need:
|
||||
return vis
|
||||
try:
|
||||
import cv2
|
||||
except Exception: # noqa: BLE001
|
||||
return vis
|
||||
vis = bgr.copy()
|
||||
if ov is not None and len(ov) >= 8:
|
||||
x1, y1, x2, y2 = [int(round(v)) for v in ov[:4]]
|
||||
cx, cy = int(round(ov[6])), int(round(ov[7]))
|
||||
cv2.rectangle(vis, (x1, y1), (x2, y2), (0, 255, 0), 2)
|
||||
cv2.drawMarker(
|
||||
vis,
|
||||
(vis.shape[1] // 2, vis.shape[0] // 2),
|
||||
(255, 255, 0),
|
||||
cv2.MARKER_CROSS,
|
||||
20,
|
||||
2,
|
||||
)
|
||||
cv2.circle(vis, (cx, cy), 6, (0, 0, 255), -1)
|
||||
conf = float(ov[4]) if len(ov) > 4 else 0.0
|
||||
z = float(ov[8]) if len(ov) > 8 else 0.0
|
||||
cv2.putText(
|
||||
vis,
|
||||
f"{conf:.2f} z={z:.3f}m",
|
||||
(x1, max(20, y1 - 8)),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.65,
|
||||
(0, 255, 0),
|
||||
2,
|
||||
)
|
||||
if status:
|
||||
is_right = ("91" in status) or ("右" in status)
|
||||
col = (0, 140, 255) if is_right else (40, 200, 255)
|
||||
cv2.rectangle(vis, (0, 0), (vis.shape[1], 40), (16, 16, 16), -1)
|
||||
cv2.putText(
|
||||
vis,
|
||||
status[:80],
|
||||
(12, 28),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.75,
|
||||
col,
|
||||
2,
|
||||
)
|
||||
return vis
|
||||
|
||||
def _set_pane(self, label: QLabel, bgr: Optional[np.ndarray], empty: str) -> None:
|
||||
vis = _to_bgr(bgr)
|
||||
if vis is None:
|
||||
vis = _placeholder(empty)
|
||||
pix = _bgr_to_pixmap(vis)
|
||||
slot = label.size()
|
||||
if slot.width() > 4 and slot.height() > 4:
|
||||
pix = pix.scaled(slot, Qt.KeepAspectRatio, Qt.FastTransformation)
|
||||
label.setPixmap(pix)
|
||||
|
||||
def _redraw(self) -> None:
|
||||
with self._lock:
|
||||
color = None if self._color is None else self._color
|
||||
depth = self._depth
|
||||
ir_l = self._ir_l
|
||||
ir_r = self._ir_r
|
||||
ov = self._overlay
|
||||
status = self._status
|
||||
if color is None:
|
||||
return
|
||||
color = self._draw_overlay(color, ov, status)
|
||||
labs = self._img_labels
|
||||
self._set_pane(labs[0], color, "无彩色")
|
||||
self._set_pane(labs[1], depth, "无深度")
|
||||
self._set_pane(labs[2], ir_l, "无 IR-L")
|
||||
self._set_pane(labs[3], ir_r, "无 IR-R")
|
||||
|
||||
|
||||
def ensure_app() -> "QApplication":
|
||||
if QApplication is None:
|
||||
raise RuntimeError("RealSense 预览窗口缺少 PySide6，无法创建")
|
||||
app = QApplication.instance()
|
||||
if app is None:
|
||||
app = QApplication([])
|
||||
return app
|
||||
+292
@@ -0,0 +1,292 @@
|
||||
#!/usr/bin/env python3
|
||||
"""realsense_view —— D405 四窗格预览（COLOR|DEPTH|IR-L|IR-R），独立进程运行。
|
||||
|
||||
相机节点（``realsense_color_pub``）只负责发布话题。所有 Qt 工作都放在这里，
|
||||
这样绘制永远不会拖垮相机采集循环（两者同进程时，Python 的 GIL 会让 ``publish``
|
||||
的墙钟耗时从 1 ms 跳到约 70 ms）。关闭窗口只是隐藏预览；Ctrl+C 才停止查看器
|
||||
（相机仍在持续发布）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
import time
|
||||
from typing import Optional
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import (
|
||||
HistoryPolicy,
|
||||
QoSProfile,
|
||||
ReliabilityPolicy,
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
from sensor_msgs.msg import Image
|
||||
from std_msgs.msg import Float32MultiArray
|
||||
|
||||
from arm_application.realsense_color_pub import _site_yaml_path
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
|
||||
def _colorize_depth_u16(d: np.ndarray) -> np.ndarray:
|
||||
"""为预览窗格做快速 numpy JET 上色（不使用 SDK 的 colorizer）。"""
|
||||
vis = np.zeros((d.shape[0], d.shape[1], 3), dtype=np.uint8)
|
||||
valid = d > 0
|
||||
if not valid.any():
|
||||
return vis
|
||||
x = d.astype(np.float32)
|
||||
lo, hi = np.percentile(x[valid], [5, 95])
|
||||
hi = max(float(hi), float(lo) + 1.0)
|
||||
gray = (np.clip((x - lo) / (hi - lo), 0.0, 1.0) * 255.0).astype(np.uint8)
|
||||
gray[~valid] = 0
|
||||
try:
|
||||
import cv2
|
||||
|
||||
vis = cv2.applyColorMap(gray, cv2.COLORMAP_JET)
|
||||
vis[~valid] = 0
|
||||
except Exception: # noqa: BLE001
|
||||
vis[:, :, 0] = gray
|
||||
vis[:, :, 1] = gray
|
||||
vis[:, :, 2] = gray
|
||||
return vis
|
||||
|
||||
|
||||
def _mono_to_bgr(mono: np.ndarray) -> np.ndarray:
|
||||
return np.ascontiguousarray(np.stack([mono, mono, mono], axis=-1))
|
||||
|
||||
|
||||
class RealsenseView(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('realsense_view')
|
||||
self.declare_parameter('camera_name', 'd405')
|
||||
self.declare_parameter('overlay_hold_sec', 0.10)
|
||||
self.declare_parameter('overlay_detection', True)
|
||||
# D405 用 workpiece/detection；D435 用 workpiece/detection_overhead（像素坐标系不同）。
|
||||
self.declare_parameter('detection_topic', 'workpiece/detection')
|
||||
self.declare_parameter('view_hz', 20.0)
|
||||
self.declare_parameter('hud_label', '')
|
||||
self._name = str(self.get_parameter('camera_name').value).strip().strip('/')
|
||||
self._hold = max(0.0, float(self.get_parameter('overlay_hold_sec').value))
|
||||
self._overlay_detection = bool(self.get_parameter('overlay_detection').value)
|
||||
self._det_topic = str(self.get_parameter('detection_topic').value or '').strip() or (
|
||||
'workpiece/detection'
|
||||
)
|
||||
self._hud = str(self.get_parameter('hud_label').value or '').strip()
|
||||
|
||||
self._color: Optional[Image] = None
|
||||
self._depth: Optional[Image] = None
|
||||
self._ir1: Optional[Image] = None
|
||||
self._ir2: Optional[Image] = None
|
||||
self._overlay: Optional[list] = None
|
||||
self._overlay_stamp = 0.0
|
||||
# stage2 孔对准叠图（已是标注好的 BGR 图），新鲜时 COLOR 窗格直接显示它
|
||||
self._stage2_img: Optional[np.ndarray] = None
|
||||
self._stage2_stamp = 0.0
|
||||
self._site_yaml = _site_yaml_path()
|
||||
self._site_mtime = 0.0
|
||||
# RX 速率表 —— 用来直观诊断卡顿（是传输问题还是绘制问题）
|
||||
self._rx_color = 0
|
||||
self._rx_depth = 0
|
||||
self._rx_ir = 0
|
||||
self._rx_t0 = 0.0
|
||||
|
||||
qos = qos_profile_sensor_data # BEST_EFFORT keep_last(1)
|
||||
base = f'/{self._name}'
|
||||
self.create_subscription(Image, f'{base}/color/image_raw', self._on_color, qos)
|
||||
self.create_subscription(
|
||||
Image, f'{base}/aligned_depth_to_color/image_raw', self._on_depth, qos
|
||||
)
|
||||
self.create_subscription(
|
||||
Image, f'{base}/infra1/image_rect_raw', self._on_ir1, qos
|
||||
)
|
||||
self.create_subscription(
|
||||
Image, f'{base}/infra2/image_rect_raw', self._on_ir2, qos
|
||||
)
|
||||
det_qos = QoSProfile(
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=5,
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
)
|
||||
if self._overlay_detection:
|
||||
self.create_subscription(
|
||||
Float32MultiArray, self._det_topic, self._on_det, det_qos
|
||||
)
|
||||
# stage2 孔对准叠图（k1/k2 标注 + 残差 HUD），有它时优先显示
|
||||
self.create_subscription(
|
||||
Image, '/stage2/overlay', self._on_stage2_overlay, qos_profile_sensor_data
|
||||
)
|
||||
|
||||
from arm_application.realsense_sdk_view import SdkPreviewWindow, ensure_app
|
||||
|
||||
self._qt_app = ensure_app()
|
||||
hud = self._hud or self._name
|
||||
self._preview = SdkPreviewWindow(
|
||||
title=f'{hud} /{self._name} COLOR|DEPTH|IR （查看器）'
|
||||
)
|
||||
self._preview.set_pane_titles(
|
||||
'COLOR', 'DEPTH', 'IR-L (左红外)', 'IR-R (右红外)'
|
||||
)
|
||||
self._preview.show()
|
||||
hz = max(5.0, min(30.0, float(self.get_parameter('view_hz').value)))
|
||||
self.create_timer(1.0 / hz, self._push)
|
||||
self.get_logger().info(
|
||||
f'查看器已启动 —— 订阅 /{self._name}/color|depth|infra1|infra2 '
|
||||
f'检测={self._det_topic if self._overlay_detection else "关"} '
|
||||
f'（保留 {self._hold:.2f}s）。关闭窗口 = 仅隐藏。'
|
||||
)
|
||||
|
||||
# 回调只存最新消息；转换在 _push 里做。
|
||||
def _on_color(self, msg: Image) -> None:
|
||||
self._color = msg
|
||||
self._rx_color += 1
|
||||
|
||||
def _on_depth(self, msg: Image) -> None:
|
||||
self._depth = msg
|
||||
self._rx_depth += 1
|
||||
|
||||
def _on_ir1(self, msg: Image) -> None:
|
||||
self._ir1 = msg
|
||||
self._rx_ir += 1
|
||||
|
||||
def _on_ir2(self, msg: Image) -> None:
|
||||
self._ir2 = msg
|
||||
|
||||
def _on_stage2_overlay(self, msg: Image) -> None:
|
||||
try:
|
||||
arr = np.frombuffer(msg.data, dtype=np.uint8).reshape(
|
||||
int(msg.height), int(msg.width), 3
|
||||
)
|
||||
self._stage2_img = np.ascontiguousarray(arr)
|
||||
self._stage2_stamp = time.monotonic()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
|
||||
def _on_det(self, msg: Float32MultiArray) -> None:
|
||||
data = list(msg.data)
|
||||
if not data:
|
||||
self._overlay = None
|
||||
self._overlay_stamp = 0.0
|
||||
if self._preview is not None:
|
||||
self._preview.set_overlay(None)
|
||||
return
|
||||
self._overlay = data
|
||||
self._overlay_stamp = time.monotonic()
|
||||
if self._preview is not None:
|
||||
self._preview.set_overlay(data)
|
||||
|
||||
def _reload_site(self) -> None:
|
||||
path = self._site_yaml
|
||||
if not path.is_file():
|
||||
return
|
||||
try:
|
||||
mtime = path.stat().st_mtime
|
||||
except OSError:
|
||||
return
|
||||
if mtime <= self._site_mtime:
|
||||
return
|
||||
self._site_mtime = mtime
|
||||
try:
|
||||
import yaml
|
||||
|
||||
data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
cam = data.get("camera") or {}
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(f"site.yaml 重新加载失败：{exc}")
|
||||
return
|
||||
if "overlay_hold_sec" in cam:
|
||||
# 顶视检测频率低，保留 launch 里设的较长 hold，别被 site 的 0.1s 冲掉。
|
||||
if "overhead" in self._det_topic:
|
||||
return
|
||||
hold = max(0.0, float(cam.get("overlay_hold_sec") or 0.0))
|
||||
if abs(hold - self._hold) > 1e-6:
|
||||
self.get_logger().info(
|
||||
f"overlay_hold_sec {self._hold:.2f}s → {hold:.2f}s (site.yaml)"
|
||||
)
|
||||
self._hold = hold
|
||||
|
||||
@staticmethod
|
||||
def _bgr(msg: Image) -> np.ndarray:
|
||||
return np.frombuffer(msg.data, dtype=np.uint8).reshape(
|
||||
int(msg.height), int(msg.width), 3
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def _mono(msg: Image) -> np.ndarray:
|
||||
return np.frombuffer(msg.data, dtype=np.uint8).reshape(
|
||||
int(msg.height), int(msg.width)
|
||||
)
|
||||
|
||||
def _push(self) -> None:
|
||||
if self._preview is None:
|
||||
return
|
||||
self._reload_site()
|
||||
now = time.monotonic()
|
||||
if self._rx_t0 <= 0.0:
|
||||
self._rx_t0 = now
|
||||
elif now - self._rx_t0 >= 5.0:
|
||||
dt = now - self._rx_t0
|
||||
self.get_logger().info(
|
||||
f'查看器接收 color={self._rx_color / dt:.1f}/s '
|
||||
f'depth={self._rx_depth / dt:.1f}/s ir={self._rx_ir / dt:.1f}/s'
|
||||
)
|
||||
self._rx_t0 = now
|
||||
self._rx_color = 0
|
||||
self._rx_depth = 0
|
||||
self._rx_ir = 0
|
||||
age = now - self._overlay_stamp
|
||||
if self._overlay is not None and age > self._hold:
|
||||
self._overlay = None
|
||||
self._preview.set_overlay(None)
|
||||
if self._overlay is None or age > self._hold:
|
||||
miss = '没有框 —— 检查 perception_node / conf'
|
||||
self._preview.set_status(f'{self._hud} {miss}' if self._hud else miss)
|
||||
else:
|
||||
self._preview.set_status(self._hud)
|
||||
if self._color is None:
|
||||
return
|
||||
color = self._bgr(self._color)
|
||||
# stage2 孔对准叠图新鲜时，COLOR 窗格直接显示它（带 k1/k2 标注 + 残差）
|
||||
if self._stage2_img is not None and (now - self._stage2_stamp) < 2.5:
|
||||
color = self._stage2_img
|
||||
depth_bgr = None
|
||||
if self._depth is not None:
|
||||
d = np.frombuffer(self._depth.data, dtype=np.uint16).reshape(
|
||||
int(self._depth.height), int(self._depth.width)
|
||||
)
|
||||
depth_bgr = _colorize_depth_u16(d)
|
||||
# 在这里（执行器线程）做 Mono→BGR，让 Qt 绘制线程保持轻量。
|
||||
ir1 = None if self._ir1 is None else _mono_to_bgr(self._mono(self._ir1))
|
||||
ir2 = None if self._ir2 is None else _mono_to_bgr(self._mono(self._ir2))
|
||||
self._preview.push_views(color, depth_bgr, ir1, ir2)
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = RealsenseView()
|
||||
app = node._qt_app
|
||||
# ROS 回调（图像反序列化、深度上色、推送）跑在执行器线程上；
|
||||
# 主线程只负责绘制 Qt —— 不会有共享的 GIL 突发。
|
||||
executor = MultiThreadedExecutor(num_threads=3)
|
||||
executor.add_node(node)
|
||||
spin_t = threading.Thread(target=executor.spin, daemon=True)
|
||||
spin_t.start()
|
||||
try:
|
||||
while rclpy.ok() and spin_t.is_alive():
|
||||
app.processEvents()
|
||||
time.sleep(0.005)
|
||||
if node._preview is not None and not node._preview.isVisible():
|
||||
node.get_logger().warn('预览已关闭 —— 查看器挂起（相机仍在发布）')
|
||||
node._preview = None
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
executor.shutdown()
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,118 @@
|
||||
"""应用层节点的统一基类：把「参数类型的宽窄」问题收在一处。
|
||||
|
||||
为什么需要这个基类（2026-09-28 真机落地实测，代价是顶置相机起不来）
|
||||
------------------------------------------------------------------
|
||||
现场 `config/site.yaml` 写的是:
|
||||
|
||||
cameras:
|
||||
d435:
|
||||
ae_limit_us: 8000 # ← YAML 整数
|
||||
|
||||
脚本把它当 `-p ae_limit_us:=8000` 传给节点，而节点里是:
|
||||
|
||||
self.declare_parameter('ae_limit_us', 3000.0) # ← double
|
||||
|
||||
rclpy **严格**比对类型，于是节点**一起就死**:
|
||||
|
||||
InvalidParameterTypeException: Trying to set parameter 'ae_limit_us'
|
||||
to '8000' of type 'INTEGER', expecting type 'DOUBLE'
|
||||
|
||||
现场看到的是「顶置 D435 起不来」，而 site.yaml 那一行看起来毫无问题 ——
|
||||
`8000` 和 `8000.0` 在运维眼里是同一个数，在 rclpy 眼里是两种类型。
|
||||
同一族的雷还有：`width: 848.0`（浮点）喂给 int 参数、`emitter_enabled: 1`
|
||||
喂给 bool 参数、`"26610799"` 与 `26610799`（带不带引号）等等。
|
||||
|
||||
**为什么不让现场人员"写对一点"**：那等于要求每个人记住「这个数必须带小数点」，
|
||||
而记错的表现是节点启动即崩、报错里还完全不提 site.yaml。规则要靠人记住的就
|
||||
一定会破（`FRAMEWORK_RULES.md` 的总纲）。
|
||||
|
||||
**为什么不在每个参数上加个 cast**：异常发生在**声明阶段**，
|
||||
`float(self.get_parameter(...).value)` 根本执行不到 —— 看着宽容，其实没救。
|
||||
|
||||
做法
|
||||
----
|
||||
用 ``ParameterDescriptor(dynamic_typing=True)`` 声明：参数类型由**传进来的值**
|
||||
决定，不做匹配检查；取值时由业务代码 cast（`float(...)` / `int(...)`），
|
||||
语义仍然由声明处的默认值表达。于是 ``8000`` / ``8000.0`` / ``"8000"`` 都能用。
|
||||
|
||||
**代价与边界**：动态类型意味着 rclpy 不再替我们拦「类型写错」。
|
||||
本项目接受这个代价，因为这些参数**全部**来自 `config/site.yaml` 这个运维手改的
|
||||
文件（不是程序内部传参），宽容比严格更符合现场；而"名字写错"仍然会被
|
||||
rclpy 拦下来（参数名是静态的，与类型无关）。
|
||||
|
||||
新增节点一律继承 ``TolerantNode``（有守卫测试
|
||||
``test_ros_node.py::test_no_node_subclasses_rclpy_node_directly`` 钉住）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any, Optional, Sequence, Union
|
||||
|
||||
from rcl_interfaces.msg import ParameterDescriptor
|
||||
from rclpy.node import Node
|
||||
|
||||
__all__ = ["TolerantNode", "tolerant_descriptor", "param_as_str"]
|
||||
|
||||
|
||||
def tolerant_descriptor() -> ParameterDescriptor:
|
||||
"""声明用的描述符：类型跟着**传进来的值**走，不做宽窄匹配。"""
|
||||
return ParameterDescriptor(dynamic_typing=True)
|
||||
|
||||
|
||||
def param_as_str(node: Node, name: str, default: str = "") -> str:
|
||||
"""读一个"本质是字符串"的参数，容忍传进来的是数字。
|
||||
|
||||
为什么需要：现场 `site.yaml` 里 `serial_number: 26610799`（不加引号）就是
|
||||
个 YAML 整数，而节点里想要的是字符串。rclpy 同样会因类型不匹配在**声明**
|
||||
阶段抛异常 —— 而字符串参数在运维手上最容易"看着是数字"。
|
||||
|
||||
以前这段逻辑在三个节点里各抄了一份（`dh_pgea_node` / `handeye_teach` /
|
||||
`realsense_color_pub`），相同的东西抄三份迟早歪（`FRAMEWORK_RULES.md` R5.4）。
|
||||
现在只此一处；取不到就返回 `default`。
|
||||
"""
|
||||
node.declare_parameter(name, default, tolerant_descriptor())
|
||||
value = node.get_parameter(name).value
|
||||
if value is None:
|
||||
return default
|
||||
return str(value).strip()
|
||||
|
||||
|
||||
class TolerantNode(Node):
|
||||
"""`rclpy.node.Node` + 「参数类型宽容」。
|
||||
|
||||
只改 `declare_parameter` 一件事：默认描述符换成动态类型。
|
||||
调用方若自己传了 `descriptor`（例如只想放宽某几个），以调用方为准。
|
||||
"""
|
||||
|
||||
def declare_parameter( # type: ignore[override]
|
||||
self,
|
||||
name: str,
|
||||
value: Any = None,
|
||||
descriptor: Optional[ParameterDescriptor] = None,
|
||||
ignore_override: bool = False,
|
||||
) -> Any:
|
||||
if descriptor is None:
|
||||
descriptor = tolerant_descriptor()
|
||||
return super().declare_parameter(name, value, descriptor, ignore_override)
|
||||
|
||||
# 列表版本是 rclpy 的另一个入口，同样要放宽（`workspace_xyz_min: [1, 2, 3]`
|
||||
# 这种"整数字面量的列表"喂给 DOUBLE_ARRAY 一样会崩）。
|
||||
def declare_parameters( # type: ignore[override]
|
||||
self,
|
||||
namespace: str,
|
||||
parameters: Sequence[
|
||||
Union[tuple, Any]
|
||||
],
|
||||
ignore_override: bool = False,
|
||||
) -> Any:
|
||||
fixed = []
|
||||
for item in parameters:
|
||||
if (
|
||||
isinstance(item, tuple)
|
||||
and len(item) == 2
|
||||
and not isinstance(item[1], ParameterDescriptor)
|
||||
):
|
||||
fixed.append((item[0], item[1], tolerant_descriptor()))
|
||||
else:
|
||||
fixed.append(item)
|
||||
return super().declare_parameters(namespace, fixed, ignore_override)
|
||||
+174
@@ -0,0 +1,174 @@
|
||||
#!/usr/bin/env python3
|
||||
"""通过 ``/follow_joint_trajectory`` 发送多点关节轨迹（真机 + mock 都适用）。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
|
||||
import rclpy
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.node import Node
|
||||
|
||||
from arm_application.site_io import joint_prefix_default
|
||||
from arm_msgs.action import FollowJointTrajectory
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
DEFAULT_JOINTS = [
|
||||
'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
|
||||
]
|
||||
NUM_JOINTS = 7
|
||||
# 扁平化存放的位置（3 个点 x 7 个关节）+ 时间 —— ROS 参数不支持列表套字典
|
||||
DEFAULT_TIMES = [3.0, 6.0, 9.0]
|
||||
DEFAULT_POSITIONS = [
|
||||
0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0,
|
||||
0.35, -0.698, 0.35, 1.57, 0.0, 0.698, 0.0,
|
||||
0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0,
|
||||
]
|
||||
|
||||
|
||||
class RunJointTrajectory(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('run_joint_trajectory')
|
||||
self.declare_parameter('joint_prefix', joint_prefix_default())
|
||||
self.declare_parameter('joint_names', DEFAULT_JOINTS)
|
||||
self.declare_parameter('velocity_scale', 0.15)
|
||||
self.declare_parameter('action_name', 'follow_joint_trajectory')
|
||||
self.declare_parameter('waypoint_times', DEFAULT_TIMES)
|
||||
self.declare_parameter('waypoint_positions', DEFAULT_POSITIONS)
|
||||
# 1 = 只跑一次（默认）；0 = 循环直到 Ctrl+C；N = 重复 N 次
|
||||
self.declare_parameter('loop_count', 1)
|
||||
self.declare_parameter('pause_between_loops', 0.5)
|
||||
|
||||
prefix = str(self.get_parameter('joint_prefix').value or '')
|
||||
base = list(
|
||||
self.get_parameter('joint_names').get_parameter_value().string_array_value
|
||||
or DEFAULT_JOINTS
|
||||
)
|
||||
self.joint_names = [f'{prefix}{n}' for n in base]
|
||||
self.velocity_scale = float(self.get_parameter('velocity_scale').value)
|
||||
action = str(self.get_parameter('action_name').value)
|
||||
self._action_name = action
|
||||
self._client = ActionClient(self, FollowJointTrajectory, action)
|
||||
|
||||
def run(self) -> int:
|
||||
if not self._client.wait_for_server(timeout_sec=15.0):
|
||||
self.get_logger().error(
|
||||
f'Action [{self._action_name}] 不可用。'
|
||||
'请先启动真机栈：bash scripts/run_real.sh'
|
||||
)
|
||||
return 1
|
||||
|
||||
times = list(self.get_parameter('waypoint_times').value)
|
||||
positions = list(self.get_parameter('waypoint_positions').value)
|
||||
n_j = len(self.joint_names)
|
||||
n_pts = len(times)
|
||||
if n_pts < 1:
|
||||
self.get_logger().error('waypoint_times 为空')
|
||||
return 1
|
||||
if len(positions) != n_pts * n_j:
|
||||
self.get_logger().error(
|
||||
f'waypoint_positions 长度 {len(positions)} != '
|
||||
f'{n_pts} 个点 x {n_j} 个关节 = {n_pts * n_j}'
|
||||
)
|
||||
return 1
|
||||
|
||||
goal = FollowJointTrajectory.Goal()
|
||||
goal.joint_names = self.joint_names
|
||||
goal.num_points = n_pts
|
||||
goal.velocity_scale = self.velocity_scale
|
||||
goal.positions = positions
|
||||
goal.time_from_start = [float(t) for t in times]
|
||||
|
||||
loop_count = int(self.get_parameter('loop_count').value)
|
||||
pause = float(self.get_parameter('pause_between_loops').value)
|
||||
duration = goal.time_from_start[-1]
|
||||
if loop_count == 0:
|
||||
self.get_logger().info(
|
||||
f'循环模式：{goal.num_points} 个点，约 {duration:.1f}s/圈 '
|
||||
f'（Ctrl+C 停止）'
|
||||
)
|
||||
elif loop_count > 1:
|
||||
self.get_logger().info(
|
||||
f'循环模式：{loop_count} 圈，{goal.num_points} 个点，'
|
||||
f'约 {duration:.1f}s/圈'
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
f'发送 {goal.num_points} 个点，v_scale={goal.velocity_scale}，'
|
||||
f'时长={duration:.1f}s'
|
||||
)
|
||||
|
||||
iteration = 0
|
||||
while rclpy.ok():
|
||||
iteration += 1
|
||||
if loop_count > 1:
|
||||
self.get_logger().info(f'第 {iteration}/{loop_count} 圈')
|
||||
|
||||
code = self._send_once(goal, duration)
|
||||
if code != 0:
|
||||
return code
|
||||
|
||||
if loop_count == 1:
|
||||
self.get_logger().info('完成：OK')
|
||||
return 0
|
||||
if loop_count > 1 and iteration >= loop_count:
|
||||
self.get_logger().info(f'完成：已跑完 {loop_count} 圈')
|
||||
return 0
|
||||
if pause > 0:
|
||||
time.sleep(pause)
|
||||
|
||||
def _send_once(self, goal: FollowJointTrajectory.Goal, duration: float) -> int:
|
||||
send = self._client.send_goal_async(goal)
|
||||
deadline = time.monotonic() + 10.0
|
||||
while rclpy.ok() and not send.done():
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
if time.monotonic() > deadline:
|
||||
self.get_logger().error('发送目标超时')
|
||||
return 2
|
||||
handle = send.result()
|
||||
if handle is None or not handle.accepted:
|
||||
self.get_logger().error('目标被拒绝')
|
||||
return 3
|
||||
|
||||
res_fut = handle.get_result_async()
|
||||
deadline = time.monotonic() + max(120.0, duration * 3.0)
|
||||
next_log = time.monotonic() + 10.0
|
||||
while rclpy.ok() and not res_fut.done():
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
now = time.monotonic()
|
||||
if now >= next_log:
|
||||
left = max(0.0, deadline - now)
|
||||
self.get_logger().info(
|
||||
f'正在执行轨迹（计划约 {duration:.0f}s），'
|
||||
f'等待机器人……（剩余超时 {left:.0f}s）'
|
||||
)
|
||||
next_log = now + 10.0
|
||||
if now > deadline:
|
||||
self.get_logger().error('等待结果超时')
|
||||
return 4
|
||||
result = res_fut.result().result
|
||||
if not result.success:
|
||||
self.get_logger().error(f'失败：{result.message}')
|
||||
return 5
|
||||
self.get_logger().info(f'本圈结束：{result.message or "OK"}')
|
||||
return 0
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = RunJointTrajectory()
|
||||
code = 1
|
||||
try:
|
||||
code = node.run()
|
||||
except KeyboardInterrupt:
|
||||
code = 0
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
raise SystemExit(code)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,290 @@
|
||||
"""供应用节点加载 ``~/ros2/config/site.yaml``。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import yaml
|
||||
|
||||
|
||||
def ros2_root() -> Path:
|
||||
"""仓库根目录。先看环境变量，再从本文件向上找 ``config/site.yaml``。
|
||||
|
||||
不能默认 ``~/ros2``：别人把仓库克隆到别的目录时，写死的家目录路径是空的，
|
||||
启动会报文件不存在。
|
||||
"""
|
||||
for key in ("SITE_ROS2_ROOT", "ROS2_WS"):
|
||||
v = os.environ.get(key, "").strip()
|
||||
if v:
|
||||
return Path(v).expanduser()
|
||||
here = Path(__file__).resolve()
|
||||
for parent in here.parents:
|
||||
if (parent / "config" / "site.yaml").is_file() and (parent / "scripts" / "env.sh").is_file():
|
||||
return parent
|
||||
return Path.home() / "ros2"
|
||||
|
||||
|
||||
_PATH_KEYS = frozenset({
|
||||
"named_poses_file",
|
||||
"handeye_file",
|
||||
"handeye_waypoints",
|
||||
"handeye_workdir",
|
||||
"handeye_calibrate",
|
||||
"yolo_weights",
|
||||
"hole_weights",
|
||||
"weights",
|
||||
"workpiece_weights",
|
||||
"config_file",
|
||||
"append_poses_file",
|
||||
})
|
||||
|
||||
|
||||
def resolve_repo_path(value: str, root: Path | None = None) -> str:
|
||||
"""仓库内的相对路径展开成绝对路径。已经是绝对路径的原样保留。"""
|
||||
text = str(value or "").strip()
|
||||
if not text:
|
||||
return ""
|
||||
path = Path(text).expanduser()
|
||||
if path.is_absolute():
|
||||
return str(path)
|
||||
base = root or ros2_root()
|
||||
return str((base / path).resolve())
|
||||
|
||||
|
||||
def _is_path_key(key: str) -> bool:
|
||||
return (
|
||||
key in _PATH_KEYS
|
||||
or key.startswith("handeye_file")
|
||||
or key.startswith("handeye_waypoints")
|
||||
or key.endswith("_weights")
|
||||
or key.endswith("_file")
|
||||
or key.endswith("_workdir")
|
||||
)
|
||||
|
||||
|
||||
def absolutize_site_paths(data: Any, root: Path | None = None) -> Any:
|
||||
"""把 site 里的仓库相对路径展开。只动路径字段，不动别的字符串。"""
|
||||
base = root or ros2_root()
|
||||
if isinstance(data, dict):
|
||||
out: dict[Any, Any] = {}
|
||||
for key, val in data.items():
|
||||
name = str(key)
|
||||
if isinstance(val, str) and _is_path_key(name):
|
||||
out[key] = resolve_repo_path(val, base)
|
||||
elif (
|
||||
name == "config"
|
||||
and isinstance(val, str)
|
||||
and (val.endswith((".yaml", ".yml", ".json")) or "/" in val)
|
||||
):
|
||||
out[key] = resolve_repo_path(val, base)
|
||||
else:
|
||||
out[key] = absolutize_site_paths(val, base)
|
||||
return out
|
||||
if isinstance(data, list):
|
||||
return [absolutize_site_paths(item, base) for item in data]
|
||||
return data
|
||||
|
||||
|
||||
def site_path() -> Path:
|
||||
env = os.environ.get("SITE_CONFIG", "").strip()
|
||||
if env:
|
||||
return Path(env).expanduser()
|
||||
return ros2_root() / "config" / "site.yaml"
|
||||
|
||||
|
||||
def load_site_dict(path: str | Path | None = None) -> dict[str, Any]:
|
||||
p = Path(path).expanduser() if path else site_path()
|
||||
if not p.is_file():
|
||||
return {}
|
||||
data = yaml.safe_load(p.read_text(encoding="utf-8")) or {}
|
||||
if not isinstance(data, dict):
|
||||
return {}
|
||||
resolved = p.resolve()
|
||||
root = resolved.parent.parent if resolved.parent.name == "config" else ros2_root()
|
||||
return absolutize_site_paths(data, root)
|
||||
|
||||
|
||||
def deep_merge(base: dict[str, Any], overlay: dict[str, Any]) -> dict[str, Any]:
|
||||
out = dict(base)
|
||||
for key, val in overlay.items():
|
||||
if isinstance(val, dict) and isinstance(out.get(key), dict):
|
||||
out[key] = deep_merge(out[key], val)
|
||||
else:
|
||||
out[key] = val
|
||||
return out
|
||||
|
||||
|
||||
def joint_prefix_default() -> str:
|
||||
"""当前臂的关节名前缀（``Rizon4-XXXXXX_``），用作 ROS 参数的默认值。
|
||||
|
||||
来源顺序：
|
||||
|
||||
1. ``SITE_JOINT_PREFIX`` —— ``scripts/env.sh`` → ``load_site_config.sh`` 从
|
||||
site.yaml 导出，这就是**权威值**；
|
||||
2. 由 ``SITE_ROBOT_SN`` 现算（``Rizon4-063989`` → ``Rizon4-063989_``）；
|
||||
3. 都没有 → 空字符串。
|
||||
|
||||
**绝不写死某个具体 SN。** 以前这里的默认值是 ``"Rizon4-063989_"``，
|
||||
于是换台机器人后 ``ros2 run`` 直接起的节点会去订**上一台臂**的关节话题，
|
||||
表现为"节点起来了、什么都不动、也不报错"（R1.1）。
|
||||
"""
|
||||
prefix = os.environ.get("SITE_JOINT_PREFIX", "").strip()
|
||||
if prefix:
|
||||
return prefix
|
||||
sn = os.environ.get("SITE_ROBOT_SN", "").strip()
|
||||
return f"{sn}_" if sn else ""
|
||||
|
||||
|
||||
def base_frame_default() -> str:
|
||||
"""当前臂的基座坐标系（``<SN>_base_link``），用作 ROS 参数默认值。"""
|
||||
frame = os.environ.get("SITE_BASE_FRAME", "").strip()
|
||||
if frame:
|
||||
return frame
|
||||
sn = os.environ.get("SITE_ROBOT_SN", "").strip()
|
||||
return f"{sn}_base_link" if sn else ""
|
||||
|
||||
|
||||
def ee_frame_default() -> str:
|
||||
"""当前臂的法兰坐标系（``<SN>_flange``），用作 ROS 参数默认值。"""
|
||||
frame = os.environ.get("SITE_EE_FRAME", "").strip()
|
||||
if frame:
|
||||
return frame
|
||||
sn = os.environ.get("SITE_ROBOT_SN", "").strip()
|
||||
return f"{sn}_flange" if sn else ""
|
||||
|
||||
|
||||
def overhead_camera(
|
||||
site: dict[str, Any], robot: dict[str, Any] | None = None
|
||||
) -> tuple[dict[str, Any], list[str]]:
|
||||
"""手外（固定）相机 → ``(相机配置字典, 同样合格但未被选中的相机名)``。
|
||||
|
||||
手外相机 = ``cameras.*`` 里 ``role`` 以 ``overhead`` 开头的那台（眼在手外时
|
||||
相机固定在工位外，不随臂动）。
|
||||
|
||||
**为什么必须有唯一实现**：这份"按 role 找，再按 arm 收敛"的规则有两个调用方——
|
||||
S03 粗定位取命名空间（本模块给节点用）、手眼标定脚本取相机名。两边各写一份的
|
||||
后果不是报错，而是"标定用的相机"和"推理用的相机"悄悄不是同一台，且没人发现。
|
||||
所以规则只写在这里，两边都调它（`export_site_env.overhead_camera` 也走这里）。
|
||||
|
||||
收敛规则（确定性的三级）：
|
||||
1. ``arm`` 与当前臂相同的优先（仿真 ``sim_d435`` 靠这级选中）；
|
||||
2. 都没有 ``arm``（= 双臂共用的那台）时回落到它；
|
||||
3. 还是多个 → 取第一个，并把其余名字放进第二个返回值，让调用方**出声告警**。
|
||||
"""
|
||||
cams = [
|
||||
cfg
|
||||
for cfg in (site.get("cameras") or {}).values()
|
||||
if isinstance(cfg, dict)
|
||||
and str(cfg.get("role") or "").strip().lower().startswith("overhead")
|
||||
]
|
||||
if not cams:
|
||||
return {}, []
|
||||
arm = str((robot or {}).get("arm") or "").strip()
|
||||
mine = [c for c in cams if str(c.get("arm") or "").strip() == arm] if arm else []
|
||||
hits = mine or [c for c in cams if not str(c.get("arm") or "").strip()] or cams
|
||||
chosen = dict(hits[0])
|
||||
others = [str(c.get("name") or "?") for c in hits[1:]]
|
||||
return chosen, others
|
||||
|
||||
|
||||
def eye_to_hand_result_name(short: str, camera_name: str) -> str:
|
||||
"""眼在手外结果文件的**文件名**（不含目录）。
|
||||
|
||||
外参绑定「这条臂的基座 + 这台相机」：换条臂或换台相机都不通用，
|
||||
所以名字必须同时带上臂号与相机。调用方只许调这里，不许自己拼
|
||||
（launch / export_site_env / S03 找文件都走这一处）。
|
||||
"""
|
||||
short = str(short or "").strip()
|
||||
cam = str(camera_name or "").strip().strip("/")
|
||||
if short and cam:
|
||||
return f"{short}_{cam}_eye_to_hand.yaml"
|
||||
if cam:
|
||||
return f"{cam}_eye_to_hand.yaml"
|
||||
if short:
|
||||
return f"{short}_eye_to_hand.yaml"
|
||||
return "eye_to_hand.yaml"
|
||||
|
||||
|
||||
def resolve_eye_to_hand_file(
|
||||
root: Path,
|
||||
*,
|
||||
explicit: str = "",
|
||||
camera_name: str = "",
|
||||
) -> Path | None:
|
||||
"""找手外相机的手眼文件。没有就返回 None。
|
||||
|
||||
路径一律按 ``root`` 解析，不跟当前工作目录走——相对 CWD 的路径会在
|
||||
"换个目录启动"时静默失效（文件找不到 → S03 直接降级），而人只会怀疑标定。
|
||||
|
||||
候选顺序：
|
||||
1. ``explicit`` —— launch 按 site.yaml 派生的确切路径
|
||||
（``<臂号>_<相机>_eye_to_hand.yaml``）；
|
||||
2. ``<相机名>_eye_to_hand.yaml`` —— 旧命名 / 仿真（``sim_d435_...``）；
|
||||
3. ``eye_to_hand.yaml`` —— 再兜底。
|
||||
"""
|
||||
candidates: list[Path] = []
|
||||
if explicit:
|
||||
p = Path(explicit).expanduser()
|
||||
candidates.append(p if p.is_absolute() else root / p)
|
||||
cam = str(camera_name or "").strip().strip("/")
|
||||
if cam:
|
||||
candidates.append(root / "handeye" / "results" / f"{cam}_eye_to_hand.yaml")
|
||||
candidates.append(root / "handeye" / "results" / "eye_to_hand.yaml")
|
||||
for cand in candidates:
|
||||
if cand.is_file():
|
||||
return cand
|
||||
return None
|
||||
|
||||
|
||||
TRACK_CAMERA_ROLES = ("wrist", "overhead")
|
||||
|
||||
|
||||
def track_camera_role(site: dict[str, Any] | None) -> str:
|
||||
"""一阶段跟踪用哪台相机：``wrist``（腕部 D405）或 ``overhead``（顶置 D435）。
|
||||
|
||||
写错**当场报错**，不会退回腕部 —— 退回会让人以为在验 D435，其实还在用 D405。
|
||||
缺省 ``wrist``：没写这个键时行为与改之前一致（腕部追踪）。
|
||||
"""
|
||||
perc = (site or {}).get("perception")
|
||||
perc = perc if isinstance(perc, dict) else {}
|
||||
raw = perc.get("track_camera")
|
||||
if raw is None or str(raw).strip() == "":
|
||||
return "wrist"
|
||||
role = str(raw).strip().lower()
|
||||
if role not in TRACK_CAMERA_ROLES:
|
||||
raise ValueError(
|
||||
f"perception.track_camera 只能是 wrist 或 overhead，收到 {raw!r}。"
|
||||
"写错不会退回腕部 —— 退回会让人以为在用 D435 其实还在用 D405。"
|
||||
)
|
||||
return role
|
||||
|
||||
|
||||
def track_handeye_mode(role: str) -> str:
|
||||
"""跟踪相机角色 → 手眼装法。overhead 必须用 cam→基座，不能用 cam→法兰。"""
|
||||
if str(role or "").strip().lower() == "overhead":
|
||||
return "eye_to_hand"
|
||||
return "eye_in_hand"
|
||||
|
||||
|
||||
def handeye_static_tf_frames(
|
||||
*,
|
||||
mode: str,
|
||||
ee_frame: str,
|
||||
base_frame: str,
|
||||
cam_frame: str,
|
||||
) -> tuple[str, str]:
|
||||
"""手眼静态 TF 的 ``(父系, 子系)``。
|
||||
|
||||
两种装法的父子关系**不同**，不能混：
|
||||
* ``eye_in_hand``：法兰 ← 相机（相机跟着臂动）
|
||||
* ``eye_to_hand``：基座 ← 相机（相机固定，不随臂动）
|
||||
|
||||
把眼在手外的文件按"法兰 ← 相机"发出去，会得到一条**看着合理**的错 TF ——
|
||||
定位总能到附近、永远差一截，而人只会去怀疑标定精度。
|
||||
"""
|
||||
if str(mode or "").strip().lower() == "eye_to_hand":
|
||||
return str(base_frame), str(cam_frame)
|
||||
return str(ee_frame), str(cam_frame)
|
||||
|
||||
+3454
File diff suppressed because it is too large
Load Diff
+1126
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,60 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Orisys 视触觉命令行工具。端口 / 配置来自 ``site.yaml`` 的 ``tactile`` 段。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import sys
|
||||
import time
|
||||
|
||||
from arm_application.orisys_tactile import make_tactile, resolve_orisys_config
|
||||
from arm_application.site_io import load_site_dict, site_path
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser(description="Orisys 触觉命令行工具（site.yaml）")
|
||||
parser.add_argument("--duration", type=float, default=10.0)
|
||||
parser.add_argument("--site", default="")
|
||||
args = parser.parse_args()
|
||||
data = load_site_dict(args.site or None)
|
||||
t = data.get("tactile") or {}
|
||||
cfg_name = resolve_orisys_config(str(t.get("config") or "dd03"), str(t.get("config_file") or ""))
|
||||
print(
|
||||
f"site={site_path() if not args.site else args.site} "
|
||||
f"backend={t.get('backend') or 'orisys'} "
|
||||
f"left={t.get('left_video', 0)} right={t.get('right_video', 2)} "
|
||||
f"config={cfg_name}"
|
||||
)
|
||||
tactile = make_tactile(data)
|
||||
try:
|
||||
tactile.connect()
|
||||
except Exception as exc:
|
||||
print(f"错误：{exc}", file=sys.stderr)
|
||||
return 1
|
||||
t0 = time.time()
|
||||
n = 0
|
||||
try:
|
||||
while time.time() - t0 < args.duration:
|
||||
sample = tactile.read_sample()
|
||||
n += 1
|
||||
raw = sample.raw or {}
|
||||
left = raw.get("left") or {}
|
||||
right = raw.get("right") or {}
|
||||
if n % 10 == 0:
|
||||
print(
|
||||
f"n={n:4d} contact={int(sample.contact)} "
|
||||
f"L F={float(left.get('fnormal') or 0):8.2f} "
|
||||
f"R F={float(right.get('fnormal') or 0):8.2f} "
|
||||
f"Fz={float(sample.force_xyz[0]):8.2f}"
|
||||
)
|
||||
time.sleep(0.01)
|
||||
except KeyboardInterrupt:
|
||||
print("已中断")
|
||||
finally:
|
||||
tactile.disconnect()
|
||||
print(f"完成 frames={n}")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
+84
@@ -0,0 +1,84 @@
|
||||
"""触觉采样 / 指令类型。后端（Orisys / mock）可替换。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
from abc import ABC, abstractmethod
|
||||
from dataclasses import dataclass
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass
|
||||
class TactileSample:
|
||||
timestamp: float
|
||||
force_xyz: np.ndarray
|
||||
torque_xyz: np.ndarray
|
||||
contact: bool
|
||||
embedding: np.ndarray | None = None
|
||||
raw: dict[str, Any] | None = None
|
||||
|
||||
|
||||
@dataclass
|
||||
class TactileCommand:
|
||||
delta_xyz: np.ndarray
|
||||
delta_rpy: np.ndarray
|
||||
gripper_pos: float | None = None
|
||||
done: bool = False
|
||||
ok: bool = True
|
||||
message: str = ""
|
||||
|
||||
|
||||
class TactileInterface(ABC):
|
||||
name: str = "tactile_base"
|
||||
|
||||
@abstractmethod
|
||||
def connect(self) -> None: ...
|
||||
|
||||
@abstractmethod
|
||||
def disconnect(self) -> None: ...
|
||||
|
||||
@abstractmethod
|
||||
def reset(self, hole_id: int) -> None: ...
|
||||
|
||||
@abstractmethod
|
||||
def step(self, sample: TactileSample) -> TactileCommand: ...
|
||||
|
||||
|
||||
class MockTactile(TactileInterface):
|
||||
name = "mock_tactile"
|
||||
|
||||
def __init__(self) -> None:
|
||||
self._i = 0
|
||||
self._hole = 0
|
||||
|
||||
def connect(self) -> None:
|
||||
self._i = 0
|
||||
|
||||
def disconnect(self) -> None:
|
||||
pass
|
||||
|
||||
def reset(self, hole_id: int) -> None:
|
||||
self._hole = hole_id
|
||||
self._i = 0
|
||||
|
||||
def read_sample(self) -> TactileSample:
|
||||
self._i += 1
|
||||
return TactileSample(
|
||||
timestamp=time.time(),
|
||||
force_xyz=np.zeros(3),
|
||||
torque_xyz=np.zeros(3),
|
||||
contact=self._i >= 8,
|
||||
)
|
||||
|
||||
def step(self, sample: TactileSample) -> TactileCommand:
|
||||
self._i += 1
|
||||
done = self._i >= 15
|
||||
return TactileCommand(
|
||||
delta_xyz=np.zeros(3),
|
||||
delta_rpy=np.zeros(3),
|
||||
done=done,
|
||||
ok=True,
|
||||
message=f"mock hole={self._hole} step={self._i}",
|
||||
)
|
||||
@@ -0,0 +1,305 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Publish Orisys dual-finger tactile status. Config from site.yaml tactile:.
|
||||
|
||||
架构（厂家示例 two_sensor_pipeline 的推荐）：每个传感器一个独立
|
||||
multiprocessing 进程（独立 CUDA 上下文），ROS 主进程只做队列→话题转发。
|
||||
原因：实测单进程内双开两路传感器会互相拖垮（30→0.8fps）；独立进程稳定 30fps。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import multiprocessing as mp
|
||||
import signal
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
|
||||
from arm_application.image_utils import UnsupportedImageError, ndarray_to_image
|
||||
from arm_application.orisys_tactile import load_orisys_from_site, tactile_worker
|
||||
from arm_application.site_io import load_site_dict, site_path
|
||||
from arm_msgs.msg import TactileStatus
|
||||
from sensor_msgs.msg import Image
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
|
||||
class TactileNode(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("tactile")
|
||||
self.declare_parameter("publish_hz", 20.0)
|
||||
# 下面两个可以用 ROS 参数覆盖 site.yaml，便于不改站点文件就做验证
|
||||
# （例如 mock 后端 + 打开图像话题来确认链路通）。
|
||||
self.declare_parameter("backend", "")
|
||||
self.declare_parameter("image_hz", -1.0)
|
||||
data = load_site_dict()
|
||||
t = data.get("tactile") or {}
|
||||
hz = float(t.get("publish_hz") or self.get_parameter("publish_hz").value or 20.0)
|
||||
backend_override = str(self.get_parameter("backend").value).strip()
|
||||
self._backend = (
|
||||
backend_override or str(t.get("backend") or "orisys")
|
||||
).strip().lower()
|
||||
self._mock = self._backend in ("mock", "mock_orisys")
|
||||
|
||||
self._pub = {
|
||||
side: self.create_publisher(TactileStatus, f"/tactile/{side}/status", 10)
|
||||
for side in ("left", "right")
|
||||
}
|
||||
self._latest: dict[str, tuple[float, dict]] = {}
|
||||
self._connected = {"left": False, "right": False}
|
||||
self._mock_n = 0
|
||||
self._workers: list = []
|
||||
self._queues: dict[str, object] = {}
|
||||
self._stop = None
|
||||
|
||||
# --- 视触觉图像 / 力矢量场 ---
|
||||
# 默认关闭（image_hz=0）：图像每帧几百 KB，不该拖累三力的 30fps 时序。
|
||||
# 打开后走一条 maxsize=1 的独立队列，满则丢旧留新 —— 主进程来不及消费
|
||||
# 只会丢图像，三力一条不受影响。
|
||||
self._image_hz = (
|
||||
float(self.get_parameter("image_hz").value)
|
||||
if float(self.get_parameter("image_hz").value) >= 0
|
||||
else float(t.get("image_hz") or 0.0)
|
||||
)
|
||||
self._image_q = None
|
||||
self._image_warned: set[str] = set()
|
||||
self._image_pub: dict[str, object] = {}
|
||||
self._vector_pub: dict[str, object] = {}
|
||||
self._image_count: dict[str, int] = {}
|
||||
self._last_mock_img = 0.0
|
||||
|
||||
# 图像 publisher 与后端无关 —— mock 后端也要能发。
|
||||
# 否则"图像话题到底通不通"只能等到有相机、上真机那天才发现。
|
||||
if self._image_hz > 0:
|
||||
for side in ("left", "right"):
|
||||
self._image_pub[side] = self.create_publisher(
|
||||
Image, f"/tactile/{side}/image", 5
|
||||
)
|
||||
self._vector_pub[side] = self.create_publisher(
|
||||
Image, f"/tactile/{side}/vector", 5
|
||||
)
|
||||
self.get_logger().info(
|
||||
f"视触觉图像已开启: {self._image_hz:.1f}Hz "
|
||||
f"→ /tactile/{{left,right}}/image 与 /vector"
|
||||
)
|
||||
|
||||
if self._mock:
|
||||
self._connected = {"left": True, "right": True}
|
||||
self.get_logger().info("触觉使用 mock 后端（假数据，不接真实传感器）")
|
||||
else:
|
||||
cfg = load_orisys_from_site(data)
|
||||
ctx = mp.get_context("spawn") # spawn：子进程不带 DDS 线程，CUDA 上下文干净
|
||||
self._stop = ctx.Event()
|
||||
if self._image_hz > 0:
|
||||
self._image_q = ctx.Queue(maxsize=1)
|
||||
for side in ("left", "right"):
|
||||
q = ctx.Queue(maxsize=2)
|
||||
self._queues[side] = q
|
||||
p = ctx.Process(
|
||||
target=tactile_worker,
|
||||
args=(side, cfg, q, self._stop, self._image_q),
|
||||
name=f"tactile_{side}", daemon=True,
|
||||
)
|
||||
p.start()
|
||||
self._workers.append(p)
|
||||
self.get_logger().info(
|
||||
f"tactile workers spawned (left pid={self._workers[0].pid} right pid={self._workers[1].pid}) "
|
||||
f"site={site_path()}"
|
||||
)
|
||||
self.create_timer(1.0 / max(1e-3, hz), self._tick)
|
||||
|
||||
def _drain(self, side: str) -> None:
|
||||
q = self._queues.get(side)
|
||||
if q is None:
|
||||
return
|
||||
latest = None
|
||||
while True:
|
||||
try:
|
||||
latest = q.get_nowait()
|
||||
except Exception:
|
||||
break
|
||||
if latest is None:
|
||||
return
|
||||
if latest.get("error"):
|
||||
self.get_logger().error(f"[{side}] {latest['error']}", throttle_duration_sec=3.0)
|
||||
return
|
||||
self._connected[side] = True
|
||||
self._latest[side] = (time.monotonic(), latest)
|
||||
|
||||
def _drain_images(self) -> None:
|
||||
"""把 worker 推来的最新图像 / 力矢量场发布出去。
|
||||
|
||||
只取最新一帧：worker 已按 ``image_hz`` 节流、队列 ``maxsize=1``，
|
||||
所以这里读到的永远是最新画面；主进程偶尔卡一下，丢掉的是中间帧
|
||||
而不是最新帧。
|
||||
"""
|
||||
if self._image_q is None:
|
||||
return
|
||||
latest = None
|
||||
while True:
|
||||
try:
|
||||
latest = self._image_q.get_nowait()
|
||||
except Exception:
|
||||
break
|
||||
if latest is None:
|
||||
return
|
||||
|
||||
side = str(latest.get("side") or "")
|
||||
if side not in self._image_pub:
|
||||
return
|
||||
stamp = self.get_clock().now().to_msg()
|
||||
frame_id = f"{side}_tactile"
|
||||
|
||||
self._publish_array(
|
||||
self._image_pub[side], latest.get("img"), frame_id, stamp, side, "image"
|
||||
)
|
||||
self._publish_array(
|
||||
self._vector_pub[side], latest.get("vector"), frame_id, stamp, side, "vector"
|
||||
)
|
||||
|
||||
def _publish_array(self, pub, array, frame_id: str, stamp, side: str, tag: str) -> None:
|
||||
"""打包并发布一个数组；形状/类型不支持时跳过该话题而不是崩溃。
|
||||
|
||||
``array is None`` 是正常情况（该侧没有矢量场输出、或本帧没算），
|
||||
不报错也不发空图 —— 发一张全零的假图会让下游以为接触到了东西。
|
||||
"""
|
||||
if array is None:
|
||||
return
|
||||
key = f"{side}_{tag}"
|
||||
try:
|
||||
msg = ndarray_to_image(array, frame_id=frame_id, stamp=stamp)
|
||||
except UnsupportedImageError as exc:
|
||||
# 编码不支持是固定的（SDK 给的 dtype 不会变），每侧每种图只提醒一次，
|
||||
# 否则 5Hz 的警告会把现场日志冲掉。
|
||||
if key not in self._image_warned:
|
||||
self._image_warned.add(key)
|
||||
self.get_logger().warn(
|
||||
f"[{key}] 无法打包成 Image，已跳过该话题: {exc}"
|
||||
)
|
||||
return
|
||||
|
||||
try:
|
||||
pub.publish(msg)
|
||||
except Exception as exc:
|
||||
if f"{key}_pub" not in self._image_warned:
|
||||
self._image_warned.add(f"{key}_pub")
|
||||
self.get_logger().warn(f"[{key}] 发布失败: {exc}")
|
||||
return
|
||||
|
||||
if self._image_count.get(key, 0) == 0:
|
||||
self.get_logger().info(
|
||||
f"[{key}] 首帧已发布 ({msg.width}x{msg.height} {msg.encoding})"
|
||||
)
|
||||
self._image_count[key] = self._image_count.get(key, 0) + 1
|
||||
|
||||
def _publish_mock_images(self, force: float) -> None:
|
||||
"""mock 后端下造一张形变图 + 一张力矢量场，用来打通图像发布链路。
|
||||
|
||||
没有传感器时也要能把 ``/tactile/<side>/image`` 与 ``/vector`` 跑起来 ——
|
||||
否则这两个话题是否真的能出图，只能等到上真机那天才知道。
|
||||
|
||||
图像内容模仿视触觉的形变：中间一个高斯隆起（法向深度），
|
||||
连同指向中心的切向流（矢量场），接触越深幅度越大。
|
||||
"""
|
||||
if not self._image_pub:
|
||||
return
|
||||
stamp = self.get_clock().now().to_msg()
|
||||
h, w = 48, 64
|
||||
yy, xx = np.mgrid[0:h, 0:w]
|
||||
cx, cy = w / 2.0, h / 2.0
|
||||
bump = np.exp(-(((xx - cx) ** 2 + (yy - cy) ** 2) / (2.0 * 9.0 ** 2)))
|
||||
|
||||
depth = (bump * (0.05 + 0.5 * float(force))).astype(np.float32)
|
||||
vx = ((cx - xx) / w * bump * float(force)).astype(np.float32)
|
||||
vy = ((cy - yy) / h * bump * float(force)).astype(np.float32)
|
||||
vector = np.dstack([vx, vy])
|
||||
|
||||
for side in ("left", "right"):
|
||||
frame = f"{side}_tactile"
|
||||
self._publish_array(self._image_pub[side], depth, frame, stamp, side, "image")
|
||||
self._publish_array(self._vector_pub[side], vector, frame, stamp, side, "vector")
|
||||
|
||||
def _tick(self) -> None:
|
||||
self._drain_images()
|
||||
if self._mock:
|
||||
self._mock_n += 1
|
||||
force = 0.2 if self._mock_n >= 8 else 0.0
|
||||
for side in ("left", "right"):
|
||||
self._publish(side, {
|
||||
"fnormal": force, "fshearx": 0.0, "fsheary": 0.0,
|
||||
"fps": 30.0, "is_contact": self._mock_n >= 8,
|
||||
"centroid": (0.0, 0.0),
|
||||
})
|
||||
# mock 的图像也按 image_hz 节流，避免比真机行为更"勤快"。
|
||||
if self._image_hz > 0:
|
||||
now = time.monotonic()
|
||||
if now - self._last_mock_img >= 1.0 / self._image_hz:
|
||||
self._last_mock_img = now
|
||||
self._publish_mock_images(force)
|
||||
return
|
||||
for side in ("left", "right"):
|
||||
self._drain(side)
|
||||
item = self._latest.get(side)
|
||||
if item is None:
|
||||
continue
|
||||
ts, data = item
|
||||
if time.monotonic() - ts > 0.6:
|
||||
continue # 数据太旧（worker 卡住）不发布
|
||||
self._publish(side, data)
|
||||
|
||||
def _publish(self, side: str, data: dict) -> None:
|
||||
msg = TactileStatus()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = f"{side}_tactile"
|
||||
msg.side = side
|
||||
msg.connected = bool(self._connected.get(side, False))
|
||||
msg.contact = bool(data.get("is_contact", False))
|
||||
msg.fnormal = float(data.get("fnormal") or 0.0)
|
||||
msg.fshearx = float(data.get("fshearx") or 0.0)
|
||||
msg.fsheary = float(data.get("fsheary") or 0.0)
|
||||
msg.fps = float(data.get("fps") or 0.0)
|
||||
c = data.get("centroid") or (0.0, 0.0)
|
||||
msg.centroid_u = float(c[0])
|
||||
msg.centroid_v = float(c[1])
|
||||
msg.error_msg = ""
|
||||
self._pub[side].publish(msg)
|
||||
|
||||
def destroy_node(self) -> bool:
|
||||
try:
|
||||
if self._stop is not None:
|
||||
self._stop.set()
|
||||
for p in self._workers:
|
||||
p.join(timeout=2.0)
|
||||
if p.is_alive():
|
||||
p.terminate()
|
||||
for p in self._workers:
|
||||
p.join(timeout=1.5)
|
||||
if p.is_alive():
|
||||
try:
|
||||
p.kill() # 还赖着就 SIGKILL，绝不留孤儿占摄像头
|
||||
except Exception:
|
||||
pass
|
||||
finally:
|
||||
return super().destroy_node()
|
||||
|
||||
|
||||
def main() -> None:
|
||||
rclpy.init()
|
||||
node = TactileNode()
|
||||
|
||||
def _sigterm(_sig, _frm) -> None:
|
||||
raise KeyboardInterrupt
|
||||
|
||||
signal.signal(signal.SIGTERM, _sigterm) # SIGTERM 也走干净退出，释放相机
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
+448
@@ -0,0 +1,448 @@
|
||||
"""权重模块的目录布局、模型发现与标签归属（纯逻辑）。
|
||||
|
||||
这个模块刻意**只依赖标准库 + model_labels**，不 import ROS、不 import
|
||||
ultralytics/tensorrt。原因：
|
||||
|
||||
1. 可以在没有 GPU、没有 ROS 的环境里跑单元测试；
|
||||
2. ``scripts/weights.sh`` 这个命令行工具能直接用上，不必先 ``source`` 一整套环境。
|
||||
|
||||
真正干重活的转换（PyTorch → ONNX → TensorRT）在 ``scripts/weights.py`` 里，
|
||||
那部分才需要 ``ros_flexiv`` 环境和 GPU。
|
||||
|
||||
## 目录约定
|
||||
|
||||
models/
|
||||
README.md ← 模块规则（要改这个模块先读它）
|
||||
staging/ ← 非标准格式先丢这里，转换工具有序地消费它
|
||||
<分组>/ ← 例如 yolo/、hole_insert/
|
||||
<模型名>.onnx
|
||||
<模型名>.engine
|
||||
<模型名>.labels.yaml ← 标签（旁挂，可手改，进 git）
|
||||
|
||||
"分组"只是一个子目录名，用来分门别类，没有语义约束。默认 ``yolo``。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from pathlib import Path
|
||||
from typing import Dict, List, Optional, Sequence, Tuple, Union
|
||||
|
||||
from .model_labels import (
|
||||
SOURCE_MODEL,
|
||||
SOURCE_ONNX,
|
||||
SOURCE_PLACEHOLDER,
|
||||
SOURCE_SIDECAR,
|
||||
LabelSet,
|
||||
label_file_status,
|
||||
labels_path_for,
|
||||
labels_from_model,
|
||||
labels_from_onnx,
|
||||
placeholder_labels,
|
||||
read_label_file,
|
||||
)
|
||||
|
||||
# 会被当成"权重"的文件后缀（不带点的小写形式）。
|
||||
WEIGHT_EXTS: Tuple[str, ...] = (
|
||||
"pt",
|
||||
"pth",
|
||||
"onnx",
|
||||
"engine",
|
||||
"trt",
|
||||
"plan",
|
||||
"tflite",
|
||||
)
|
||||
|
||||
# 转换工具认识的输入格式。其它格式一律归到 staging 等人工处理。
|
||||
# 注意 pth 也收：虽然 Ultralytics 主要用 .pt，但用户手里常是 .pth。
|
||||
CONVERTIBLE_INPUT_EXTS: Tuple[str, ...] = ("pt", "pth", "onnx")
|
||||
|
||||
# 已经被转换过的产物，重转没有意义。
|
||||
FINAL_EXTS: Tuple[str, ...] = ("engine", "trt", "plan")
|
||||
|
||||
# 非标准格式/待转换文件放这里。
|
||||
STAGING_DIRNAME = "staging"
|
||||
|
||||
# 读标签时挑格式的顺序：onnx 有内嵌 metadata 且读取代价极低（毫秒级），
|
||||
# 所以排在前面；engine 排最后是因为它很可能什么都没有。
|
||||
LABEL_READ_ORDER: Tuple[str, ...] = ("onnx", "pt", "pth", "engine", "trt")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 仓库根 / 权重根
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def find_repo_root(start: Optional[Union[str, Path]] = None) -> Optional[Path]:
|
||||
"""从 ``start`` 往上找仓库根（以存在 ``config/site.yaml`` 为标志）。
|
||||
|
||||
这样无论代码是从 ``src/`` 直接跑、还是从 ``install/`` 跑，
|
||||
都能定位到同一份 ``models/``，不会出现"两份权重"。
|
||||
"""
|
||||
here = Path(start).resolve() if start else Path(__file__).resolve()
|
||||
for cand in [here, *here.parents]:
|
||||
if (cand / "config" / "site.yaml").is_file():
|
||||
return cand
|
||||
return None
|
||||
|
||||
|
||||
def default_weights_root() -> Path:
|
||||
"""``models/`` 的默认位置。找不到仓库根时退回当前工作目录下的 models/。"""
|
||||
import os
|
||||
|
||||
env = os.environ.get("SITE_WEIGHTS_ROOT")
|
||||
if env:
|
||||
return Path(env).expanduser()
|
||||
root = find_repo_root()
|
||||
if root is not None:
|
||||
return root / "models"
|
||||
return Path.cwd() / "models"
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 模型条目
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
@dataclass
|
||||
class ModelEntry:
|
||||
"""一个"模型" = 权重目录里同名前缀的一组文件。"""
|
||||
|
||||
group: str
|
||||
name: str
|
||||
files: Dict[str, Path] = field(default_factory=dict)
|
||||
|
||||
@property
|
||||
def dir(self) -> Path:
|
||||
if not self.files:
|
||||
return Path(".")
|
||||
return next(iter(self.files.values())).parent
|
||||
|
||||
@property
|
||||
def labels_path(self) -> Path:
|
||||
return self.dir / (self.name + ".labels.yaml")
|
||||
|
||||
@property
|
||||
def exts(self) -> List[str]:
|
||||
"""已存在的格式，按固定顺序返回，保证输出稳定。
|
||||
|
||||
顺序就是转换流水线的顺序：``pt → onnx → engine``（源头 → 中间 → 最终）。
|
||||
这样列表里一眼能看出这个模型走到哪一步了。
|
||||
"""
|
||||
order = list(WEIGHT_EXTS)
|
||||
return sorted(self.files, key=lambda e: (order.index(e) if e in order else 99, e))
|
||||
|
||||
def path(self, ext: str) -> Optional[Path]:
|
||||
return self.files.get(ext.lower().lstrip("."))
|
||||
|
||||
def sorted_files(self) -> List[Path]:
|
||||
"""按读标签的推荐顺序给出文件列表。"""
|
||||
out: List[Path] = []
|
||||
for ext in LABEL_READ_ORDER:
|
||||
p = self.files.get(ext)
|
||||
if p:
|
||||
out.append(p)
|
||||
for ext in self.exts:
|
||||
p = self.files.get(ext)
|
||||
if p and p not in out:
|
||||
out.append(p)
|
||||
return out
|
||||
|
||||
def missing_of(self, wanted: Sequence[str]) -> List[str]:
|
||||
return [e for e in wanted if e not in self.files]
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 发现
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def is_staging(path: Path) -> bool:
|
||||
return path.name == STAGING_DIRNAME
|
||||
|
||||
|
||||
def groups(root: Union[str, Path]) -> List[str]:
|
||||
"""列出所有权重分组目录（跳过 staging 与隐藏目录）。"""
|
||||
r = Path(root)
|
||||
if not r.is_dir():
|
||||
return []
|
||||
return sorted(
|
||||
p.name
|
||||
for p in r.iterdir()
|
||||
if p.is_dir() and not p.name.startswith(".") and not is_staging(p)
|
||||
)
|
||||
|
||||
|
||||
def staging_dir(root: Union[str, Path]) -> Path:
|
||||
return Path(root) / STAGING_DIRNAME
|
||||
|
||||
|
||||
def infer_group(
|
||||
root: Union[str, Path],
|
||||
source: Union[str, Path],
|
||||
fallback: str = "yolo",
|
||||
) -> str:
|
||||
"""转换产物该落到**哪个分组**：默认沿用源文件所在的分组。
|
||||
|
||||
为什么不能一律落到 ``yolo``（2026-09-28 真机踩到，代价是一次白转）
|
||||
----------------------------------------------------------------
|
||||
现场把 ``models/hole_insert/detect_best_1.onnx`` 转 engine，工具默认
|
||||
``--group yolo``，于是产物落到 ``models/yolo/detect_best_1.engine`` ——
|
||||
而运行时找 engine 的规则是**同级同名**（``yolo_detector.resolve_weights``：
|
||||
``p.with_suffix(".engine")``，即 site.yaml 指的 ``.onnx`` 旁边）。两边
|
||||
一分家，engine 就等于不存在：现象是"启动即报缺 engine"，而文件明明躺着。
|
||||
这正是本项目最忌讳的**静默失效** —— 工具没报错，位置也"合理"。
|
||||
|
||||
规则：
|
||||
- 源文件已在权重根目录下的某个分组里（例如 ``models/hole_insert/x.onnx``）
|
||||
→ 沿用 ``hole_insert``。这样"就地转换"永远落在原地，运行时必然找得到；
|
||||
- 源文件在 ``staging/``、权重根目录之外或根目录正下方 → ``fallback``
|
||||
（默认 ``yolo``）：那才是"新来的模型，还没决定分组"的正常情形。
|
||||
|
||||
Args:
|
||||
root: 权重根目录（``models/``）。
|
||||
source: 输入文件。
|
||||
fallback: 无法沿用时的分组名。
|
||||
|
||||
Returns:
|
||||
分组名（不带路径）。
|
||||
"""
|
||||
r = Path(root)
|
||||
try:
|
||||
rel = Path(source).expanduser().resolve().relative_to(r.resolve())
|
||||
except ValueError:
|
||||
# 不在权重根目录下（/tmp、外部磁盘、staging 之类）→ 没有"原地"可言
|
||||
return fallback
|
||||
if len(rel.parts) < 2:
|
||||
# 直接躺在 models/ 正下方：没有分组信息，用默认分组
|
||||
return fallback
|
||||
group = rel.parts[0]
|
||||
if is_staging(r / group) or group.startswith("."):
|
||||
return fallback
|
||||
return group
|
||||
|
||||
|
||||
def staging_files(root: Union[str, Path]) -> List[Path]:
|
||||
"""staging 里的待处理文件。
|
||||
|
||||
跳过隐藏文件（``.gitkeep`` 等）和 ``.md`` 说明文档 —— 那些是给人和 git 看的，
|
||||
不是待转换的模型。否则 ``doctor`` 会把 README 报成"格式不支持"。
|
||||
"""
|
||||
d = staging_dir(root)
|
||||
if not d.is_dir():
|
||||
return []
|
||||
return sorted(
|
||||
p
|
||||
for p in d.iterdir()
|
||||
if p.is_file() and not p.name.startswith(".") and p.suffix.lower() != ".md"
|
||||
)
|
||||
|
||||
|
||||
def discover(root: Optional[Union[str, Path]] = None) -> List[ModelEntry]:
|
||||
"""扫描权重根，按"分组 + 模型名"归并出所有模型。
|
||||
|
||||
同名不同格式（m.onnx / m.engine / m.pt）会被合成一个条目 —— 因为它们
|
||||
共用一个模型名、一份标签。``.labels.yaml`` 本身不算权重。
|
||||
"""
|
||||
r = Path(root) if root is not None else default_weights_root()
|
||||
out: List[ModelEntry] = []
|
||||
if not r.is_dir():
|
||||
return out
|
||||
for g in groups(r):
|
||||
gdir = r / g
|
||||
by_name: Dict[str, ModelEntry] = {}
|
||||
for p in sorted(gdir.iterdir()):
|
||||
if not p.is_file() or p.name.startswith("."):
|
||||
continue
|
||||
ext = p.suffix.lower().lstrip(".")
|
||||
if ext not in WEIGHT_EXTS:
|
||||
continue
|
||||
# m.labels.yaml 的后缀是 .yaml，不会落进 WEIGHT_EXTS，天然被排除。
|
||||
stem = p.name[: -(len(ext) + 1)]
|
||||
entry = by_name.get(stem)
|
||||
if entry is None:
|
||||
entry = ModelEntry(group=g, name=stem, files={})
|
||||
by_name[stem] = entry
|
||||
entry.files[ext] = p
|
||||
out.extend(by_name.values())
|
||||
return out
|
||||
|
||||
|
||||
def find_model(
|
||||
root: Optional[Union[str, Path]] = None,
|
||||
name: str = "",
|
||||
group: Optional[str] = None,
|
||||
) -> Optional[ModelEntry]:
|
||||
"""按模型名找模型。
|
||||
|
||||
- ``name`` 是完整路径 → 直接按路径构造条目（不要求它在权重目录里）
|
||||
- ``name`` 是 ``分组/模型名`` → 按分组+名字精确查找
|
||||
- ``name`` 是裸名字 → 在所有分组里找同名模型；``group`` 可缩小范围
|
||||
|
||||
裸名字跨分组同名时会优先返回 ``group`` 指定的那个；都没指定就返回找到的第一个，
|
||||
并由调用方提示存在歧义。
|
||||
"""
|
||||
p = Path(name).expanduser()
|
||||
if p.suffix and (p.is_file() or p.parent.is_dir() and str(name).count("/")):
|
||||
ext = p.suffix.lower().lstrip(".")
|
||||
if ext in WEIGHT_EXTS:
|
||||
entry = ModelEntry(group=group or p.parent.name, name=p.stem, files={})
|
||||
entry.files[ext] = p
|
||||
return entry
|
||||
|
||||
for e in discover(root):
|
||||
if e.name != name:
|
||||
continue
|
||||
if group and e.group != group:
|
||||
continue
|
||||
return e
|
||||
|
||||
# 裸名字没匹配上，再试 ``分组/模型名``。
|
||||
# ``list`` 的输出里模型是带分组一起显示的，用户照着敲 ``yolo/detect_best``
|
||||
# 非常自然；以前这里会直接报"找不到模型"，而报错又只列出裸名字，很别扭。
|
||||
# 放在裸名字查找之后，是为了不把真正的相对路径（``a/b.onnx``）误判成分组名。
|
||||
if group is None and "/" in name:
|
||||
head, _, tail = name.rpartition("/")
|
||||
if head and tail:
|
||||
return find_model(root, tail, head)
|
||||
return None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 标签归属
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def model_labels(entry: ModelEntry, *, deep: bool = False) -> LabelSet:
|
||||
"""给出某个模型当前生效的标签，附带来源。
|
||||
|
||||
优先级：旁挂文件 > ONNX 内嵌 > 模型内嵌 > 占位符。
|
||||
|
||||
``deep=False``（默认）只读旁挂文件和 ONNX metadata —— 都快。
|
||||
``deep=True`` 才会去加载模型本身（``.pt`` / ``.engine``），
|
||||
代价是要起 Ultralytics、加载权重，秒级，所以默认不开。
|
||||
这样 ``weights.sh list`` 能秒出结果，不因为几个 .pt 卡十几秒。
|
||||
"""
|
||||
files = entry.sorted_files()
|
||||
if not files:
|
||||
return LabelSet(names={}, source=SOURCE_PLACEHOLDER, path=None)
|
||||
|
||||
side = label_file_status(files[0])[1]
|
||||
names = read_label_file(side) if side else None
|
||||
if names:
|
||||
return LabelSet(names=names, source=SOURCE_SIDECAR, path=side)
|
||||
|
||||
for p in files:
|
||||
ext = p.suffix.lower().lstrip(".")
|
||||
if ext == "onnx":
|
||||
got = labels_from_onnx(p)
|
||||
if got:
|
||||
return LabelSet(names=got[0], source=SOURCE_ONNX, path=p)
|
||||
elif deep and ext in ("pt", "pth", "engine", "trt", "plan"):
|
||||
# engine 在当前的 TensorRT 下读不到（没有 metadata API），
|
||||
# 但万一用户是拿 Ultralytics 导出的，还是值得试一次。
|
||||
got = labels_from_model(p)
|
||||
if got:
|
||||
return LabelSet(names=got[0], source=SOURCE_MODEL, path=p)
|
||||
|
||||
return LabelSet(names=placeholder_labels(0), source=SOURCE_PLACEHOLDER, path=None)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 转换规划
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ConversionChain:
|
||||
"""从某个输入格式到目标格式，需要依次产出哪些文件。"""
|
||||
|
||||
source_ext: str
|
||||
target: str
|
||||
produce: Tuple[str, ...]
|
||||
|
||||
@property
|
||||
def is_noop(self) -> bool:
|
||||
return not self.produce
|
||||
|
||||
def describe(self) -> str:
|
||||
if self.is_noop:
|
||||
return f"{self.source_ext} → 无需转换"
|
||||
return f"{self.source_ext} → " + " → ".join(self.produce)
|
||||
|
||||
|
||||
def conversion_chain(source_ext: str, target: str = "all") -> ConversionChain:
|
||||
"""算出转换链。
|
||||
|
||||
TensorRT 的 engine 只能由 ONNX 构建，所以任何通往 engine 的路都必须
|
||||
先得到 ONNX。这点在这里集中表达，避免调用方各写一遍。
|
||||
|
||||
- ``target="all"``：能产出多少产出多少（.pt → ONNX + engine）
|
||||
- ``target="onnx"``：只要 ONNX（engine 无法反向拆回 ONNX，返回空）
|
||||
- ``target="engine"``：只要 engine（.pt 会顺带产出 ONNX 作为中间产物）
|
||||
"""
|
||||
src = source_ext.lower().lstrip(".")
|
||||
tgt = (target or "all").lower().strip()
|
||||
if tgt not in ("all", "onnx", "engine"):
|
||||
raise ValueError(f"未知转换目标: {target}（可选 all/onnx/engine）")
|
||||
|
||||
if src not in CONVERTIBLE_INPUT_EXTS:
|
||||
# 已经是最终产物，或干脆不认识 —— 都交给调用方去报错/跳过。
|
||||
return ConversionChain(src, tgt, ())
|
||||
|
||||
if src in ("pt", "pth"):
|
||||
if tgt == "onnx":
|
||||
return ConversionChain(src, tgt, ("onnx",))
|
||||
# engine 需要 ONNX 做中间产物，两条路都带上它。
|
||||
return ConversionChain(src, tgt, ("onnx", "engine"))
|
||||
|
||||
# src == "onnx"
|
||||
if tgt == "onnx":
|
||||
return ConversionChain(src, tgt, ())
|
||||
return ConversionChain(src, tgt, ("engine",))
|
||||
|
||||
|
||||
def canonical_path(root: Union[str, Path], group: str, name: str, ext: str) -> Path:
|
||||
"""转换产物该落在哪：``models/<group>/<name>.<ext>``。
|
||||
|
||||
这是"自动改名并归位"的落点。无论输入文件叫什么、放在哪
|
||||
（哪怕在 /tmp 或 staging 里叫 ``best_v3_final(1).pt``），
|
||||
产出都用调用方给的 ``name`` 和 ``group``。
|
||||
"""
|
||||
return Path(root) / group / f"{name}.{ext.lstrip('.')}"
|
||||
|
||||
|
||||
def validate_name(name: str) -> Optional[str]:
|
||||
"""检查模型名是否安全可用作文件名。返回错误信息或 None。"""
|
||||
n = (name or "").strip()
|
||||
if not n:
|
||||
return "模型名不能为空"
|
||||
if n != name:
|
||||
return f"模型名首尾不能有空格: {name!r}"
|
||||
if any(c.isspace() for c in n):
|
||||
# 名字里有空格会让命令行、site.yaml、ROS 参数到处都要加引号，
|
||||
# 早晚有人忘。直接禁掉，改用下划线或短横线。
|
||||
return "模型名不能含有空格（建议用 _ 或 - 代替）"
|
||||
bad = set('/\\:*?"<>|')
|
||||
hit = sorted(bad & set(n))
|
||||
if hit:
|
||||
return f"模型名不能含有这些字符: {''.join(hit)}"
|
||||
if n.startswith("."):
|
||||
return "模型名不能以点开头"
|
||||
if n.endswith(".labels"):
|
||||
return "不要带 .labels 后缀，标签文件会自动生成"
|
||||
return None
|
||||
|
||||
|
||||
def plan_conversion(
|
||||
root: Union[str, Path],
|
||||
source: Union[str, Path],
|
||||
*,
|
||||
name: str,
|
||||
group: str = "yolo",
|
||||
target: str = "all",
|
||||
) -> Tuple[ConversionChain, List[Path]]:
|
||||
"""算出一条完整的转换计划：产出链 + 每个产物的最终落点。"""
|
||||
src = Path(source).expanduser()
|
||||
chain = conversion_chain(src.suffix, target)
|
||||
outs = [canonical_path(root, group, name, ext) for ext in chain.produce]
|
||||
return chain, outs
|
||||
+618
@@ -0,0 +1,618 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Ultralytics YOLO / TensorRT 封装。
|
||||
|
||||
对外接口：``resolve_weights``、``YoloDetector``、``Detection``、``pick_best``、
|
||||
``MpYoloDetector``。当 ``backend='tensorrt'`` 时，TensorRT 会加载与
|
||||
``.onnx`` / ``.pt`` 路径同级的 ``.engine`` 文件。
|
||||
|
||||
类别名（标签）由 ``model_labels`` 统一提供：优先读权重旁边的
|
||||
``<模型名>.labels.yaml``（用户可随时手改），其次读模型内嵌 metadata。
|
||||
这样做是因为 TensorRT engine 在当前的 TensorRT Python API 下**无法携带标签**，
|
||||
只能靠旁挂文件；详见 ``model_labels`` 模块头。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import multiprocessing as mp
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import List, Optional, Sequence, Union
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .model_labels import (
|
||||
SOURCE_PLACEHOLDER,
|
||||
LabelSet,
|
||||
describe,
|
||||
parse_names_field,
|
||||
placeholder_labels,
|
||||
read_labels,
|
||||
)
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
# 哨兵值：worker 还停在上一次的那帧 —— 调用方不能把它算作漏检。
|
||||
INFER_BUSY = object()
|
||||
|
||||
|
||||
@dataclass
|
||||
class Detection:
|
||||
xyxy: np.ndarray # (4,) float：x1,y1,x2,y2
|
||||
conf: float
|
||||
cls_id: int
|
||||
cls_name: str
|
||||
# pose 模型才有：(K, 3) = 每个关键点的 x, y, conf（**原图像素坐标**）。
|
||||
# 检测模型恒为 None —— 下游必须先判 None，别假设它有。
|
||||
kpts: Optional[np.ndarray] = None
|
||||
|
||||
@property
|
||||
def cx(self) -> float:
|
||||
return float(0.5 * (self.xyxy[0] + self.xyxy[2]))
|
||||
|
||||
@property
|
||||
def cy(self) -> float:
|
||||
return float(0.5 * (self.xyxy[1] + self.xyxy[3]))
|
||||
|
||||
@property
|
||||
def area(self) -> float:
|
||||
return float(max(0.0, self.xyxy[2] - self.xyxy[0]) * max(0.0, self.xyxy[3] - self.xyxy[1]))
|
||||
|
||||
|
||||
def _resolve_device(device: str) -> str:
|
||||
try:
|
||||
import torch
|
||||
except Exception: # noqa: BLE001
|
||||
return "cpu"
|
||||
d = (device or "").strip().lower()
|
||||
if d in ("", "auto", "cuda", "cuda:0", "0", "gpu"):
|
||||
return "0" if torch.cuda.is_available() else "cpu"
|
||||
return device.strip() or "cpu"
|
||||
|
||||
|
||||
def resolve_weights(weights: Union[str, Path], backend: str = "auto") -> Path:
|
||||
"""当 backend=auto/tensorrt 时，优先使用与 .pt 同级的 TensorRT .engine。"""
|
||||
p = Path(weights).expanduser()
|
||||
b = (backend or "auto").strip().lower()
|
||||
engine = p.with_suffix(".engine")
|
||||
onnx = p.with_suffix(".onnx")
|
||||
if b in ("tensorrt", "engine"):
|
||||
if engine.is_file():
|
||||
return engine
|
||||
raise FileNotFoundError(
|
||||
f"缺少 TensorRT engine：{engine}。"
|
||||
f"导出命令：bash ~/ros2/scripts/weights.sh convert {p} --name {p.stem}"
|
||||
)
|
||||
if b == "onnx":
|
||||
if onnx.is_file():
|
||||
return onnx
|
||||
raise FileNotFoundError(f"缺少 ONNX 文件：{onnx}")
|
||||
if b in ("", "auto"):
|
||||
if engine.is_file():
|
||||
return engine
|
||||
if p.is_file():
|
||||
return p
|
||||
if onnx.is_file():
|
||||
return onnx
|
||||
return p
|
||||
|
||||
|
||||
def resolve_class_labels(path: Union[str, Path], model=None) -> LabelSet:
|
||||
"""给出一个权重该用的类别标签（旁挂文件优先）。
|
||||
|
||||
``model`` 是已经加载好的 Ultralytics 模型对象（可为 None）。顺序：
|
||||
|
||||
1. ``<模型名>.labels.yaml`` —— 用户可编辑的权威来源
|
||||
2. 传入的 ``model.names`` —— 模型内嵌 metadata
|
||||
3. 占位符 ``class_0 / class_1 …``
|
||||
|
||||
第 3 步刻意不用 ``str(cid)``。原先读不到名字时会退化成类名 `"0"`，
|
||||
和"真的有个叫 0 的类"无法区分，标签丢了都看不出来。
|
||||
占位符 + 一条 warning 让这件事在日志里可见。
|
||||
"""
|
||||
p = Path(path).expanduser()
|
||||
|
||||
# 先只看旁挂文件（快，不加载模型）：它必须能覆盖模型内嵌的旧值，
|
||||
# 否则"我改了标签为什么不生效"会变成很难查的问题。
|
||||
labels = read_labels(p, allow_model_metadata=False)
|
||||
if not labels.is_placeholder:
|
||||
return labels
|
||||
|
||||
raw = getattr(model, "names", None) if model is not None else None
|
||||
names = parse_names_field(raw or {})
|
||||
if names:
|
||||
from .model_labels import SOURCE_MODEL
|
||||
|
||||
return LabelSet(names=names, source=SOURCE_MODEL, path=p)
|
||||
|
||||
nc = 0
|
||||
try:
|
||||
nc = int(getattr(model, "nc", 0) or 0)
|
||||
except (TypeError, ValueError):
|
||||
nc = 0
|
||||
labels = LabelSet(names=placeholder_labels(nc), source=SOURCE_PLACEHOLDER, path=None)
|
||||
logger.warning(
|
||||
"模型 %s 没有类别标签，暂时用占位名显示。补一个 %s 即可（改完重跑节点生效）。"
|
||||
"命令行: bash ~/ros2/scripts/weights.sh labels %s --from-model",
|
||||
p.name,
|
||||
p.stem + ".labels.yaml",
|
||||
p.stem,
|
||||
)
|
||||
return labels
|
||||
|
||||
|
||||
class YoloDetector:
|
||||
def __init__(
|
||||
self,
|
||||
weights: Union[str, Path],
|
||||
*,
|
||||
conf: float = 0.5,
|
||||
iou: float = 0.45,
|
||||
device: str = "",
|
||||
imgsz: int = 640,
|
||||
class_ids: Optional[Sequence[int]] = None,
|
||||
class_names: Optional[Sequence[str]] = None,
|
||||
) -> None:
|
||||
try:
|
||||
from ultralytics import YOLO
|
||||
except Exception as exc: # noqa: BLE001
|
||||
raise RuntimeError(
|
||||
"未安装 ultralytics。在 ros_flexiv 环境里执行：pip install ultralytics"
|
||||
) from exc
|
||||
|
||||
path = Path(weights).expanduser()
|
||||
if not path.is_file():
|
||||
raise FileNotFoundError(
|
||||
f"找不到 YOLO 权重：{path}。"
|
||||
f"请把你的 .pt 放到 ~/ros2/models/yolo/workpiece.pt"
|
||||
)
|
||||
try:
|
||||
import torch
|
||||
|
||||
# 别让 PyTorch 占满所有 CPU 核心（相机 + ROS 都要用）。
|
||||
torch.set_num_threads(1)
|
||||
torch.set_num_interop_threads(1)
|
||||
if torch.cuda.is_available():
|
||||
torch.backends.cudnn.benchmark = True
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
self.device = _resolve_device(device)
|
||||
self.conf = float(conf)
|
||||
self.iou = float(iou)
|
||||
self.imgsz = int(imgsz)
|
||||
self._half_ok = False
|
||||
self._is_engine = path.suffix.lower() in {".engine", ".trt"}
|
||||
self._model = YOLO(str(path), task="detect")
|
||||
if not self._is_engine:
|
||||
try:
|
||||
self._model.fuse()
|
||||
except Exception:
|
||||
pass
|
||||
self.class_ids = set(int(v) for v in class_ids) if class_ids else None
|
||||
self.class_names = (
|
||||
{str(n).strip().lower() for n in class_names if str(n).strip()}
|
||||
if class_names
|
||||
else None
|
||||
)
|
||||
self._labels = resolve_class_labels(path, self._model)
|
||||
self._id_to_name = dict(self._labels.names)
|
||||
self._warmed = False
|
||||
self._pending = None
|
||||
|
||||
def warmup(self, *, half: bool = False) -> None:
|
||||
if self._warmed:
|
||||
return
|
||||
if half and (not self._is_engine) and self.device != "cpu":
|
||||
try:
|
||||
self._model.to("cuda")
|
||||
self._model.model.half()
|
||||
self._half_ok = True
|
||||
except Exception:
|
||||
self._half_ok = False
|
||||
dummy = np.zeros((self.imgsz, self.imgsz, 3), dtype=np.uint8)
|
||||
self._predict(dummy)
|
||||
self._warmed = True
|
||||
|
||||
def _predict(self, image_bgr: np.ndarray):
|
||||
kwargs = dict(
|
||||
source=image_bgr,
|
||||
conf=self.conf,
|
||||
iou=self.iou,
|
||||
imgsz=self.imgsz,
|
||||
device=self.device,
|
||||
verbose=False,
|
||||
max_det=20,
|
||||
)
|
||||
if self._half_ok and not self._is_engine:
|
||||
kwargs["quantize"] = 16
|
||||
try:
|
||||
import torch
|
||||
|
||||
ctx = torch.inference_mode()
|
||||
except Exception:
|
||||
from contextlib import nullcontext
|
||||
|
||||
ctx = nullcontext()
|
||||
with ctx:
|
||||
return self._model.predict(**kwargs)
|
||||
|
||||
def detect(
|
||||
self,
|
||||
image_bgr: np.ndarray,
|
||||
conf: float | None = None,
|
||||
iou: float | None = None,
|
||||
imgsz: int | None = None,
|
||||
**_kwargs,
|
||||
) -> List[Detection]:
|
||||
"""给孔检测器用的别名接口（xyxy + conf）。"""
|
||||
if conf is not None:
|
||||
self.conf = float(conf)
|
||||
if iou is not None:
|
||||
self.iou = float(iou)
|
||||
if imgsz is not None:
|
||||
self.imgsz = int(imgsz)
|
||||
return self.infer(image_bgr)
|
||||
|
||||
def infer(self, image_bgr: np.ndarray, *, half: bool = False, seq: int = 0) -> List[Detection]:
|
||||
del seq
|
||||
if not self._warmed:
|
||||
self.warmup(half=half)
|
||||
results = self._predict(image_bgr)
|
||||
if not results:
|
||||
return []
|
||||
r0 = results[0]
|
||||
boxes = getattr(r0, "boxes", None)
|
||||
if boxes is None or len(boxes) == 0:
|
||||
return []
|
||||
|
||||
out: List[Detection] = []
|
||||
xyxy = boxes.xyxy.detach().cpu().numpy()
|
||||
confs = boxes.conf.detach().cpu().numpy()
|
||||
clss = boxes.cls.detach().cpu().numpy().astype(int)
|
||||
for i in range(len(xyxy)):
|
||||
cid = int(clss[i])
|
||||
# 用 LabelSet.get：读不到名字时给 class_N 占位，而不是退化成裸数字 "0"。
|
||||
name = self._labels.get(cid)
|
||||
if self.class_ids is not None and cid not in self.class_ids:
|
||||
continue
|
||||
if self.class_names is not None and name.lower() not in self.class_names:
|
||||
continue
|
||||
out.append(
|
||||
Detection(
|
||||
xyxy=xyxy[i].astype(float),
|
||||
conf=float(confs[i]),
|
||||
cls_id=cid,
|
||||
cls_name=name,
|
||||
)
|
||||
)
|
||||
out.sort(key=lambda d: d.conf, reverse=True)
|
||||
return out
|
||||
|
||||
def submit(self, image_bgr: np.ndarray, seq: int = 0) -> None:
|
||||
self._pending = (int(seq), image_bgr)
|
||||
|
||||
def wait_result(self, timeout: float = 0.045):
|
||||
del timeout
|
||||
if self._pending is None:
|
||||
return None
|
||||
seq, img = self._pending
|
||||
self._pending = None
|
||||
t0 = time.monotonic()
|
||||
dets = self.infer(img, seq=seq)
|
||||
return seq, dets, (time.monotonic() - t0) * 1000.0
|
||||
|
||||
|
||||
def build_detector(
|
||||
weights: Union[str, Path],
|
||||
*,
|
||||
backend: str = "auto",
|
||||
conf: float = 0.25,
|
||||
iou: float = 0.45,
|
||||
device: str = "",
|
||||
imgsz: int = 640,
|
||||
):
|
||||
"""按权重类型挑检测器：pose 模型走 ``PoseDetector``，否则走 ``YoloDetector``。
|
||||
|
||||
为什么要这个工厂：ultralytics 解不了 pose 的 TensorRT engine（见
|
||||
``pose_detector`` 模块头），所以 pose 必须换成自解码实现。判断依据是模型
|
||||
是不是 ``task=pose`` —— 直接读旁边 ``.onnx`` 的 metadata，不靠调用方声明。
|
||||
|
||||
返回对象统一有 ``detect()/infer()/warmup()/imgsz/class_names/class_ids``。
|
||||
"""
|
||||
path = Path(weights).expanduser()
|
||||
from .pose_detector import PoseDetector, is_pose_model
|
||||
|
||||
if is_pose_model(path):
|
||||
logger.info("检测到 pose 模型 %s → 用 PoseDetector（自解码关键点）", path.name)
|
||||
return PoseDetector(
|
||||
path,
|
||||
conf=conf,
|
||||
iou=iou,
|
||||
device=device or "0",
|
||||
imgsz=imgsz,
|
||||
)
|
||||
return YoloDetector(
|
||||
path,
|
||||
conf=conf,
|
||||
iou=iou,
|
||||
device=device,
|
||||
imgsz=imgsz,
|
||||
)
|
||||
|
||||
|
||||
def _yolo_worker_main(q_in, q_out, cfg: dict) -> None:
|
||||
"""spawn 出来的子进程：只跑 GPU 上的检测。这里绝不能 import rclpy。"""
|
||||
try:
|
||||
path = resolve_weights(cfg["weights"], cfg.get("backend") or "auto")
|
||||
det = build_detector(
|
||||
path,
|
||||
backend=cfg.get("backend") or "auto",
|
||||
conf=float(cfg.get("conf") or 0.25),
|
||||
iou=float(cfg.get("iou") or 0.45),
|
||||
device=str(cfg.get("device") or "0"),
|
||||
imgsz=int(cfg.get("imgsz") or 416),
|
||||
)
|
||||
det.warmup(half=bool(cfg.get("half", True)))
|
||||
q_out.put(("ready", str(path)))
|
||||
except Exception as exc: # noqa: BLE001
|
||||
q_out.put(("error", str(exc)))
|
||||
return
|
||||
while True:
|
||||
item = q_in.get()
|
||||
if item is None:
|
||||
return
|
||||
# 一直排空到队列里最新的帧 —— 绝不把 GPU 浪费在过期图像上。
|
||||
try:
|
||||
while True:
|
||||
newer = q_in.get_nowait()
|
||||
if newer is None:
|
||||
return
|
||||
item = newer
|
||||
except Exception:
|
||||
pass
|
||||
seq, image = item
|
||||
try:
|
||||
t0 = time.monotonic()
|
||||
dets = det.infer(image, half=bool(cfg.get("half", True)))
|
||||
gpu_ms = (time.monotonic() - t0) * 1000.0
|
||||
packed = [
|
||||
(
|
||||
d.xyxy.astype(np.float32),
|
||||
float(d.conf),
|
||||
int(d.cls_id),
|
||||
str(d.cls_name),
|
||||
None if d.kpts is None else np.asarray(d.kpts, dtype=np.float32),
|
||||
)
|
||||
for d in dets
|
||||
]
|
||||
_put_drop_old(q_out, ("ok", seq, packed, gpu_ms))
|
||||
except Exception as exc: # noqa: BLE001
|
||||
_put_drop_old(q_out, ("error", str(exc)))
|
||||
|
||||
|
||||
def _put_drop_old(q, item) -> None:
|
||||
"""绝不让 GPU 进程因为结果队列满了而阻塞。"""
|
||||
try:
|
||||
q.put_nowait(item)
|
||||
return
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
q.get_nowait()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
q.put_nowait(item)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
class YoloInferProcess:
|
||||
"""把 YOLO 放进 spawn() 子进程，让 ROS 回调永远不用和 PyTorch 抢 GIL。"""
|
||||
|
||||
def __init__(self, cfg: dict) -> None:
|
||||
self._cfg = dict(cfg)
|
||||
self._p = None
|
||||
self._start()
|
||||
|
||||
def _start(self) -> None:
|
||||
ctx = mp.get_context("spawn")
|
||||
# maxsize=2 + worker 排空到最新：父进程永远不需要主动丢帧
|
||||
# （之前 Queue put_nowait/get_nowait 的竞争，正是 worker 明明活着
|
||||
# 却出现数秒“静默”冻结的根源）。
|
||||
self._q_in = ctx.Queue(maxsize=2)
|
||||
self._q_out = ctx.Queue(maxsize=4)
|
||||
self._p = ctx.Process(
|
||||
target=_yolo_worker_main,
|
||||
args=(self._q_in, self._q_out, self._cfg),
|
||||
name="yolo_infer",
|
||||
daemon=True,
|
||||
)
|
||||
self._p.start()
|
||||
msg = self._q_out.get(timeout=90.0)
|
||||
if not msg or msg[0] != "ready":
|
||||
err = msg[1] if msg and len(msg) > 1 else "worker failed"
|
||||
raise RuntimeError(f"YOLO 工作进程：{err}")
|
||||
self.weights_path = str(msg[1])
|
||||
self.imgsz = int(self._cfg.get("imgsz") or 416)
|
||||
|
||||
def restart(self) -> None:
|
||||
"""杀掉卡死 / 挂掉的 worker，再起一个新的（重新加载 engine 约 2-5 s）。"""
|
||||
self.close()
|
||||
self._start()
|
||||
|
||||
@property
|
||||
def alive(self) -> bool:
|
||||
return bool(self._p is not None and self._p.is_alive())
|
||||
|
||||
def submit(self, image_bgr: np.ndarray, seq: int = 0) -> bool:
|
||||
"""把最新一帧排进队列。如果 worker 正忙就跳过 —— 它在每次推理后都会
|
||||
排空到队列里最新的帧，所以不会跑任何过期数据。
|
||||
|
||||
返回是否真的进队。进不去必须告诉调用方，否则一次性会以为「已经在跑」
|
||||
而空等，把 D405 也卡住。
|
||||
"""
|
||||
if not self.alive:
|
||||
raise RuntimeError("YOLO 工作进程已死亡")
|
||||
try:
|
||||
self._q_in.put((int(seq), image_bgr), timeout=0.010)
|
||||
return True
|
||||
except Exception:
|
||||
return False
|
||||
|
||||
def wait_result(self, timeout: float = 0.045):
|
||||
"""返回 ``(seq, list[Detection], gpu_ms)`` 或 ``None``。"""
|
||||
if not self.alive:
|
||||
raise RuntimeError("YOLO 工作进程已死亡")
|
||||
deadline = time.monotonic() + max(0.004, float(timeout))
|
||||
latest = None
|
||||
while time.monotonic() < deadline:
|
||||
try:
|
||||
msg = self._q_out.get(
|
||||
timeout=max(0.001, deadline - time.monotonic())
|
||||
)
|
||||
except Exception:
|
||||
break
|
||||
latest = self._parse_out(msg)
|
||||
while True:
|
||||
try:
|
||||
extra = self._q_out.get_nowait()
|
||||
except Exception:
|
||||
break
|
||||
parsed = self._parse_out(extra)
|
||||
if parsed is not None:
|
||||
latest = parsed
|
||||
if latest is not None:
|
||||
break
|
||||
return latest
|
||||
|
||||
def _parse_out(self, msg):
|
||||
if not msg:
|
||||
return None
|
||||
if msg[0] == "error":
|
||||
raise RuntimeError(str(msg[1]))
|
||||
if msg[0] != "ok":
|
||||
return None
|
||||
seq, packed = int(msg[1]), msg[2]
|
||||
gpu_ms = float(msg[3]) if len(msg) > 3 else 0.0
|
||||
dets = []
|
||||
for row in packed:
|
||||
# 兼容旧格式（4 元组，无关键点），免得子进程和父进程版本不一致时炸掉。
|
||||
xyxy, conf, cid, name = row[0], row[1], row[2], row[3]
|
||||
kpts = row[4] if len(row) > 4 else None
|
||||
dets.append(
|
||||
Detection(
|
||||
xyxy=np.asarray(xyxy, dtype=float),
|
||||
conf=float(conf),
|
||||
cls_id=int(cid),
|
||||
cls_name=str(name),
|
||||
kpts=None if kpts is None else np.asarray(kpts, dtype=float),
|
||||
)
|
||||
)
|
||||
return seq, dets, gpu_ms
|
||||
|
||||
def infer(self, image_bgr: np.ndarray, *, half: bool = False, seq: int = 0):
|
||||
"""兼容接口：提交 + 等待。要做流水线的话优先用 submit/wait_result。"""
|
||||
del half
|
||||
self.submit(image_bgr, seq)
|
||||
got = self.wait_result(timeout=0.08)
|
||||
if got is None:
|
||||
return INFER_BUSY
|
||||
return got[1]
|
||||
|
||||
def close(self) -> None:
|
||||
if self._p is None:
|
||||
return
|
||||
try:
|
||||
self._q_in.put_nowait(None)
|
||||
except Exception:
|
||||
pass
|
||||
self._p.join(timeout=2.0)
|
||||
if self._p.is_alive():
|
||||
self._p.terminate()
|
||||
self._p.join(timeout=1.0)
|
||||
if self._p.is_alive():
|
||||
self._p.kill() # 卡在 CUDA 调用里出不来 —— 直接 SIGKILL
|
||||
self._p.join(timeout=1.0)
|
||||
self._p = None
|
||||
|
||||
|
||||
def _iou_xyxy(a: np.ndarray, b: np.ndarray) -> float:
|
||||
ax1, ay1, ax2, ay2 = [float(v) for v in a]
|
||||
bx1, by1, bx2, by2 = [float(v) for v in b]
|
||||
ix1, iy1 = max(ax1, bx1), max(ay1, by1)
|
||||
ix2, iy2 = min(ax2, bx2), min(ay2, by2)
|
||||
inter = max(0.0, ix2 - ix1) * max(0.0, iy2 - iy1)
|
||||
ua = (
|
||||
max(0.0, ax2 - ax1) * max(0.0, ay2 - ay1)
|
||||
+ max(0.0, bx2 - bx1) * max(0.0, by2 - by1)
|
||||
- inter
|
||||
)
|
||||
return float(inter / ua) if ua > 1e-6 else 0.0
|
||||
|
||||
|
||||
def pick_best(
|
||||
dets: Sequence[Detection],
|
||||
*,
|
||||
prefer_center: Optional[tuple] = None,
|
||||
prev_uv: Optional[np.ndarray] = None,
|
||||
prev_xyxy: Optional[np.ndarray] = None,
|
||||
max_track_px: float = 320.0,
|
||||
reacquire_conf: float = 0.18,
|
||||
lock_iou: float = 0.08,
|
||||
min_conf: float = 0.12,
|
||||
min_area: float = 4000.0,
|
||||
) -> Optional[Detection]:
|
||||
"""让当前工件在快速运动下也能被持续跟踪。
|
||||
|
||||
用一个较宽的匹配门限去和**预测出的**上一帧检测框做匹配。如果 IoU 不达标、
|
||||
但 YOLO 仍以高于 min_conf 的置信度报出了目标，就保留该检测 —— 不要返回
|
||||
None（那正是运动过程中目标丢失的主要来源）。
|
||||
过小的框（logo / 通止规尖）会被拒掉，避免机械臂锁到垫子上。
|
||||
"""
|
||||
if not dets:
|
||||
return None
|
||||
# 已锁定的跟踪在运动（模糊）时可能掉到 min_conf 以下；但新锁定不允许这样。
|
||||
floor = max(0.06, 0.50 * float(min_conf))
|
||||
usable = [d for d in dets if float(d.conf) >= floor and float(d.area) >= float(min_area)]
|
||||
if not usable:
|
||||
return None
|
||||
|
||||
locked: List[Detection] = []
|
||||
if prev_xyxy is not None:
|
||||
pw = max(8.0, float(prev_xyxy[2] - prev_xyxy[0]))
|
||||
ph = max(8.0, float(prev_xyxy[3] - prev_xyxy[1]))
|
||||
gate = max(0.90 * float(max_track_px), 1.25 * float(np.hypot(pw, ph)))
|
||||
pcx = 0.5 * (float(prev_xyxy[0]) + float(prev_xyxy[2]))
|
||||
pcy = 0.5 * (float(prev_xyxy[1]) + float(prev_xyxy[3]))
|
||||
for d in usable:
|
||||
iou = _iou_xyxy(d.xyxy, prev_xyxy)
|
||||
dpx = float(np.hypot(d.cx - pcx, d.cy - pcy))
|
||||
if iou >= float(lock_iou) or dpx <= gate:
|
||||
locked.append(d)
|
||||
|
||||
if locked:
|
||||
return max(
|
||||
locked,
|
||||
key=lambda d: (float(d.conf) * (float(d.area) ** 0.25), float(d.conf)),
|
||||
)
|
||||
|
||||
def score_new(d: Detection) -> float:
|
||||
s = float(d.conf) * 10.0 + 0.015 * np.sqrt(max(float(d.area), 1.0))
|
||||
if prefer_center is not None:
|
||||
cx0, cy0 = float(prefer_center[0]), float(prefer_center[1])
|
||||
s -= 1e-6 * ((d.cx - cx0) ** 2 + (d.cy - cy0) ** 2)
|
||||
if prev_uv is not None:
|
||||
dpx = float(np.hypot(d.cx - float(prev_uv[0]), d.cy - float(prev_uv[1])))
|
||||
if dpx <= max_track_px:
|
||||
s += 2.0 * (1.0 - dpx / max(max_track_px, 1.0))
|
||||
return s
|
||||
|
||||
best = max(usable, key=score_new)
|
||||
# 快速移动：即使 YOLO 报出的框离上一帧很远，也保留这个实时检测。
|
||||
need = max(float(min_conf), float(reacquire_conf))
|
||||
if float(best.conf) >= need:
|
||||
return best
|
||||
return None
|
||||
Reference in New Issue
Block a user