arm_dev
883ed6b0b9
新增包
- arm_kinematics：从 arm_application 提取几何雅可比 + DLS 逆解（纯 numpy）。
它被应用层和驱动层共用，留在应用层会让驱动反向依赖应用层。
arm_application/geom_ik.py 改为 re-export shim，现有 import 一行未改。
- arm_driver_flexiv：非夕驱动适配器。只补 manifest 声明而现有节点缺失的接口
（/home /enable /clear_fault /switch_tool /soft_drag /compliant_descent），
已有能力不重复实现。RDK 访问经 rdk_py310 下的长驻桥，本包不 import flexivrdk。
新接口
- arm_msgs/SwitchTool.srv、arm_msgs/CompliantDescent.action
- 合并 capabilities 里语义重复的 set_tool / switch_tool
修复（都是只在真机上暴露、且难以定位的问题）
- rdk_client 会永久挂死：select 的"可读"不等于"有完整一行"，readline 遇到
半行 JSON 会一直阻塞等换行符，超时机制完全失效。改用 os.read + 自己做行缓冲。
- CompliantDescent 行程少一步：descended + step > max 在浮点累加下会提前跳出，
永远到不了标称行程。改用整数步数计数。
- CompliantDescentRunner 的 TF frame 退化成 _base_link：没继承节点已解析的 SN。
- resolve_orisys_config 仍指向旧路径；load_orisys_from_site 读的是旧键 config。
触觉四路数据打通
- 深度图 / 力矢量场原先被 tactile_worker 有意丢弃（注释写"图像不过队列"），
从未发布到 ROS。新增 image_utils（数组→Image，编码自动判定）、独立的图像
队列（maxsize=1 满则丢旧）与 /tactile/<side>/{image,vector}。
默认关闭（tactile.image_hz=0），开启也不影响三力的 30fps 时序。
- mock 后端也会发图像，无硬件即可验证整条发布链路。
配置
- site.yaml 新增 tactile.image_hz；移除与 model 重复的旧键 tactile.config
- 型号配置从 src/arm_application/config/orisys/ 迁到
devices/sensors/orisys/models/dd03/（随 SDK 一起放）
测试：198 个（原 139）
- arm_kinematics 24（雅可比/逆解的解析算例与不变量）
- arm_driver_flexiv 69（下压状态机 34 + RDK 协议层 35）
- arm_application 59（图像打包 36 + 矢量场探测 23）
51 lines
3.0 KiB
Markdown
51 lines
3.0 KiB
Markdown
# Orisys 视触觉
|
||
|
||
夹爪左右指上的视触觉相机（**不是**腕部 D405）。SDK 用厂家 wheel 安装
|
||
（`pip install --no-deps ~/sensor_ws/0.4.2/orisys-0.4.2-cp311-*.whl` 到 ros_flexiv 环境），
|
||
本仓库只放适配器、现场参数和 `dd03.json`。
|
||
|
||
源码：
|
||
|
||
| 文件 | 作用 |
|
||
|------|------|
|
||
| `arm_application/orisys_tactile.py` | SDK 适配 + `load_orisys_from_site` + `tactile_worker`（多进程工作器） |
|
||
| `arm_application/tactile_interface.py` | 可替换接口 / Mock |
|
||
| `arm_application/tactile_node.py` | 发布 `/tactile/left\|right/status`（三力），以及可选的 `/image`（深度图）与 `/vector`（力矢量场） |
|
||
| `arm_application/image_utils.py` | numpy 数组 → `sensor_msgs/Image` 的打包（编码自动判定） |
|
||
| `devices/sensors/orisys/models/dd03/config.json` | 传感器几何/跟踪（厂家配置，可进 git）。原先在 `src/arm_application/config/orisys/`，已迁到设备目录随 SDK 一起放 |
|
||
|
||
**四路数据怎么拿到**：三力在 `/tactile/<side>/status`（`TactileStatus` 的
|
||
`fnormal` / `fshearx` / `fsheary`），深度图在 `/tactile/<side>/image`，
|
||
力矢量场在 `/tactile/<side>/vector`（`32FC2`，每像素 dx/dy）。
|
||
|
||
后两路默认**关闭**：设 `site.yaml` 的 `tactile.image_hz > 0` 才发布。原因见下节 ——
|
||
图像不该和 30fps 的三力挤同一条队列。没有硬件时可以用
|
||
`ros2 run arm_application tactile --ros-args -p backend:=mock -p image_hz:=5.0`
|
||
把这两条话题跑起来验证链路。
|
||
|
||
**架构：每侧手指一个独立进程**（厂家示例 `two_sensor_pipeline.py` 的推荐做法，
|
||
每进程独立 CUDA 上下文）。实测单进程双开两路会互相拖垮（30→0.8fps），
|
||
双进程各 30fps。`tactile` 节点内部用 `multiprocessing(spawn)` 起两个 worker，
|
||
主进程只做队列→话题转发。worker 进程 comm 名 `orisys_tac_w`，runner 脚本会
|
||
连带清理（**孤儿 worker 会占死 /dev/video\***，表现为下次启动
|
||
"Failed to open video source"，手动清：`pkill orisys_tac_w`）。
|
||
|
||
现场要点（都在 **`config/site.yaml` → `tactile:`**）：
|
||
|
||
- `left_video/right_video` 用 **`/dev/v4l/by-path/...` 稳定路径**——Orisys 是
|
||
"USB Camera"（不是 RealSense！），video 编号随插拔漂移，by-path 绑定物理 USB 口
|
||
- `cuda: false` + `infer_backend: cpu`——双路并行抢 GPU 上下文会互相拖慢，
|
||
CPU 解算仅 ~9ms/帧足够（单路想快可开 cuda）
|
||
- 读数是**形变任意单位**（非牛顿）且夹持基线会漂移；三阶段节点启动时自动测
|
||
静止基线，所有力阈值都是相对增量（见 `docs/HOLE_DETECT.md` 三阶段段落）
|
||
|
||
```bash
|
||
bash ~/ros2/scripts/run_tactile.sh --cli # 打印力 若干秒
|
||
bash ~/ros2/scripts/run_tactile.sh # ROS 节点
|
||
```
|
||
|
||
无硬件时：`tactile.backend: mock`。
|
||
|
||
搜孔插入运动（机械臂螺旋 + 力闭环）已由三阶段实现：`stage3_search_node.py`，
|
||
见 `docs/HOLE_DETECT.md` 三阶段段落，跑 `bash ~/ros2/scripts/run_stage3_search.sh --arm 89`。
|