orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

注释与提示信息全面中文化：源码 66 个 .py + 脚本/配置 45 个文件

Browse Source 
背景：现场读代码的人只读中文。此前代码里的 docstring 是英文、行内注释是中文，
一半英文一半中文反而更难看懂 —— 新手打开一个 .py 不知道这个模块为什么存在。

范围（本仓库自己的代码，厂商上游不动）
  src/arm_application/**   37 个文件：servo_node / perception_node / handeye_* / dh_pgea /
                           yolo_detector / kalman_filter / goto_ee_plane / dual_gripper …
  src/arm_interfaces/**    11 个文件：paths / manifest / loader / config_guard / errors …
  src/arm_control/**        5 个文件
  src/arm_bringup/**        3 个 launch
  scripts/**                6 个 .py + 29 个 .sh（scripts/ 是"唯一操作入口"，用户屏幕上
                           看到的就是这些提示，所以提示信息也一并中文化）
  config/ handeye/ devices/ src/arm_*/config/   17 个 .yaml/.xml/.xacro/.cfg

口径（写进 docs/FRAMEWORK_RULES.md 新增的 R6.5，作为长期约束）
  注释、docstring、print/日志/异常里给人看的句子 = 中文；
  标识符、API 名、话题名、参数名、字段名、命令名 = 保留英文原文（用 `` `` 包起来）；
  单位、十六进制、协议报文、CRC 值、命令示例 = 原样保留；
  src/flexiv_*/ = 厂商上游代码，不许改也不许翻译（R4.4）。
  翻译保原意与原有详细程度：那些"为什么不能用另一种做法""踩过什么坑"的说明
  全部完整保留 —— 它们才是这份代码里最贵的部分。

新增 scripts/check_text_only.py：只改文字时的自证工具
  人工 review 靠不住（115 个文件、3000 多行 diff）。这个脚本把改动前后的源码解析成
  语法树，删掉 docstring、抹平字符串常量、把 f-string 只留"插值了几次 + 插值是什么"，
  再比对：完全相同 = 改动范围内没有逻辑变化。
  留了一个口子：f-string 的插值按内容排序后再比 —— 因为中文语序常常要和英文反着来
  （f"on {port} read {idx} failed" → f"在 {port} 上读 {idx} 失败"），只换显示顺序不该判失败；
  但多插一个、少插一个、换掉被插入的表达式，立刻会报出来。
  本次 66 个 .py 全部通过该闸门。

验收
  改动的 66 个 .py：scripts/check_text_only.py 全部通过（零逻辑变化）
  45 个非 .py 文件：去注释后与 HEAD 逐字节相同（零配置变化）
  docstring 总数：没有一个文件减少（不存在"翻译时顺手删了说明"）
  单测：arm_application 233 / arm_driver_flexiv 69 / arm_interfaces 46 /
        arm_kinematics 24 / arm_sim_gazebo 236 = 608 passed，与改动前一致
  全部 .sh：bash -n 通过；并额外实际执行了 --help 验证（见下一个提交：
        bash -n 对 CRLF 脚本会漏报，只有真跑才暴露）

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
2026-09-23 17:40:55 +08:00
parent d78e66ce98
commit 72353f3773
115 changed files with 1758 additions and 1573 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.cursor/rules/framework-core.mdc
+15
View File
@@ -34,6 +34,21 @@ alwaysApply: true
- 把"降级跳过"写成"通过"。
- 在中文路径下 `colcon build`。
- 提交信息只写"改了什么"而不写"为什么"（中文，附实测验收数据）。
- 在 `src/arm_*/**`、`scripts/**` 里写英文注释 / docstring / 提示信息（R6.5）。
注释和给人看的句子**一律中文**；标识符、话题名、字段名、命令名**保留英文原文**
（写在中文句子里时用 `` `` 包起来）。`src/flexiv_*/` 是厂商上游代码，不许翻译（R4.4）。
## 只改文字时，必须自证没碰逻辑
翻译注释、补文档这类"纯文本"改动最容易出事（漏掉 f-string 插值、顺手改个变量名），
而且人工 review 看不出来。提交前跑：
```bash
python3 scripts/check_text_only.py
```
它比对语法树（删 docstring + 抹平字符串常量 + f-string 只留插值），
不一致就报出来。基准是 `git HEAD`，所以要在提交前跑。
## 出问题先查症状表
config/fastdds_visual_servo.xml
+10 -10
View File
@@ -1,22 +1,22 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!--
FastDDS profile for visual-servo hard paths (Ubuntu 22.04 / Humble default RMW).
visual-servo 硬实时路径用的 FastDDS 配置（Ubuntu 22.04 / Humble 默认 RMW）。
Use:
用法：
export FASTRTPS_DEFAULT_PROFILES_FILE=~/ros2/config/fastdds_visual_servo.xml
export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
Goals:
- Prefer UDP unicast for low-latency control topics
- Shrink discovery / participant lease so late joiners do not stall 100 Hz
- Keep history shallow so subscribers never chase a backlog
目标：
- 低延迟的控制话题优先使用 UDP 单播
- 缩短发现 / 参与者租约，避免后加入的节点把 100 Hz 卡住
- 保持较浅的历史队列，让订阅者永远不必去追积压的旧数据
-->
<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
<transport_descriptors>
<transport_descriptor>
<transport_id>udp_transport</transport_id>
<type>UDPv4</type>
<!-- Avoid shared-memory backlog stalls between heavy perception and servo -->
<!-- 避免繁重的感知与伺服之间因共享内存积压而卡顿 -->
<maxMessageSize>65500</maxMessageSize>
</transport_descriptor>
</transport_descriptors>
@@ -29,7 +29,7 @@
<useBuiltinTransports>false</useBuiltinTransports>
<builtin>
<discovery_config>
<!-- Faster discovery; reduce lease so dead peers drop quickly -->
<!-- 加快发现；缩短租约，让已经死掉的节点尽快被剔除 -->
<leaseDuration>
<sec>10</sec>
<nanosec>0</nanosec>
@@ -50,7 +50,7 @@
</rtps>
</participant>
<!-- Perception image topics: BEST_EFFORT + KEEP_LAST (drop old frames) -->
<!-- 感知图像话题：BEST_EFFORT + KEEP_LAST（丢弃旧帧） -->
<data_reader profile_name="sensor_best_effort">
<qos>
<reliability>
@@ -66,7 +66,7 @@
</qos>
</data_reader>
<!-- /visual_servo/cmd_vel: always only the newest Twist -->
<!-- /visual_servo/cmd_vel：永远只保留最新的 Twist -->
<data_writer profile_name="cmd_vel_latest">
<qos>
<reliability>
docs/FRAMEWORK_RULES.md
+16
View File
@@ -351,6 +351,22 @@ ROS 构建对非 ASCII 路径会出各种问题。工作目录保持 ASCII（推
格式：`<阶段号> <一句话结果>` + 正文说明"问题是什么、为什么这么改、怎么验证的"。
修静默失效时，把"症状"写进去 —— 那是后来人搜索时唯一能匹配上的东西。
### R6.5 注释、docstring、提示信息一律用中文
现场操作的人读中文。所以本仓库（`src/arm_*/**`、`scripts/**`）里：
- **注释、docstring、`print`/日志/异常里给人看的句子 = 中文**
- **标识符、API 名、话题名、参数名、字段名、文件名、命令名 = 保留英文原文**，
写在中文句子里时用双反引号包起来，例如 ``SITE_ROS2_ROOT``、``devices/<kind>/<brand>/``
- 单位、十六进制、协议报文、CRC 值、寄存器地址、命令示例**原样保留**
**例外：`src/flexiv_*/` 是厂商上游代码，不许改、也不许翻译**（见 R4.4）。
只改文本的提交，必须能自证"没碰到逻辑"。做法：把新旧两份源码都解析成 AST，
删掉 docstring、把字符串常量抹平、把 f-string 只留"插值了几次 + 插值是什么"，
再比对 —— 必须完全相同。这条检查会抓出"顺手改名""顺手调整语句顺序""漏掉一个
f-string 插值"这类问题；而"为了中文语序把两个插值换个显示顺序"属于允许的改动。
---
## 7. 改动前的检查清单
handeye/calibrate.yaml
+4 -4
View File
@@ -1,8 +1,8 @@
# Hand-eye auto calibration config.
# 手眼自动标定配置。
#
# Workspace: ~/ros2
# 工作空间：~/ros2
#
# Teach poses, then solve (prefer the wrapper):
# 先示教位姿，再求解（推荐用封装脚本）：
# bash ~/ros2/scripts/run_handeye.sh teach
# bash ~/ros2/scripts/run_handeye.sh merge
# bash ~/ros2/scripts/run_handeye.sh calibrate
@@ -33,7 +33,7 @@ handeye_calibrate:
start_index: 0
min_poses: 5
# Camera on flange (eye-in-hand): D405
# 相机装在法兰上（eye-in-hand）：D405
camera_source: "d405" # d405 | d435 | manual
camera_serial: ""
camera_namespace: ""
handeye/poses/89_eye_in_hand.yaml
+16 -16
View File
@@ -1,38 +1,38 @@
# Hand-eye taught joint poses (7 joints × N). Shared by calibrate + waypoints.
# 手眼标定示教的关节位姿（7 关节 × N）。由 calibrate + waypoints 共用。
#
# Edit here, or:
# 在这里编辑，或者：
# bash ~/ros2/scripts/run_handeye.sh teach
# bash ~/ros2/scripts/run_handeye.sh merge
#
# /** so any node name receives these params.
# /** 这样任意节点名都能收到这些参数。
/**:
ros__parameters:
num_poses: 12
# Taught 2026-09-02 via handeye_teach (RDK free-drive)
# 2026-09-02 通过 handeye_teach 示教（RDK 自由驱动）
waypoint_positions: [
# pose 0
# 位姿 0
-0.751865, -0.769544, 0.713051, 1.140021, -0.189690, 0.133294, -0.342184,
# pose 1
# 位姿 1
-0.541318, -0.867448, 0.864868, 1.233428, -0.348623, 0.146046, -0.249719,
# pose 2
# 位姿 2
-0.343594, -0.948720, 0.848287, 1.273279, -0.954339, 0.019624, 0.071314,
# pose 3
# 位姿 3
-0.379884, -1.002193, 1.166394, 1.579194, -1.097644, -0.115142, -0.347640,
# pose 4
# 位姿 4
-0.694826, -0.943610, 0.820178, 0.963907, -0.427763, 0.165888, -0.761562,
# pose 5
# 位姿 5
-0.964023, -1.025531, 1.013898, 1.032490, -0.167151, 0.017429, -0.566073,
# pose 6
# 位姿 6
-0.812664, -0.984951, 1.072695, 1.473414, -0.772970, 0.267382, 0.401278,
# pose 7
# 位姿 7
-0.967527, -1.067191, 1.101680, 1.434183, -0.915388, 0.259486, 0.205330,
# pose 8
# 位姿 8
-0.451483, -1.257408, 0.895481, 0.806594, -0.652401, -0.221453, 0.184234,
# pose 9
# 位姿 9
-0.981877, -1.415295, 1.339004, 1.131348, -0.863194, 0.170952, 1.615403,
# pose 10
# 位姿 10
-0.967163, -1.377051, 1.337671, 1.399175, -1.241886, 0.284547, 1.927756,
# pose 11
# 位姿 11
-0.575271, -1.141574, 1.121935, 1.502172, -1.452029, 0.088544, 2.200019,
]
handeye/poses/91_eye_in_hand.yaml
+15 -15
View File
@@ -1,36 +1,36 @@
# Right arm 91 taught joints for hand-eye.
# Fill with:
# 右臂 91 手眼标定示教的关节位姿。
# 用以下命令填充：
# bash ~/ros2/scripts/run_handeye.sh --arm 91 teach
# bash ~/ros2/scripts/run_handeye.sh --arm 91 merge
#
# Do not reuse 89's waypoint_positions.
# 不要复用 89 的 waypoint_positions。
/**:
ros__parameters:
num_poses: 12
waypoint_positions: [
# pose 0
# 位姿 0
0.387144, -0.514770, -0.072695, 1.399918, 0.326501, 0.308501, -0.298212,
# pose 1
# 位姿 1
0.459205, -0.747691, 0.042380, 0.908430, 0.436933, -0.099413, -0.398336,
# pose 2
# 位姿 2
0.113162, -0.929267, -0.132969, 0.776759, 0.579499, -0.081271, -0.588715,
# pose 3
# 位姿 3
0.823056, -1.181803, -0.157161, 0.344860, 0.073450, -0.436449, -0.333484,
# pose 4
# 位姿 4
0.561660, 0.126479, -0.217063, 2.400061, 1.116363, 1.205544, -0.955972,
# pose 5
# 位姿 5
0.749011, 0.058548, -0.125615, 1.951037, 0.399065, 0.388437, -1.057397,
# pose 6
# 位姿 6
0.707611, -0.557010, -0.302672, 1.263980, 0.333093, 0.208229, -0.938372,
# pose 7
# 位姿 7
0.197846, -0.400530, 0.263064, 1.879643, 0.062586, 1.133715, -0.791285,
# pose 8
# 位姿 8
0.804027, -0.518162, 0.353947, 1.985246, -0.941195, 0.481238, -1.551113,
# pose 9
# 位姿 9
0.296161, -0.030159, 0.317117, 2.335652, 1.072158, 1.524893, -0.591372,
# pose 10
# 位姿 10
0.346492, -0.906418, -0.118014, 0.810448, 0.420788, -0.122104, -0.197859,
# pose 11
# 位姿 11
1.031802, -0.422436, -0.640939, 1.656629, 0.375163, 0.470365, -0.328041,
]
handeye/results/89_eye_in_hand.yaml
+2 -2
View File
@@ -1,5 +1,5 @@
# Left arm 89 eye-in-hand. Do not use for 91.
# Hand-eye result
# 左臂 89 eye-in-hand. 不要用于 91。
# 手眼标定结果
# mode: eye_in_hand
# camera_source: d405
# views: 12
handeye/waypoints.yaml
+3 -3
View File
@@ -1,7 +1,7 @@
# Hand-eye interactive waypoint tour (preview + Free Drive confirm).
# Poses: sibling handeye_poses.yaml
# 手眼标定交互式路点巡回（预览 + 自由驱动确认）。
# 位姿来源：同目录的 handeye_poses.yaml
#
# Run:
# 运行：
# bash ~/ros2/scripts/run_handeye.sh waypoints
handeye_waypoints:
scripts/README.md
+9
View File
@@ -27,6 +27,15 @@
- 注意 `run_sim.sh` 不是 Gazebo，它走 mock 驱动（假数据 + RViz），永远不会产生接触力
**环境：** `env.sh` · `build_all.sh`
**RDK：** `test_rdk_python.sh` · `rdk_switch_tool.sh` · `rdk_drag_bridge.py`
**改注释/写文档后自检：** `python3 scripts/check_text_only.py`
- 用途：证明这次改动**只动了文字**（注释、docstring、提示信息），没碰逻辑。
它把改动前后的源码解析成语法树，删掉 docstring、抹平字符串常量后比对；
"翻译 f-string 时漏掉一个 `{}` 插值""顺手改了个变量名"这类问题会被抓出来。
- `python3 scripts/check_text_only.py` 查所有改过的 `.py`；也可以直接跟文件名。
基准是 `git HEAD`，所以要在**提交之前**跑。老手滑手滑的代价比跑一条命令高得多。
- 规矩写在 [docs/FRAMEWORK_RULES.md](../docs/FRAMEWORK_RULES.md) 的 R6.5。
一句话：**注释/docstring/提示信息用中文；标识符、话题名、字段名保留英文**
（但 `src/flexiv_*/` 是厂商上游代码，不许翻译，见 R4.4）。
**搁置未用：** `fetch_isaac_flexiv.sh`（拉 Flexiv 的 Isaac Sim / RDK 源码到 `thirdparty/`；Isaac 路线目前搁置，Gazebo 是选定平台）
`lib/parse_arm_args.sh` 解析 `--arm 89|91`。
scripts/build_all.sh
+1 -1
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env bash
# Build entire workspace: arm stack + flexiv real drivers
# 构建整个工作区：机械臂栈 + Flexiv 真机驱动
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
scripts/build_flexiv_rdk.sh
+2 -2
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env bash
# Build Flexiv RDK v1.x into ~/flexiv_install (required for real robot / flexiv_hardware).
# 把 Flexiv RDK v1.x 构建安装到 ~/flexiv_install（真机 / `flexiv_hardware` 必需）。
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
RDK="${ROOT}/src/flexiv_rdk"
@@ -11,7 +11,7 @@ if [[ ! -d "$RDK" ]]; then
fi
echo "Building RDK thirdparty deps into $PREFIX ..."
# Prefer system /opt/ros if present; else conda env already has compilers
# 有系统 /opt/ros 就优先用它；否则 conda 环境里已经有编译器
if [[ -f /opt/ros/humble/setup.bash ]]; then
# shellcheck disable=SC1091
source /opt/ros/humble/setup.bash
scripts/build_real.sh
+1 -1
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env bash
# Build Flexiv hardware packages for real robot (after RDK installed to ~/flexiv_install)
# 构建真机用的 Flexiv 硬件包（前提：RDK 已装到 ~/flexiv_install）
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
scripts/check_robot_network.sh
+1 -1
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env bash
# Check PC ↔ Flexiv robot network (addresses from site.yaml)
# 检查 PC ↔ Flexiv 机器人网络（地址取自 site.yaml）
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
scripts/check_text_only.py
+170
View File
@@ -0,0 +1,170 @@
#!/usr/bin/env python3
"""纯文本改动的安全闸门：证明一次改动**只动了文字**，没碰逻辑。
=============================================================================
为什么需要这个工具
=============================================================================
"把英文注释翻成中文""补一批说明文档"这类改动，看起来零风险，其实经常出事：
改注释的人手滑动了变量名、顺手调了语句顺序、翻译 f-string 时漏掉一个 ``{}`` 插值。
这些都不会报错，只在某天变成一句"结果怎么不对了"。
人工 review 靠不住（几十个文件、几千行 diff，没人真的逐行看）。
所以这里用 AST 做机器判据：**把两份源码解析成语法树，抹掉所有文字，再比对。**
抹掉规则：
1. 删掉所有 docstring
2. 所有字符串常量换成同一个占位符
3. f-string 只保留"插值了几次、插值的是什么表达式"，
字面量部分整段丢掉、插值按内容排序
第 3 条是为中文语序专门留的口子。英文 ``f"on {port} read {idx} failed"``
翻成 ``f"在 {port} 上读 {idx} 失败"`` 只是把两个插值的**显示顺序**换了，
语义没变，不该判失败。但只要有人**多插一个、少插一个、或换掉被插入的表达式**，
这里立刻报出来。
=============================================================================
用法
=============================================================================
python3 scripts/check_text_only.py # 检查所有改过的 .py
python3 scripts/check_text_only.py <文件> [...] # 只检查指定文件
python3 scripts/check_text_only.py --staged # 只检查已 git add 的
比较基准固定为 ``git HEAD``（也就是"上一次提交"），所以要用在**提交之前**。
退出码 0 = 改动范围内没有逻辑变化；非 0 = 有文件动了不该动的东西（会打印差异）。
=============================================================================
"""
from __future__ import annotations
import argparse
import ast
import difflib
import subprocess
import sys
from pathlib import Path
REPO_ROOT = Path(__file__).resolve().parents[1]
# 被抹平后的字符串统一长这样。用控制字符是为了保证它不会和真实内容撞车。
PLACEHOLDER = "\0STR\0"
class _Normalize(ast.NodeTransformer):
"""把语法树里的"文字"全部抹掉，只留下结构与标识符。"""
def _strip_docstring(self, node):
"""删掉模块 / 类 / 函数的第一个字符串表达式（docstring）。"""
self.generic_visit(node)
body = getattr(node, "body", None)
if body and isinstance(body[0], ast.Expr) and isinstance(body[0].value, ast.Constant) \
and isinstance(body[0].value.value, str):
node.body = body[1:]
return node
visit_Module = _strip_docstring
visit_FunctionDef = _strip_docstring
visit_AsyncFunctionDef = _strip_docstring
visit_ClassDef = _strip_docstring
def visit_Constant(self, node: ast.Constant):
if isinstance(node.value, str):
return ast.copy_location(ast.Constant(value=PLACEHOLDER), node)
return node
def visit_JoinedStr(self, node: ast.JoinedStr):
"""f-string：只留插值，不留字面量；插值按内容排序（见模块 docstring）。"""
self.generic_visit(node)
parts = [ast.dump(v) for v in node.values if isinstance(v, ast.FormattedValue)]
return ast.copy_location(
ast.Constant(value="FSTR[" + "|".join(sorted(parts)) + "]"), node)
def normalize(source: str) -> str:
"""源码 -> 抹掉文字后的规范化结构字符串。"""
return ast.dump(_Normalize().visit(ast.parse(source)))
def _git(*args: str) -> subprocess.CompletedProcess:
return subprocess.run(["git", *args], capture_output=True, text=True, cwd=REPO_ROOT)
def head_source(path: str) -> str | None:
"""取该文件在 HEAD 里的内容；返回 None 表示 HEAD 里没有它（新文件）。"""
r = _git("show", f"HEAD:{path}")
return r.stdout if r.returncode == 0 else None
def changed_python_files() -> list[str]:
r = _git("diff", "--name-only", "HEAD")
return [p for p in r.stdout.split() if p.endswith(".py")]
def staged_python_files() -> list[str]:
r = _git("diff", "--cached", "--name-only")
return [p for p in r.stdout.split() if p.endswith(".py")]
def first_diff(old: str, new: str) -> str:
"""给出一段人能看懂的差异上下文。"""
matcher = difflib.SequenceMatcher(None, old, new, autojunk=False)
for tag, i1, i2, j1, j2 in matcher.get_opcodes():
if tag != "equal":
return (f" 结构差异类型: {tag}\n"
f" 改前: ...{old[max(0, i1 - 70):i2 + 70]}...\n"
f" 改后: ...{new[max(0, j1 - 70):j2 + 70]}...")
return " （未找到差异片段——理论上不该走到这里）"
def main() -> int:
ap = argparse.ArgumentParser(
description="证明一次改动只动了注释/字符串，没有改逻辑",
)
ap.add_argument("paths", nargs="*", help="要检查的文件；留空 = 所有改过的 .py")
ap.add_argument("--staged", action="store_true", help="只检查已 git add 的文件")
args = ap.parse_args()
paths = args.paths or (staged_python_files() if args.staged else changed_python_files())
if not paths:
print("没有需要检查的 .py 文件（工作区是干净的？）。")
return 0
ok, problems, skipped = 0, [], []
for path in paths:
disk = REPO_ROOT / path
if not disk.is_file():
skipped.append((path, "工作区里不存在（被删除了？）"))
continue
old = head_source(path)
if old is None:
skipped.append((path, "HEAD 里没有它（新增文件），没有基准可比"))
continue
try:
same = normalize(old) == normalize(disk.read_text(encoding="utf-8"))
except SyntaxError as exc:
problems.append((path, f" 语法错误，先修好再看：{exc}"))
continue
if same:
ok += 1
else:
problems.append((path, first_diff(normalize(old),
normalize(disk.read_text(encoding="utf-8")))))
print(f"通过（只改了文字）: {ok}")
for path, why in skipped:
print(f"跳过 {path} —— {why}")
if problems:
print(f"\n!! 有 {len(problems)} 个文件动了文字之外的东西：")
for path, why in problems:
print(f" {path}")
print(why)
print("\n注意：如果差异只出现在 f-string 的插值顺序上，那是允许的（见脚本说明）。")
return 1
print("\n闸门通过：改动范围内没有逻辑变化。")
return 0
if __name__ == "__main__":
sys.exit(main())
scripts/convert_flexiv_traj.py
+15 -15
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""Convert Flexiv Elements .traj (text) -> arm_application YAML waypoints."""
"""把 Flexiv Elements 的 .traj（文本）转换成 arm_application 的 YAML 路点。"""
from __future__ import annotations
@@ -12,7 +12,7 @@ from typing import Any
def parse_flexiv_traj(text: str) -> list[dict[str, Any]]:
"""Parse Elements Studio scatter-point .traj file."""
"""解析 Elements Studio 导出的散点 .traj 文件。"""
blocks = re.split(r'pattern_params\s*\{', text)[1:]
waypoints: list[dict[str, Any]] = []
for block in blocks:
@@ -35,7 +35,7 @@ def parse_flexiv_traj(text: str) -> list[dict[str, Any]]:
def assign_times(waypoints: list[dict[str, Any]], min_seg: float = 3.0) -> None:
"""Cumulative time_from_start from joint-space segment length."""
"""按关节空间的段长累加出 time_from_start。"""
t = 0.0
prev = None
for wp in waypoints:
@@ -44,7 +44,7 @@ def assign_times(waypoints: list[dict[str, Any]], min_seg: float = 3.0) -> None:
t += min_seg
else:
delta = max(abs(a - b) for a, b in zip(pos, prev))
# ~4 s per radian max joint move, floored at min_seg
# 关节最大运动量每弧度约 4 秒，且不低于 min_seg
t += max(min_seg, delta * 4.0)
wp['time'] = round(t, 2)
prev = pos
@@ -65,17 +65,17 @@ def to_yaml(
pos_line = ', '.join(f'{v:.6f}' for v in flat)
time_line = ', '.join(str(t) for t in times)
names = ', '.join(f' # {wp.get("name", "")}' if i == 0 else '' for i, wp in enumerate(waypoints))
_ = names # names kept in comments below
_ = names # 名称保留在下面的注释里
lines = [
'# Auto-converted from Flexiv Elements .traj (joints: deg -> rad)',
'# ROS params: flat waypoint_positions + waypoint_times (no nested dicts)',
'# Edit velocity_scale / loop_count as needed (no rebuild).',
'# 由 Flexiv Elements .traj 自动转换而来（关节角：度 -> 弧度）',
'# ROS 参数：扁平的 waypoint_positions + waypoint_times（不使用嵌套字典）',
'# 按需修改 velocity_scale / loop_count（无需重新编译）。',
'run_joint_trajectory:',
' ros__parameters:',
f' joint_prefix: "{joint_prefix}"',
' joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]',
f' velocity_scale: {velocity_scale}',
' loop_count: 1 # 1=once, 0=forever, N=repeat N',
' loop_count: 1 # 1=执行一次，0=无限循环，N=重复 N 次',
' pause_between_loops: 0.5',
f' waypoint_times: [{time_line}]',
' waypoint_positions: [',
@@ -89,16 +89,16 @@ def to_yaml(
def main() -> int:
ap = argparse.ArgumentParser(description=__doc__)
ap.add_argument('input', type=Path, help='Flexiv Elements .traj file')
ap.add_argument('-o', '--output', type=Path, help='Output YAML path')
ap.add_argument('--prefix', default='Rizon4-063989_', help='Joint name prefix (site SN + _)')
ap.add_argument('--min-seg', type=float, default=3.0, help='Min seconds per segment')
ap.add_argument('input', type=Path, help='Flexiv Elements 的 .traj 文件')
ap.add_argument('-o', '--output', type=Path, help='输出的 YAML 路径')
ap.add_argument('--prefix', default='Rizon4-063989_', help='关节名前缀（现场 SN + _）')
ap.add_argument('--min-seg', type=float, default=3.0, help='每段的最小秒数')
args = ap.parse_args()
text = args.input.read_text(encoding='utf-8')
wps = parse_flexiv_traj(text)
if not wps:
print('ERROR: no waypoints parsed', file=sys.stderr)
print('错误：没有解析到任何路点', file=sys.stderr)
return 1
assign_times(wps, min_seg=args.min_seg)
yaml_text = to_yaml(wps, joint_prefix=args.prefix)
@@ -107,7 +107,7 @@ def main() -> int:
if out:
out.parent.mkdir(parents=True, exist_ok=True)
out.write_text(yaml_text, encoding='utf-8')
print(f'Wrote {len(wps)} waypoints -> {out}')
print(f'已写入 {len(wps)} 个路点 -> {out}')
else:
print(yaml_text)
return 0
scripts/convert_traj.sh
+2 -2
View File
@@ -1,6 +1,6 @@
#!/usr/bin/env bash
# Convert Flexiv Elements .traj → arm_application YAML (standard entry for all trajectories).
# Usage:
# 把 Flexiv Elements 的 .traj 转成 `arm_application` 的 YAML（所有轨迹的统一入口）。
# 用法：
# ./scripts/convert_traj.sh ~/文档/my_task.traj
# ./scripts/convert_traj.sh ~/文档/my_task.traj -o /path/to/out.yaml
# ./scripts/convert_traj.sh ~/文档/my_task.traj --min-seg 2.5 --no-copy
scripts/env.sh
+4 -4
View File
@@ -1,6 +1,6 @@
#!/usr/bin/env bash
# Source this workspace environment (conda + ROS + flexiv libs).
# Usage: source scripts/env.sh
# 加载本工作区环境（conda + ROS + flexiv 库）。
# 用法：source scripts/env.sh
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
if [[ -f /opt/ros/humble/setup.bash ]]; then
@@ -33,7 +33,7 @@ fi
# shellcheck disable=SC1091
source "${ROOT}/install/setup.bash"
export LD_LIBRARY_PATH="${HOME}/flexiv_install/lib:${LD_LIBRARY_PATH:-}"
# Pip TensorRT (tensorrt-cu12) — needed to load YOLO .engine
# pip 版 TensorRT（tensorrt-cu12）—— 加载 YOLO `.engine` 需要它
_TRT_SITE="${CONDA_PREFIX:-${HOME}/anaconda3/envs/ros_flexiv}/lib/python3.11/site-packages"
for _d in "${_TRT_SITE}/tensorrt" "${_TRT_SITE}/tensorrt_libs" "${_TRT_SITE}/nvidia/tensorrt/lib"; do
if [[ -d "${_d}" ]]; then
@@ -44,6 +44,6 @@ unset _d _TRT_SITE
export YOLO_AUTOINSTALL=false
export ROS2_WS="${ROOT}"
# Master site config → SITE_* (YOLO weights, camera, robot SN, …)
# 主站点配置 → `SITE_*`（YOLO 权重、相机、机器人 SN……）
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true
scripts/fetch_flexiv.sh
+4 -4
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env bash
# Clone official Flexiv ROS 2 stack for Rizon (RDK v1.x → humble-v1 branch).
# 克隆 Flexiv 官方 ROS 2 栈（Rizon 用，RDK v1.x → humble-v1 分支）。
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
@@ -9,7 +9,7 @@ BRANCH="${FLEXIV_ROS2_BRANCH:-humble-v1}"
mkdir -p "${SRC}"
cd "${SRC}"
# Always ensure official URDF package is present (meshes + xacro)
# 无论如何都要保证官方 URDF 包存在（mesh + xacro）
if [[ ! -d flexiv_description ]]; then
echo "Fetching flexiv_description (${BRANCH})..."
if ! git clone --depth 1 -b "${BRANCH}" https://github.com/flexivrobotics/flexiv_description.git; then
@@ -58,7 +58,7 @@ if ! command -v vcs >/dev/null 2>&1; then
exit 1
fi
# Prefer local Flexiv_Rizon4 RDK (avoids GitHub clone when present)
# 优先用本地 Flexiv_Rizon4 里的 RDK（有本地副本时就不去 GitHub 克隆）
LOCAL_FLEXIV="${LOCAL_FLEXIV:-$HOME/文档/Flexiv_Rizon4}"
if [[ -d "${LOCAL_FLEXIV}/flexiv_rdk" ]]; then
echo "Using local RDK: ${LOCAL_FLEXIV}/flexiv_rdk"
@@ -73,7 +73,7 @@ else
fi
fi
# Always import remaining deps from repos (description already handled; skip-existing)
# 其余依赖一律从 .repos 导入（description 已单独处理；skip-existing 跳过已有）
vcs import "${SRC}" < "${REPOS_FILE}" --recursive --skip-existing || true
cat <<EOF
scripts/fetch_isaac_flexiv.sh
+2 -2
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env bash
# Clone Flexiv Isaac Sim integration repos into thirdparty/
# 把 Flexiv Isaac Sim 集成仓库克隆到 thirdparty/
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
@@ -23,7 +23,7 @@ clone_or_update https://github.com/flexivrobotics/flexiv_sim_plugin.git flexiv_s
clone_or_update https://github.com/flexivrobotics/isaac_sim_ws.git isaac_sim_ws
clone_or_update https://github.com/flexivrobotics/flexiv_description.git flexiv_description
# Optional: RDK sources next to sim (if not already under src/)
# 可选：在仿真目录旁放一份 RDK 源码（如果 src/ 下还没有）
if [[ ! -d "${ROOT}/src/flexiv_rdk" ]]; then
clone_or_update https://github.com/flexivrobotics/flexiv_rdk.git flexiv_rdk
fi
scripts/fix_robot_lan.sh
+2 -2
View File
@@ -15,7 +15,7 @@ source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true
PC_IP_PRIMARY="${SITE_PC_IP:-192.168.2.10}"
PC_ADDRS=("192.168.2.10/24" "192.168.100.10/24")
ROBOT_IPS=("${SITE_ROBOT_IP:-192.168.2.100}")
# Catalog extras (second arm)
# catalog 里的额外项（第二条臂）
if [[ -n "${SITE_ROBOT_CATALOG:-}" ]]; then
:
fi
@@ -132,7 +132,7 @@ for ip in 192.168.2.100 192.168.101.1 192.168.2.101 "${SITE_ROBOT_IP:-}"; do
[[ -z "$ip" ]] && continue
ping_one "$ip" && ok=1 || true
done
# unique
# 去重
echo
if ping -c 1 -W 1 "${SITE_ROBOT_IP:-192.168.2.100}" >/dev/null 2>&1; then
echo "OK — 当前臂 ${SITE_ROBOT_SN:-} @ ${SITE_ROBOT_IP} 可达。RDK 用 SN 连接。"
scripts/gen_site_example.py
+1 -1
View File
@@ -88,7 +88,7 @@ REGEX_REPLACEMENTS: list[tuple[str, str]] = [
def render(site_text: str) -> str:
"""Turn the live config text into the anonymised template text."""
"""把现场配置文本转换成匿名化后的模板文本。"""
out = site_text
for old, new in LITERAL_REPLACEMENTS:
out = out.replace(old, new)
scripts/go_home.sh
+15 -15
View File
@@ -1,10 +1,10 @@
#!/usr/bin/env bash
# Move Rizon4 to a named pose (default: home) and FORCE stop.
# 把 Rizon4 移动到指定命名的位姿（默认：home）并强制停止。
#
# Critical: visual_servo.launch / servo_node must be killed first, otherwise
# they keep streaming JointTrajectory and the arm never settles.
# 关键：必须先杀掉 visual_servo.launch / servo_node，否则
# 它们会一直发 JointTrajectory，机械臂永远稳定不下来。
#
# Usage:
# 用法：
# bash ~/ros2/scripts/go_home.sh
# bash ~/ros2/scripts/go_home.sh --pose ready --speed 0.3
@@ -39,7 +39,7 @@ while [[ $# -gt 0 ]]; do
--pose) POSE="${2:?}"; shift 2 ;;
--speed) SPEED="${2:?}"; shift 2 ;;
--keep-servo) KEEP_SERVO=1; shift ;;
--stop-track) shift ;; # legacy
--stop-track) shift ;; # 历史遗留
-h|--help) usage; exit 0 ;;
*) echo "Unknown: $1" >&2; usage >&2; exit 1 ;;
esac
@@ -56,10 +56,10 @@ SPEED="${SPEED:-${SITE_GO_HOME_SPEED:-0.25}}"
kill_streamers() {
echo "=== killing visual-servo streamers ==="
# Prefer disable control before kill (if node still up)
# 能先禁用控制就禁用（如果节点还活着），再动手 kill
ros2 param set /servo_node enable_control false >/dev/null 2>&1 || true
# Kill launch parent first, then executables (SIGTERM then SIGKILL)
# 先杀 launch 父进程，再杀可执行文件（先 SIGTERM 再 SIGKILL）
local patterns=(
'visual_servo.launch.py'
'/lib/arm_application/servo_node'
@@ -115,7 +115,7 @@ n.destroy_node(); rclpy.shutdown()
PY
}
# Strong settle via controller ACTION (not topic — matches JTC reliably)
# 用控制器的 ACTION 做强力稳定（不走话题 —— 这样能可靠地匹配 JTC）
settle_hold_action() {
local pose="$1"
python3 - "$pose" <<'PY'
@@ -160,7 +160,7 @@ def spin_until(pred, timeout=3.0):
spin_until(lambda: js_box[0] is not None, 3.0)
# Two-point: current -> target (or target hold), BOTH with zero velocity
# 两点轨迹：当前位姿 -> 目标位姿（或保持在目标位姿），两点速度都为零
q_now = list(target)
if js_box[0] is not None:
m = dict(zip(js_box[0].name, js_box[0].position))
@@ -172,14 +172,14 @@ if js_box[0] is not None:
def send_hold(q, T=0.40):
traj = JointTrajectory()
traj.joint_names = list(JOINTS)
# start
# 起点
p0 = JointTrajectoryPoint()
p0.positions = [float(v) for v in q]
p0.velocities = [0.0] * 7
p0.accelerations = [0.0] * 7
p0.time_from_start.sec = 0
p0.time_from_start.nanosec = 50_000_000
# end same pose, still zero vel (pin)
# 终点是同一姿势，速度仍为零（钉住）
p1 = JointTrajectoryPoint()
p1.positions = [float(v) for v in q]
p1.velocities = [0.0] * 7
@@ -203,11 +203,11 @@ def send_hold(q, T=0.40):
spin_until(lambda: rfut.done(), T + 3.0)
return True
# 1) snap toward named pose with zero end vel
# 1) 先朝命名位姿快速靠拢，末端速度为零
print(f" action hold → {name} (zero vel)")
send_hold(target, T=0.60)
time.sleep(0.4)
# 2–4) re-pin measured pose several times (align JTC open-loop cmd to encoder)
# 2–4) 反复把实测位姿重新钉住（让 JTC 的开环指令与编码器读数对齐）
for i in range(3):
spin_until(lambda: js_box[0] is not None, 1.0)
if js_box[0] is None:
@@ -218,7 +218,7 @@ for i in range(3):
send_hold(q_pin, T=0.45)
time.sleep(0.25)
# Wait until |qd| small
# 等到 |qd| 足够小
print(" waiting for joints to stop ...")
t0 = time.monotonic()
while time.monotonic() - t0 < 5.0:
@@ -270,7 +270,7 @@ sleep "${WAIT_SEC}"
echo "=== settle (action, vel=0) ==="
settle_hold_action "${POSE}"
# Final check: servo must still be dead
# 最后确认：servo 必须仍然是死的
if pgrep -f '/lib/arm_application/servo_node' >/dev/null 2>&1; then
echo "WARNING: servo_node came back — killing again"
kill_streamers
scripts/install_rdk_offline.sh
+3 -3
View File
@@ -1,6 +1,6 @@
#!/usr/bin/env bash
# Offline-friendly Flexiv RDK v1.9.1 install for ROS (humble-v1 / flexiv_hardware).
# CMakeLists already supports FLEXIV_RDK_OFFLINE_LIB / vendor/rdk_libs/.
# 面向离线场景的 Flexiv RDK v1.9.1 安装脚本（供 ROS 的 humble-v1 / `flexiv_hardware` 使用）。
# CMakeLists 已经支持 `FLEXIV_RDK_OFFLINE_LIB` / `vendor/rdk_libs/`。
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
@@ -11,7 +11,7 @@ EXPECTED_HASH="$(tr -d '[:space:]' < "${RDK_SRC}/lib/${LIB_NAME}.sha256")"
VENDOR_LIB="${ROOT}/vendor/rdk_libs/${LIB_NAME}"
JOBS="${JOBS:-$(nproc)}"
# Prefer conda ros_flexiv toolchain (cmake/g++ often only there)
# 优先用 conda 的 ros_flexiv 工具链（cmake/g++ 往往只在那里有）
if ! command -v cmake >/dev/null 2>&1; then
# shellcheck disable=SC1091
source "${HOME}/anaconda3/etc/profile.d/conda.sh" 2>/dev/null \
scripts/lib/export_site_env.py
+43 -43
View File
@@ -1,7 +1,7 @@
#!/usr/bin/env python3
"""Load ~/ros2/config/site.yaml and print bash export lines or JSON.
"""读取 ``~/ros2/config/site.yaml``，打印 bash 的 export 行或 JSON。
Usage:
用法:
eval "$(python3 ~/ros2/scripts/lib/export_site_env.py)"
python3 ~/ros2/scripts/lib/export_site_env.py --json
python3 ~/ros2/scripts/lib/export_site_env.py --print
@@ -35,18 +35,18 @@ def site_path() -> Path:
def load_site(path: Path | None = None) -> dict:
path = path or site_path()
if not path.is_file():
raise FileNotFoundError(f"site config not found: {path}")
raise FileNotFoundError(f"找不到 site 配置: {path}")
text = path.read_text(encoding="utf-8")
if yaml is None:
raise RuntimeError("PyYAML required: pip/conda install pyyaml")
raise RuntimeError("需要 PyYAML：pip/conda install pyyaml")
data = yaml.safe_load(text) or {}
if not isinstance(data, dict):
raise ValueError(f"invalid site.yaml: {path}")
raise ValueError(f"无效的 site.yaml: {path}")
return data
def list_robots(data: dict) -> list[dict]:
"""Catalog entries from robot.robots (list or SN-keyed map)."""
"""从 robot.robots 取清单条目（列表或按 SN 索引的映射）。"""
raw = (data.get("robot") or {}).get("robots") or []
out: list[dict] = []
if isinstance(raw, dict):
@@ -90,7 +90,7 @@ def _deep_merge(base: dict, overlay: dict) -> dict:
def normalize_robot_sn(data: dict, want: str) -> str:
"""Map 89/91/left/right/label → full Flexiv SN."""
"""把 89/91/left/right/label 映射成完整的 Flexiv SN。"""
w = (want or "").strip()
if not w:
return w
@@ -109,7 +109,7 @@ def normalize_robot_sn(data: dict, want: str) -> str:
def resolve_robot(data: dict, sn: str | None = None) -> dict:
"""Active arm: ROBOT_SN env overrides robot.sn; catalog fills ip/iface/frames."""
"""当前生效的机械臂：环境变量 ``ROBOT_SN`` 覆盖 robot.sn；清单补齐 ip/iface/frames。"""
robot = dict(data.get("robot") or {})
catalog = list_robots(data)
want = normalize_robot_sn(
@@ -146,7 +146,7 @@ def resolve_robot(data: dict, sn: str | None = None) -> dict:
def apply_active_arm(data: dict, sn: str | None = None) -> tuple[dict, dict]:
"""Merge robots[] camera/gripper/paths/servo onto site defaults for the active SN."""
"""把 robots[] 里的 camera/gripper/paths/servo 合并到当前 SN 对应的 site 默认值上。"""
robot = resolve_robot(data, sn)
merged = dict(data)
for key in _ARM_SECTION_KEYS:
@@ -157,14 +157,14 @@ def apply_active_arm(data: dict, sn: str | None = None) -> tuple[dict, dict]:
def camera_ns(data: dict) -> str:
"""Topic namespace for the active wrist camera (d405_left / d405_right)."""
"""当前腕部相机（d405_left / d405_right）的话题命名空间。"""
cam = data.get("camera") or {}
name = str(cam.get("name") or "d405").strip().strip("/")
return name or "d405"
def camera_frames(data: dict) -> tuple[str, str, str]:
"""(namespace, optical_frame, depth_topic) for the active arm."""
"""当前机械臂的（命名空间, 光学坐标系, 深度话题）。"""
ns = camera_ns(data)
yolo = data.get("yolo") or {}
frame = str(yolo.get("camera_frame") or "").strip()
@@ -177,7 +177,7 @@ def camera_frames(data: dict) -> tuple[str, str, str]:
def flatten(data: dict) -> dict:
"""Flat SITE_* env map for bash scripts."""
"""供 bash 脚本使用的扁平 ``SITE_*`` 环境变量映射。"""
data, robot = apply_active_arm(data)
paths = data.get("paths") or {}
cam = data.get("camera") or {}
@@ -341,7 +341,7 @@ def flatten(data: dict) -> dict:
def gripper_overrides(data: dict) -> dict:
"""ROS parameter dict for dh_pgea_node (DH Modbus, not Flexiv gripper)."""
"""供 dh_pgea_node 使用的 ROS 参数字典（DH Modbus，不是 Flexiv 夹爪）。"""
data, _robot = apply_active_arm(data)
g = data.get("gripper") or {}
return {
@@ -362,7 +362,7 @@ def gripper_overrides(data: dict) -> dict:
def perception_overrides(data: dict) -> dict:
"""ROS parameter dict for perception_node."""
"""供 perception_node 使用的 ROS 参数字典。"""
data, _robot = apply_active_arm(data)
paths = data.get("paths") or {}
yolo = data.get("yolo") or {}
@@ -415,7 +415,7 @@ def perception_overrides(data: dict) -> dict:
def servo_overrides(data: dict) -> dict:
"""ROS parameter dict for servo_node."""
"""供 servo_node 使用的 ROS 参数字典。"""
data, robot = apply_active_arm(data)
paths = data.get("paths") or {}
yolo = data.get("yolo") or {}
@@ -431,7 +431,7 @@ def servo_overrides(data: dict) -> dict:
"arm_label": str(robot.get("label") or robot.get("arm") or ""),
"z_ref_m": float(servo.get("z_ref_m") or yolo.get("z_ref_m") or 0.40),
}
# Pass through remaining servo keys that match node params
# 透传其余与节点参数同名的 servo 键
keys = [
"control_hz", "watchdog_timeout_sec", "lost_timeout_sec", "stale_hold_sec",
"max_extrapolate_sec", "predict_enable", "predict_sec",
@@ -460,7 +460,7 @@ def main() -> int:
ap.add_argument("--json", action="store_true")
ap.add_argument("--print", action="store_true", dest="do_print")
ap.add_argument("--config", default="")
ap.add_argument("--sn", "--arm", dest="sn", default="", help="89|91|left|right|full SN")
ap.add_argument("--sn", "--arm", dest="sn", default="", help="89|91|left|right|完整 SN")
args = ap.parse_args()
if args.sn:
os.environ["ROBOT_SN"] = str(args.sn)
@@ -471,35 +471,35 @@ def main() -> int:
print(json.dumps({"site": data, "env": flat}, ensure_ascii=False, indent=2))
return 0
if args.do_print:
print(f"site config: {path}")
print(f"site 配置: {path}")
print(
f" robot SN : {flat['SITE_ROBOT_SN']}"
f" 机械臂 SN : {flat['SITE_ROBOT_SN']}"
f"{(' (' + flat['SITE_ROBOT_LABEL'] + ')') if flat.get('SITE_ROBOT_LABEL') else ''}"
f" arm={flat.get('SITE_ROBOT_ARM') or '?'}"
)
print(f" catalog : {flat.get('SITE_ROBOT_CATALOG') or '(none)'}")
print(f" robot IP : {flat['SITE_ROBOT_IP']} pc={flat['SITE_PC_IP']} iface={flat.get('SITE_NET_IFACE') or '?'}")
print(f" frames : prefix={flat['SITE_JOINT_PREFIX']} ee={flat['SITE_EE_FRAME']}")
print(f" tool name : {flat.get('SITE_ROBOT_TOOL_NAME') or '(keep current)'}")
print(f" 清单 : {flat.get('SITE_ROBOT_CATALOG') or '（无）'}")
print(f" 机械臂 IP : {flat['SITE_ROBOT_IP']} pc={flat['SITE_PC_IP']} iface={flat.get('SITE_NET_IFACE') or '?'}")
print(f" 坐标系 : prefix={flat['SITE_JOINT_PREFIX']} ee={flat['SITE_EE_FRAME']}")
print(f" 工具名 : {flat.get('SITE_ROBOT_TOOL_NAME') or '（保持当前）'}")
catalog_sns = [str(r.get("sn") or "") for r in list_robots(data)]
if catalog_sns and flat["SITE_ROBOT_SN"] not in catalog_sns:
print(
f" WARNING: robot.sn {flat['SITE_ROBOT_SN']!r} is not in robots[]",
f" 警告：robot.sn {flat['SITE_ROBOT_SN']!r} 不在 robots[] 里",
file=sys.stderr,
)
print(f" yolo weights : {flat['SITE_YOLO_WEIGHTS']}")
print(f" resolved : {flat['SITE_YOLO_WEIGHTS_RESOLVED']}")
print(f" handeye : {flat['SITE_HANDEYE_FILE']}")
print(f" handeye wp : {flat['SITE_HANDEYE_WAYPOINTS']}")
print(f" handeye dir : {flat['SITE_HANDEYE_WORKDIR']}")
print(f" named poses : {flat.get('SITE_NAMED_POSES_FILE')}")
print(f" YOLO 权重 : {flat['SITE_YOLO_WEIGHTS']}")
print(f" 解析后 : {flat['SITE_YOLO_WEIGHTS_RESOLVED']}")
print(f" 手眼 : {flat['SITE_HANDEYE_FILE']}")
print(f" 手眼路点 : {flat['SITE_HANDEYE_WAYPOINTS']}")
print(f" 手眼目录 : {flat['SITE_HANDEYE_WORKDIR']}")
print(f" 命名位姿 : {flat.get('SITE_NAMED_POSES_FILE')}")
print(
" board : "
" 标定板 : "
f"{flat['SITE_HANDEYE_BOARD_COLS']}x{flat['SITE_HANDEYE_BOARD_ROWS']} "
f"inner, square={flat['SITE_HANDEYE_SQUARE_M']} m"
f"内角点, square={flat['SITE_HANDEYE_SQUARE_M']} m"
)
print(
f" camera : serial={flat['SITE_CAM_SERIAL'] or '(empty)'} "
f" 相机 : serial={flat['SITE_CAM_SERIAL'] or '（空）'} "
f"{flat['SITE_CAM_WIDTH']}x{flat['SITE_CAM_HEIGHT']}@{flat['SITE_CAM_FPS']} "
f"ae={flat.get('SITE_CAM_AUTO_EXPOSURE')} "
f"exp={flat.get('SITE_CAM_EXPOSURE_US')}us "
@@ -507,26 +507,26 @@ def main() -> int:
f"ae_limit={flat.get('SITE_CAM_AE_LIMIT_US')}us emitter={flat.get('SITE_CAM_EMITTER')}"
)
print(
f" catalog cam : leftD405={flat.get('SITE_D405_LEFT_SERIAL')} "
f" 清单相机 : leftD405={flat.get('SITE_D405_LEFT_SERIAL')} "
f"rightD405={flat.get('SITE_D405_RIGHT_SERIAL')} "
f"D435={flat.get('SITE_D435_SERIAL')}"
)
print(
f" gripper : {flat.get('SITE_GRIP_MODEL')} port={flat.get('SITE_GRIP_PORT')}"
f" 夹爪 : {flat.get('SITE_GRIP_MODEL')} port={flat.get('SITE_GRIP_PORT')}"
)
print(
f" hole_detect : backend={flat.get('SITE_HOLE_BACKEND')} "
f"weights={flat.get('SITE_HOLE_WEIGHTS')}"
)
print(
f" tactile : backend={flat.get('SITE_TACTILE_BACKEND')} "
f" 触觉 : backend={flat.get('SITE_TACTILE_BACKEND')} "
f"L={flat.get('SITE_TACTILE_LEFT')} R={flat.get('SITE_TACTILE_RIGHT')} "
f"cfg={flat.get('SITE_TACTILE_CONFIG')}"
)
print(f" go_home : pose={flat['SITE_GO_HOME_POSE']} speed={flat['SITE_GO_HOME_SPEED']}")
he_path = Path(flat["SITE_HANDEYE_FILE"]).expanduser()
if not he_path.is_file():
print(f" WARNING: hand-eye file missing: {he_path}", file=sys.stderr)
print(f" 警告：手眼文件缺失: {he_path}", file=sys.stderr)
else:
try:
he_data = yaml.safe_load(he_path.read_text(encoding="utf-8")) or {}
@@ -536,28 +536,28 @@ def main() -> int:
he_data.get("uncalibrated") or "T_4x4" not in he_data
):
print(
f" WARNING: {he_path.name} is not calibrated for this arm "
f" 警告：{he_path.name} 没有针对该机械臂标定 "
f"(run_handeye.sh teach/calibrate --arm {flat.get('SITE_ROBOT_ARM') or '91'})",
file=sys.stderr,
)
if "063991" in flat["SITE_ROBOT_SN"] and flat.get("SITE_CAM_SERIAL") == flat.get("SITE_D405_LEFT_SERIAL"):
print(
" WARNING: 91 is using the LEFT wrist D405 serial — check robots[].camera.serial",
" 警告：91 用的是左腕 D405 的序列号 — 请检查 robots[].camera.serial",
file=sys.stderr,
)
if "RGI" in str(flat.get("SITE_GRIP_MODEL") or ""):
print(
" NOTE: 本臂是 RGI。CLI: bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open|rotate",
" 提示：本臂是 RGI。CLI: bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open|rotate",
file=sys.stderr,
)
w = Path(flat["SITE_YOLO_WEIGHTS"]).expanduser()
if not w.is_file():
print(f" WARNING: weights file missing: {w}", file=sys.stderr)
print(f" 警告：权重文件缺失: {w}", file=sys.stderr)
return 2
return 0
# bash exports
# bash 导出
for k, v in flat.items():
# escape for eval
# 为 eval 转义
vv = str(v).replace("'", "'\"'\"'")
print(f"export {k}='{vv}'")
return 0
scripts/lib/load_site_config.sh
+3 -3
View File
@@ -1,6 +1,6 @@
#!/usr/bin/env bash
# Load ~/ros2/config/site.yaml into SITE_* environment variables.
# Usage (from other scripts):
# 把 ~/ros2/config/site.yaml 加载成 `SITE_*` 环境变量。
# 用法（在其它脚本里）：
# source "${ROOT}/scripts/lib/load_site_config.sh"
_SITE_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)"
@@ -13,7 +13,7 @@ else
# shellcheck disable=SC1090
eval "$(python3 "${_SITE_EXPORTER}" --config "${_SITE_FILE}")"
export SITE_CONFIG="${_SITE_FILE}"
# Resolve 89/91/left/right aliases to the full Flexiv SN for RDK.
# 把 89/91/left/right 别名解析成完整的 Flexiv SN，供 RDK 使用。
if [[ -n "${SITE_ROBOT_SN:-}" ]]; then
export ROBOT_SN="${SITE_ROBOT_SN}"
fi
scripts/lib/parse_arm_args.sh
+3 -3
View File
@@ -1,12 +1,12 @@
#!/usr/bin/env bash
# Parse --sn / --arm from argv. Must be sourced BEFORE load_site_config.sh
# so ROBOT_SN is visible to export_site_env.py.
# 从 argv 里解析 --sn / --arm。必须在 `load_site_config.sh` 之前 source，
# 这样 `ROBOT_SN` 才能被 export_site_env.py 看到。
#
# source "${ROOT}/scripts/lib/parse_arm_args.sh"
# parse_arm_args "$@"
# set -- "${PARSE_ARM_REST[@]}"
#
# Aliases (89|91|left|right) are resolved in export_site_env.py.
# 别名（89|91|left|right）在 export_site_env.py 里解析。
parse_arm_args() {
PARSE_ARM_REST=()
scripts/list_realsense.sh
+1 -1
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env bash
# List plugged RealSense cameras (serials used in site.yaml).
# 列出已插上的 RealSense 相机（序列号在 site.yaml 里使用）。
set -eo pipefail
python3 - <<'PY'
import pyrealsense2 as rs
scripts/merge_handeye_poses.py
+4 -4
View File
@@ -73,7 +73,7 @@ def format_positions_block(poses: list[list[float]], indent: str = " ") -> st
def merge(yaml_path: Path, poses: list[list[float]]) -> None:
text = yaml_path.read_text(encoding="utf-8").replace("\r\n", "\n").replace("\r", "\n")
# Detect indent of waypoint_positions (2 or 4 spaces typical)
# 探测 waypoint_positions 的缩进（通常 2 空格或 4 空格）
m_ind = re.search(r"^([ \t]*)waypoint_positions:\s*\[", text, re.MULTILINE)
indent = m_ind.group(1) if m_ind else " "
block = format_positions_block(poses, indent=indent)
@@ -82,7 +82,7 @@ def merge(yaml_path: Path, poses: list[list[float]]) -> None:
rf"{re.escape(indent)}waypoint_positions:\s*\[[\s\S]*?{re.escape(indent)}\]",
re.MULTILINE,
)
# Also match a one-line empty list: waypoint_positions: []
# 也要匹配"单行空列表"的写法：waypoint_positions: []
one_line = re.compile(
rf"{re.escape(indent)}waypoint_positions:\s*\[\s*\]",
re.MULTILINE,
@@ -94,7 +94,7 @@ def merge(yaml_path: Path, poses: list[list[float]]) -> None:
else:
raise RuntimeError("waypoint_positions block not found in YAML")
# Keep num_poses in sync when present
# 顺手把 num_poses 同步成实际点数（如果原本就有这个键）
n = len(poses)
if re.search(r"^\s*num_poses:\s*\d+", new_text, re.MULTILINE):
new_text = re.sub(
@@ -105,7 +105,7 @@ def merge(yaml_path: Path, poses: list[list[float]]) -> None:
flags=re.MULTILINE,
)
else:
# Insert num_poses just before waypoint_positions
# 原本没有 num_poses，就插在 waypoint_positions 前面
new_text = new_text.replace(
block,
f"{indent}num_poses: {n}\n{block}",
scripts/rdk_drag_bridge.py
+24 -24
View File
@@ -105,13 +105,13 @@ class SoftDrag:
robot.SetJointImpedance(g, soft)
log(f'SetJointImpedance {g} K={soft}')
except Exception as exc:
log(f'SetJointImpedance group {g}: {exc}')
log(f'SetJointImpedance 组 {g}: {exc}')
else:
n = max(7, len(self.latest_q))
try:
robot.SetJointImpedance([35.0] * n, [0.7] * n)
except Exception as exc:
log(f'SetJointImpedance flat: {exc}')
log(f'SetJointImpedance flat（不分组）: {exc}')
self._stop.clear()
self._thread = threading.Thread(target=self._loop, name='soft-drag', daemon=True)
@@ -144,9 +144,9 @@ class SoftDrag:
n = len(q)
robot.SendJointPosition(q, [0.0] * n, [1.5] * n, [2.0] * n)
except Exception as exc:
log(f'soft loop: {exc}')
log(f'soft 循环: {exc}')
time.sleep(0.15)
# ~20 Hz — keep lock released most of the time
# 约 20 Hz —— 大部分时间让锁保持释放状态
for _ in range(10):
if self._stop.is_set():
return
@@ -158,7 +158,7 @@ class SoftDrag:
if th is not None:
th.join(timeout=4.0)
if th.is_alive():
log('soft loop still alive after join (will idle anyway)')
log('join 之后 soft 循环仍然存活（无论如何都会切到 IDLE）')
self._thread = None
@@ -168,7 +168,7 @@ def _ensure_operational(robot) -> Optional[str]:
def _idle_async(robot, lock: threading.Lock, flexivrdk, timeout: float = 4.0) -> bool:
"""Switch to IDLE without blocking the stdin loop forever."""
"""切到 IDLE，同时避免把 stdin 循环永久阻塞住。"""
done = threading.Event()
def run() -> None:
@@ -193,7 +193,7 @@ def _idle_async(robot, lock: threading.Lock, flexivrdk, timeout: float = 4.0) ->
threading.Thread(target=run, name='idle', daemon=True).start()
ok = done.wait(timeout)
if not ok:
log('IDLE/Stop timed out — stdin loop continues')
log('IDLE/Stop 超时 —— stdin 循环继续运行')
return ok
@@ -213,7 +213,7 @@ def main() -> int:
try:
msg = json.loads(line)
except Exception as exc: # noqa: BLE001
reply({'ok': False, 'error': f'bad json: {exc}'})
reply({'ok': False, 'error': f'JSON 解析失败: {exc}'})
continue
cmd = str(msg.get('cmd', '')).strip()
@@ -233,17 +233,17 @@ def main() -> int:
for args in attempts:
try:
robot = flexivrdk.Robot(*args)
log(f'connected {sn} args={args!r}')
log(f'已连接 {sn} args={args!r}')
last_err = None
break
except TypeError:
continue
except Exception as exc:
last_err = exc
log(f'connect {args!r}: {exc}')
log(f'连接 {args!r}: {exc}')
robot = None
if robot is None:
reply({'ok': False, 'error': str(last_err or 'connect failed')})
reply({'ok': False, 'error': str(last_err or '连接失败')})
continue
err = _ensure_operational(robot)
if err:
@@ -256,7 +256,7 @@ def main() -> int:
elif cmd == 'drag_on':
if robot is None:
reply({'ok': False, 'error': 'not connected'})
reply({'ok': False, 'error': '尚未连接'})
continue
err = _ensure_operational(robot)
if err:
@@ -274,7 +274,7 @@ def main() -> int:
reply({'ok': True, 'drag': True, 'mode': 'floating', 'primitive': name})
continue
except Exception as exc:
log(f'Floating failed, fallback soft: {exc}')
log(f'Floating 失败，退回 soft 模式: {exc}')
try:
if soft is not None:
@@ -284,18 +284,18 @@ def main() -> int:
if soft.latest_q:
cached_q = list(soft.latest_q)
drag_on = True
log('soft drag ON')
log('软拖拽已开启')
reply({
'ok': True,
'drag': True,
'mode': 'soft',
'hint': 'Push the arm gently; low joint impedance. Enter=photo (s=hold).',
'hint': '轻推机械臂即可，关节阻抗已调低。Enter=拍照（s=保持）。',
})
except Exception as exc:
reply({'ok': False, 'error': f'soft drag failed: {exc}'})
reply({'ok': False, 'error': f'软拖拽失败: {exc}'})
elif cmd == 'drag_off':
log('drag_off requested')
log('收到 drag_off 请求')
if soft is not None:
if soft.latest_q:
cached_q = list(soft.latest_q)
@@ -304,7 +304,7 @@ def main() -> int:
if robot is not None:
_idle_async(robot, rlock, flexivrdk, timeout=4.0)
drag_on = False
log('drag_off done')
log('drag_off 已完成')
reply({'ok': True, 'drag': False, 'q': cached_q})
elif cmd == 'joints':
@@ -314,14 +314,14 @@ def main() -> int:
elif len(cached_q) >= 7:
q = list(cached_q)
if q is None or len(q) < 7:
reply({'ok': False, 'error': 'no joint cache'})
reply({'ok': False, 'error': '没有关节缓存'})
continue
cached_q = q
reply({'ok': True, 'q': q, 'drag': drag_on})
elif cmd == 'enable':
if robot is None:
reply({'ok': False, 'error': '尚未 connect'})
reply({'ok': False, 'error': '尚未连接'})
continue
err = ensure_operational(robot)
reply({
@@ -332,7 +332,7 @@ def main() -> int:
elif cmd == 'clear_fault':
if robot is None:
reply({'ok': False, 'error': '尚未 connect'})
reply({'ok': False, 'error': '尚未连接'})
continue
err = rdk_clear_fault(robot)
if err:
@@ -349,7 +349,7 @@ def main() -> int:
elif cmd == 'switch_tool':
if robot is None:
reply({'ok': False, 'error': '尚未 connect'})
reply({'ok': False, 'error': '尚未连接'})
continue
want = str(msg.get('name', '')).strip()
@@ -428,9 +428,9 @@ def main() -> int:
return 0
else:
reply({'ok': False, 'error': f'unknown cmd {cmd}'})
reply({'ok': False, 'error': f'未知命令 {cmd}'})
except Exception as exc: # noqa: BLE001
log(f'cmd={cmd} error: {exc}')
log(f'cmd={cmd} 出错: {exc}')
reply({'ok': False, 'error': str(exc)})
return 0
scripts/rdk_gripper.py
+27 -27
View File
@@ -1,10 +1,10 @@
#!/usr/bin/env python3
"""Control Studio device GripperDahuan / GripperDahuanModbus via Flexiv RDK.
"""通过 Flexiv RDK 控制 Studio 设备 GripperDahuan / GripperDahuanModbus。
Must run under RDK Python (rdk_py310). Uses a *lite* RDK instance so it can
share the robot with run_real.sh (ros2_control holds the normal RDK).
必须在 RDK 的 Python（rdk_py310）下运行。使用 *lite*（轻量）RDK 实例，
以便和 run_real.sh 共用机器人（常规 RDK 被 ros2_control 占用）。
The controller talks RS485 itself (its own /dev/ttyUSB0), not the IPC serial.
控制器自己走 RS485（用自带的 /dev/ttyUSB0），不走工控机的串口。
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py --list
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py status
@@ -65,28 +65,28 @@ def _move(grip, params, width: float, vel: float, force: float) -> None:
vmax = float(params.max_vel)
if vmax <= 0:
vmax = 0.05
# Move velocity cannot be 0 (RDK).
# 移动速度不能为 0（RDK 限制）。
vel = _clamp(vel, max(vmin, 1e-4), vmax)
force = _clamp(force, float(params.min_force), float(params.max_force))
width = _clamp(width, float(params.min_width), float(params.max_width))
print(f"Move width={width:.4f} m vel={vel:.4f} m/s force={force:.1f} N")
print(f"移动 width={width:.4f} m vel={vel:.4f} m/s force={force:.1f} N")
grip.Move(width, vel, force)
def main() -> int:
ap = argparse.ArgumentParser(description="Flexiv RDK gripper (GripperDahuanModbus)")
ap = argparse.ArgumentParser(description="Flexiv RDK 夹爪（GripperDahuanModbus）")
ap.add_argument("--sn", default="")
ap.add_argument("--iface", default="")
ap.add_argument("--device", default="", help="Studio Device name")
ap.add_argument("--force-n", type=float, default=0.0, help="force limit [N]; 0=mid")
ap.add_argument("--vel", type=float, default=0.0, help="speed [m/s]; 0=half max")
ap.add_argument("--device", default="", help="Studio 设备名")
ap.add_argument("--force-n", type=float, default=0.0, help="力上限 [N]；0=取中值")
ap.add_argument("--vel", type=float, default=0.0, help="速度 [m/s]；0=最大速度的一半")
ap.add_argument(
"cmd",
nargs="?",
default="status",
choices=["list", "status", "init", "open", "close", "pos", "enable"],
)
ap.add_argument("value", nargs="?", default="", help="pos: meters or 0–1000")
ap.add_argument("value", nargs="?", default="", help="pos: 米，或 0–1000")
args = ap.parse_args()
sys.path.insert(0, str(ROOT / "scripts" / "lib"))
@@ -99,18 +99,18 @@ def main() -> int:
iface = (args.iface or robot_cfg.get("iface") or "").strip()
device = (args.device or g.get("device_name") or "GripperDahuanModbus").strip()
if not sn:
print("ERROR: robot SN missing", file=sys.stderr)
print("错误：没拿到机械臂序列号（SN）", file=sys.stderr)
return 2
import flexivrdk
print(f"=== RDK lite gripper SN={sn} device={device} iface={iface or 'auto'} ===")
print(f"=== RDK lite 夹爪 SN={sn} device={device} iface={iface or 'auto'} ===")
robot = connect_lite(sn, iface)
dev = flexivrdk.Device(robot)
names = list(dev.list() or [])
if args.cmd == "list":
print("Studio devices:")
print("Studio 设备列表:")
for n in names:
try:
en = dev.enabled(n)
@@ -123,7 +123,7 @@ def main() -> int:
if device not in names:
print(
f"ERROR: device {device!r} not in Studio pool.\n available: {', '.join(names)}",
f"错误：设备 {device!r} 不在 Studio 设备池里。\n 可用: {', '.join(names)}",
file=sys.stderr,
)
return 1
@@ -133,7 +133,7 @@ def main() -> int:
)
if not dev.connected(device):
print(
"WARN: connected=False — 控制器自己的 /dev/ttyUSB0 没接到爪。\n"
"警告：connected=False —— 控制器自己的 /dev/ttyUSB0 没接到夹爪。\n"
" USB–RS485 要插 Flexiv 机箱 USB（不是工控机），A/B + 24V 到 PGEA。\n"
" 或用法兰工具口 485（腕部白绿+/绿-）。"
)
@@ -142,14 +142,14 @@ def main() -> int:
if args.cmd in ("enable", "status", "init", "open", "close", "pos"):
try:
grip.Enable(device)
print(f"Enable [{device}] OK")
print(f"使能 [{device}] 成功")
except Exception as exc:
print(f"ERROR: Enable failed: {exc}", file=sys.stderr)
print(f"错误：使能失败: {exc}", file=sys.stderr)
return 1
params = grip.params()
print(" params:", _fmt_params(params))
print(" states:", _fmt_states(grip.states()))
print(" 参数:", _fmt_params(params))
print(" 状态:", _fmt_states(grip.states()))
force = float(args.force_n or g.get("force_n") or 0.0)
if force <= 0:
@@ -164,27 +164,27 @@ def main() -> int:
return 0
if args.cmd == "init":
print("Init (fingers home, ~10 s) …")
print("初始化（手指回零，约 10 秒）…")
grip.Init()
time.sleep(2.0)
print(" states:", _fmt_states(grip.states()))
print(" 状态:", _fmt_states(grip.states()))
return 0
if args.cmd == "open":
_move(grip, params, float(params.max_width), vel, force)
time.sleep(0.4)
print(" states:", _fmt_states(grip.states()))
print(" 状态:", _fmt_states(grip.states()))
return 0
if args.cmd == "close":
_move(grip, params, float(params.min_width), vel, force)
time.sleep(0.4)
print(" states:", _fmt_states(grip.states()))
print(" 状态:", _fmt_states(grip.states()))
return 0
if args.cmd == "pos":
if args.value == "":
print("usage: pos <meters|0-1000>", file=sys.stderr)
print("用法: pos <米|0-1000>", file=sys.stderr)
return 2
raw = float(args.value)
if raw > 1.5:
@@ -194,7 +194,7 @@ def main() -> int:
width = raw
_move(grip, params, width, vel, force)
time.sleep(0.4)
print(" states:", _fmt_states(grip.states()))
print(" 状态:", _fmt_states(grip.states()))
return 0
return 0
@@ -203,5 +203,5 @@ if __name__ == "__main__":
try:
raise SystemExit(main())
except Exception as exc:
print(f"ERROR: {exc}", file=sys.stderr)
print(f"错误：{exc}", file=sys.stderr)
raise SystemExit(1)
scripts/rdk_switch_tool.py
+38 -38
View File
@@ -1,8 +1,8 @@
#!/usr/bin/env python3
"""Switch the Flexiv active tool (TCP + payload) to a Studio-configured name.
"""把 Flexiv 当前工具（TCP + 负载）切换成 Studio 里配置好的名字。
Must run under RDK Python (rdk_py310). Robot must be IDLE; stop run_real.sh first
if ros2_control already owns the RDK connection.
必须在 RDK 的 Python（rdk_py310）下运行。机器人必须处于 IDLE；如果
ros2_control 已经占住了 RDK 连接，请先停掉 run_real.sh。
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_switch_tool.py --list
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_switch_tool.py --name Flange
@@ -17,7 +17,7 @@ import time
def _single_arm_groups(robot):
"""RDK 1.9 Tool API is single-arm (no JointGroup). Older builds use groups."""
"""RDK 1.9 的 Tool API 是单臂的（没有 JointGroup）。老版本用 groups。"""
info = robot.info()
groups = getattr(info, "single_arm_groups", None) or {}
if isinstance(groups, dict):
@@ -33,7 +33,7 @@ def _single_arm_groups(robot):
return [flexivrdk.JointGroup.ARM_1]
except Exception:
pass
return [None] # sentinel: call Tool.name()/Switch() without a group
return [None] # 哨兵：调用 Tool.name()/Switch() 时不带 group
def _tool_name(tool, group):
@@ -57,7 +57,7 @@ def _tool_switch(tool, group, target: str) -> None:
def _group_name(group) -> str:
if group is None:
return "arm"
return "单臂"
try:
import flexivrdk
@@ -91,7 +91,7 @@ def _ensure_idle(robot, mode) -> None:
time.sleep(0.25)
if not robot.operational():
raise RuntimeError(
"robot not operational — AUTO Remote + Enable, release E-stop"
"机器人未处于 operational 状态 —— 请在示教器切 AUTO Remote 并使能，松开急停"
)
robot.SwitchMode(mode.IDLE)
@@ -108,13 +108,13 @@ def _resolve_name(tool, want: str) -> str:
if hit:
return hit
raise SystemExit(
"tool %r not found in Studio pool.\n available: %s"
% (want, ", ".join(names) or "(empty)")
"工具 %r 不在 Studio 工具池里。\n 可选: %s"
% (want, ", ".join(names) or "（空）")
)
def _iface_ipv4s(name: str) -> list:
"""FastDDS whitelist wants IPv4 addresses, not NIC names like enp4s0."""
"""FastDDS 白名单要的是 IPv4 地址，而不是 enp4s0 这类网卡名。"""
import socket
import fcntl
import struct
@@ -147,7 +147,7 @@ def _iface_ipv4s(name: str) -> list:
addrs.append(cidr.split("/")[0])
except Exception:
pass
# unique, keep order
# 去重，保持原有顺序
seen = set()
uniq = []
for a in addrs:
@@ -170,7 +170,7 @@ def _rdk_whitelist(iface: str) -> list[list[str]]:
robot_lan = [a for a in _iface_ipv4s(iface) if a.startswith("192.168.2.")]
if robot_lan:
lists.append(robot_lan)
# unique
# 去重
seen: set[tuple[str, ...]] = set()
out: list[list[str]] = []
for item in lists:
@@ -185,7 +185,7 @@ def connect_rdk(sn: str, iface: str = ""):
import flexivrdk
for listen in _rdk_whitelist(iface):
print("RDK whitelist:", ", ".join(listen))
print("RDK 白名单:", ", ".join(listen))
for args in ((sn, listen), (sn, listen, True), (sn, listen, True, False)):
try:
return flexivrdk.Robot(*args)
@@ -194,51 +194,51 @@ def connect_rdk(sn: str, iface: str = ""):
except RuntimeError as exc:
msg = str(exc)
if "whitelist" in msg.lower() or "filtered out" in msg.lower():
print(" whitelist rejected, try next")
print(" 白名单被拒，试下一个")
break
if "Failed to connect" in msg or "failed to connect" in msg.lower():
print(" search timeout with this whitelist")
print(" 用这个白名单搜索超时")
break
raise
print("retry without iface filter")
print("不带 iface 过滤再试一次")
return flexivrdk.Robot(sn)
def main() -> int:
ap = argparse.ArgumentParser(description="Switch Flexiv Studio tool / TCP")
ap.add_argument("--sn", default="", help="robot serial (default: site.yaml)")
ap.add_argument("--iface", default="", help="NIC whitelist for RDK (e.g. enp4s0)")
ap = argparse.ArgumentParser(description="切换 Flexiv Studio 工具 / TCP")
ap.add_argument("--sn", default="", help="机械臂序列号（默认取 site.yaml）")
ap.add_argument("--iface", default="", help="RDK 用的网卡白名单（如 enp4s0）")
ap.add_argument(
"--name",
default="",
help="Studio tool name (Flange = no tool). Empty = site.yaml robot.tool_name",
help="Studio 工具名（Flange = 不装工具）。留空 = 用 site.yaml 的 robot.tool_name",
)
ap.add_argument("--list", action="store_true", help="list tools and exit")
ap.add_argument("--list", action="store_true", help="列出工具后退出")
args = ap.parse_args()
sn = (args.sn or "").strip()
want = (args.name or "").strip()
if not sn:
print("ERROR: --sn is required (run via run_real.sh / rdk_switch_tool.sh)", file=sys.stderr)
print("错误：必须提供 --sn（请通过 run_real.sh / rdk_switch_tool.sh 运行）", file=sys.stderr)
return 2
import flexivrdk
mode = flexivrdk.Mode
iface = (args.iface or "").strip()
print(f"=== RDK tool SN={sn} iface={iface or 'auto'} ===")
print(f"=== RDK 工具 SN={sn} iface={iface or 'auto'} ===")
robot = None
try:
robot = connect_rdk(sn, iface)
if robot.fault():
print("fault present, ClearFault ...")
print("检测到故障，正在 ClearFault ...")
if not robot.ClearFault():
print("ERROR: fault cannot be cleared", file=sys.stderr)
print("错误：故障无法清除", file=sys.stderr)
return 1
tool = flexivrdk.Tool(robot)
names = list(tool.list() or [])
groups = _single_arm_groups(robot)
print("Studio tools:", ", ".join(names) or "(none)")
print("Studio 工具:", ", ".join(names) or "（无）")
for g in groups:
cur = _tool_name(tool, g)
extra = ""
@@ -246,23 +246,23 @@ def main() -> int:
extra = " " + _fmt_params(_tool_params(tool, g))
except Exception:
pass
print(f" active [{_group_name(g)}]: {cur}{extra}")
print(f" 当前 [{_group_name(g)}]: {cur}{extra}")
if args.list:
return 0
if not want:
print("tool_name empty — keep current (set robot.tool_name in site.yaml to switch)")
print("RDK OK")
print("tool_name 为空 —— 保持当前工具（要切换请在 site.yaml 里设置 robot.tool_name）")
print("RDK 正常")
return 0
target = _resolve_name(tool, want)
already = all(_tool_name(tool, g) == target for g in groups)
if already:
print(f"already on [{target}]")
print("RDK OK")
print(f"已经处于 [{target}]")
print("RDK 正常")
return 0
print(f"switching → [{target}] (requires IDLE)")
print(f"正在切换到 → [{target}]（需要 IDLE）")
_ensure_idle(robot, mode)
for g in groups:
_tool_switch(tool, g, target)
@@ -273,16 +273,16 @@ def main() -> int:
extra = " " + _fmt_params(_tool_params(tool, g))
except Exception:
pass
print(f" now [{_group_name(g)}]: {cur}{extra}")
print(f" 现在 [{_group_name(g)}]: {cur}{extra}")
if cur != target:
print(f"ERROR: switch failed, still {cur!r}", file=sys.stderr)
print(f"错误：切换失败，当前仍是 {cur!r}", file=sys.stderr)
return 1
print("RDK OK")
print("RDK 正常")
return 0
except Exception as exc:
print(f"ERROR: {exc}", file=sys.stderr)
print(" Stop run_real.sh first (RDK is exclusive).", file=sys.stderr)
print(" Pendant: AUTO Remote + Enable Ethernet/RDK.", file=sys.stderr)
print(f"错误：{exc}", file=sys.stderr)
print(" 请先停掉 run_real.sh（RDK 是独占的）。", file=sys.stderr)
print(" 示教器：切 AUTO Remote，并使能 Ethernet/RDK。", file=sys.stderr)
return 1
finally:
robot = None
scripts/rdk_switch_tool.sh
+4 -4
View File
@@ -1,11 +1,11 @@
#!/usr/bin/env bash
# Switch / list Flexiv Studio tools via RDK (reads name from site.yaml).
# 通过 RDK 切换 / 列出 Flexiv Studio 工具（工具名从 site.yaml 读取）。
#
# bash ~/ros2/scripts/rdk_switch_tool.sh # switch to robot.tool_name
# bash ~/ros2/scripts/rdk_switch_tool.sh --list # print Studio tool pool
# bash ~/ros2/scripts/rdk_switch_tool.sh # 切换到 robot.tool_name
# bash ~/ros2/scripts/rdk_switch_tool.sh --list # 打印 Studio 工具池
# bash ~/ros2/scripts/rdk_switch_tool.sh --name Flange
#
# Stop run_real.sh first — RDK allows only one client.
# 先停掉 `run_real.sh` —— RDK 只允许一个客户端。
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
scripts/run_camera_d405.sh
+8 -8
View File
@@ -1,17 +1,17 @@
#!/usr/bin/env bash
# Terminal: D405 via official pyrealsense2 SDK — color + depth + stereo IR topics.
# With --preview the Qt window runs in a SEPARATE viewer process (realsense_view)
# so painting never slows the camera stream. Closing the window ≠ stop camera.
# Do NOT start view_realsense_depth.sh at the same time (USB is exclusive).
# 终端方式运行 D405：走官方 pyrealsense2 SDK —— 发布彩色 + 深度 + 双目 IR 话题。
# 带 --preview 时，Qt 窗口跑在**独立的**查看器进程（`realsense_view`）里，
# 这样绘制界面永远不会拖慢相机数据流。关掉窗口 ≠ 停止相机。
# 不要同时启动 `view_realsense_depth.sh`（USB 是独占的）。
#
# Usage:
# 用法：
# bash ~/ros2/scripts/run_camera_d405.sh
# bash ~/ros2/scripts/run_camera_d405.sh --preview
# bash ~/ros2/scripts/run_camera_d405.sh --arm 91 --preview
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
PREVIEW=-1 # -1 = use site; 0/1 = force
PREVIEW=-1 # -1 = 沿用 site；0/1 = 强制
HIGH_FPS=0
# shellcheck disable=SC1091
@@ -143,8 +143,8 @@ fi
echo "=== next: bash ~/ros2/scripts/run_visual_servo_rt.sh --arm ${SITE_ROBOT_ARM:-89} [--control] ==="
if [[ "${DO_PREVIEW}" == "1" ]]; then
# Camera publishes only; the Qt viewer runs in its own process so painting
# can never starve the capture loop (shared GIL cost ~70 ms/frame before).
# 相机进程只负责发布；Qt 查看器跑在自己的进程里，所以界面绘制
# 永远不会把采集循环饿死（以前共用 GIL 时每帧要花约 70 ms）。
pkill -f 'lib/arm_application/realsense_view' 2>/dev/null || true
ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}" -p show_preview:=false &
CAM_PID=$!
scripts/run_camera_d435.sh
+1 -1
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env bash
# Overhead D435 between the two arms (eye-to-hand). Does not open either D405.
# 两臂之间的顶置 D435（`eye-to-hand`，相机固定在外）。不会打开任何一个 D405。
#
# bash ~/ros2/scripts/run_camera_d435.sh --preview
#
scripts/run_display.sh
+1 -1
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env bash
# View URDF only (joint_state_publisher_gui + RViz)
# 只看 URDF（joint_state_publisher_gui + RViz）
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
scripts/run_dual_gripper.sh
+3 -3
View File
@@ -1,8 +1,8 @@
#!/usr/bin/env bash
# Both DH grippers (left PGEA + right RGI) as one ROS 2 node.
# 把两个 DH 夹爪（左 PGEA + 右 RGI）作为同一个 ROS 2 节点启动。
# bash ~/ros2/scripts/run_dual_gripper.sh
# Topics: /dual_gripper_driver/left|right/{command,status}
# Services: /dual_gripper_driver/left|right/execute (arm_msgs/srv/GripperExecute)
# 话题：/dual_gripper_driver/left|right/{command,status}
# 服务：/dual_gripper_driver/left|right/execute (arm_msgs/srv/GripperExecute)
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
scripts/run_gripper.sh
+6 -6
View File
@@ -1,8 +1,8 @@
#!/usr/bin/env bash
# DH grippers on the IPC (Modbus RTU). Not Flexiv Studio / RDK.
# 工控机上的 DH 夹爪（Modbus RTU）。不是 Flexiv Studio / RDK 那一套。
#
# Left 89 PGEA parallel /dev/ttyUSB0
# Right 91 RGI rotary /dev/ttyUSB1
# 左臂 89 PGEA 平行夹爪 /dev/ttyUSB0
# 右臂 91 RGI 旋转夹爪 /dev/ttyUSB1
#
# bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli probe
# bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli init
@@ -13,9 +13,9 @@
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli close
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli rotate 90
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli revolutions 2
# bash ~/ros2/scripts/run_gripper.sh --arm 89 # ROS node PGEA
# bash ~/ros2/scripts/run_gripper.sh --arm 91 # ROS node RGI
# bash ~/ros2/scripts/run_dual_gripper.sh # both ROS nodes
# bash ~/ros2/scripts/run_gripper.sh --arm 89 # ROS 节点 PGEA
# bash ~/ros2/scripts/run_gripper.sh --arm 91 # ROS 节点 RGI
# bash ~/ros2/scripts/run_dual_gripper.sh # 两个 ROS 节点都起
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
scripts/run_handeye.sh
+6 -6
View File
@@ -1,14 +1,14 @@
#!/usr/bin/env bash
# Hand-eye calibration helpers.
# Scripts: ~/ros2/scripts/run_handeye.sh
# Config & results: ~/ros2/handeye/ ← 见 handeye/README.md
# 手眼标定辅助脚本。
# 脚本：~/ros2/scripts/run_handeye.sh
# 配置与结果：~/ros2/handeye/ ← 见 handeye/README.md
# poses/<臂号>_<模式>.yaml 采点（输入）
# results/<臂号>_<模式>.yaml 标定结果（输出）
# calibrate.yaml 板尺寸 / 运动 / 稳定性参数
# tmp/ 临时产物
# Site overrides: ~/ros2/config/site.yaml → handeye:
# 站点覆盖项：~/ros2/config/site.yaml → handeye:
#
# Usage:
# 用法：
# bash ~/ros2/scripts/run_handeye.sh calibrate
# bash ~/ros2/scripts/run_handeye.sh waypoints
# bash ~/ros2/scripts/run_handeye.sh teach # RDK 软拖拽（推荐）
@@ -61,7 +61,7 @@ source "${ROOT}/scripts/env.sh"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
# Prefer package source tree (editable); fall back to install share
# 优先用包的源码树（editable 模式）；退而用 install 里的 share
APP_CFG="${ROOT}/src/arm_application/config"
if [[ ! -d "${APP_CFG}" ]]; then
APP_CFG="$(ros2 pkg prefix arm_application 2>/dev/null)/share/arm_application/config" || true
scripts/run_hole_detect.sh
+5 -5
View File
@@ -1,13 +1,13 @@
#!/usr/bin/env bash
# Hole detection on the current-arm wrist D405. Detect only — no insert motion.
# 用当前臂腕部 D405 做孔检测。只检测 —— 不做插入动作。
#
# bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
# bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview
#
# backend from site.yaml hole_detect.backend:
# two_stage YOLO workpiece bbox + HoughCircles (camera_test hole_detect.py)
# yolo YOLO boxes treated as holes
# hough full-image HoughCircles
# 后端取自 site.yaml 的 hole_detect.backend:
# two_stage YOLO 工件 bbox + HoughCircles（camera_test 的 hole_detect.py）
# yolo 把 YOLO 框直接当作孔
# hough 整图 HoughCircles
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
scripts/run_real.sh
+4 -4
View File
@@ -1,6 +1,6 @@
#!/usr/bin/env bash
# Real Rizon4 bringup — sets RDK runtime libs + launches stack backend:=real
# Note: no `set -u` — conda/RoboStack hooks reference unset vars (CONDA_BUILD, COLCON_TRACE)
# 真机 Rizon4 启动入口 —— 设置 RDK 运行时库，然后以 backend:=real 启动整栈
# 注意：这里不加 `set -u` —— conda/RoboStack 的钩子会引用未绑定的变量（CONDA_BUILD、COLCON_TRACE）
#
# bash ~/ros2/scripts/run_real.sh
# bash ~/ros2/scripts/run_real.sh --arm 91
@@ -31,10 +31,10 @@ source "${ROOT}/install/setup.bash"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
# flexiv_hardware must load spdlog/RBDyn from flexiv_install (not conda fmt v11)
# `flexiv_hardware` 必须从 flexiv_install 加载 spdlog/RBDyn（不能用 conda 的 fmt v11）
export LD_LIBRARY_PATH="${FLEXIV_PREFIX}/lib:${LD_LIBRARY_PATH:-}"
# SITE_ROBOT_SN is the resolved Flexiv serial (89/91 aliases already mapped).
# `SITE_ROBOT_SN` 是解析后的 Flexiv 序列号（89/91 别名已经映射好了）。
ROBOT_SN="${SITE_ROBOT_SN:-${ROBOT_SN:-Rizon4-063989}}"
ROBOT_IP="${ROBOT_IP:-${SITE_ROBOT_IP:-192.168.100.1}}"
NET_IFACE="${SITE_NET_IFACE:-}"
scripts/run_sim.sh
+1 -1
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env bash
# Quick-start: mock arm + RViz
# 快速启动：mock 机械臂 + RViz
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
scripts/run_tactile.sh
+2 -2
View File
@@ -1,8 +1,8 @@
#!/usr/bin/env bash
# Orisys visuo-tactile (finger cameras, not D405). Config: site.yaml tactile:
# Orisys 视触觉（指端相机，不是 D405）。配置：site.yaml 的 tactile:
#
# bash ~/ros2/scripts/run_tactile.sh --cli
# bash ~/ros2/scripts/run_tactile.sh # ROS node /tactile/left|right/status
# bash ~/ros2/scripts/run_tactile.sh # ROS 节点 /tactile/left|right/status
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
scripts/run_visual_servo_rt.sh
+6 -6
View File
@@ -1,14 +1,14 @@
#!/usr/bin/env bash
# Real-time visual servo (reads ALL tunables from ~/ros2/config/site.yaml).
# 实时视觉伺服（所有可调参数全部从 ~/ros2/config/site.yaml 读取）。
#
# bash ~/ros2/scripts/run_visual_servo_rt.sh
# bash ~/ros2/scripts/run_visual_servo_rt.sh --control
# bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 91 --control
#
# --control starts tracking from the current pose (no lift, no return-to-home).
# --control 从当前位姿直接开始跟踪（不抬升，也不回 home）。
# 89/91 手眼与相机各自独立：必须与 run_real.sh 同一 --arm。
#
# Switch YOLO weights: edit paths.yolo_weights in site.yaml
# 切换 YOLO 权重：改 site.yaml 里的 paths.yolo_weights
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
@@ -42,9 +42,9 @@ fi
if [[ "${CONTROL}" == true ]]; then
echo "=== waiting for rizon_arm_controller ACTIVE (T1: run_real.sh --arm ${SITE_ROBOT_ARM:-?}) ==="
# NOTE: action-list can show zombie entries from a crashed ros2_control,
# and `ros2 control` CLI is broken in this env (xmlrpc Fault). A raw
# service call only answers when the controller manager is actually alive.
# 注意：action-list 可能显示出崩溃过的 ros2_control 留下的僵尸条目，
# 而且这个环境里的 `ros2 control` CLI 是坏的（xmlrpc Fault）。直接发
# 服务请求只有控制器管理器真的活着时才会应答。
ready=false
for _i in $(seq 1 40); do
if timeout 5 ros2 service call /controller_manager/list_controllers \
scripts/show_site_config.sh
+1 -1
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env bash
# Show / validate the master site config.
# 显示 / 校验主站点配置。
# bash ~/ros2/scripts/show_site_config.sh
# SITE_CONFIG=/path/to/other.yaml bash ~/ros2/scripts/show_site_config.sh
scripts/test_rdk_python.sh
+1 -1
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env bash
# Smoke-test Flexiv RDK Python against site robot (no ROS / no GitHub).
# 针对站点机器人对 Flexiv RDK Python 做冒烟测试（不需要 ROS / 不访问 GitHub）。
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
scripts/use_local_flexiv.sh
+3 -3
View File
@@ -1,6 +1,6 @@
#!/usr/bin/env bash
# Wire local Flexiv trees under ~/文档/Flexiv_Rizon4 into this workspace.
# Avoids re-cloning RDK / sim plugin from GitHub.
# 把 ~/文档/Flexiv_Rizon4 下的本地 Flexiv 代码树接入本工作区。
# 免得再从 GitHub 重新克隆 RDK / 仿真插件。
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
@@ -30,7 +30,7 @@ if [[ -d "${LOCAL_FLEXIV}/flexiv_sim_plugin" ]]; then
echo "Linked thirdparty/flexiv_sim_plugin -> ${LOCAL_FLEXIV}/flexiv_sim_plugin"
fi
# humble-v1 ROS hardware needs RDK 1.9.x (kept separately if present)
# humble-v1 的 ROS 硬件需要 RDK 1.9.x（如果存在，会单独保留一份）
if [[ -d "${SRC}/flexiv_rdk.bak.1787646725" ]]; then
ln -sfn "${SRC}/flexiv_rdk.bak.1787646725" "${SRC}/flexiv_rdk_v1"
touch "${SRC}/flexiv_rdk.bak.1787646725/COLCON_IGNORE"
src/arm_application/arm_application/__init__.py
+28 -28
View File
@@ -1,32 +1,32 @@
"""arm_application — field nodes for Flexiv Rizon + D405 + DH grippers.
"""arm_application —— Flexiv Rizon + D405 + DH 夹爪的现场节点包。
Public ROS executables (see setup.py console_scripts)
----------------------------------------------------
Camera: realsense_color_pub
Perception: perception_node
Servo: servo_node
Hand-eye: handeye_teach, handeye_waypoints, handeye_calibrate
Gripper: dh_pgea, dh_pgea_cli, dual_gripper, dual_gripper_cli
Hole detect: hole_detect
Tactile: tactile, tactile_cli
Helpers: goto_ee_plane, demo_named_poses, run_joint_trajectory
对外可直接运行的 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
Reusable Python modules (import from your own nodes)
----------------------------------------------------
yolo_detector YOLO / TensorRT inference + pick_best
box_tracker pixel-space constant-velocity box coast
plane_feature stereo plane centroid inside a YOLO box
dh_pgea official Modbus RTU client for DH PGEA
dh_official DH Modbus client used by dual_gripper
dual_gripper PGEA left + RGI right
hole_detector yolo / hough / two_stage hole centers
orisys_tactile Orisys visuo-tactile adapter
geom_ik Rizon geometric Jacobian / DLS / hand-eye load
handeye_lib chessboard + OpenCV calibrateHandEye
handeye_realsense topic / serial helpers for D405
pid_controller PID with integral separation
kalman_filter 3D constant-velocity KF
可复用的 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`` 三维匀速模型卡尔曼滤波
Operators should use ``scripts/*.sh`` and ``config/site.yaml``, not these
imports. See ``docs/INTERFACES.md``.
现场操作请走 ``scripts/*.sh`` 和 ``config/site.yaml``，不要直接 import 这些模块。
接口契约见 ``docs/INTERFACES.md``。
"""
src/arm_application/arm_application/box_tracker.py
+4 -4
View File
@@ -1,8 +1,8 @@
#!/usr/bin/env python3
"""Constant-velocity tracker for a YOLO xyxy box (pixel space).
"""针对 YOLO ``xyxy`` 框（像素坐标系）的匀速跟踪器。
Used so fast workpiece motion does not drop the lock: the next-frame gate
is the *predicted* box, and brief YOLO misses coast the overlay.
用于避免工件快速运动时丢失锁定：下一帧的闸门使用*预测*框，
短暂的 YOLO 漏检则由惯性滑行（coast）维持叠加显示。
"""
from __future__ import annotations
@@ -38,7 +38,7 @@ class BoxTracker:
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) # center px/s
self.v = np.zeros(2, dtype=float) # 中心点 px/s
self.t: Optional[float] = None
def reset(self) -> None:
src/arm_application/arm_application/demo_named_poses.py
+7 -7
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""Demo: cycle named poses via /set_named_pose (works for any backend)."""
"""演示：通过 ``/set_named_pose`` 循环切换命名位姿（对任何后端都适用）。"""
from __future__ import annotations
@@ -31,10 +31,10 @@ class DemoNamedPoses(Node):
def run(self) -> int:
if not self._cli.wait_for_service(timeout_sec=15.0):
self.get_logger().error('/set_named_pose not available')
self.get_logger().error('``/set_named_pose`` 服务不可用')
return 1
for loop in range(max(self.loops, 1)):
self.get_logger().info(f'loop {loop + 1}/{self.loops}')
self.get_logger().info(f'第 {loop + 1}/{self.loops} 轮')
for pose in self.poses:
req = SetNamedPose.Request()
req.pose_name = pose
@@ -45,17 +45,17 @@ class DemoNamedPoses(Node):
while rclpy.ok() and not fut.done():
rclpy.spin_once(self, timeout_sec=0.05)
if time.monotonic() > deadline:
self.get_logger().error(f'timeout on {pose}')
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} failed: {res.message if res else "no response"}'
f'{pose} 执行失败：{res.message if res else "无响应"}'
)
return 3
self.get_logger().info(f'{pose}: {res.message}')
self.get_logger().info(f'{pose}：{res.message}')
time.sleep(self.dwell_sec)
self.get_logger().info('demo done')
self.get_logger().info('演示结束')
return 0
src/arm_application/arm_application/dh_official.py
+31 -31
View File
@@ -20,34 +20,34 @@ from typing import Iterable
# =============================================================================
# DH-Robotics Modbus register map
# DH-Robotics Modbus 寄存器映射
# =============================================================================
# Basic control registers (holding registers, function code 03/06/10)
REG_INIT = 0x0100 # 0x01 initialize, 0xA5 fully initialize
REG_FORCE = 0x0101 # gripper closing force, 20~100 %
REG_POSITION = 0x0103 # finger reference position, 0~1000 per-mille
REG_SPEED = 0x0104 # finger open/close speed, 1~100 %
# 基础控制寄存器（保持寄存器，功能码 03/06/10）
REG_INIT = 0x0100 # 0x01 初始化，0xA5 完全初始化
REG_FORCE = 0x0101 # 夹爪闭合力，20~100 %
REG_POSITION = 0x0103 # 手指参考位置，0~1000 千分比
REG_SPEED = 0x0104 # 手指开合速度，1~100 %
# RGI rotation control registers
REG_ROTATION_ANGLE = 0x0105 # target rotation angle, signed degree value
REG_ROTATION_SPEED = 0x0107 # rotation speed, 1~100 %
REG_ROTATION_FORCE = 0x0108 # rotation force/torque, 20~100 %
# RGI 旋转控制寄存器
REG_ROTATION_ANGLE = 0x0105 # 目标旋转角度，有符号角度值
REG_ROTATION_SPEED = 0x0107 # 旋转速度，1~100 %
REG_ROTATION_FORCE = 0x0108 # 旋转力/力矩，20~100 %
# Feedback registers
REG_INIT_STATE = 0x0200 # 0 uninitialized, 1 initialized, 2 initializing
REG_GRIPPER_STATE = 0x0201 # 0 moving, 1 reached, 2 object caught, 3 object dropped
REG_CURRENT_POSITION = 0x0202 # current finger position, per-mille
REG_ROTATION_ANGLE_FB = 0x0208 # current rotation angle, signed degree value
REG_ROTATION_INIT_STATE = 0x020A # 0 uninitialized, 1 initialized, 2 initializing
REG_ROTATION_STATE = 0x020B # 0 moving, 1 reached, 2 blocking, 3 had been blocked
# 反馈寄存器
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 曾经受阻
# Configuration registers
REG_SAVE_PARAMS = 0x0300 # write 1 to save all parameters to flash
# 配置寄存器
REG_SAVE_PARAMS = 0x0300 # 写入 1 可把所有参数保存到 flash
class DHModbusError(RuntimeError):
"""DH Modbus communication or protocol error."""
"""DH Modbus 通信或协议错误。"""
def _clamp_int(value: float | int, lo: int, hi: int) -> int:
@@ -55,9 +55,9 @@ def _clamp_int(value: float | int, lo: int, hi: int) -> int:
def _encode_signed(value: float | int) -> int:
"""Encode signed degree value as DH one's-complement register value.
"""把有符号角度值编码为 DH 的反码（one's complement）寄存器值。
RGI manual example: +360 -> 0x0168, -360 -> 0xFE97.
RGI 手册示例：+360 -> 0x0168，-360 -> 0xFE97。
"""
value = _clamp_int(value, -32767, 32767)
if value >= 0:
@@ -66,7 +66,7 @@ def _encode_signed(value: float | int) -> int:
def _decode_signed(raw: int) -> int:
"""Decode DH one's-complement register value to signed integer."""
"""把 DH 的反码（one's complement）寄存器值解码为有符号整数。"""
raw = int(raw) & 0xFFFF
if raw & 0x8000:
mag = (~raw) & 0x7FFF
@@ -75,7 +75,7 @@ def _decode_signed(raw: int) -> int:
class DHModbusClient:
"""Minimal thread-safe Modbus RTU client for DH-Robotics grippers."""
"""面向 DH-Robotics 夹爪的最小线程安全 Modbus RTU 客户端。"""
def __init__(
self,
@@ -91,7 +91,7 @@ class DHModbusClient:
try:
import minimalmodbus
import serial
except ImportError as exc: # pragma: no cover - depends on runtime env
except ImportError as exc: # pragma: no cover - 取决于运行时环境
raise DHModbusError(
"缺少 minimalmodbus/pyserial。请先安装：pip install minimalmodbus pyserial"
) from exc
@@ -124,7 +124,7 @@ class DHModbusClient:
pass
# ------------------------------------------------------------------
# Low-level register access
# 底层寄存器读写
# ------------------------------------------------------------------
def write_register(self, address: int, value: int) -> None:
value = _clamp_int(value, 0, 0xFFFF)
@@ -170,10 +170,10 @@ class DHModbusClient:
return _decode_signed(self.read_register(address))
# ------------------------------------------------------------------
# Common PGE/PGEA/RGI control
# PGE/PGEA/RGI 通用控制
# ------------------------------------------------------------------
def initialize(self, *, full: bool = False, rotation: bool = False, timeout: float = 10.0) -> None:
"""Initialize/homing the gripper and wait until ready."""
"""初始化/回零夹爪，并等待其就绪。"""
self.write_register(REG_INIT, 0xA5 if full else 0x01)
start = time.monotonic()
while time.monotonic() - start < timeout:
@@ -229,11 +229,11 @@ class DHModbusClient:
return self.read_register(REG_INIT_STATE)
def hold_current_position(self) -> None:
"""Best-effort stop/hold for DH grippers without a dedicated stop register."""
"""在没有专用停止寄存器的 DH 夹爪上尽力停止/保持。"""
self.set_position_per_mille(self.get_position_per_mille())
# ------------------------------------------------------------------
# RGI rotation control
# RGI 旋转控制
# ------------------------------------------------------------------
def set_rotation_speed_pct(self, speed_pct: float | int) -> int:
value = _clamp_int(speed_pct, 1, 100)
@@ -263,7 +263,7 @@ class DHModbusClient:
return self.read_register(REG_ROTATION_STATE)
def hold_current_rotation(self) -> None:
"""Stop rotation by re-commanding the current feedback angle."""
"""重新下发当前反馈角度来停止旋转。"""
self.write_register(REG_ROTATION_ANGLE, self.get_rotation_angle_raw())
def hold_current_all(self) -> None:
src/arm_application/arm_application/dh_pgea.py
+46 -46
View File
@@ -1,21 +1,21 @@
#!/usr/bin/env python3
"""DH Robotics PGE / PGEA Modbus RTU client.
"""DH Robotics PGE / PGEA Modbus RTU 客户端。
Port of the official C++ driver:
移植自官方 C++ 驱动：
DH-Robotics/dh_gripper_ros → DH_Modbus_Gripper
(factory maps model names containing ``PGE`` / ``PGEA`` onto this class)
（工厂会按型号名里是否包含 ``PGE`` / ``PGEA`` 映射到本类）
Flexiv RDK ``Gripper`` cannot drive this claw: it is not a Studio Device.
Talk RS485 (USB–UART) with the same registers the DH ROS driver uses.
Flexiv RDK 的 ``Gripper`` 驱动不了这个夹爪：它不是 Studio Device。
改用 RS485（USB–UART）通信，寄存器与 DH 的 ROS 驱动完全一致。
Registers (holding, FC03 read / FC06 write, CRC-16/Modbus LE):
0x0100 init write 0xA5
0x0101 force 20–100 %
0x0103 position 0–1000 ‰
0x0104 speed 1–100 %
0x0200 init state 0=need 1=done 2=doing
0x0201 grip state 0=moving 1=arrived 2=caught 3=dropped
0x0202 current position
寄存器（保持寄存器，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
@@ -31,11 +31,11 @@ try:
import serial
except ImportError as exc: # pragma: no cover
raise SystemExit(
"pyserial missing. In conda ros_flexiv: pip install pyserial"
"缺少 pyserial。在 conda 环境 ros_flexiv 里执行：pip install pyserial"
) from exc
# Official CRC-16/Modbus (same table as dh_modbus_gripper.cpp).
# 官方 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,
@@ -105,7 +105,7 @@ INIT_STATE_NAME = {
def crc16_modbus(data: bytes) -> int:
"""Official DH CRC16 (Modbus, little-endian on the wire)."""
"""官方 DH CRC16（Modbus，线路上按小端发送）。"""
crc = 0xFFFF
for byte in data:
crc = (crc >> 8) ^ _CRC_TABLE[(crc ^ byte) & 0xFF]
@@ -176,7 +176,7 @@ WIRING_HINT = (
class DhPgeaGripper:
"""Thread-safe Modbus RTU client for PGE / PGEA (official register map)."""
"""面向 PGE / PGEA 的线程安全 Modbus RTU 客户端（官方寄存器映射）。"""
def __init__(
self,
@@ -196,7 +196,7 @@ class DhPgeaGripper:
if mode in ("", "off", "none", "false", "0"):
mode = ""
if mode not in ("", "tx", "tx_inv"):
raise DhPgeaError(f"rs485_rts must be '', tx, or tx_inv, got {rs485_rts!r}")
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()
@@ -220,8 +220,8 @@ class DhPgeaGripper:
)
except Exception as exc:
raise DhPgeaError(
f"cannot open {self.port}: {exc}. "
"Need 24 V DC + RS485, and user in group dialout."
f"无法打开 {self.port}：{exc}。"
"需要 24 V DC 供电 + RS485 接线，且当前用户在 dialout 组内。"
) from exc
try:
self._ser.dtr = True
@@ -248,7 +248,7 @@ class DhPgeaGripper:
self.close()
def write_register(self, index: int, value: int, retries: int = 3) -> bool:
"""FC06 write, wait for echo (official WriteRegisterFunc)."""
"""FC06 写，等待回显（对应官方 WriteRegisterFunc）。"""
frame = _frame(self.slave_id, 0x06, index, int(value) & 0xFFFF)
with self._lock:
for _ in range(max(1, retries)):
@@ -258,7 +258,7 @@ class DhPgeaGripper:
return False
def read_register(self, index: int, retries: int = 8) -> int:
"""FC03 read one holding register (official ReadRegisterFunc)."""
"""FC03 读一个保持寄存器（对应官方 ReadRegisterFunc）。"""
req = _frame(self.slave_id, 0x03, index, 1)
last: Optional[bytes] = None
with self._lock:
@@ -275,13 +275,13 @@ class DhPgeaGripper:
return (rsp[3] << 8) | (rsp[4] & 0xFF)
hint = f" last={last.hex() if last else '(empty)'}"
raise DhPgeaError(
f"Modbus read 0x{index:04X} failed on {self.port} "
f"(no valid reply{hint}). baud={self.baudrate} slave={self.slave_id} "
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:
"""One short read of init-state. Raises if the claw is silent."""
"""短读一次初始化状态。夹爪无应答时会抛异常。"""
return self.read_register(REG_INIT_STATE, retries=2)
def _set_tx(self, tx: bool) -> None:
@@ -297,7 +297,7 @@ class DhPgeaGripper:
def _transact(self, frame: bytes, expect: int) -> Optional[bytes]:
if self._ser is None or not self._ser.is_open:
raise DhPgeaError("serial not open")
raise DhPgeaError("串口未打开")
bit_s = 10.0 / max(1, self.baudrate)
self._set_tx(True)
if self.rs485_rts:
@@ -320,12 +320,12 @@ class DhPgeaGripper:
return self.write_register(REG_INIT, INIT_CMD)
def initialize(self, timeout_s: float = 20.0, poll_s: float = 0.2) -> bool:
"""Write 0xA5 then wait until init state == finished (official bringup)."""
"""写 0xA5，然后等到初始化状态 == 完成（对应官方上电流程）。"""
state = self.get_init_state()
if state == S_INIT_FINISHED:
return True
if not self.initialization():
raise DhPgeaError("init write 0x0100=0xA5 failed (no Modbus echo)")
raise DhPgeaError("初始化写 0x0100=0xA5 失败（无 Modbus 回显）")
deadline = time.monotonic() + float(timeout_s)
while time.monotonic() < deadline:
state = self.get_init_state()
@@ -333,8 +333,8 @@ class DhPgeaGripper:
return True
time.sleep(poll_s)
raise DhPgeaError(
f"init timeout after {timeout_s:.0f}s (state={state}). "
"Fingers should home once on first power-up."
f"初始化在 {timeout_s:.0f}s 后超时（state={state}）。"
"首次上电时手指本应先回零一次。"
)
def set_target_force(self, force_pct: int) -> bool:
@@ -427,11 +427,11 @@ class DhPgeaGripper:
def _selfcheck_crc() -> None:
"""Official short-manual example: set position 500."""
"""官方简短手册示例：设置位置 500。"""
frame = _frame(1, 0x06, 0x0103, 500)
expect = bytes([0x01, 0x06, 0x01, 0x03, 0x01, 0xF4, 0x78, 0x21])
if frame != expect:
raise AssertionError(f"CRC mismatch: {frame.hex()} != {expect.hex()}")
raise AssertionError(f"CRC 不匹配：{frame.hex()} != {expect.hex()}")
def _site_defaults() -> dict:
@@ -465,11 +465,11 @@ def _site_defaults() -> dict:
def _scan_bus(port: str) -> int:
"""Try baud × slave × RTS until one combination answers."""
"""遍历波特率 × 从站号 × RTS，直到某个组合有应答。"""
bauds = (115200, 57600, 38400, 19200, 9600)
slaves = (1, 2, 3)
rts_modes = ("", "tx", "tx_inv")
print(f"scan {port} (stop the ROS node first — port is exclusive)")
print(f"扫描 {port}（请先停掉 ROS 节点 —— 该串口是独占的）")
for baud in bauds:
for slave in slaves:
for rts in rts_modes:
@@ -492,7 +492,7 @@ def _scan_bus(port: str) -> int:
print(f" miss {label}")
finally:
g.close()
print("no reply on any combination.", file=sys.stderr)
print("所有组合都无应答。", file=sys.stderr)
print(WIRING_HINT, file=sys.stderr)
return 1
@@ -500,24 +500,24 @@ def _scan_bus(port: str) -> int:
def _cli(argv: Optional[list[str]] = None) -> int:
site = _site_defaults()
ap = argparse.ArgumentParser(
description="DH PGEA Modbus CLI (official register map, not Flexiv RDK)"
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: empty | tx | tx_inv")
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="position 0–1000 for 'pos'")
ap.add_argument("value", nargs="?", default="", help="'pos' 命令的位置，0–1000")
args = ap.parse_args(argv)
if args.cmd == "selfcheck":
_selfcheck_crc()
print("CRC self-check OK (01 06 01 03 01 F4 78 21)")
print("CRC 自检通过（01 06 01 03 01 F4 78 21）")
return 0
if args.cmd == "scan":
return _scan_bus(args.port)
@@ -532,7 +532,7 @@ def _cli(argv: Optional[list[str]] = None) -> int:
try:
grip.open()
except DhPgeaError as exc:
print(f"ERROR: {exc}", file=sys.stderr)
print(f"错误：{exc}", file=sys.stderr)
return 1
try:
@@ -541,7 +541,7 @@ def _cli(argv: Optional[list[str]] = None) -> int:
print(st.summary())
return 0
if args.cmd == "init":
print("initializing (fingers home)…")
print("正在初始化（手指回零）…")
grip.initialize()
print(grip.get_run_states().summary())
return 0
@@ -550,25 +550,25 @@ def _cli(argv: Optional[list[str]] = None) -> int:
return 0
if args.cmd == "open":
if not grip.open_jaw(args.force, args.speed):
raise DhPgeaError("open write failed")
raise DhPgeaError("张开写入失败")
print(grip.wait_settle(1000).summary())
return 0
if args.cmd == "close":
if not grip.close_jaw(args.force, args.speed):
raise DhPgeaError("close write failed")
raise DhPgeaError("闭合写入失败")
print(grip.wait_settle(0).summary())
return 0
if args.cmd == "pos":
if args.value == "":
print("usage: pos <0-1000>", file=sys.stderr)
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("position write failed")
raise DhPgeaError("位置写入失败")
print(grip.wait_settle(pos).summary())
return 0
except DhPgeaError as exc:
print(f"ERROR: {exc}", file=sys.stderr)
print(f"错误：{exc}", file=sys.stderr)
print(WIRING_HINT, file=sys.stderr)
return 1
finally:
src/arm_application/arm_application/dh_pgea_node.py
+14 -14
View File
@@ -1,18 +1,18 @@
#!/usr/bin/env python3
"""ROS 2 node for DH PGEA (official Modbus RTU, not Flexiv gripper).
"""DH PGEA 的 ROS 2 节点（官方 Modbus RTU，不用 Flexiv 夹爪）。
Topics (layout matches DH-Robotics/dh_gripper_ros):
话题（布局与 DH-Robotics/dh_gripper_ros 一致）：
/gripper/ctrl std_msgs/Float32MultiArray
data: [initialize, position, force, speed]
initialize>0.5 → write 0xA5; else set force/speed/position
initialize>0.5 → 写 0xA5；否则设置力/速度/位置
/gripper/states std_msgs/Float32MultiArray
data: [initialized, grip_state, position, target_pos, target_force]
/gripper/joint_states sensor_msgs/JointState
Services (std_srvs/Trigger):
服务（std_srvs/Trigger）：
/gripper/initialize /gripper/open /gripper/close
Do not set Flexiv bringup load_gripper:=true — that is Flexiv-GN01 only.
不要设置 Flexiv bringup 的 load_gripper:=true —— 那只是 Flexiv-GN01 用的。
"""
from __future__ import annotations
@@ -36,7 +36,7 @@ from arm_application.dh_pgea import (
def _param_as_str(node: Node, name: str, default: str = "") -> str:
"""String param that also accepts numeric CLI overrides (e.g. SN 26610799)."""
"""字符串参数，同时允许命令行用数字覆盖（例如 SN 26610799）。"""
node.declare_parameter(name, default, ParameterDescriptor(dynamic_typing=True))
val = node.get_parameter(name).value
if val is None or val == "":
@@ -105,7 +105,7 @@ class DhPgeaNode(Node):
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"(official Modbus RTU; Flexiv RDK gripper API unused)"
f"（官方 Modbus RTU；未使用 Flexiv RDK 夹爪 API）"
)
try:
self._grip.open()
@@ -121,14 +121,14 @@ class DhPgeaNode(Node):
self.get_logger().error(str(exc))
for line in WIRING_HINT.splitlines():
self.get_logger().error(line)
self.get_logger().error("will keep retrying; open/close will fail until the bus answers")
self.get_logger().error("会持续重试；在总线恢复应答前 open/close 都会失败")
if self._auto_init and self._online:
try:
self._grip.initialize()
self.get_logger().info("gripper initialized")
self.get_logger().info("夹爪已初始化")
except DhPgeaError as exc:
self.get_logger().error(f"auto_init failed: {exc}")
self.get_logger().error(f"auto_init 失败：{exc}")
self._online = False
period = 1.0 / max(1.0, hz)
@@ -150,7 +150,7 @@ class DhPgeaNode(Node):
if speed is not None:
self._speed = s
if not self._grip.move(int(position), f, s):
raise DhPgeaError("move write failed")
raise DhPgeaError("移动写入失败")
return f"pos={int(position)} force={f}% speed={s}%"
def _on_ctrl(self, msg: Float32MultiArray) -> None:
@@ -158,7 +158,7 @@ class DhPgeaNode(Node):
try:
if initialize > 0.5:
self._grip.initialize()
self.get_logger().info("initialize via /gripper/ctrl")
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
@@ -210,7 +210,7 @@ class DhPgeaNode(Node):
self._state = st
if not self._online:
self._online = True
self.get_logger().info("gripper bus recovered")
self.get_logger().info("夹爪总线已恢复")
except DhPgeaError as exc:
self._online = False
self.get_logger().warn(str(exc), throttle_duration_sec=5.0)
@@ -227,7 +227,7 @@ class DhPgeaNode(Node):
js = JointState()
js.header.stamp = self.get_clock().now().to_msg()
js.name = ["gripper_finger1_joint"]
# Official ROS driver maps 0–1000 → ~0–0.637 rad; we publish mm as well.
# 官方 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)
src/arm_application/arm_application/dual_gripper.py
+1 -1
View File
@@ -750,7 +750,7 @@ def load_dual_arm_gripper_config(path: str | Path) -> DualArmGripperConfig:
def load_from_site(data: dict) -> DualArmGripperConfig:
"""Build DualArmGripperConfig from ~/ros2/config/site.yaml."""
"""从 ``~/ros2/config/site.yaml`` 构建 ``DualArmGripperConfig``。"""
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"), {})
src/arm_application/arm_application/dual_gripper_cli.py
+24 -24
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""CLI for DH PGEA (left) and RGI (right). Loads ports from site.yaml."""
"""DH PGEA（左臂）与 RGI（右臂）的命令行工具。从 site.yaml 读取串口。"""
from __future__ import annotations
@@ -39,67 +39,67 @@ def _normalize_arm(raw: str) -> str:
return "right"
if t in ("both", "dual", "all"):
return "both"
raise SystemExit(f"unknown --arm {raw!r} (use 89|91|left|right|both)")
raise SystemExit(f"未知的 --arm {raw!r}（可用 89|91|left|right|both）")
def test_parallel(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
gripper = manager.left
print(f"=== PGEA parallel ({gripper.cfg.port}) ===")
print(f"=== PGEA 平行夹爪（{gripper.cfg.port}）===")
gripper.connect()
_print_status("connected", gripper.get_status())
_print_status("已连接", gripper.get_status())
if args.test in ("init", "probe", "all"):
if args.test != "probe":
gripper.initialize(full=args.init_full)
_print_status("init", gripper.get_status())
_print_status("初始化", gripper.get_status())
if args.test in ("open", "all"):
_print_status("open", gripper.open(force=args.force, speed=args.speed))
_print_status("张开", gripper.open(force=args.force, speed=args.speed))
if args.test in ("close", "all"):
_print_status("close", gripper.close(force=args.force, speed=args.speed))
_print_status("闭合", gripper.close(force=args.force, speed=args.speed))
if args.test in ("move", "pos") and args.pos is not None:
_print_status("move", gripper.move_to(args.pos, force=args.force, speed=args.speed))
_print_status("移动", gripper.move_to(args.pos, force=args.force, speed=args.speed))
if args.test in ("status", "probe"):
_print_status("status", gripper.get_status())
_print_status("状态", gripper.get_status())
def test_rotary(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
gripper = manager.right
print(f"=== RGI rotary ({gripper.cfg.port}) ===")
print(f"=== RGI 旋转夹爪（{gripper.cfg.port}）===")
gripper.connect()
_print_status("connected", gripper.get_status())
_print_status("已连接", gripper.get_status())
if args.test in ("init", "all"):
gripper.initialize(full=args.init_full)
_print_status("init", gripper.get_status())
_print_status("初始化", gripper.get_status())
if args.test in ("open", "all"):
_print_status("open", gripper.open_fingers(force=args.force, speed=args.speed))
_print_status("张开", gripper.open_fingers(force=args.force, speed=args.speed))
if args.test in ("close", "all"):
_print_status("close", gripper.close_fingers(force=args.force, speed=args.speed))
_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("fingers", gripper.move_fingers(int(args.pos), force=args.force, speed=args.speed))
_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(
"revolutions",
"圈数",
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("rotate", gripper.rotate_to(angle, speed=args.speed, torque=args.force))
_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("rotate_by", gripper.rotate_by(delta, speed=args.speed, torque=args.force))
_print_status("相对旋转", gripper.rotate_by(delta, speed=args.speed, torque=args.force))
if args.test in ("status", "probe"):
_print_status("status", gripper.get_status())
_print_status("状态", gripper.get_status())
def main() -> int:
parser = argparse.ArgumentParser(description="DH PGEA / RGI CLI (site.yaml ports)")
parser = argparse.ArgumentParser(description="DH PGEA / RGI 命令行工具（串口来自 site.yaml）")
parser.add_argument("--arm", default=os.environ.get("ROBOT_ARM") or os.environ.get("ROBOT_SN") or "89")
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 or RGI finger 0-1000")
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)
@@ -110,7 +110,7 @@ def main() -> int:
site = Path(args.site).expanduser() if args.site else _site_path()
if not site.is_file():
print(f"ERROR: site.yaml missing: {site}", file=sys.stderr)
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))
@@ -126,11 +126,11 @@ def main() -> int:
test_parallel(manager, args)
test_rotary(manager, args)
except KeyboardInterrupt:
print("interrupt — stop")
print("收到中断 —— 停止")
manager.emergency_stop()
return 130
except Exception as exc:
print(f"ERROR: {exc}", file=sys.stderr)
print(f"错误：{exc}", file=sys.stderr)
return 1
finally:
manager.disconnect_all()
src/arm_application/arm_application/dual_gripper_node.py
+7 -7
View File
@@ -1,4 +1,4 @@
"""ROS 2 dual DH gripper node (PGEA left + RGI right). Uses arm_msgs, not Flexiv RDK."""
"""ROS 2 双臂 DH 夹爪节点（左臂 PGEA + 右臂 RGI）。使用 arm_msgs，而非 Flexiv RDK。"""
from __future__ import annotations
@@ -61,7 +61,7 @@ class DualGripperNode(Node):
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"gripper config from site.yaml: {site}")
self.get_logger().info(f"夹爪配置来自 site.yaml：{site}")
self._connect_errors: dict[str, str] = {}
if _as_bool(self.get_parameter("auto_connect").value, True):
@@ -112,12 +112,12 @@ class DualGripperNode(Node):
continue
try:
gripper.connect()
self.get_logger().info(f"{side} gripper connected: {gripper.cfg.port}")
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} gripper connect failed: {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"gripper connect failed: {self._connect_errors}")
raise RuntimeError(f"夹爪连接失败：{self._connect_errors}")
def _gripper(self, side: str):
return self._manager.left if side == "left" else self._manager.right
@@ -245,14 +245,14 @@ class DualGripperNode(Node):
status = gripper.get_status()
status.state = GripperState.ERROR
status.error_msg = f"unsupported command {command!r} (side={side})"
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} command {msg.command!r} failed: {status.error_msg}")
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]:
src/arm_application/arm_application/goto_ee_plane.py
+48 -50
View File
@@ -1,12 +1,12 @@
#!/usr/bin/env python3
"""Startup return-to-initial before perception/servo starts.
"""感知/伺服启动前的开机回初始位流程。
Sequence:
[A] if the flange is below servo.safe_z_m, lift it STRAIGHT UP first
(closed-loop Z hops) so the home move can't sweep walls/fixtures;
[B] call /set_named_pose (Flexiv named pose, default "home") — the arm
returns to the Flexiv initial pose with Flexiv's own planner;
[C] hold (pin) so the servo takes over from a clean, stationary pose.
流程：
[A] 若法兰低于 ``servo.safe_z_m``，先让它竖直上升（闭环 Z 小步跳），
以免回 home 时扫到墙面/工装；
[B] 调用 ``/set_named_pose``（Flexiv 命名位姿，默认 "home"）—— 由 Flexiv
自己的规划器把机械臂送回 Flexiv 初始位姿；
[C] 保持（pin）住，让伺服从一个干净、静止的位姿接手。
"""
from __future__ import annotations
@@ -49,13 +49,13 @@ def _load_ee_cfg() -> dict:
"base": str(robot.get("base_frame") or "Rizon4-063989_base_link"),
"ee": str(robot.get("ee_frame") or "Rizon4-063989_flange"),
"z": ee_z,
# Lift straight up to this Z before the home move (avoid hitting
# walls/fixtures); then return to the Flexiv named initial pose.
# 回 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),
# only used by the (currently unused) verticalize planner path
# 仅用于（目前未使用的）verticalize 规划路径
"kp": float(servo.get("ee_hold_kp") or 2.0),
"w_max": float(servo.get("ee_w_max") or 0.50),
}
@@ -170,7 +170,7 @@ class GotoEePlane(Node):
self._spin(0.05)
handle = fut.result() if fut.done() else None
if handle is None or not handle.accepted:
print(" ERROR: JTC rejected the height trajectory", flush=True)
print(" 错误：JTC 拒绝了抬升轨迹", flush=True)
return False
res = handle.get_result_async()
t0 = time.monotonic()
@@ -183,13 +183,13 @@ class GotoEePlane(Node):
if Tee is not None:
p = Tee[:3, 3]
print(
f" moving ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f}) "
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(" WARN: JTC result timeout — checking TF anyway", flush=True)
print(" 警告：JTC 结果超时 —— 仍然继续检查 TF", flush=True)
return False
return True
@@ -203,25 +203,24 @@ class GotoEePlane(Node):
max_seg: float = 0.08,
v: float = 0.05,
) -> bool:
"""Plan and execute ONE short cartesian segment (≤ max_seg) toward
target. Long moves are handled by the caller in closed-loop hops,
because the Jacobian is frozen at the current pose — over >10 cm the
linearization error makes single-shot plans land far off target."""
"""规划并执行一段朝向 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: # clamp this hop; caller loops with fresh TF
if dist > max_seg: # 限制本次小步跳的长度；调用方会用最新的 TF 循环执行
dp = dp * (max_seg / dist)
dist = max_seg
print(
f" plan {tag} Δxyz=({dp[0]*1000:+.0f},{dp[1]*1000:+.0f},{dp[2]*1000:+.0f})mm "
f"tilt={ang * 180 / np.pi:.1f}deg",
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("ERROR: missing link TF for Jacobian", flush=True)
print("错误：缺少用于雅可比的 link TF", flush=True)
return False
q = q0.copy()
@@ -253,14 +252,14 @@ class GotoEePlane(Node):
planned = dist - float(np.linalg.norm(remain))
if len(waypoints) < 2 or planned < 0.4 * dist:
print(
f" WARN {tag}: planner stalled "
f"(planned {planned*1000:.0f}/{dist*1000:.0f}mm) — skip hop",
f" 警告 {tag}：规划器停滞 "
f"（已规划 {planned*1000:.0f}/{dist*1000:.0f}mm）—— 跳过本次小步跳",
flush=True,
)
return False
# joint-excursion sanity gate: refuse wild plans
# 关节偏移量的合理性闸门：拒绝过于夸张的规划
if float(np.max(np.abs(waypoints[-1] - q0))) > 1.0:
print(f" ERROR {tag}: joint excursion >1 rad — abort hop", flush=True)
print(f" 错误 {tag}：关节偏移 >1 rad —— 放弃本次小步跳", flush=True)
return False
duration = max(1.6, dist / max(min(v, 0.06), 1e-3))
@@ -278,10 +277,10 @@ class GotoEePlane(Node):
)
qdot = np.clip(qdot, -0.40, 0.40)
traj.points.append(_pt(qi, qdot, t))
# force zero velocity at the last point
# 强制最后一个点的速度为零
traj.points[-1].velocities = [0.0] * 7
print(
f" send JTC action {tag} {n} steps T={duration:.1f}s",
f" 发送 JTC action {tag}，{n} 步 T={duration:.1f}s",
flush=True,
)
return self._send_action(traj, wait_s=duration + 6.0)
@@ -298,9 +297,8 @@ class GotoEePlane(Node):
v: float,
max_hops: int = 8,
) -> Tuple[bool, np.ndarray, np.ndarray]:
"""Closed-loop point-to-point: plan ≤8 cm hop → execute → re-measure
TF → repeat, until within tolerance. Robust to Jacobian linearization
error over long distances."""
"""闭环点到点：规划 ≤8 cm 的小步跳 → 执行 → 重新测量 TF → 重复，
直到落在容差内。这样对长距离下的雅可比线性化误差更鲁棒。"""
for hop in range(max_hops):
p = Tee[:3, 3]
dp, ang, _ = self._target_err(p, Tee[:3, :3], target, verticalize)
@@ -308,8 +306,8 @@ class GotoEePlane(Node):
if dist < tol_m and ((not verticalize) or ang < tol_tilt):
return True, q, Tee
print(
f" {tag} hop{hop + 1} remain={dist*1000:.0f}mm "
f"tilt={ang * 180 / np.pi:.1f}deg",
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):
@@ -319,7 +317,7 @@ class GotoEePlane(Node):
self._spin(0.05)
q2, Tee2 = self._wait_state(2.0)
if q2 is None or Tee2 is None:
print("ERROR: lost state during move", flush=True)
print("错误：移动过程中丢失状态", flush=True)
return False, q, Tee
q, Tee = q2, Tee2
p = Tee[:3, 3]
@@ -340,10 +338,10 @@ class GotoEePlane(Node):
self._send_action(traj, wait_s=3.0)
def _named_pose(self, name: str, speed: float) -> bool:
"""Call Flexiv /set_named_pose and wait until the arm is stationary."""
"""调用 Flexiv 的 ``/set_named_pose``，并等待机械臂静止。"""
cli = self.create_client(SetNamedPose, "/set_named_pose")
if not cli.wait_for_server(timeout_sec=8.0):
print("ERROR: /set_named_pose unavailable — start run_real.sh", flush=True)
print("错误：``/set_named_pose`` 不可用 —— 请启动 run_real.sh", flush=True)
return False
req = SetNamedPose.Request()
req.pose_name = name
@@ -355,16 +353,16 @@ class GotoEePlane(Node):
while not fut.done() and time.monotonic() - t0 < 90.0:
self._spin(0.1)
if not fut.done():
print("ERROR: set_named_pose timeout", flush=True)
print("错误：set_named_pose 超时", flush=True)
return False
resp = fut.result()
print(
f" [B] resp success={resp.success} msg={resp.message}",
f" [B] 响应 success={resp.success} msg={resp.message}",
flush=True,
)
if not resp.success:
return False
# wait until the flange stops moving (Δp < 1 mm over ~0.8 s)
# 等待法兰停止运动（约 0.8 s 内 Δp < 1 mm）
last = None
still = 0.0
t0 = time.monotonic()
@@ -380,57 +378,57 @@ class GotoEePlane(Node):
else:
still = 0.0
last = p.copy()
print(" WARN: still-moving wait timeout — continue anyway", flush=True)
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("ERROR: no joint_states / TF — start run_real.sh", flush=True)
print("错误：没有 joint_states / TF —— 请启动 run_real.sh", flush=True)
return 2
if not self._cli.wait_for_server(timeout_sec=8.0):
print("ERROR: JTC action unavailable — start run_real.sh", flush=True)
print("错误：JTC action 不可用 —— 请启动 run_real.sh", flush=True)
return 3
p = Tee[:3, 3]
safe_z = float(cfg["safe_z"])
print(
f"=== return to Flexiv initial pose '{cfg['home_pose']}' "
f"=== 回到 Flexiv 初始位姿 '{cfg['home_pose']}' "
f"safe_z={safe_z*1000:.0f}mm ===",
flush=True,
)
print(
f" now ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f})",
f" 当前 ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f})",
flush=True,
)
# --- Phase A: pure-Z lift to the safe height before the home move ---
# --- 阶段 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] lift Z {p[2]*1000:.0f} → {safe_z*1000:.0f}mm", flush=True)
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("ERROR: lift phase failed — pin here", flush=True)
print("错误：抬升阶段失败 —— 就地 pin 住", flush=True)
self._pin(q)
return 4
# --- Phase B: Flexiv named pose → real initial pose, good posture ---
# --- 阶段 B：Flexiv 命名位姿 → 真正的初始位姿，姿态良好 ---
if not self._named_pose(cfg["home_pose"], cfg["home_speed"]):
print("ERROR: named-pose return failed — pin here", flush=True)
print("错误：回命名位姿失败 —— 就地 pin 住", flush=True)
self._pin(q)
return 4
# --- Phase C: hold so the servo takes over from a stationary pose ---
# --- 阶段 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" at initial pose ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f})",
f" 已到初始位姿 ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f})",
flush=True,
)
return 0
src/arm_application/arm_application/handeye_calibrate.py
+71 -66
View File
@@ -1,14 +1,14 @@
#!/usr/bin/env python3
"""Full hand-eye pipeline: move -> stabilize chessboard -> capture -> solve -> cleanup.
"""完整手眼标定流水线：移动 → 棋盘稳定判定 → 采集 → 求解 → 清理。
Supports eye_in_hand / eye_to_hand.
支持 eye_in_hand / eye_to_hand。
Camera intrinsics priority:
1) ROS CameraInfo topic (if publishing)
2) RealSense D405/D435 EEPROM via pyrealsense2 (camera_source)
3) Manual camera_fx/fy/cx/cy in YAML
相机内参优先级：
1) ROS 的 CameraInfo 话题（若正在发布）
2) 通过 pyrealsense2 读取 RealSense D405/D435 的 EEPROM（camera_source）
3) YAML 中手写的 camera_fx/fy/cx/cy
Chessboard size (board_cols / board_rows / square_size_m) is user-configurable.
棋盘尺寸（board_cols / board_rows / square_size_m）可由用户配置。
"""
from __future__ import annotations
@@ -68,7 +68,7 @@ def _image_to_bgr(msg: Image) -> np.ndarray:
img = data.reshape(h, w, 4)
bgr = img[:, :, :3]
return bgr[:, :, ::-1].copy() if enc == 'rgba8' else bgr
raise RuntimeError(f'Unsupported image encoding: {msg.encoding!r}')
raise RuntimeError(f'不支持的图像编码：{msg.encoding!r}')
def _camera_info_to_intrinsics(msg: CameraInfo) -> CameraIntrinsics:
@@ -93,7 +93,7 @@ class HandeyeCalibrate(Node):
self.declare_parameter('waypoint_positions', [0.0] * (12 * 7))
self.declare_parameter('min_poses', 5)
# Camera: manual | d405 | d435
# 相机来源：manual | d405 | d435
self.declare_parameter('camera_source', 'manual')
self.declare_parameter('camera_serial', '')
self.declare_parameter('camera_namespace', '')
@@ -107,12 +107,12 @@ class HandeyeCalibrate(Node):
self.declare_parameter('camera_cy', 0.0)
self.declare_parameter('camera_dist', [0.0, 0.0, 0.0, 0.0, 0.0])
# Chessboard (customizable)
# 棋盘（可自定义）
# 默认值必须与 site.yaml 的 handeye.board_* 及结果文件头里的实际板子一致。
# 真实使用的板：内角点 8x11，方格 15mm（见 handeye/results/*.yaml 的头部注释）。
# 这里原先写的是 9x6 / 0.025（另一张早期的板），一旦 site.yaml 漏配就会被
# 静默启用 —— 症状是"标定能顺利跑完，但结果明显不对"，极难排查。
self.declare_parameter('board_cols', 8) # inner corners
self.declare_parameter('board_cols', 8) # 内角点
self.declare_parameter('board_rows', 11)
self.declare_parameter('square_size_m', 0.015)
@@ -205,18 +205,18 @@ class HandeyeCalibrate(Node):
if not img_topic:
self.get_logger().error(
'image_topic empty. Set camera_source:=d405|d435 or fill image_topic.'
'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'Subscribed image: {img_topic} (BEST_EFFORT)')
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'Subscribed camera_info: {info_topic} (BEST_EFFORT)')
self.get_logger().info(f'已订阅 camera_info：{info_topic}（BEST_EFFORT）')
def _on_js(self, msg: JointState) -> None:
self._latest_js = msg
@@ -230,20 +230,20 @@ class HandeyeCalibrate(Node):
def run(self) -> int:
if self.mode not in ('eye_in_hand', 'eye_to_hand'):
self.get_logger().error(
f'Unknown mode={self.mode!r}; use eye_in_hand or eye_to_hand'
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'Only {len(poses)} filled poses (need >= {self.min_poses}). '
'Teach with: bash ~/ros2/scripts/run_handeye.sh teach && merge.'
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} >= filled poses {len(poses)}'
f'start_index {self.start_index} >= 已填写位姿数 {len(poses)}'
)
return 1
@@ -252,8 +252,8 @@ class HandeyeCalibrate(Node):
if not self._client.wait_for_server(timeout_sec=15.0):
self.get_logger().error(
f'Action [{self.action_name}] unavailable. '
'Start: bash ~/ros2/scripts/run_real.sh'
f'Action [{self.action_name}] 不可用。'
'请启动：bash ~/ros2/scripts/run_real.sh'
)
return 2
@@ -262,11 +262,11 @@ class HandeyeCalibrate(Node):
eye_to_hand = self.mode == 'eye_to_hand'
self.get_logger().info(
f'Hand-eye [{self.mode}] camera_source={self.camera_source}: '
f'{len(poses)} poses, board={self.board.cols}x{self.board.rows} '
f'square={self.board.square_size}m, v={self.velocity_scale}'
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}'
)
self.get_logger().warn('Clear workspace; E-Stop ready. Ctrl+C aborts.')
self.get_logger().warn('清理工作空间；确认急停就绪。Ctrl+C 可中止。')
Rg: List[np.ndarray] = []
tg: List[np.ndarray] = []
@@ -277,17 +277,17 @@ class HandeyeCalibrate(Node):
try:
for i in range(self.start_index, len(poses)):
q = poses[i]
self._log_progress(i, len(poses), 'moving...')
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}] arm did not reach target joints '
'(controller reported OK but joints unchanged). '
'Restart run_real.sh; pendant AUTO Remote; clear fault. '
'Do NOT trust this calibration.'
f'[{i + 1}] 机械臂未到达目标关节位置 '
'（控制器报告成功，但关节没有变化）。'
'请重启 run_real.sh；示教器切 AUTO Remote；清除故障。'
'不要相信这次标定结果。'
)
return 10
@@ -301,16 +301,16 @@ class HandeyeCalibrate(Node):
ok, std, corners, pattern, bgr = self._wait_stable()
if not ok or corners is None or bgr is None:
msg = f'[{i + 1}] chessboard not stable (std={std:.3f})'
msg = f'[{i + 1}] 棋盘不稳定（std={std:.3f}）'
if self.skip_unstable:
self.get_logger().warn(f'{msg} — skip')
self.get_logger().warn(f'{msg} —— 跳过')
continue
self.get_logger().error(f'{msg}. Abort.')
self.get_logger().error(f'{msg}。中止。')
return 7
ee = self._read_ee_pose()
if ee is None:
self.get_logger().error('Failed to read ee_pose')
self.get_logger().error('读取 ee_pose 失败')
return 8
assert self._intrinsics is not None
@@ -349,20 +349,20 @@ class HandeyeCalibrate(Node):
}
)
self.get_logger().info(
f'[{i + 1}/{len(poses)}] CAPTURED '
f'[{i + 1}/{len(poses)}] 已采集 '
f'(corner_std={std:.3f}px) -> {img_path.name}'
)
except KeyboardInterrupt:
self.get_logger().info('Interrupted')
self.get_logger().info('已中断')
return 0
if len(Rg) < self.min_poses:
self.get_logger().error(
f'Only {len(Rg)} captures; need >= {self.min_poses} to solve'
f'只采集到 {len(Rg)} 次；求解至少需要 {self.min_poses} 次'
)
return 9
self.get_logger().info(f'Solving hand-eye with {len(Rg)} views...')
self.get_logger().info(f'用 {len(Rg)} 个视角求解手眼标定...')
R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=eye_to_hand)
name = (
'T_cam_to_base (eye_to_hand)'
@@ -371,7 +371,7 @@ class HandeyeCalibrate(Node):
)
yaml_txt = format_transform_yaml(R, t, name)
header = (
f'# Hand-eye result\n'
f'# 手眼标定结果\n'
f'# mode: {self.mode}\n'
f'# camera_source: {self.camera_source}\n'
f'# views: {len(Rg)}\n'
@@ -385,17 +385,17 @@ class HandeyeCalibrate(Node):
json.dumps(meta, indent=2), encoding='utf-8'
)
self.get_logger().info('======== Hand-eye result ========')
self.get_logger().info('======== 手眼标定结果 ========')
for line in (header + yaml_txt).strip().splitlines():
self.get_logger().info(line)
self.get_logger().info(f'Saved -> {self.result_file}')
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('Calibration images deleted')
self.get_logger().info('标定图像已删除')
else:
self.get_logger().info(f'Images kept in {self.images_dir}')
self.get_logger().info(f'图像保留在 {self.images_dir}')
return 0
def _load_poses(self) -> List[List[float]]:
@@ -407,8 +407,8 @@ class HandeyeCalibrate(Node):
need = n_cfg * n_j
if len(flat) < need:
self.get_logger().warn(
f'waypoint_positions length {len(flat)} < num_poses*{n_j}={need}; '
'padding with zeros'
f'waypoint_positions 长度 {len(flat)} < num_poses*{n_j}={need}；'
'用零补齐'
)
flat = flat + [0.0] * (need - len(flat))
poses: List[List[float]] = []
@@ -417,7 +417,7 @@ class HandeyeCalibrate(Node):
if is_pose_filled(q):
poses.append(q)
else:
self.get_logger().warn(f'Pose slot {i} empty (all-zero) — skipped')
self.get_logger().warn(f'位姿槽 {i} 为空（全零）—— 已跳过')
return poses
def _resolve_intrinsics(self) -> bool:
@@ -430,7 +430,7 @@ class HandeyeCalibrate(Node):
self._intrinsics = _camera_info_to_intrinsics(self._latest_info)
K = self._intrinsics.K
self.get_logger().info(
f'Intrinsics from CameraInfo ({self._info_topic}): '
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
@@ -445,19 +445,19 @@ class HandeyeCalibrate(Node):
height=self.rs_height,
)
except Exception as exc: # noqa: BLE001
self.get_logger().error(f'RealSense intrinsics query failed: {exc}')
self.get_logger().error(f'RealSense 内参查询失败：{exc}')
else:
self._intrinsics = CameraIntrinsics(K=intr.as_K(), dist=intr.as_dist())
self.get_logger().info(
f'Intrinsics from RealSense {intr.model.upper()} '
f'serial={intr.serial} {intr.width}x{intr.height}: '
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'Requested {self.rs_width}x{self.rs_height}, '
f'got {intr.width}x{intr.height} (closest profile)'
f'请求 {self.rs_width}x{self.rs_height}，'
f'实际得到 {intr.width}x{intr.height}（最接近的 profile）'
)
return True
@@ -468,8 +468,8 @@ class HandeyeCalibrate(Node):
cy = float(self.get_parameter('camera_cy').value)
if fx <= 0 or fy <= 0:
self.get_logger().error(
'No intrinsics. Set camera_source:=d405|d435, '
'or provide camera_info_topic, or fill camera_fx/fy/cx/cy.'
'没有内参。请设置 camera_source:=d405|d435，'
'或提供 camera_info_topic，或填写 camera_fx/fy/cx/cy。'
)
return False
dist = np.array(
@@ -478,7 +478,7 @@ class HandeyeCalibrate(Node):
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'Intrinsics from YAML: fx={fx:.2f} fy={fy:.2f} cx={cx:.2f} cy={cy:.2f}'
f'内参来自 YAML：fx={fx:.2f} fy={fy:.2f} cx={cx:.2f} cy={cy:.2f}'
)
return True
@@ -561,7 +561,7 @@ class HandeyeCalibrate(Node):
try:
bgr = _image_to_bgr(self._latest_image)
except Exception as exc: # noqa: BLE001
self.get_logger().error(f'Image convert failed: {exc}')
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
@@ -574,9 +574,9 @@ class HandeyeCalibrate(Node):
last_bgr = bgr
now = time.monotonic()
if now - last_log > 1.0:
seen = 'yes' if corners is not None else 'NO'
seen = '有' if corners is not None else '无'
self.get_logger().info(
f' board={seen} corner_std='
f' 棋盘={seen} 角点标准差='
f'{std if std < 1e8 else float("nan"):.3f}px'
)
last_log = now
@@ -586,7 +586,12 @@ class HandeyeCalibrate(Node):
def _verify_joints(self, target: List[float], tol: float = 0.08) -> bool:
"""Confirm joint_states reached target (detect fake trajectory success)."""
"""确认 ``joint_states`` 真的走到了目标位置。
用来识破"假的成功"：有些执行接口会立刻返回成功，但关节其实没动
（或只动了一半）。这里在 2 秒内轮询关节角，逐轴比对容差 ``tol``（弧度），
只有全部落在容差内才算真到位。
"""
deadline = time.monotonic() + 2.0
while rclpy.ok() and time.monotonic() < deadline:
rclpy.spin_once(self, timeout_sec=0.05)
@@ -602,10 +607,10 @@ class HandeyeCalibrate(Node):
continue
err = float(np.sqrt(np.mean([(a - b) ** 2 for a, b in zip(actual, target)])))
self.get_logger().info(
f' joint RMSE vs target: {err:.4f} rad (tol={tol})'
f' 关节相对目标的 RMSE：{err:.4f} rad（tol={tol}）'
)
return err <= tol
self.get_logger().error('No joint_states while verifying motion')
self.get_logger().error('校验运动时没有 joint_states')
return False
def _move_one(self, positions: List[float], duration: float) -> int:
@@ -617,18 +622,18 @@ class HandeyeCalibrate(Node):
goal.time_from_start = [float(duration)]
send = self._client.send_goal_async(goal)
if not self._wait_future(send, 10.0, 'send 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('Goal rejected')
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), 'result'):
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'Move failed: {result.message}')
self.get_logger().error(f'移动失败：{result.message}')
return 6
return 0
@@ -637,7 +642,7 @@ class HandeyeCalibrate(Node):
while rclpy.ok() and not fut.done():
rclpy.spin_once(self, timeout_sec=0.05)
if time.monotonic() > deadline:
self.get_logger().error(f'Timeout waiting for {label}')
self.get_logger().error(f'等待 {label} 超时')
return False
return fut.done()
@@ -656,7 +661,7 @@ class HandeyeCalibrate(Node):
import cv2
if not cv2.imwrite(str(path), bgr):
raise RuntimeError(f'cv2.imwrite failed: {path}')
raise RuntimeError(f'cv2.imwrite 失败：{path}')
def main(args=None) -> None:
src/arm_application/arm_application/handeye_lib.py
+16 -16
View File
@@ -1,8 +1,8 @@
#!/usr/bin/env python3
"""Hand-eye helpers: chessboard stabilize + AX=XB solve.
"""手眼标定辅助工具：棋盘稳定判定 + AX=XB 求解。
Uses ``cv2.calibrateHandEye`` when present; otherwise Tsai-Lenz (NumPy).
Chessboard detection always needs OpenCV.
有 ``cv2.calibrateHandEye`` 时用它，否则用 Tsai-Lenz（NumPy 实现）。
棋盘检测始终依赖 OpenCV。
"""
from __future__ import annotations
@@ -21,9 +21,9 @@ except Exception: # noqa: BLE001
@dataclass
class BoardConfig:
cols: int # inner corners (width)
rows: int # inner corners (height)
square_size: float # meters
cols: int # 内角点数（宽）
rows: int # 内角点数（高）
square_size: float # 米
@dataclass
@@ -78,9 +78,9 @@ def board_object_points(cfg: BoardConfig) -> np.ndarray:
def detect_chessboard(
image_bgr: np.ndarray, cfg: BoardConfig
) -> Optional[Tuple[np.ndarray, Tuple[int, int]]]:
"""Return (corners, pattern_size) or None. pattern is (cols, rows) used."""
"""返回 (角点, pattern_size) 或 None。pattern 为实际使用的 (cols, rows)。"""
if cv2 is None:
raise RuntimeError('OpenCV (cv2) is required for chessboard detection')
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
@@ -105,7 +105,7 @@ def solve_board_pose(
pattern: Optional[Tuple[int, int]] = None,
) -> Tuple[np.ndarray, np.ndarray]:
if cv2 is None:
raise RuntimeError('OpenCV (cv2) is required for solvePnP')
raise RuntimeError('solvePnP 需要 OpenCV（cv2）')
if pattern is None:
pattern = (cfg.cols, cfg.rows)
board = BoardConfig(pattern[0], pattern[1], cfg.square_size)
@@ -114,7 +114,7 @@ def solve_board_pose(
obj, corners, cam.K, cam.dist, flags=cv2.SOLVEPNP_ITERATIVE
)
if not ok:
raise RuntimeError('solvePnP failed')
raise RuntimeError('solvePnP 失败')
R, _ = cv2.Rodrigues(rvec)
return R.astype(float), np.asarray(tvec, dtype=float).reshape(3, 1)
@@ -172,10 +172,10 @@ def calibrate_hand_eye_tsai(
R_target2cam: List[np.ndarray],
t_target2cam: List[np.ndarray],
) -> Tuple[np.ndarray, np.ndarray]:
"""Tsai-Lenz. Returns R_cam2gripper, t_cam2gripper."""
"""Tsai-Lenz 方法。返回 R_cam2gripper、t_cam2gripper。"""
n = len(R_gripper2base)
if n < 3:
raise ValueError('Need >= 3 poses')
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)]
@@ -220,8 +220,8 @@ def calibrate_hand_eye(
eye_to_hand: bool = False,
) -> Tuple[np.ndarray, np.ndarray]:
"""
eye_in_hand -> X = cam -> gripper (mounted on flange)
eye_to_hand -> X = cam -> base (fixed camera)
eye_in_hand -> X = cam -> gripper（相机装在法兰上）
eye_to_hand -> X = cam -> base（相机固定安装）
"""
Rg = list(R_gripper2base)
tg = list(t_gripper2base)
@@ -240,7 +240,7 @@ def calibrate_hand_eye(
)
return np.asarray(R, float), np.asarray(t, float).reshape(3, 1)
except Exception:
pass # fall back to NumPy Tsai
pass # 退回 NumPy 版 Tsai
return calibrate_hand_eye_tsai(Rg, tg, R_target2cam, t_target2cam)
@@ -257,7 +257,7 @@ def format_transform_yaml(R: np.ndarray, t: np.ndarray, name: str) -> str:
def is_pose_filled(joints: Sequence[float]) -> bool:
"""False if all zeros / NaN placeholder."""
"""若全为零 / NaN 占位则返回 False。"""
arr = np.asarray(joints, dtype=float)
if arr.size != 7:
return False
src/arm_application/arm_application/handeye_realsense.py
+22 -22
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""Query RealSense (D405 / D435) factory color intrinsics via pyrealsense2."""
"""通过 pyrealsense2 查询 RealSense（D405 / D435）的出厂彩色内参。"""
from __future__ import annotations
@@ -19,7 +19,7 @@ class RealsenseIntrinsics:
fy: float
cx: float
cy: float
dist: List[float] # OpenCV order, length 5
dist: List[float] # OpenCV 顺序，长度 5
def as_K(self) -> np.ndarray:
return np.array(
@@ -32,7 +32,7 @@ class RealsenseIntrinsics:
return np.array(d[:5], dtype=float).reshape(-1, 1)
# Default ROS topic namespaces when camera_source is d405/d435.
# camera_source 为 d405/d435 时的默认 ROS 话题命名空间。
REALSENSE_TOPIC_DEFAULTS = {
'd405': {
'image_topic': '/d405/color/image_raw',
@@ -55,13 +55,13 @@ def _norm_model(model: str) -> str:
def list_realsense_devices() -> List[Tuple[str, str]]:
"""Return [(name, serial), ...]."""
"""返回 [(name, serial), ...]。"""
try:
import pyrealsense2 as rs
except Exception as exc: # noqa: BLE001
raise RuntimeError(
'pyrealsense2 not installed. '
'Install in ros_flexiv: pip/conda install pyrealsense2'
'未安装 pyrealsense2。'
'请在 ros_flexiv 环境中安装：pip/conda install pyrealsense2'
) from exc
out: List[Tuple[str, str]] = []
for d in rs.context().devices:
@@ -79,27 +79,27 @@ def query_realsense_color_intrinsics(
height: int = 720,
) -> RealsenseIntrinsics:
"""
Read on-device color intrinsics for D405/D435 at the given resolution.
在给定分辨率下读取 D405/D435 设备上的彩色内参。
Does not start a streaming pipeline (uses profile metadata).
不会启动流管道（只读取 profile 元数据）。
"""
try:
import pyrealsense2 as rs
except Exception as exc: # noqa: BLE001
raise RuntimeError(
'pyrealsense2 not installed. '
'Install in ros_flexiv: pip/conda install pyrealsense2'
'未安装 pyrealsense2。'
'请在 ros_flexiv 环境中安装：pip/conda install pyrealsense2'
) from exc
want = _norm_model(model)
if want not in ('d405', 'd435'):
raise ValueError(f'Unsupported model {model!r}; use d405 or 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('No RealSense device connected')
raise RuntimeError('未连接任何 RealSense 设备')
chosen = None
for d in devices:
@@ -119,14 +119,14 @@ def query_realsense_color_intrinsics(
for d in devices
)
raise RuntimeError(
f'No RealSense matching model={want!r} serial={serial!r}. '
f'Connected: {avail or "none"}'
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 # (score, width, height, fx, fy, cx, cy, coeffs)
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:
@@ -155,7 +155,7 @@ def query_realsense_color_intrinsics(
best = cand
if best is None:
raise RuntimeError(f'No color stream profile on {name} ({sn})')
raise RuntimeError(f'{name} ({sn}) 上没有彩色流 profile')
_, w, h, fx, fy, cx, cy, coeffs = best
while len(coeffs) < 5:
@@ -174,7 +174,7 @@ def query_realsense_color_intrinsics(
def is_realsense_source(name: str) -> bool:
"""True for d405 / d435 and namespaced aliases (d405_left, d405_right)."""
"""对 d405 / d435 及带命名空间的别名（d405_left、d405_right）返回 True。"""
return _norm_model(name or "") in ("d405", "d435")
@@ -185,9 +185,9 @@ def resolve_realsense_topics(
image_topic: str = '',
camera_info_topic: str = '',
) -> Tuple[str, str]:
"""Fill default image/camera_info topics for d405/d435 if empty.
"""为空时，为 d405/d435 填充默认的 image/camera_info 话题。
``model`` may be a topic namespace such as ``d405_left`` / ``d405_right``.
``model`` 可以是 ``d405_left`` / ``d405_right`` 这类话题命名空间。
"""
ns = (camera_namespace or model or 'd405').strip().strip('/')
img = (image_topic or '').strip()
@@ -210,7 +210,7 @@ def apply_realsense_exposure(
gain: float | None = None,
log=None,
) -> list[str]:
"""Cap AE / kill projector. Wrist D405 overexposure is usually long AE + emitter."""
"""限制自动曝光 / 关闭投影器。腕部 D405 过曝通常是自动曝光过长 + 发射器导致。"""
import pyrealsense2 as rs
notes: list[str] = []
@@ -230,7 +230,7 @@ def apply_realsense_exposure(
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))
# Must disable AE *before* writing exposure; D405 firmware ignores exposure while AE is on.
# 必须在写曝光*之前*关闭自动曝光；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)
@@ -247,5 +247,5 @@ def apply_realsense_exposure(
if gain is not None and _set(sensor, rs.option.gain, float(gain)):
notes.append(f"gain={gain}")
if log and notes:
log("RealSense exposure: " + ", ".join(dict.fromkeys(notes)))
log("RealSense 曝光设置：" + ", ".join(dict.fromkeys(notes)))
return notes
src/arm_application/arm_application/handeye_teach.py
+86 -86
View File
@@ -1,26 +1,26 @@
#!/usr/bin/env python3
"""Hand-eye teach: live preview + Free Drive + Enter to photo/record joints.
"""手眼示教：实时预览 + Free Drive + 按 Enter 拍照/记录关节。
Two backends
两种后端
------------
ros (default, stack running):
- Joints from /get_arm_state
- Drag: teach pendant Free Drive + hold enable
Keep: bash ~/ros2/scripts/run_real.sh
ros（默认，整套栈正在运行）：
- 关节来自 ``/get_arm_state``
- 拖拽：示教器 Free Drive + 按住使能键
需保持：bash ~/ros2/scripts/run_real.sh
rdk (software Free Drive):
- Stop run_real.sh first (RDK exclusive)
- Press 'd' to enable FloatingJoint drag via RDK
- Joints from RDK robot.states
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
UI keys (preview window)
Enter / Space photo + record joints
d enable Free Drive (rdk) / show pendant tip (ros)
s stop Free Drive (rdk) / hold
q / Esc finish
界面按键（预览窗口）
Enter / Space 拍照 + 记录关节
d 启用 Free Drive（rdk）/ 显示示教器提示（ros）
s 停止 Free Drive（rdk）/ 保持
q / Esc 结束
"""
from __future__ import annotations
@@ -40,21 +40,21 @@ from rclpy.node import Node
HELP = """
================================================
Hand-eye TEACH: preview -> drag -> photo+pose
手眼示教 TEACH：预览 -> 拖拽 -> 拍照+位姿
================================================
Window keys:
Enter / Space = take photo + save joints
d = Free Drive ON
s = Free Drive OFF / hold
q / Esc = finish
Tip: make board clear & fill ~30-70% of image
Prefer 8-12 poses with varied angles
窗口按键：
Enter / Space = 拍照 + 保存关节
d = 开启 Free Drive
s = 关闭 Free Drive / 保持
q / Esc = 结束
提示：让棋盘清晰，并占到画面的约 30-70%
建议采 8-12 个角度有变化的位姿
================================================
"""
def _setup_matplotlib_font():
"""Avoid Chinese mojibake: prefer CJK font if present."""
"""避免中文乱码：优先选用存在的中日韩（CJK）字体。"""
import matplotlib
matplotlib.use('QtAgg')
@@ -77,7 +77,7 @@ def _setup_matplotlib_font():
chosen = name
break
if chosen is None:
# try by file path
# 按文件路径再试一次
for path in (
'/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc',
'/usr/share/fonts/opentype/noto/NotoSansCJK-Medium.ttc',
@@ -104,7 +104,7 @@ def _param_as_str(node: Node, name: str, default: str = '') -> str:
class RdkBridge:
"""Talk to Flexiv RDK via rdk_py310 subprocess (Python 3.10 ABI)."""
"""通过 rdk_py310 子进程（Python 3.10 ABI）与 Flexiv RDK 通信。"""
def __init__(self, robot_sn: str, logger=None) -> None:
import json
@@ -120,9 +120,9 @@ class RdkBridge:
if not bridge.is_file():
bridge = Path('/home/cc/ros2/scripts/rdk_drag_bridge.py')
if not bridge.is_file():
raise FileNotFoundError(f'rdk_drag_bridge.py not found: {bridge}')
raise FileNotFoundError(f'找不到 rdk_drag_bridge.py：{bridge}')
# Only Flexiv libs — do not inherit conda LD_LIBRARY_PATH (can hang RDK).
# 只保留 Flexiv 的库 —— 不要继承 conda 的 LD_LIBRARY_PATH（会让 RDK 卡死）。
flexiv_lib = os.path.expanduser('~/flexiv_install/lib')
env = {
'HOME': os.environ.get('HOME', ''),
@@ -130,9 +130,9 @@ class RdkBridge:
'LD_LIBRARY_PATH': flexiv_lib,
'LANG': 'C.UTF-8',
}
# Do NOT inherit conda PYTHONPATH.
# IMPORTANT: do not use PIPE for stderr — RDK logs fill the pipe and
# deadlock the bridge (stdout never replies). Log to a file instead.
# 不要继承 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(
@@ -145,14 +145,14 @@ class RdkBridge:
env=env,
)
if logger:
logger.info(f'RDK bridge started; log → {log_path}')
logger.info(f'RDK 桥接已启动；日志 → {log_path}')
pong = self._ask({'cmd': 'ping'}, timeout=8.0)
if not pong.get('ok'):
raise RuntimeError(
f'RDK bridge ping failed: {pong}. See {log_path}'
f'RDK 桥接 ping 失败：{pong}。参见 {log_path}'
)
if logger:
logger.info('RDK bridge ping OK, connecting to robot...')
logger.info('RDK 桥接 ping 正常，正在连接机器人...')
conn = self._ask(
{
'cmd': 'connect',
@@ -163,11 +163,11 @@ class RdkBridge:
)
if not conn.get('ok'):
raise RuntimeError(
f'RDK connect failed: {conn.get("error")}. '
f'Stop run_real.sh first; AUTO Remote + clear fault. See {log_path}'
f'RDK 连接失败：{conn.get("error")}。'
f'请先停掉 run_real.sh；切 AUTO Remote 并清除故障。参见 {log_path}'
)
if logger:
logger.info(f'RDK bridge OK (py={pong.get("py")}) sn={robot_sn}')
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]]
@@ -175,9 +175,9 @@ class RdkBridge:
def _ask(self, msg: dict, timeout: float = 10.0) -> dict:
import select
# One in-flight command: camera/GUI must never share this pipe.
# 同一时刻只允许一条在途命令：相机/GUI 绝不能共用这个管道。
if not self._io.acquire(timeout=max(timeout, 0.2)):
return {'ok': False, 'error': 'bridge busy'}
return {'ok': False, 'error': '桥接忙'}
try:
assert self._proc.stdin and self._proc.stdout
self._proc.stdin.write(self._json.dumps(msg) + '\n')
@@ -188,7 +188,7 @@ class RdkBridge:
tip = '/tmp/rdk_drag_bridge.log'
return {
'ok': False,
'error': f'bridge exited rc={self._proc.returncode}; see {tip}',
'error': f'桥接已退出 rc={self._proc.returncode}；参见 {tip}',
}
ready, _, _ = select.select([self._proc.stdout], [], [], 0.05)
if not ready:
@@ -202,8 +202,8 @@ class RdkBridge:
try:
return self._json.loads(line)
except Exception as exc: # noqa: BLE001
return {'ok': False, 'error': f'bad reply: {exc} {line!r}'}
return {'ok': False, 'error': 'bridge timeout'}
return {'ok': False, 'error': f'应答异常：{exc} {line!r}'}
return {'ok': False, 'error': '桥接超时'}
finally:
self._io.release()
@@ -296,14 +296,14 @@ class HandeyeTeach(Node):
self._start_gui()
print(HELP)
if self.backend == 'rdk':
print('[backend=rdk] Press d = soft-drag (works in AUTO Remote, no Floating license)')
print(' run_real.sh must be STOPPED.')
print('[backend=rdk] 按 d = 软拖拽（在 AUTO Remote 下可用，无需 Floating 许可）')
print(' run_real.sh 必须已停止。')
print(' 拖着就能按 Enter 拍照，不必先按 s')
else:
print('[backend=ros] Drag with pendant Free Drive + hold enable.')
print(' Or stop run_real.sh and use: -p backend:=rdk')
print('[backend=ros] 用示教器 Free Drive + 按住使能键来拖拽。')
print(' 或停掉 run_real.sh 后改用：-p backend:=rdk')
if font:
self.get_logger().info(f'UI font: {font}')
self.get_logger().info(f'界面字体：{font}')
self.create_timer(1.0 / max(1, self._fps), self._tick)
def _init_ros_state(self) -> None:
@@ -312,15 +312,15 @@ class HandeyeTeach(Node):
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 unavailable. Start run_real.sh, '
'or use: -p backend:=rdk (after stopping the stack)'
'get_arm_state 不可用。请启动 run_real.sh，'
'或改用：-p backend:=rdk（需先停掉整套栈）'
)
def _init_rdk(self) -> None:
self.get_logger().info(f'Connecting RDK bridge to {self.robot_sn} ...')
self.get_logger().info(f'正在把 RDK 桥接连到 {self.robot_sn} ...')
self._rdk = RdkBridge(self.robot_sn, logger=self.get_logger())
self._robot = True # marker: rdk backend ready
self.get_logger().info('RDK connected. Press d for Free Drive.')
self._robot = True # 标记：RDK 后端已就绪
self.get_logger().info('RDK 已连接。按 d 启用 Free Drive。')
def _start_camera(self) -> None:
import pyrealsense2 as rs
@@ -342,7 +342,7 @@ class HandeyeTeach(Node):
cfg.enable_stream(rs.stream.color, ww, hh, rs.format.bgr8, ff)
profile = pipe.start(cfg)
self._fps = ff
self.get_logger().info(f'Camera {ww}x{hh}@{ff}Hz')
self.get_logger().info(f'相机 {ww}x{hh}@{ff}Hz')
break
except Exception as exc: # noqa: BLE001
last_err = exc
@@ -353,7 +353,7 @@ class HandeyeTeach(Node):
pipe = rs.pipeline()
if profile is None:
raise RuntimeError(
f'Cannot open D405 — stop realsense_color_pub first. ({last_err})'
f'无法打开 D405 —— 请先停掉 realsense_color_pub。（{last_err}）'
)
self._pipe = pipe
try:
@@ -371,7 +371,7 @@ class HandeyeTeach(Node):
log=self.get_logger().info,
)
except Exception as exc: # noqa: BLE001
self.get_logger().warn(f'exposure apply: {exc}')
self.get_logger().warn(f'曝光设置应用失败：{exc}')
self._cam_thread = threading.Thread(target=self._cam_loop, name='d405', daemon=True)
self._cam_thread.start()
@@ -395,7 +395,7 @@ class HandeyeTeach(Node):
self._plt = plt
self._fig, self._ax = plt.subplots(1, 1, figsize=(11, 7))
self._fig.canvas.manager.set_window_title(
'handeye_teach | Enter=photo d=drag s=stop q=quit'
'handeye_teach | Enter=拍照 d=拖拽 s=停止 q=退出'
)
self._im = None
self._status = self._ax.text(
@@ -415,21 +415,21 @@ class HandeyeTeach(Node):
plt.show(block=False)
def _status_text(self) -> str:
drag = 'DRAG ON' if self._drag_on else 'drag off'
drag = '拖拽开启' if self._drag_on else '拖拽关闭'
if self.backend == 'rdk':
tip = 'd=soft-drag s=hold'
tip = 'd=软拖拽 s=保持'
else:
tip = 'Pendant FreeDrive+Enable'
tip = '示教器 FreeDrive+使能'
return (
f'captured:{len(self._captures)} [{drag}] '
f'{tip} | Enter=photo q=quit'
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('Quit requested')
self.get_logger().info('收到退出请求')
return
if self._cmd_pending:
return
@@ -446,7 +446,7 @@ class HandeyeTeach(Node):
elif key == 's':
self._disable_drag()
except Exception as exc: # noqa: BLE001
self.get_logger().error(f'key {key}: {exc}')
self.get_logger().error(f'按键 {key}：{exc}')
finally:
self._cmd_pending = False
@@ -454,34 +454,34 @@ class HandeyeTeach(Node):
if self.backend != 'rdk':
self._drag_on = True
self.get_logger().warn(
'ROS backend: use teach pendant Free Drive + hold enable button. '
'For software drag: stop run_real.sh, then '
'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('starting soft-drag (preview stays live) ...')
# soft = joint impedance drag under AUTO Remote (no Floating license)
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'Drag ON mode={res.get("mode")} — push the arm, Enter=photo (s=hold optional)'
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'Drag failed: {res.get("error")}')
# status text is refreshed by _tick on the GUI thread
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'Stop drag: {res.get("error")}')
self.get_logger().warn(f'停止拖拽：{res.get("error")}')
self._drag_on = False
self.get_logger().info('Free Drive OFF / hold')
self.get_logger().info('Free Drive 已关闭 / 保持')
def _tick(self) -> None:
if self._quit:
@@ -510,7 +510,7 @@ class HandeyeTeach(Node):
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 joints failed')
self.get_logger().error('读取 RDK 关节失败')
return q
from arm_msgs.srv import GetArmState
@@ -532,11 +532,11 @@ class HandeyeTeach(Node):
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('No camera frame yet')
self.get_logger().warn('还没有相机帧')
return
q = self._read_joints()
if q is None:
self.get_logger().error('Failed to read joints')
self.get_logger().error('读取关节失败')
return
idx = len(self._captures)
@@ -569,7 +569,7 @@ class HandeyeTeach(Node):
if target > 0:
if n < target:
self.get_logger().info(
f'[{n}/{target}] saved photo+joints —— 还差 {target - n} 个点'
f'[{n}/{target}] 已保存照片+关节 —— 还差 {target - n} 个点'
)
elif n == target:
self.get_logger().info(
@@ -579,18 +579,18 @@ class HandeyeTeach(Node):
)
else:
self.get_logger().info(
f'[{n}/{target}] saved photo+joints —— 已超出目标 '
f'[{n}/{target}] 已保存照片+关节 —— 已超出目标 '
f'{n - target} 个点（会用全部点求解）'
)
else:
self.get_logger().info(f'[{n}] saved photo+joints')
self.get_logger().info(f'[{n}] 已保存照片+关节')
self._write_yaml_snippet()
finally:
self._busy = False
def _write_yaml_snippet(self) -> None:
lines = [
'# Paste into handeye_calibrate.yaml -> waypoint_positions',
'# 粘贴到 handeye_calibrate.yaml -> waypoint_positions',
f'# num_poses: {len(self._captures)}',
'waypoint_positions: [',
]
@@ -612,17 +612,17 @@ class HandeyeTeach(Node):
except Exception:
pass
n = len(self._captures)
print('\n========== TEACH DONE ==========')
print(f'poses: {n}')
print(f'joints: {self.poses_file}')
print('\n========== 示教完成 ==========')
print(f'位姿: {n}')
print(f'关节: {self.poses_file}')
print(f'yaml: {self.yaml_file}')
print(f'images: {self.images_dir}')
print(f'图像: {self.images_dir}')
if n >= 1:
print('\n--- paste into handeye_calibrate.yaml ---\n')
print('\n--- 粘贴到 handeye_calibrate.yaml ---\n')
print(self.yaml_file.read_text(encoding='utf-8'))
print('Also set num_poses:', n)
print('同时设置 num_poses:', n)
if n < 5:
self.get_logger().warn(f'Only {n} poses; need >=5 (prefer 8-12).')
self.get_logger().warn(f'只有 {n} 个位姿；至少需要 5 个（建议 8-12 个）。')
def main(args=None) -> None:
src/arm_application/arm_application/handeye_waypoints.py
+44 -45
View File
@@ -1,17 +1,16 @@
#!/usr/bin/env python3
"""Hand-eye calibration waypoint runner.
"""手眼标定路点执行器。
Moves to each joint waypoint one-by-one, waits for arrival, then dwells so a
camera (or operator) can capture a calibration image.
逐个移动到每个关节路点，等到位后停留一段时间，
让相机（或操作者）采集一张标定图像。
capture_mode:
auto — dwell_sec then next pose
keypress — terminal Enter to continue
preview — OpenCV live camera + Enter/Space confirm (recommended for teaching)
auto — 停留 dwell_sec 后进入下一个位姿
keypress — 在终端按 Enter 继续
preview — OpenCV 实时相机 + Enter/Space 确认（示教时推荐）
During ``preview`` mode, use the teach pendant **Free Drive** to drag-adjust
while the preview window is open (ros2_control keeps AUTO Remote; drag is
via pendant, not RDK).
处于 ``preview`` 模式时，可在预览窗口打开期间用示教器的 **Free Drive**
拖拽微调（ros2_control 保持 AUTO Remote；拖拽经由示教器，而非 RDK）。
"""
from __future__ import annotations
@@ -106,7 +105,7 @@ def _image_to_bgr(msg: Image) -> np.ndarray:
img = data.reshape(h, w, 4)
bgr = img[:, :, :3]
return bgr[:, :, ::-1].copy() if enc == 'rgba8' else bgr
raise RuntimeError(f'Unsupported image encoding: {msg.encoding!r}')
raise RuntimeError(f'不支持的图像编码：{msg.encoding!r}')
class HandeyeWaypoints(Node):
@@ -165,14 +164,14 @@ class HandeyeWaypoints(Node):
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 has no GUI (headless). Preview on /handeye/preview_image; '
'confirm with Enter in this terminal.'
'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'Preview image: {self.image_topic}')
self.get_logger().info(f'预览图像：{self.image_topic}')
def _on_image(self, msg: Image) -> None:
try:
@@ -183,10 +182,10 @@ class HandeyeWaypoints(Node):
def run(self) -> int:
if self.capture_mode == 'preview':
if cv2 is None:
self.get_logger().error('preview mode needs OpenCV (cv2)')
self.get_logger().error('preview 模式需要 OpenCV（cv2）')
return 1
if not self.image_topic:
self.get_logger().error('preview mode needs image_topic or camera_source')
self.get_logger().error('preview 模式需要 image_topic 或 camera_source')
return 1
times = list(self.get_parameter('waypoint_times').value)
@@ -194,23 +193,23 @@ class HandeyeWaypoints(Node):
n_j = len(self.joint_names)
n_pts = len(times)
if n_pts < 1:
self.get_logger().error('waypoint_times is empty')
self.get_logger().error('waypoint_times 为空')
return 1
if len(positions) != n_pts * n_j:
self.get_logger().error(
f'waypoint_positions length {len(positions)} != {n_pts}*{n_j}'
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} >= num waypoints {n_pts}'
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}] not available. '
'Start stack first: bash scripts/run_real.sh'
f'Action [{self.action_name}] 不可用。'
'请先启动整套栈：bash scripts/run_real.sh'
)
return 2
@@ -226,18 +225,18 @@ class HandeyeWaypoints(Node):
+ [f'target_{n}' for n in self.joint_names]
+ [f'measured_{n}' for n in self.joint_names]
)
self.get_logger().info(f'Logging captures → {path}')
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'Hand-eye: {n_pts} poses, start={self.start_index}, '
f'v_scale={self.velocity_scale}, dwell={self.dwell_sec}s, '
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(
'Clear workspace; keep board in FOV. Ctrl+C aborts.'
'清理工作空间；让棋盘保持在视野内。Ctrl+C 可中止。'
)
win = 'handeye_preview'
@@ -246,7 +245,7 @@ class HandeyeWaypoints(Node):
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}] moving (~{move_t:.1f}s)...'
f'[{i + 1}/{n_pts}] 移动中（约 {move_t:.1f}s）...'
)
code = self._move_one(q, move_t)
if code != 0:
@@ -264,10 +263,10 @@ class HandeyeWaypoints(Node):
csv_file.flush()
if not self._wait_capture(i, n_pts):
self.get_logger().info('Aborted by user')
self.get_logger().info('用户已中止')
return 0
# After confirm in preview mode, save taught joints
# 在 preview 模式下确认后，保存示教关节
if self.capture_mode == 'preview':
taught = self._read_joints()
if taught:
@@ -281,10 +280,10 @@ class HandeyeWaypoints(Node):
except Exception: # noqa: BLE001
pass
self.get_logger().info('Hand-eye waypoint sequence done')
self.get_logger().info('手眼标定路点序列执行完毕')
if self.append_poses_file and self.capture_mode == 'preview':
self.get_logger().info(
f'Taught poses saved → {self.append_poses_file}'
f'示教位姿已保存 → {self.append_poses_file}'
)
return 0
@@ -299,7 +298,7 @@ class HandeyeWaypoints(Node):
with path.open('a', encoding='utf-8') as f:
f.write(f'# pose {index}\n')
f.write(line + '\n')
self.get_logger().info(f'Saved pose {index} → {path}')
self.get_logger().info(f'已保存位姿 {index} → {path}')
def _move_one(self, positions: List[float], duration: float) -> int:
goal = FollowJointTrajectory.Goal()
@@ -310,19 +309,19 @@ class HandeyeWaypoints(Node):
goal.time_from_start = [float(duration)]
send = self._client.send_goal_async(goal)
if not self._wait_future(send, 10.0, 'send 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('Goal rejected')
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), 'result'):
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'Move failed: {result.message}')
self.get_logger().error(f'移动失败：{result.message}')
return 6
return 0
@@ -331,7 +330,7 @@ class HandeyeWaypoints(Node):
while rclpy.ok() and not fut.done():
rclpy.spin_once(self, timeout_sec=0.05)
if time.monotonic() > deadline:
self.get_logger().error(f'Timeout waiting for {label}')
self.get_logger().error(f'等待 {label} 超时')
return False
return fut.done()
@@ -359,11 +358,11 @@ class HandeyeWaypoints(Node):
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}] ARRIVED — Free Drive adjust, then Enter/Space'
f'[{index + 1}/{n_pts}] 已到位 —— 用 Free Drive 调整，然后按 Enter/Space'
)
else:
self.get_logger().info(
f'[{index + 1}/{n_pts}] ARRIVED — CAPTURE NOW q=[{q_str}]'
f'[{index + 1}/{n_pts}] 已到位 —— 现在采集 q=[{q_str}]'
)
def _wait_capture(self, index: int, n_pts: int) -> bool:
@@ -372,7 +371,7 @@ class HandeyeWaypoints(Node):
if self.capture_mode in ('key', 'keypress', 'manual'):
self.get_logger().info(
'Press Enter after capturing (q+Enter to quit)...'
'采集完成后按 Enter（q+Enter 退出）...'
)
for _ in range(5):
rclpy.spin_once(self, timeout_sec=0.05)
@@ -394,7 +393,7 @@ class HandeyeWaypoints(Node):
return vis
cv2.putText(
vis,
f'Pose {index + 1}/{n_pts}',
f'位姿 {index + 1}/{n_pts}',
(12, 28),
cv2.FONT_HERSHEY_SIMPLEX,
0.7,
@@ -403,7 +402,7 @@ class HandeyeWaypoints(Node):
)
cv2.putText(
vis,
'Free Drive drag | Enter=next q=quit',
'Free Drive 拖拽 | Enter=下一个 q=退出',
(12, 56),
cv2.FONT_HERSHEY_SIMPLEX,
0.6,
@@ -420,7 +419,7 @@ class HandeyeWaypoints(Node):
msg.encoding = 'bgr8'
msg.is_bigendian = 0
msg.step = msg.width * 3
msg.data = array.array('B', bgr.tobytes()) # fast path; bytes costs ~40 ms
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:
@@ -444,8 +443,8 @@ class HandeyeWaypoints(Node):
confirmed.set()
self.get_logger().info(
f'[{index + 1}/{n_pts}] Adjust with Free Drive, then press Enter '
'(q+Enter to quit)'
f'[{index + 1}/{n_pts}] 用 Free Drive 调整，然后按 Enter '
'（q+Enter 退出）'
)
reader = threading.Thread(target=_stdin_reader, daemon=True)
reader.start()
@@ -464,7 +463,7 @@ class HandeyeWaypoints(Node):
if bgr is None:
if not no_frame_warn:
self.get_logger().warn(
f'Waiting for {self.image_topic} ...',
f'正在等待 {self.image_topic} ...',
throttle_duration_sec=3.0,
)
no_frame_warn = True
@@ -485,7 +484,7 @@ class HandeyeWaypoints(Node):
if bgr is None:
if not no_frame_warn:
self.get_logger().warn(
f'Waiting for {self.image_topic} ...',
f'正在等待 {self.image_topic} ...',
throttle_duration_sec=3.0,
)
no_frame_warn = True
src/arm_application/arm_application/hole_detect_node.py
+1 -1
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""Hole detection on the current-arm D405 color topic. Detect-only (no insert motion)."""
"""在当前机械臂的 D405 彩色话题上做入孔检测。只检测，不做插入动作。"""
from __future__ import annotations
src/arm_application/arm_application/hole_detector.py
+4 -1
View File
@@ -60,7 +60,10 @@ _HOLE_SITE_SKIP = {"weights", "workpiece_weights", "config"}
def config_from_site(data: dict[str, Any] | None = None) -> dict[str, Any]:
"""Package yaml defaults, then overlay site.yaml hole_detect: (operator source of truth)."""
"""先取包内 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()
src/arm_application/arm_application/kalman_filter.py
+9 -5
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""Constant-velocity Kalman filter for a 3D point (camera frame)."""
"""三维点的匀速模型卡尔曼滤波（坐标系为相机系）。"""
from __future__ import annotations
@@ -9,7 +9,7 @@ import numpy as np
class KalmanFilter3D:
"""State ``[x, y, z, vx, vy, vz]``. Measurement is position only."""
"""状态量为 ``[x, y, z, vx, vy, vz]``；观测只有位置，没有速度。"""
def __init__(
self,
@@ -43,7 +43,11 @@ class KalmanFilter3D:
return self.x[3:].copy()
def advance(self, t: float) -> None:
"""Predict the state forward to time ``t`` (seconds, monotonic)."""
"""把状态预测到时刻 ``t``（秒，必须单调递增）。
超过 0.5 秒的空档按"目标可能已经跑掉"处理：位置不动、速度清零，
否则旧速度会把位置外推得很离谱。
"""
if not self.initialized or self._t is None:
self._t = t
return
@@ -51,7 +55,7 @@ class KalmanFilter3D:
if dt <= 1e-6:
return
if dt > 0.5:
# Long gap — keep position, zero velocity.
# 空档太长——位置保持不动，速度清零。
self.x[3:] = 0.0
self._t = t
return
@@ -59,7 +63,7 @@ class KalmanFilter3D:
self._t = t
def update(self, z: np.ndarray) -> None:
"""Measurement update with a 3D position."""
"""用一次三维位置观测做量测更新。"""
z = np.asarray(z, dtype=float).reshape(3)
if not self.initialized:
self.reset(z)
src/arm_application/arm_application/move_to_pose_client.py
+8 -8
View File
@@ -1,7 +1,7 @@
#!/usr/bin/env python3
"""Example client for arm_msgs/action/MoveToPose (Cartesian goal).
"""``arm_msgs/action/MoveToPose``（笛卡尔目标）的示例客户端。
Wire this to MoveIt or your IK solver action server when available.
等你有了 MoveIt 或自己的 IK 求解 action server 后，把它接上去即可。
"""
from __future__ import annotations
@@ -24,8 +24,8 @@ class MoveToPoseClient(Node):
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 not available. '
'Start a Cartesian planner (e.g. MoveIt) that implements it.'
'move_to_pose action 不可用。'
'请启动一个实现了它的笛卡尔规划器（例如 MoveIt）。'
)
return
@@ -36,18 +36,18 @@ class MoveToPoseClient(Node):
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.position.z = z
# Identity orientation
# 单位姿态（不旋转）
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'Sending Cartesian goal: ({x:.3f}, {y:.3f}, {z:.3f})')
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('Goal rejected')
self.get_logger().error('目标被拒绝')
return
result_future = handle.get_result_async()
@@ -59,7 +59,7 @@ class MoveToPoseClient(Node):
def main(args=None) -> None:
rclpy.init(args=args)
node = MoveToPoseClient()
# Default demo pose; override via CLI: x y z
# 默认演示位姿；可通过命令行覆盖：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])]
src/arm_application/arm_application/orisys_tactile.py
+3 -3
View File
@@ -1,4 +1,4 @@
"""Orisys visuo-tactile adapter. SDK is pip-installed; site.yaml holds device ids."""
"""Orisys 视触觉适配器。SDK 通过 pip 安装；设备 id 存放在 ``site.yaml`` 里。"""
from __future__ import annotations
@@ -185,8 +185,8 @@ class OrisysTactile(TactileInterface):
import orisys
except ImportError as exc:
raise RuntimeError(
"orisys SDK missing. pip install -e /path/to/orisys-sdk "
"(git.orisys-tech.cn/orisys/test). Config still comes from site.yaml tactile."
"缺少 orisys SDK。请执行 pip install -e /path/to/orisys-sdk "
"（git.orisys-tech.cn/orisys/test）。配置依旧从 ``site.yaml`` 的 ``tactile`` 段读取。"
) from exc
self._sensors = []
src/arm_application/arm_application/perception_node.py
+75 -76
View File
@@ -1,13 +1,13 @@
#!/usr/bin/env python3
"""perception_node — YOLO + depth + KF (own process).
"""perception_node —— YOLO + 深度 + 卡尔曼滤波（独立进程）。
Hard rules:
- Image callbacks ONLY store latest frames (never run YOLO in the callback)
- YOLO runs on a dedicated worker thread so MutuallyExclusive I/O is never blocked
- SensorDataQoS BEST_EFFORT keep_last(5)
- Publish filtered 3D as geometry_msgs/PointStamped (no Services)
- Dynamic object TF at vision rate only
- High-confidence detections always reacquire (no freeze on fast motion)
硬性规则：
- 图像回调**只**存最新帧（绝不在回调里跑 YOLO）
- YOLO 跑在专门的工作线程上，保证 ``MutuallyExclusive`` 的 I/O 回调永远不会被阻塞
- 用 ``SensorDataQoS`` ``BEST_EFFORT`` ``keep_last(5)``
- 滤波后的三维点用 ``geometry_msgs/PointStamped`` 发布（不用 Service）
- 动态物体 TF 只在视觉帧率下发布
- 高置信度检测始终重新捕获（快速运动时不会冻结）
"""
from __future__ import annotations
@@ -50,7 +50,7 @@ from arm_application.yolo_detector import (
def _bgr_copy(msg: Image) -> np.ndarray:
"""BGR uint8 owned by caller (safe for async YOLO; ROS may drop the msg)."""
"""返回调用方持有的 BGR uint8 数组（对异步 YOLO 安全；ROS 可能把原消息丢掉）。"""
h, w = int(msg.height), int(msg.width)
enc = (msg.encoding or "").lower()
buf = np.frombuffer(msg.data, dtype=np.uint8)
@@ -58,7 +58,7 @@ def _bgr_copy(msg: Image) -> np.ndarray:
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}")
raise RuntimeError(f"不支持的编码 {msg.encoding!r}")
class PerceptionNode(Node):
@@ -66,7 +66,7 @@ class PerceptionNode(Node):
super().__init__("perception_node")
tid = threading.get_ident()
self.get_logger().warn(
f"perception_node started pid={os.getpid()} thread_id={tid}"
f"perception_node 已启动 pid={os.getpid()} thread_id={tid}"
)
self.declare_parameter("camera_source", "d405")
@@ -86,30 +86,30 @@ class PerceptionNode(Node):
self.declare_parameter("jump_z_m", 0.20)
self.declare_parameter("min_depth_m", 0.04)
self.declare_parameter("max_depth_m", 0.55)
self.declare_parameter("work_z_max_m", 0.50) # above this = hole/background
self.declare_parameter("work_z_max_m", 0.50) # 高于此值 = 孔 / 背景
self.declare_parameter("object_frame", "tracked_object")
self.declare_parameter("camera_frame", "d405_color_optical_frame")
self.declare_parameter("kf_q_pos", 2.0e-3)
self.declare_parameter("kf_q_vel", 5.0e-2)
self.declare_parameter("kf_r_pos", 8.0e-4)
self.declare_parameter("meas_blend", 0.65) # 1=raw meas, 0=pure KF
self.declare_parameter("meas_blend", 0.65) # 1 = 原始测量值，0 = 纯卡尔曼滤波
self.declare_parameter("half_precision", True)
self.declare_parameter("backend", "auto") # auto|torch|tensorrt
self.declare_parameter("mp_infer", True) # YOLO in a spawn() process
# Adaptive Z LPF: alpha_z = clamp(z_alpha_min + |vel_z|*z_alpha_gain, min, max)
self.declare_parameter("mp_infer", True) # YOLO 跑在 spawn() 子进程里
# 自适应 Z 低通滤波：alpha_z = clamp(z_alpha_min + |vel_z|*z_alpha_gain, min, max)
self.declare_parameter("z_alpha_min", 0.10)
self.declare_parameter("z_alpha_max", 0.60)
self.declare_parameter("z_alpha_gain", 2.0)
# Faster YOLO: downscale before infer; boxes scaled back to full res
# 加速 YOLO：推理前先降采样；检测框再缩放回全分辨率
self.declare_parameter("infer_max_width", 0)
# Box EMA (1=raw YOLO every frame — tight follow; lower = smoother/lag)
# 检测框 EMA（1 = 每帧都用原始 YOLO —— 紧跟目标；更低 = 更平滑但滞后）
self.declare_parameter("box_ema_alpha", 0.85)
# Brief YOLO misses: keep last box on preview; clear only after N frames
# YOLO 短暂漏检：预览里保留上一个框；只有连续 N 帧都漏检才清除
self.declare_parameter("miss_clear_frames", 12)
self.declare_parameter("overlay_hold_on_miss", True)
self.declare_parameter("hold_point_on_miss", True)
self.declare_parameter("miss_hold_point_frames", 2)
# plane = stereo surface centroid (recommended); box = YOLO box center
# plane = 立体表面质心（推荐）；box = YOLO 检测框中心
self.declare_parameter("feature_mode", "plane")
self.declare_parameter("plane_inlier_m", 0.008)
self.declare_parameter("plane_band_m", 0.06)
@@ -170,7 +170,7 @@ class PerceptionNode(Node):
self._stop = threading.Event()
self._wake = threading.Event()
# I/O group: only lightweight store-latest (never YOLO)
# I/O 回调组：只做轻量的“存最新帧”（绝不跑 YOLO）
self._io_cbg = MutuallyExclusiveCallbackGroup()
self._wake_cbg = MutuallyExclusiveCallbackGroup()
@@ -195,7 +195,7 @@ class PerceptionNode(Node):
self._yolo_proc: Optional[YoloInferProcess] = None
if self.mp_infer:
self._yolo_proc = YoloInferProcess(yolo_cfg)
self.detector = self._yolo_proc # duck-typed infer()
self.detector = self._yolo_proc # 鸭子类型，只要求有 infer()
imgsz = self._yolo_proc.imgsz
self.get_logger().info(
f"YOLO worker pid={self._yolo_proc._p.pid} weights={self._yolo_proc.weights_path}"
@@ -221,7 +221,7 @@ class PerceptionNode(Node):
try:
self.detector.warmup(half=self._yolo_half)
except Exception as exc: # noqa: BLE001
self.get_logger().warn(f"YOLO warmup failed: {exc}")
self.get_logger().warn(f"YOLO 预热失败：{exc}")
imgsz = self.detector.imgsz
image_qos = QoSProfile(
@@ -256,8 +256,8 @@ class PerceptionNode(Node):
depth_topic = f"/{cam_ns}/aligned_depth_to_color/image_raw"
if not img_topic:
raise RuntimeError(
f"image_topic empty (camera_source={cam_src!r} ns={ns!r}); "
"use d405 / d405_left / d405_right"
f"image_topic 为空（camera_source={cam_src!r} ns={ns!r}）；"
"请使用 d405 / d405_left / d405_right"
)
self.get_logger().info(
f"perception image={img_topic} depth={depth_topic} info={info_topic or '-'}"
@@ -283,7 +283,7 @@ class PerceptionNode(Node):
self._point_pub = self.create_publisher(
PointStamped, "visual_servo/filtered_point", point_qos
)
# Reliable + keep_last(5): camera preview tolerates brief DDS gaps
# RELIABLE + keep_last(5)：相机预览能容忍短暂的 DDS 丢包间隙
det_qos = QoSProfile(
history=HistoryPolicy.KEEP_LAST,
depth=5,
@@ -298,7 +298,7 @@ class PerceptionNode(Node):
self._worker.start()
self.get_logger().info(
f"perception ready vision≤{vision_hz:.0f}Hz imgsz={imgsz} "
f"perception 就绪 vision≤{vision_hz:.0f}Hz imgsz={imgsz} "
f"infer_max_w={self.infer_max_width} mp={int(self.mp_infer)} "
f"backend={self.backend_name} half={self._yolo_half} "
f"feature={self.feature_mode} image={img_topic}"
@@ -328,8 +328,8 @@ class PerceptionNode(Node):
w = float(msg.width)
self._fx = 654.8 if w >= 1200 else 393.9 * (w / 640.0)
self._fy = 654.0 if w >= 1200 else 393.4 * (float(msg.height) / 480.0)
# Do NOT pickle the image into the YOLO queue here — that stalls the
# camera callback and looks like inference freeze. Infer thread submits.
# 千万不要在这里把图像 pickle 进 YOLO 队列 —— 那会卡住相机回调，
# 看起来就像是推理冻结。由推理线程负责提交。
self._wake.set()
def _on_info(self, msg: CameraInfo) -> None:
@@ -349,7 +349,7 @@ class PerceptionNode(Node):
self._wake.set()
def _submit_latest(self) -> None:
"""Copy the newest camera frame and hand it to the GPU process."""
"""复制最新的相机帧，交给 GPU 进程。"""
msg = self._latest_image
seq = int(self._frame_seq)
if msg is None or seq <= self._submitted_seq:
@@ -361,38 +361,37 @@ class PerceptionNode(Node):
self.detector.submit(small, seq)
self._submitted_seq = seq
except Exception as exc: # noqa: BLE001
self.get_logger().warn(f"yolo submit: {exc}", throttle_duration_sec=2.0)
self.get_logger().warn(f"yolo 提交失败：{exc}", throttle_duration_sec=2.0)
def _restart_yolo(self, why: str) -> None:
"""Self-heal a hung/dead GPU worker (engine reload takes a few sec)."""
"""自愈：重启卡死 / 挂掉的 GPU 工作进程（重新加载 engine 需要几秒）。"""
now = time.monotonic()
if now - self._last_restart_t < 5.0:
return
if not hasattr(self.detector, "restart"):
return
self._last_restart_t = now
self.get_logger().error(f"YOLO worker {why} — restarting (box coasts)")
self.get_logger().error(f"YOLO 工作进程 {why} —— 正在重启（检测框靠惯性滑行）")
try:
self.detector.restart()
self._submitted_seq = -1
self._last_gpu_t = time.monotonic()
self.get_logger().info("YOLO worker restarted")
self.get_logger().info("YOLO 工作进程已重启")
except Exception as exc: # noqa: BLE001
self.get_logger().error(f"YOLO restart failed: {exc}")
self.get_logger().error(f"YOLO 重启失败：{exc}")
def _infer_loop(self) -> None:
"""Submit latest frame, collect GPU result, then stereo.
"""提交最新帧，收集 GPU 结果，再做立体匹配。
GPU must never wait on stereo: the worker drops old results if we are
briefly busy. We also submit *before* waiting so the next infer is
already queued.
GPU 绝不能等立体匹配：如果我们短暂正忙，工作进程会直接丢掉旧结果。
我们还会**先**提交再去等，这样下一次推理已经排进队列了。
"""
while not self._stop.is_set():
self._submit_latest()
try:
got = self.detector.wait_result(timeout=0.030)
except Exception as exc: # noqa: BLE001
self.get_logger().error(f"infer: {exc}", throttle_duration_sec=2.0)
self.get_logger().error(f"推理：{exc}", throttle_duration_sec=2.0)
if not getattr(self.detector, "alive", True):
self._restart_yolo("died")
time.sleep(0.02)
@@ -402,13 +401,13 @@ class PerceptionNode(Node):
silent = (now - self._last_gpu_t) if self._last_gpu_t else 0.0
if self._last_gpu_t is not None and silent > 0.25:
self.get_logger().warn(
f"YOLO silent {silent*1000:.0f}ms "
f"(worker={'up' if getattr(self.detector, 'alive', True) else 'DEAD'})",
f"YOLO 静默 {silent*1000:.0f}ms "
f"(worker={'存活' if getattr(self.detector, 'alive', True) else '已死'})",
throttle_duration_sec=1.0,
)
outstanding = self._submitted_seq > self._done_seq
if outstanding and self._last_gpu_t is not None and silent > 2.0:
self._restart_yolo(f"hung {silent:.1f}s")
self._restart_yolo(f"卡死 {silent:.1f}s")
continue
if (
self._last_yolo_t is not None
@@ -429,7 +428,7 @@ class PerceptionNode(Node):
try:
self._apply_detections(dets, scale, t_img)
except Exception as exc: # noqa: BLE001
self.get_logger().error(f"apply: {exc}", throttle_duration_sec=2.0)
self.get_logger().error(f"应用检测结果出错：{exc}", throttle_duration_sec=2.0)
self._done_seq = int(seq)
self._submit_latest()
dt_ms = (time.monotonic() - t0) * 1000.0
@@ -457,13 +456,13 @@ class PerceptionNode(Node):
def _depth_in_box(self, xyxy) -> tuple:
msg = self._latest_depth
if msg is None:
return None, "no depth"
return None, "无深度"
if not self._depth_fresh(msg):
return None, "stale depth"
return None, "深度过期"
enc = (msg.encoding or "").lower()
buf = self._decode_depth_m(msg)
if buf is None:
return None, "bad depth"
return None, "深度无效"
h, w = buf.shape
x1, y1, x2, y2 = [float(v) for v in xyxy]
img = self._latest_image
@@ -471,7 +470,7 @@ class PerceptionNode(Node):
sx, sy = w / float(img.width), h / float(img.height)
x1, y1, x2, y2 = x1 * sx, y1 * sy, x2 * sx, y2 * sy
bw, bh = max(8.0, x2 - x1), max(8.0, y2 - y1)
# Inner ROI — avoid box edges; still keep enough pixels on a holey workpiece
# 内部 ROI —— 避开检测框边缘；但对有孔的工件仍要保留足够像素
ix0 = int(np.clip(x1 + 0.20 * bw, 0, w - 1))
iy0 = int(np.clip(y1 + 0.20 * bh, 0, h - 1))
ix1 = int(np.clip(x2 - 0.20 * bw, ix0 + 2, w))
@@ -491,7 +490,7 @@ class PerceptionNode(Node):
scale = 1.0
vals = meters[np.isfinite(meters) & (meters > self.min_depth) & (meters < self.max_depth)]
if vals.size < 8:
# Inner ROI is often holes; retry the full box.
# 内部 ROI 经常落在孔上；改用整个检测框重试。
y0 = int(np.clip(y1, 0, h - 1))
x0 = int(np.clip(x1, 0, w - 1))
y1b = int(np.clip(y2, y0 + 2, h))
@@ -509,8 +508,8 @@ class PerceptionNode(Node):
if vals.size < 8:
med_m = float(np.median(meters)) if meters.size else 0.0
return None, f"ok={vals.size} scale={scale} med={med_m:.3f}"
# Near-surface: holes / table / far background must NOT pull Z out.
# Take closest 40%, then keep a 6 cm band around the 10th percentile.
# 只取近表面：孔 / 桌面 / 远处背景绝不能把 Z 拉偏。
# 先取最近的 40%，再围绕第 10 百分位保留一个 6 cm 的带宽。
vals = np.sort(vals)
n_near = max(8, int(0.40 * vals.size))
near = vals[:n_near]
@@ -524,7 +523,7 @@ class PerceptionNode(Node):
return z, f"ok={vals.size} near={band.size}"
def _adaptive_filter_z(self, z_raw: float, now: float) -> float:
"""Item 4: adaptive LPF on depth — quiet when still, responsive when moving."""
"""第 4 项：对深度做自适应低通滤波 —— 静止时更稳，运动时更灵敏。"""
if self._z_filt is None or self._z_filt_t is None:
self._z_filt = float(z_raw)
self._z_filt_t = now
@@ -563,7 +562,7 @@ class PerceptionNode(Node):
)
def _publish_detection_clear(self) -> None:
"""Empty detection → preview clears green box (must NOT reuse last xyxy)."""
"""空检测 → 预览清掉绿框（绝不能复用上一次的 xyxy）。"""
self._last_det_payload = None
msg = Float32MultiArray()
msg.data = []
@@ -589,7 +588,7 @@ class PerceptionNode(Node):
self._det_pub.publish(msg)
def _prepare_infer(self, bgr: np.ndarray) -> tuple:
"""Optional downscale for YOLO; return (infer_bgr, scale) full = infer * scale."""
"""可选的 YOLO 降采样；返回 (infer_bgr, scale)，全分辨率 = 推理图 * scale。"""
max_w = self.infer_max_width
h, w = bgr.shape[:2]
if max_w <= 0 or w <= max_w:
@@ -612,7 +611,7 @@ class PerceptionNode(Node):
return (int(msg.width), int(msg.height))
def _coast_box(self, note: str) -> bool:
"""Keep the green box moving with last velocity; do not drive 3D."""
"""让绿框按上一次的速度继续移动；不要用它驱动三维。"""
now = time.monotonic()
pred = self._box_trk.predict(now, self._img_wh())
if pred is None and self._last_raw_xyxy is not None:
@@ -626,7 +625,7 @@ class PerceptionNode(Node):
self._publish_detection_box(
pred, float(self._last_box_conf), float(self._last_box_cls), uv, z
)
self.get_logger().info(f"box coast {note}", throttle_duration_sec=0.5)
self.get_logger().info(f"检测框惯滑 {note}", throttle_duration_sec=0.5)
return True
def _clear_track(self) -> None:
@@ -640,7 +639,7 @@ class PerceptionNode(Node):
self._publish_detection_clear()
def _apply_detections(self, dets, scale: float, t_img: float) -> None:
"""Match YOLO, publish a *now*-predicted box, then stereo on that box."""
"""匹配 YOLO 结果，发布按当前时刻预测的检测框，再在该框上做立体匹配。"""
if scale != 1.0:
for d in dets:
d.xyxy = d.xyxy * scale
@@ -665,7 +664,7 @@ class PerceptionNode(Node):
self._clear_track()
self._last_yolo_t = None
self.get_logger().info(
"detection miss — cleared overlay",
"检测漏检 —— 已清除叠加显示",
throttle_duration_sec=1.0,
)
else:
@@ -691,7 +690,7 @@ class PerceptionNode(Node):
self._last_box_conf = float(best.conf)
self._last_box_cls = float(best.cls_id)
self._box_trk.update(xyxy_raw, t_img)
# Compensate infer delay: draw where the part is *now*, not where YOLO saw it
# 补偿推理延迟：画工件**当前**所在的位置，而不是 YOLO 看到它时的位置
xyxy = self._box_trk.predict(now, self._img_wh())
if xyxy is None:
xyxy = xyxy_raw
@@ -705,8 +704,8 @@ class PerceptionNode(Node):
xyxy, float(best.conf), float(best.cls_id), box_uv, z_preview
)
# Hover target = stereo 3D on the workpiece inside the box (not a ray
# through a hole / table, and not z_ref). Red dot = that 3D point.
# 悬停目标 = 检测框内工件表面的立体三维点（不是穿过孔 / 桌面的射线，
# 也不是 z_ref）。红点 = 该三维点。
feat = None
p_cam: Optional[np.ndarray] = None
note = "box"
@@ -727,13 +726,13 @@ class PerceptionNode(Node):
z_ok = z_meas is not None
z, note = self._metric_z(z_meas, note)
if z is None:
# Never had usable depth (target beyond D405 range / not in the work
# area / camera mount or handeye off). Do NOT publish a z_ref point —
# that would fake-lock the gripper over a phantom. Hold instead.
# 从未拿到过可用深度（目标超出 D405 量程 / 不在工作区内 / 相机安装或
# 手眼标定有问题）。不要发布 z_ref 点 —— 那会在幻影位置上假装锁定夹爪。
# 此时应保持不动。
self.get_logger().warn(
f"target has NO usable depth ({note}) — NOT tracking. "
f"Put the workpiece in view at {self.min_depth:.2f}-{self.work_z_max:.2f} m "
"from the camera, or check camera mount / handeye calibration.",
f"目标没有可用深度（{note}）—— 不跟踪。"
f"请把工件放到相机视野内 {self.min_depth:.2f}-{self.work_z_max:.2f} m 处，"
"或检查相机安装 / 手眼标定。",
throttle_duration_sec=1.0,
)
return
@@ -754,7 +753,7 @@ class PerceptionNode(Node):
if dpx > self.jump_px or (z_ok and dz > self.jump_z_m):
self.kf.reset(p_cam, now)
self.get_logger().info(
f"reacquire conf={best.conf:.2f} dpx={dpx:.0f}",
f"重新捕获 conf={best.conf:.2f} dpx={dpx:.0f}",
throttle_duration_sec=0.5,
)
else:
@@ -765,7 +764,7 @@ class PerceptionNode(Node):
kf_p = self.kf.position
blend = self.meas_blend
filt = blend * p_cam + (1.0 - blend) * kf_p
# Keep XY scale honest even if KF still has an old far-Z
# 即使卡尔曼里还残留着旧的远距离 Z，也要保证 XY 的尺度真实
z_use = float(np.clip(filt[2], self.min_depth, self.work_z_max))
if abs(z_use - float(p_cam[2])) > 0.04:
filt = p_cam.copy()
@@ -775,14 +774,14 @@ class PerceptionNode(Node):
self._publish_point(filt, stamp)
self.get_logger().info(
f"track {self.feature_mode} conf={best.conf:.2f} uv=({uv[0]:.0f},{uv[1]:.0f}) "
f"跟踪 {self.feature_mode} conf={best.conf:.2f} uv=({uv[0]:.0f},{uv[1]:.0f}) "
f"z={filt[2]:.3f} z_ok={int(z_ok)} {note}",
throttle_duration_sec=0.4,
)
def _depth_fresh(self, msg: Image) -> bool:
"""Reject stale depth — on a fast-moving part, old depth at the current
box reads the background (table at ~2.4 m) and poisons the 3D point."""
"""拒绝过期的深度 —— 对快速运动的工件，当前检测框处的旧深度会读到背景
（约 2.4 m 处的桌面），从而污染三维点。"""
try:
t = rclpy.time.Time.from_msg(msg.header.stamp)
age = (self.get_clock().now() - t).nanoseconds * 1e-9
@@ -816,7 +815,7 @@ class PerceptionNode(Node):
band_m=self.plane_band_m,
)
except Exception as exc: # noqa: BLE001
self.get_logger().warn(f"inner3d feature: {exc}", throttle_duration_sec=2.0)
self.get_logger().warn(f"inner3d 特征计算失败：{exc}", throttle_duration_sec=2.0)
return None
def _plane_center(self, xyxy):
@@ -846,13 +845,13 @@ class PerceptionNode(Node):
band_m=self.plane_band_m,
)
except Exception as exc: # noqa: BLE001
self.get_logger().warn(f"plane feature: {exc}", throttle_duration_sec=2.0)
self.get_logger().warn(f"平面特征计算失败：{exc}", throttle_duration_sec=2.0)
return None
def _metric_z(self, z_meas: Optional[float], note: str) -> tuple:
"""Z used for pixel→metre. Never use hole/floor depth (inflates XY error).
Returns (None, note) when this target NEVER had usable depth — publishing
a z_ref point then would fake-lock the servo onto a phantom XY."""
"""用于像素→米的 Z。绝不能用孔 / 地面的深度（会放大 XY 误差）。
当这个目标**从未**有过可用深度时返回 (None, note) —— 那种情况下发布
z_ref 点会让伺服假锁到一个幻影 XY 上。"""
z_ref = max(0.12, float(self.z_ref))
if z_meas is not None and self.min_depth <= z_meas <= self.work_z_max:
self._last_good_z = float(z_meas)
src/arm_application/arm_application/pid_controller.py
+5 -5
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""PID with integral clamp, derivative-on-measurement, and integral separation."""
"""PID 控制器，带积分限幅、微分作用于测量值（derivative-on-measurement）以及积分分离。"""
from __future__ import annotations
@@ -7,10 +7,10 @@ from typing import Optional
class PidController:
"""``u = Kp e + Ki ∫e + Kd (−dmeas/dt)``, then saturate.
"""``u = Kp e + Ki ∫e + Kd (−dmeas/dt)``，然后再做饱和限幅。
Integral separation: when ``|error| > i_sep_thresh``, integral is forced to 0
and does not accumulate (P+D only for fast chase / less overshoot).
积分分离：当 ``|error| > i_sep_thresh`` 时，积分被强制置 0 且不再累积
（只用 P+D，以便快速追赶 / 减少超调）。
"""
def __init__(
@@ -50,7 +50,7 @@ class PidController:
dterm = -(x - self._meas_prev) / dt
self._meas_prev = x
# Integral separation — large error: clear I, do not accumulate
# 积分分离 —— 误差较大：清空 I，不再累积
if self.i_sep_thresh > 0.0 and abs(err) > self.i_sep_thresh:
self._i = 0.0
else:
src/arm_application/arm_application/plane_feature.py
+13 -14
View File
@@ -1,10 +1,9 @@
#!/usr/bin/env python3
"""Stereo plane Z at the YOLO box center.
"""在 YOLO 检测框中心处求立体平面的 Z。
The hover target is the box-center ray intersected with the fitted near
plane. That stays on the detection, with metric Z (30° camera: wrong Z
would shift base XY). The cloud-mean centroid is not used — holes / table
inliers pull it off the green box.
悬停目标是「检测框中心射线」与「拟合出的近平面」的交点。这样既能留在检测结果上，
又带有真实的米制 Z（30° 相机下，Z 错了会把 base 系的 XY 也带偏）。不使用点云均值
质心 —— 孔 / 桌面的内点会把它从绿框上拉走。
"""
from __future__ import annotations
@@ -17,9 +16,9 @@ import numpy as np
@dataclass
class PlaneFeature:
uv: np.ndarray # (2,) color-pixel coords of the 3D centroid
z: float # camera-frame Z of the 3D centroid
xyz: np.ndarray # (3,) stereo-cloud centroid in camera frame
uv: np.ndarray # (2,) 三维质心对应的彩色像素坐标
z: float # 三维质心在相机坐标系下的 Z
xyz: np.ndarray # (3,) 相机坐标系下的立体点云质心
n_inliers: int
note: str
@@ -128,10 +127,10 @@ def workpiece_xyz_in_box(
inner: float = 0.40,
band_m: float = 0.06,
) -> Optional[PlaneFeature]:
"""Physical workpiece point: median 3D of the near surface in the inner box.
"""工件上的实体点：内层检测框里近表面深度的三维中位数。
Not a monocular ray (holes punch through to the table) and not the
whole-ROI cloud mean (table / one-sided depth pulls it off the part).
不是单目射线（孔会穿透到桌面），也不是整个 ROI 的点云均值
（桌面 / 单侧深度会把它从工件上拉走）。
"""
meters, _ = _depth_meters(depth_buf, encoding)
if meters.ndim != 2:
@@ -212,7 +211,7 @@ def geometric_center_on_plane(
inlier_m: float = 0.008,
band_m: float = 0.06,
) -> Optional[PlaneFeature]:
"""Box-center point on the fitted near plane, or None."""
"""拟合出的近平面上、位于检测框中心的点，拿不到则返回 None。"""
meters, _ = _depth_meters(depth_buf, encoding)
if meters.ndim != 2:
return None
@@ -224,7 +223,7 @@ def geometric_center_on_plane(
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
# Keep uv in color pixels: work in depth pixels then scale back.
# uv 保持用彩色像素坐标：先在深度像素下计算，再缩放回来。
scale_u, scale_v = 1.0 / sx, 1.0 / sy
else:
scale_u = scale_v = 1.0
@@ -246,7 +245,7 @@ def geometric_center_on_plane(
yy, xx = np.nonzero(near)
zs = roi[yy, xx]
# Color-equivalent pixel for back-projection (aligned depth uses color K).
# 反投影用的彩色等效像素（对齐后的深度使用的是彩色内参 K）。
us = (xx + x0) * scale_u
vs = (yy + y0) * scale_v
pts = np.stack(
src/arm_application/arm_application/realsense_color_pub.py
+90 -90
View File
@@ -1,18 +1,18 @@
#!/usr/bin/env python3
"""Publish RealSense color stream to ROS topics (no realsense2_camera apt pkg).
"""把 RealSense 彩色流发布到 ROS 话题（不依赖 realsense2_camera 的 apt 包）。
Use when ``ros-humble-realsense2-camera`` is not installed but ``pyrealsense2`` works.
当你没装 ``ros-humble-realsense2-camera``、但 ``pyrealsense2`` 能正常工作时用它。
Default topics (camera_name=d405):
默认话题（camera_name=d405）：
/d405/color/image_raw
/d405/color/camera_info
/d405/aligned_depth_to_color/image_raw (16UC1 mm, if enable_depth)
/d405/aligned_depth_to_color/image_raw (16UC1 mm，需开启 enable_depth)
Performance notes
-----------------
* Publishes from the capture thread (no timer lag).
* Uses SensorDataQoS (BEST_EFFORT, depth=1) so slow subscribers drop old frames.
* Optional SDK preview: ``-p show_preview:=true`` (Intel sample path: pipeline → numpy → window).
性能说明
--------
* 在采集线程里直接发布（没有定时器带来的延迟）。
* 使用 SensorDataQoS（BEST_EFFORT，depth=1），这样慢的订阅者会主动丢旧帧。
* 可选的 SDK 预览：``-p show_preview:=true``（沿用 Intel 示例的链路：pipeline → numpy → 窗口）。
"""
from __future__ import annotations
@@ -34,7 +34,7 @@ from std_msgs.msg import Float32MultiArray
def _param_as_str(node: Node, name: str, default: str = '') -> str:
"""String param that also accepts numeric CLI overrides (e.g. serial numbers)."""
"""字符串参数，同时兼容命令行传数字的情况（例如序列号）。"""
node.declare_parameter(
name, default, ParameterDescriptor(dynamic_typing=True)
)
@@ -82,7 +82,7 @@ def _site_yaml_path() -> Path:
class RealsenseColorPub(Node):
def __init__(self) -> None:
super().__init__('realsense_color_pub')
self.declare_parameter('camera_name', 'd405') # topic namespace
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)
@@ -94,8 +94,8 @@ class RealsenseColorPub(Node):
self.declare_parameter('enable_depth', True)
self.declare_parameter('enable_infra', True)
self.declare_parameter('preview_stereo', True)
# Render the Qt preview only every Nth frame — publishing stays at full
# rate; the CPU colorizer + 4-pane paint must not slow the stream.
# Qt 预览每 N 帧才渲染一次 —— 发布始终保持满帧率；
# CPU 上色 + 四窗格绘制绝不能拖慢数据流。
self.declare_parameter('preview_decimation', 2)
self.declare_parameter('auto_exposure', True)
self.declare_parameter('ae_limit_us', 3000.0)
@@ -122,7 +122,7 @@ class RealsenseColorPub(Node):
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 meter: hardware frame numbers vs actually published (loop rate)
# FPS 表：硬件帧号对比实际发布帧数（循环速率）
self._fps_t0 = 0.0
self._fps_pub_color = 0
self._fps_pub_depth = 0
@@ -132,7 +132,7 @@ class RealsenseColorPub(Node):
self._t_align = 0.0
self._t_pub = 0.0
qos = qos_profile_sensor_data # BEST_EFFORT, keep last 1
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
@@ -143,8 +143,8 @@ class RealsenseColorPub(Node):
self._depth_pub = self.create_publisher(
Image, f'/{self._name}/aligned_depth_to_color/image_raw', qos
)
# IR streams are published too so the preview can live in a separate
# viewer process (Qt must not share the GIL with the capture loop).
# IR 流也会一起发布，这样预览可以放在独立的查看器进程里
# （Qt 不能和采集循环共享 GIL）。
self._ir1_pub = None
self._ir2_pub = None
if self._enable_infra_param and self._preview_stereo:
@@ -163,8 +163,8 @@ class RealsenseColorPub(Node):
self._infra_mode = 'none'
self._colorizer = None
self._logged_depth = False
# Device depth scale → multiply raw uint16 by this to get true mm.
# D405 = 0.1 mm/unit, D435 = 1.0 mm/unit. Read from the sensor at startup.
# 设备深度尺度 → 用原始 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
@@ -205,13 +205,13 @@ class RealsenseColorPub(Node):
det_qos,
)
self.get_logger().info(
'SDK preview ON — COLOR|DEPTH|IR-L|IR-R in this process '
f'(hold {self._overlay_hold_sec:.1f}s). Close window ≠ stop camera.'
'SDK 预览已开启 —— 本进程内显示 COLOR|DEPTH|IR-L|IR-R '
f'（保留 {self._overlay_hold_sec:.1f}s）。关窗口 ≠ 停相机。'
)
self._start_pipeline()
def _pick_device(self, devices, rs):
"""Prefer configured serial, then D405 by name (this PC also has a D435)."""
"""优先用配置的序列号，其次按名字选 D405（这台机器上还接了 D435）。"""
hint = (self._name or 'd405').lower()
if self._serial:
for d in devices:
@@ -222,7 +222,7 @@ class RealsenseColorPub(Node):
for d in devices
)
raise RuntimeError(
f'Serial {self._serial!r} not found. Connected: {avail}'
f'找不到序列号 {self._serial!r}。当前已连接：{avail}'
)
for d in devices:
name = str(d.get_info(rs.camera_info.name) or '').lower()
@@ -231,7 +231,7 @@ class RealsenseColorPub(Node):
return devices[0]
def _reload_site_preview(self, stream_w: int = 0, stream_h: int = 0) -> None:
"""Pick up overlay_hold_sec (and warn if width/height changed) without restart."""
"""不重启就能读到新的 overlay_hold_sec（宽高变了则给出警告）。"""
path = self._site_yaml
if not path.is_file():
return
@@ -248,7 +248,7 @@ class RealsenseColorPub(Node):
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 reload failed: {exc}")
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))
@@ -264,14 +264,14 @@ class RealsenseColorPub(Node):
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"preview stream is {live_w}x{live_h}, site.yaml camera is "
f"{want_w}x{want_h} — restart: bash ~/ros2/scripts/run_camera_d405.sh --preview"
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:
# Empty payload = perception miss → clear green box immediately
# 空负载 = 感知漏检 → 立刻清掉绿框
if not msg.data or len(msg.data) < 8:
self._overlay = None
self._overlay_stamp = 0.0
@@ -295,8 +295,8 @@ class RealsenseColorPub(Node):
img.encoding = 'bgr8'
img.is_bigendian = 0
img.step = w * 3
# array.array('B', ...) hits the setter's fast path — plain bytes gets
# an element-wise debug check that costs ~40 ms per frame here.
# array.array('B', ...) 会走 setter 的快路径 —— 直接传 bytes 会触发
# 逐元素的 debug 检查，在这里每帧要多花约 40 ms。
img.data = array.array('B', np.ascontiguousarray(bgr).tobytes())
self._img_pub.publish(img)
@@ -324,7 +324,7 @@ class RealsenseColorPub(Node):
if not self._logged_depth:
self._logged_depth = True
self.get_logger().info(
f'first aligned depth {dw}x{dh} → '
f'首个对齐深度 {dw}x{dh} → '
f'/{self._name}/aligned_depth_to_color/image_raw'
)
@@ -333,10 +333,10 @@ class RealsenseColorPub(Node):
import pyrealsense2 as rs
except Exception as exc: # noqa: BLE001
raise RuntimeError(
'pyrealsense2 not available. conda activate ros_flexiv'
'pyrealsense2 不可用。请执行 conda activate ros_flexiv'
) from exc
# After killing a previous publisher, USB may take 1–3s to re-enumerate.
# 杀掉上一个发布进程后，USB 需要 1–3s 才能重新枚举。
ctx = rs.context()
devices = []
for attempt in range(8):
@@ -347,9 +347,9 @@ class RealsenseColorPub(Node):
ctx = rs.context()
if not devices:
raise RuntimeError(
'No RealSense connected (USB empty after retries). '
'Plug D405 into THIS machine on a USB3 (blue) port, unplug/replug, '
'and close realsense-viewer / the 192.168.0.108 capture site if it holds the camera.'
'没有连接 RealSense（多次重试后 USB 仍为空）。'
'请把 D405 插到**本机**的 USB3（蓝色）口，拔插一次，'
'并关闭 realsense-viewer / 192.168.0.108 上占用该相机的采集端。'
)
found = self._pick_device(devices, rs)
@@ -375,8 +375,8 @@ class RealsenseColorPub(Node):
)
if usb2:
self.get_logger().warn(
f'{cam_name} is on USB {usb_desc} — keeping depth/IR at '
'640x480@15. Plug D405 into a USB3 (blue) port for 848@30.'
f'{cam_name} 接在 USB {usb_desc} 上 —— 深度/IR 将保持 '
'640x480@15。把 D405 插到 USB3（蓝色）口可获得 848@30。'
)
self._width, self._height, self._fps = 640, 480, 15
@@ -399,7 +399,7 @@ class RealsenseColorPub(Node):
]
if usb2:
candidates = [(640, 480, 15), (640, 480, 5), (424, 240, 15)]
# Drop firmware junk (848@90 etc.) that ioctl-fails and wedges USB.
# 丢弃固件里的垃圾模式（848@90 之类），它们会 ioctl 失败并卡死 USB。
seen = set()
trimmed = []
for item in candidates:
@@ -425,8 +425,8 @@ class RealsenseColorPub(Node):
have_depth = False
have_infra = False
# USB3: try real IR-L+IR-R first (same stereo AE). Color+IR-L often
# fails on D405 — then IR-R + color-as-left (different look, not AE).
# USB3：先尝试真正的 IR-L+IR-R（共享立体自动曝光）。彩色+IR-L 在 D405 上
# 经常失败 —— 那就退回 IR-R + 把彩色当左目（观感不同，且不共享自动曝光）。
def _modes():
if want_depth and want_infra and not usb2:
yield True, 'both'
@@ -461,25 +461,25 @@ class RealsenseColorPub(Node):
self._infra_mode = infra_mode
if (w, h, fps) != want:
self.get_logger().warn(
f'Requested {want[0]}x{want[1]}@{want[2]}Hz '
f'→ using {w}x{h}@{fps}Hz'
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 color+depth failed — color only'
f'{w}x{h}@{fps}Hz 彩色+深度失败 —— 只用彩色'
)
elif want_infra and infra_mode == 'none':
self.get_logger().warn(
f'{w}x{h}@{fps}Hz stereo IR failed — color+depth preview'
f'{w}x{h}@{fps}Hz 立体 IR 失败 —— 改用彩色+深度预览'
)
elif infra_mode == 'both':
self.get_logger().info(
f'{w}x{h}@{fps}Hz stereo IR-L+IR-R (shared AE) + depth'
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=color gray, IR-R=raw IR '
'(looks brighter/darker — not two AE loops)'
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)
@@ -487,7 +487,7 @@ class RealsenseColorPub(Node):
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} failed: {exc}')
self.get_logger().warn(f'{w}x{h}@{fps}Hz {kind} 失败：{exc}')
profile = None
have_depth = False
have_infra = False
@@ -501,7 +501,7 @@ class RealsenseColorPub(Node):
break
if profile is None:
raise RuntimeError(
f'No supported color profile for D405. Last error: {last_err}'
f'D405 没有可用的彩色配置。最后的错误：{last_err}'
)
self._pipe = pipe
@@ -509,18 +509,18 @@ class RealsenseColorPub(Node):
self._have_depth = have_depth
self._have_infra = have_infra
self._logged_depth = False
# Publish depth in TRUE millimetres. D405 raw units are 0.1 mm (D435 1 mm);
# downstream (perception/servo) assumes mm, so normalise by the device scale.
# 深度按**真实毫米**发布。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/unit → mm/unit
self._depth_to_mm = ds * 1000.0 # m/单位 → mm/单位
self.get_logger().info(
f'depth scale {ds * 1000.0:.4f} mm/unit → publishing true mm'
f'深度尺度 {ds * 1000.0:.4f} mm/单位 → 按真实 mm 发布'
)
except Exception as exc: # noqa: BLE001
self.get_logger().warn(f'depth scale read failed, assume mm: {exc}')
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
@@ -546,11 +546,11 @@ class RealsenseColorPub(Node):
log=self.get_logger().info,
)
apply_realsense_exposure(profile.get_device(), **kwargs)
# Firmware often resets AE on the first frames; stamp it again.
# 固件经常会在最初几帧重置自动曝光；所以再设置一次。
_time.sleep(0.25)
apply_realsense_exposure(profile.get_device(), **kwargs)
except Exception as exc: # noqa: BLE001
self.get_logger().warn(f'exposure apply: {exc}')
self.get_logger().warn(f'应用曝光参数失败：{exc}')
if self._preview is not None:
if self._infra_mode == 'both':
self._preview.set_pane_titles(
@@ -575,8 +575,8 @@ class RealsenseColorPub(Node):
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 failed ({exc}); using D405 defaults for '
f'{self._width}x{self._height}'
f'get_intrinsics 失败（{exc}）；改用 D405 的默认内参，'
f'分辨率 {self._width}x{self._height}'
)
w, h = float(self._width), float(self._height)
if w >= 1200:
@@ -595,10 +595,10 @@ class RealsenseColorPub(Node):
depth_txt = (
f' + aligned depth /{self._name}/aligned_depth_to_color/image_raw'
if have_depth
else ' (color only)'
else ' （仅彩色）'
)
if have_infra:
depth_txt += ' + IR-L/IR-R preview'
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 '
@@ -611,30 +611,30 @@ class RealsenseColorPub(Node):
self._thread.start()
def _enable_color_ae_fast(self, profile, rs) -> None:
"""Color auto-exposure that prefers short exposure (less motion blur)."""
"""彩色自动曝光，优先用短曝光（减少运动模糊）。"""
try:
sensor = profile.get_device().first_color_sensor()
except Exception as exc: # noqa: BLE001
self.get_logger().warn(f'color AE: no color sensor ({exc})')
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 = keep FPS, shorten exposure when the scene is bright enough
# 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; less motion blur at 30 Hz
sensor.set_option(limit, 5000.0) # µs；30 Hz 下减少运动模糊
except Exception:
pass
self.get_logger().info('color auto-exposure ON (prefer FPS / less blur)')
self.get_logger().info('彩色自动曝光已开启（优先帧率 / 减少模糊）')
except Exception as exc: # noqa: BLE001
self.get_logger().warn(f'color AE: {exc}')
self.get_logger().warn(f'彩色 AE：{exc}')
def _enable_stereo_ae(self, profile, rs) -> None:
"""One auto-exposure for both IR imagers (stereo module)."""
"""两路 IR 成像器共用一路自动曝光（立体模块）。"""
try:
sensor = profile.get_device().first_depth_sensor()
if sensor.supports(rs.option.enable_auto_exposure):
@@ -642,15 +642,15 @@ class RealsenseColorPub(Node):
limit = getattr(rs.option, "auto_exposure_limit", None)
if limit is not None and sensor.supports(limit):
try:
# Cap IR exposure well under the 33 ms frame period so a
# dim scene cannot silently drop the stream below 30 Hz.
# 把 IR 曝光上限压到远低于 33 ms 的帧周期，这样光线暗的场景
# 也不会悄悄把数据流掉到 30 Hz 以下。
sensor.set_option(limit, 20000.0)
self.get_logger().info('stereo IR exposure limit 20ms')
self.get_logger().info('立体 IR 曝光上限 20ms')
except Exception:
pass
self.get_logger().info('stereo IR auto-exposure ON (shared L/R)')
self.get_logger().info('立体 IR 自动曝光已开启（左右共享）')
except Exception as exc: # noqa: BLE001
self.get_logger().warn(f'stereo AE: {exc}')
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:
@@ -695,7 +695,7 @@ class RealsenseColorPub(Node):
return self._pipe is not None
except Exception as exc: # noqa: BLE001
self.get_logger().error(
f'reconnect failed: {exc}. Unplug/replug USB3, slack the cable.',
f'重连失败：{exc}。请拔插 USB3，并给线缆留点余量。',
throttle_duration_sec=5.0,
)
return False
@@ -738,15 +738,15 @@ class RealsenseColorPub(Node):
if dropped or misses >= 4:
usb = getattr(self, '_usb_desc', '?')
self.get_logger().error(
f'Camera stream lost (USB={usb}): {text[:120]}. '
'Reconnecting — slack the USB cable so the arm does not yank it.'
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}')
self.get_logger().warn(f'wait_for_frames：{exc}')
continue
if not color:
misses += 1
@@ -756,14 +756,14 @@ class RealsenseColorPub(Node):
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:
# raw device units → true mm (D405 raw is 0.1 mm/unit)
# 原始设备单位 → 真实 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)
# Preview work (CPU colorizer, IR copies, 4-pane paint) only every
# Nth frame — it must not slow down publishing.
# 预览相关的工作（CPU 上色、IR 拷贝、四窗格绘制）每 N 帧才做一次
# —— 它绝不能拖慢发布。
do_push = (
self._preview is not None
and (self._frame_i % self._preview_every) == 0
@@ -776,13 +776,13 @@ class RealsenseColorPub(Node):
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
# Only substitute color-gray when the raw left IR stream is unavailable.
# 只有在拿不到原始左目 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'first frame {sw}x{sh}')
self.get_logger().info(f'首帧 {sw}x{sh}')
if self._preview is not None:
self._preview.set_pane_titles(
f"COLOR {sw}x{sh}",
@@ -809,7 +809,7 @@ class RealsenseColorPub(Node):
self._overlay is None or age > hold
):
self._preview.set_status(
'NO box — check perception_node / conf'
'没有框 —— 检查 perception_node / conf'
)
else:
self._preview.set_status('')
@@ -817,15 +817,15 @@ class RealsenseColorPub(Node):
bgr.copy(), depth_bgr, ir_l_img, ir_r_img
)
except Exception as exc: # noqa: BLE001
# Context may be shutting down
# 上下文可能正在关闭
if not self._running:
break
self.get_logger().warn(
f'publish failed: {exc}', throttle_duration_sec=2.0
f'发布失败：{exc}', throttle_duration_sec=2.0
)
def _publish_ir(self, frame, pub) -> None:
"""Raw IR imager as mono8 for the external viewer process."""
"""把原始 IR 成像器以 mono8 发布，供外部查看器进程使用。"""
arr = np.asanyarray(frame.get_data())
h, w = int(arr.shape[0]), int(arr.shape[1])
msg = Image()
@@ -840,8 +840,8 @@ class RealsenseColorPub(Node):
pub.publish(msg)
def _fps_meter(self, color, depth, depth_u16) -> None:
"""Log published vs hardware frame rate — separates loop slowness
(preview/CPU) from SDK-side drops (AE long exposure / USB)."""
"""记录发布帧率与硬件帧率 —— 用来区分循环太慢
（预览/CPU）和 SDK 侧掉帧（自动曝光长曝光 / USB）。"""
now = time.monotonic()
try:
fn_c = int(color.get_frame_number())
@@ -886,13 +886,13 @@ def main(args=None) -> None:
preview = getattr(node, '_preview', None)
if preview is not None:
app = node._qt_app
# Closing the window must NOT stop the camera — track needs the stream.
# 关闭窗口绝不能停掉相机 —— 跟踪需要这条数据流。
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(
'Preview closed — camera keeps publishing'
'预览已关闭 —— 相机继续发布'
)
node._preview = None
else:
src/arm_application/arm_application/realsense_sdk_view.py
+8 -8
View File
@@ -1,8 +1,8 @@
#!/usr/bin/env python3
"""RealSense SDK preview: fixed 2x2 Qt panes (color / depth / IR-L / IR-R).
"""RealSense SDK 预览：固定的 2x2 Qt 窗格（彩色 / 深度 / IR-L / IR-R）。
Stitching four streams into one pixmap looked like a single camera view when
depth/IR were missing. Each stream now has its own labeled pane.
把四路画面拼成一张 pixmap 时，一旦缺了深度 / IR 看起来就像单相机视图。
现在每路画面都有自己带标题的独立窗格。
"""
from __future__ import annotations
@@ -221,15 +221,15 @@ class SdkPreviewWindow(QMainWindow):
return
color = self._draw_overlay(color, ov, status)
labs = self._img_labels
self._set_pane(labs[0], color, "NO COLOR")
self._set_pane(labs[1], depth, "NO DEPTH")
self._set_pane(labs[2], ir_l, "NO IR-L")
self._set_pane(labs[3], ir_r, "NO IR-R")
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("PySide6 not available for RealSense preview window")
raise RuntimeError("RealSense 预览窗口缺少 PySide6，无法创建")
app = QApplication.instance()
if app is None:
app = QApplication([])
src/arm_application/arm_application/realsense_view.py
+18 -19
View File
@@ -1,11 +1,10 @@
#!/usr/bin/env python3
"""realsense_view — D405 4-pane preview (COLOR|DEPTH|IR-L|IR-R), own process.
"""realsense_view —— D405 四窗格预览（COLOR|DEPTH|IR-L|IR-R），独立进程运行。
The camera node (``realsense_color_pub``) only publishes topics. All Qt work
runs here so painting can never starve the camera capture loop (Python GIL
made ``publish`` wall-time jump from 1 ms to ~70 ms when they shared a
process). Closing the window only hides the preview; Ctrl+C stops the viewer
(the camera keeps publishing).
相机节点（``realsense_color_pub``）只负责发布话题。所有 Qt 工作都放在这里，
这样绘制永远不会拖垮相机采集循环（两者同进程时，Python 的 GIL 会让 ``publish``
的墙钟耗时从 1 ms 跳到约 70 ms）。关闭窗口只是隐藏预览；Ctrl+C 才停止查看器
（相机仍在持续发布）。
"""
from __future__ import annotations
@@ -31,7 +30,7 @@ from arm_application.realsense_color_pub import _site_yaml_path
def _colorize_depth_u16(d: np.ndarray) -> np.ndarray:
"""Fast numpy JET colorize for the preview pane (no SDK colorizer)."""
"""为预览窗格做快速 numpy JET 上色（不使用 SDK 的 colorizer）。"""
vis = np.zeros((d.shape[0], d.shape[1], 3), dtype=np.uint8)
valid = d > 0
if not valid.any():
@@ -79,7 +78,7 @@ class RealsenseView(Node):
self._stage2_stamp = 0.0
self._site_yaml = _site_yaml_path()
self._site_mtime = 0.0
# RX rate meter — visible stutter diagnosis (transport vs paint)
# RX 速率表 —— 用来直观诊断卡顿（是传输问题还是绘制问题）
self._rx_color = 0
self._rx_depth = 0
self._rx_ir = 0
@@ -115,7 +114,7 @@ class RealsenseView(Node):
self._qt_app = ensure_app()
hud = self._hud or self._name
self._preview = SdkPreviewWindow(
title=f'{hud} /{self._name} COLOR|DEPTH|IR (viewer)'
title=f'{hud} /{self._name} COLOR|DEPTH|IR （查看器）'
)
self._preview.set_pane_titles(
'COLOR', 'DEPTH', 'IR-L (左红外)', 'IR-R (右红外)'
@@ -124,11 +123,11 @@ class RealsenseView(Node):
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'viewer ON — subscribing /{self._name}/color|depth|infra1|infra2 '
f'(hold {self._hold:.2f}s). Close window = hide only.'
f'查看器已启动 —— 订阅 /{self._name}/color|depth|infra1|infra2 '
f'（保留 {self._hold:.2f}s）。关闭窗口 = 仅隐藏。'
)
# Callbacks only store the newest message; conversion happens in _push.
# 回调只存最新消息；转换在 _push 里做。
def _on_color(self, msg: Image) -> None:
self._color = msg
self._rx_color += 1
@@ -184,7 +183,7 @@ class RealsenseView(Node):
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 reload failed: {exc}")
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))
@@ -216,7 +215,7 @@ class RealsenseView(Node):
elif now - self._rx_t0 >= 5.0:
dt = now - self._rx_t0
self.get_logger().info(
f'viewer rx color={self._rx_color / dt:.1f}/s '
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
@@ -228,7 +227,7 @@ class RealsenseView(Node):
self._overlay = None
self._preview.set_overlay(None)
if self._overlay is None or age > self._hold:
miss = 'NO box — check perception_node / conf'
miss = '没有框 —— 检查 perception_node / conf'
self._preview.set_status(f'{self._hud} {miss}' if self._hud else miss)
else:
self._preview.set_status(self._hud)
@@ -244,7 +243,7 @@ class RealsenseView(Node):
int(self._depth.height), int(self._depth.width)
)
depth_bgr = _colorize_depth_u16(d)
# Mono→BGR here (executor thread) so the Qt paint thread stays cheap.
# 在这里（执行器线程）做 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)
@@ -254,8 +253,8 @@ def main(args=None) -> None:
rclpy.init(args=args)
node = RealsenseView()
app = node._qt_app
# ROS callbacks (image deserialization, depth colorize, push) run on
# executor threads; the main thread only paints Qt — no shared GIL bursts.
# ROS 回调（图像反序列化、深度上色、推送）跑在执行器线程上；
# 主线程只负责绘制 Qt —— 不会有共享的 GIL 突发。
executor = MultiThreadedExecutor(num_threads=3)
executor.add_node(node)
spin_t = threading.Thread(target=executor.spin, daemon=True)
@@ -265,7 +264,7 @@ def main(args=None) -> None:
app.processEvents()
time.sleep(0.005)
if node._preview is not None and not node._preview.isVisible():
node.get_logger().warn('Preview closed — viewer idles (camera keeps publishing)')
node.get_logger().warn('预览已关闭 —— 查看器挂起（相机仍在发布）')
node._preview = None
except KeyboardInterrupt:
pass
src/arm_application/arm_application/run_joint_trajectory.py
+24 -24
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""Send a multi-point joint trajectory via /follow_joint_trajectory (real + mock)."""
"""通过 ``/follow_joint_trajectory`` 发送多点关节轨迹（真机 + mock 都适用）。"""
from __future__ import annotations
@@ -16,7 +16,7 @@ DEFAULT_JOINTS = [
'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
]
NUM_JOINTS = 7
# flat positions (3 pts x 7 joints) + times — ROS params do not support dict lists
# 扁平化存放的位置（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,
@@ -34,7 +34,7 @@ class RunJointTrajectory(Node):
self.declare_parameter('action_name', 'follow_joint_trajectory')
self.declare_parameter('waypoint_times', DEFAULT_TIMES)
self.declare_parameter('waypoint_positions', DEFAULT_POSITIONS)
# 1 = run once (default); 0 = loop until Ctrl+C; N = repeat N times
# 1 = 只跑一次（默认）；0 = 循环直到 Ctrl+C；N = 重复 N 次
self.declare_parameter('loop_count', 1)
self.declare_parameter('pause_between_loops', 0.5)
@@ -52,8 +52,8 @@ class RunJointTrajectory(Node):
def run(self) -> int:
if not self._client.wait_for_server(timeout_sec=15.0):
self.get_logger().error(
f'Action [{self._action_name}] not available. '
'Start real stack first: bash scripts/run_real.sh'
f'Action [{self._action_name}] 不可用。'
'请先启动真机栈：bash scripts/run_real.sh'
)
return 1
@@ -62,12 +62,12 @@ class RunJointTrajectory(Node):
n_j = len(self.joint_names)
n_pts = len(times)
if n_pts < 1:
self.get_logger().error('waypoint_times is empty')
self.get_logger().error('waypoint_times 为空')
return 1
if len(positions) != n_pts * n_j:
self.get_logger().error(
f'waypoint_positions length {len(positions)} != '
f'{n_pts} points x {n_j} joints = {n_pts * n_j}'
f'waypoint_positions 长度 {len(positions)} != '
f'{n_pts} 个点 x {n_j} 个关节 = {n_pts * n_j}'
)
return 1
@@ -83,35 +83,35 @@ class RunJointTrajectory(Node):
duration = goal.time_from_start[-1]
if loop_count == 0:
self.get_logger().info(
f'Loop mode: {goal.num_points} points, ~{duration:.1f}s/cycle '
f'(Ctrl+C to stop)'
f'循环模式：{goal.num_points} 个点，约 {duration:.1f}s/圈 '
f'（Ctrl+C 停止）'
)
elif loop_count > 1:
self.get_logger().info(
f'Loop mode: {loop_count} cycles, {goal.num_points} points, '
f'~{duration:.1f}s/cycle'
f'循环模式：{loop_count} 圈，{goal.num_points} 个点，'
f'约 {duration:.1f}s/圈'
)
else:
self.get_logger().info(
f'Sending {goal.num_points} points, v_scale={goal.velocity_scale}, '
f'duration={duration:.1f}s'
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'Cycle {iteration}/{loop_count}')
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('Done: OK')
self.get_logger().info('完成：OK')
return 0
if loop_count > 1 and iteration >= loop_count:
self.get_logger().info(f'Done: completed {loop_count} cycles')
self.get_logger().info(f'完成：已跑完 {loop_count} 圈')
return 0
if pause > 0:
time.sleep(pause)
@@ -122,11 +122,11 @@ class RunJointTrajectory(Node):
while rclpy.ok() and not send.done():
rclpy.spin_once(self, timeout_sec=0.05)
if time.monotonic() > deadline:
self.get_logger().error('Timed out sending goal')
self.get_logger().error('发送目标超时')
return 2
handle = send.result()
if handle is None or not handle.accepted:
self.get_logger().error('Goal rejected')
self.get_logger().error('目标被拒绝')
return 3
res_fut = handle.get_result_async()
@@ -138,18 +138,18 @@ class RunJointTrajectory(Node):
if now >= next_log:
left = max(0.0, deadline - now)
self.get_logger().info(
f'Executing trajectory (~{duration:.0f}s planned), '
f'waiting for robot... ({left:.0f}s timeout left)'
f'正在执行轨迹（计划约 {duration:.0f}s），'
f'等待机器人……（剩余超时 {left:.0f}s）'
)
next_log = now + 10.0
if now > deadline:
self.get_logger().error('Timed out waiting for result')
self.get_logger().error('等待结果超时')
return 4
result = res_fut.result().result
if not result.success:
self.get_logger().error(f'Failed: {result.message}')
self.get_logger().error(f'失败：{result.message}')
return 5
self.get_logger().info(f'Cycle finished: {result.message or "OK"}')
self.get_logger().info(f'本圈结束：{result.message or "OK"}')
return 0
src/arm_application/arm_application/servo_node.py
+114 -116
View File
@@ -1,13 +1,13 @@
#!/usr/bin/env python3
"""servo_node — 100 Hz Twist PBVS (own process, no YOLO).
"""servo_node —— 100 Hz 的 Twist 视觉伺服（PBVS，独立进程，不跑 YOLO）。
Hard rules:
- MultiThreadedExecutor + Reentrant ControlCallbackGroup for 100 Hz timer
- Subscribe PointStamped only (no Service)
- cmd_vel: SystemDefaults + keep_last(1) so driver always sees latest
- Watchdog: lost target → freeze extrapolate, zero vel, clear I, zero traj
- Latency compensation only while target is fresh
- Never sendTransform in the 100 Hz loop
硬性规则：
- 用 ``MultiThreadedExecutor`` + ``Reentrant`` 的 ``ControlCallbackGroup`` 驱动 100 Hz 定时器
- 只订阅 ``PointStamped``（不用 Service）
- ``cmd_vel`` 用 ``SystemDefaults`` + ``keep_last(1)``，保证驱动器总能看到最新指令
- 看门狗：目标丢失 → 冻结外推、速度置零、清 I、轨迹置零
- 只有在目标数据新鲜时才做延迟补偿
- 绝不在 100 Hz 循环里调用 ``sendTransform``
"""
from __future__ import annotations
@@ -59,7 +59,7 @@ class ServoNode(Node):
super().__init__("servo_node")
tid = threading.get_ident()
self.get_logger().warn(
f"servo_node started pid={os.getpid()} thread_id={tid}"
f"servo_node 已启动 pid={os.getpid()} thread_id={tid}"
)
self.declare_parameter("joint_prefix", "Rizon4-063989_")
@@ -87,9 +87,9 @@ class ServoNode(Node):
self.declare_parameter("predict_v_max", 0.35)
self.declare_parameter("predict_jump_m", 0.08)
# --- Watchdog (lost target hard stop) ---
# --- 看门狗（丢失目标即硬停） ---
self.declare_parameter("watchdog_timeout_sec", 1.2)
self.declare_parameter("stale_hold_sec", 0.28) # freeze XY before watchdog trip
self.declare_parameter("stale_hold_sec", 0.28) # 触发看门狗前先冻结 XY
# --- PID ---
self.declare_parameter("kp_xy", 2.00)
@@ -100,7 +100,7 @@ class ServoNode(Node):
self.declare_parameter("kd_z", 0.45)
self.declare_parameter("i_limit_xy", 0.03)
self.declare_parameter("i_limit_z", 0.025)
# Integral separation thresholds
# 积分分离阈值
self.declare_parameter("i_sep_pos_m", 0.005)
self.declare_parameter("i_sep_ang_rad", 0.02)
@@ -109,27 +109,27 @@ class ServoNode(Node):
self.declare_parameter("v_max_xy", 0.06)
self.declare_parameter("v_max_z", 0.04)
self.declare_parameter("a_max", 0.45)
self.declare_parameter("lost_timeout_sec", 1.2) # alias / stamp-age backup
self.declare_parameter("e_xy_clip_m", 0.080) # never treat error as >8 cm
self.declare_parameter("lost_timeout_sec", 1.2) # 别名 / 用时间戳年龄兜底
self.declare_parameter("e_xy_clip_m", 0.080) # 误差绝不按大于 8 cm 处理
# --- Velocity LPF (smooth 100Hz → traj) ---
# --- 速度低通滤波（把 100Hz 平滑成轨迹） ---
self.declare_parameter("vel_lpf_alpha", 0.40)
self.declare_parameter("wz_lpf_alpha", 0.40)
# --- JTC dq sliding window ---
# --- JTC 的 dq 滑动窗口 ---
self.declare_parameter("dq_window_size", 5)
self.declare_parameter("min_ee_z_m", 0.20)
self.declare_parameter("workspace_xyz_min", [0.15, -0.50, 0.18])
self.declare_parameter("workspace_xyz_max", [0.80, 0.50, 0.60])
# Base-frame EE hold (flange). Z/XY in metres, editable via site.yaml.
# base 系下的末端保持（法兰）。Z/XY 单位为米，可通过 site.yaml 修改。
self.declare_parameter("ee_x_m", 0.25)
self.declare_parameter("ee_y_m", 0.00)
self.declare_parameter("ee_z_m", 0.25)
self.declare_parameter("ee_hold_xy", False)
self.declare_parameter("ee_vertical_down", True)
# hover = flange/gripper-center XY on the 3D feature (any lock height).
# lookat = image center. hover_offset_ee_xy_m is flange-XY to jaw center.
# hover = 让法兰/夹爪中心 XY 停在三维特征点上（任意锁定高度都可）。
# lookat = 对准图像中心。hover_offset_ee_xy_m 是法兰 XY 到爪中心的偏移。
self.declare_parameter("xy_track", "hover")
self.declare_parameter("hover_offset_ee_xy_m", [0.0, 0.0])
self.declare_parameter("arm_label", "")
@@ -144,14 +144,14 @@ class ServoNode(Node):
self.declare_parameter("camera_fy", 390.0)
self.declare_parameter("camera_cx", 320.0)
self.declare_parameter("camera_cy", 240.0)
# Seek: at lock height with no valid target, scan the work area so the
# eye-in-hand camera can find the workpiece on its own (自适应找工件).
# Seek：到达锁定高度却没有有效目标时，扫描工作区域，让 eye-in-hand
# 相机自己去找到工件（自适应找工件）。
self.declare_parameter("seek_enabled", True)
self.declare_parameter("seek_radius_max_m", 0.16)
self.declare_parameter("seek_speed_mps", 0.05)
self.declare_parameter("seek_timeout_sec", 45.0)
# Hover integral: breaks the ~4mm joint-command deadband that otherwise
# stalls the last few mm of XY convergence onto the workpiece.
# 悬停积分：用来突破约 4mm 的关节指令死区，否则 XY 收敛到最后几毫米
# 时就会停滞、贴不到工件上。
self.declare_parameter("ki_hover", 0.8)
# 串联用：XY 入死区 + Z 到锁高持续这么久 → /visual_servo/locked=True
self.declare_parameter("lock_hold_sec", 2.0)
@@ -253,7 +253,7 @@ class ServoNode(Node):
f"HANDEYE yaml={he_path} "
f"cam_in_flange=({t_he[0]*1000:.1f},{t_he[1]*1000:.1f},{t_he[2]*1000:.1f})mm "
f"optical_vs_flangeZ={self._cam_tilt_deg:.1f}deg "
f"— hover uses this T, not TF lookup"
f"— 悬停使用这个 T，而不是用 TF 查询"
)
self.pid_x = PidController(
@@ -287,12 +287,12 @@ class ServoNode(Node):
self._lock = threading.Lock()
self._p_meas = np.zeros(3, dtype=float)
self._stamp_sec = -1.0
self._last_seen_time = -1.0 # monotonic, updated on every PointStamped
self._last_seen_time = -1.0 # 单调时钟，每次收到 PointStamped 都会更新
self._have_target = False
self._target_lost_flag = True
self._v_raw = np.zeros(3, dtype=float)
self._v_filt = np.zeros(3, dtype=float) # LPF linear (cam)
self._wz_filt = 0.0 # reserved angular z LPF
self._v_filt = np.zeros(3, dtype=float) # 线速度低通（相机系）
self._wz_filt = 0.0 # 预留的角速度 z 低通
self._wz_raw = 0.0
self._zeroed = True
self._last_js: Optional[JointState] = None
@@ -305,14 +305,14 @@ class ServoNode(Node):
self._p_feat_base: Optional[np.ndarray] = None
self._he_cmp_done = False
self._feat_kf = KalmanFilter3D(q_pos=2.0e-3, q_vel=1.5e-1, r_pos=4.0e-4)
# XY hover only after flange Z is on the lock plane (fresh depth).
# 只有当法兰 Z 已经落到锁定平面上（深度新鲜）才开启 XY 悬停。
self._xy_armed = bool(self.ee_z_m <= 0.0)
# Seek (search) state: scan for the workpiece when at lock_z with no target.
# Seek（搜索）状态：在锁定高度且没有目标时扫描工件。
self._seek_t0 = 0.0
self._seek_theta = 0.0
self._seek_origin = (0.0, 0.0)
self._seek_warned = False
self._hover_ixy = np.zeros(2, dtype=float) # hover integral (anti-deadband)
self._hover_ixy = np.zeros(2, dtype=float) # 悬停积分（抗死区）
point_topic = str(self.get_parameter("point_topic").value)
point_qos = QoSProfile(
@@ -363,10 +363,10 @@ class ServoNode(Node):
callback_group=self._control_cbg,
)
mode = "CONTROL ON" if self.enable_control else "cmd_vel only"
mode = "控制已开启" if self.enable_control else "仅 cmd_vel"
arm = self.arm_label or self.ee_frame
self.get_logger().info(
f"servo ready [{arm}] [{mode}] {control_hz:.0f}Hz watchdog={self.watchdog_timeout:.2f}s "
f"伺服就绪 [{arm}] [{mode}] {control_hz:.0f}Hz watchdog={self.watchdog_timeout:.2f}s "
f"vel_lpf_α={self.vel_lpf_alpha:.2f} dq_win={self.dq_window_size} "
f"lock_z={self.ee_z_m:.3f}m (site servo.ee_z_m) "
f"xy_track={self.xy_track} cam_tilt={self._cam_tilt_deg:.1f}° "
@@ -381,7 +381,7 @@ class ServoNode(Node):
self._last_js = msg
def _on_point(self, msg: PointStamped) -> None:
"""Fresh vision measurement — arm the watchdog and clear lost flag."""
"""新的视觉测量 —— 给看门狗“上弦”并清除丢失标志。"""
p_cam = np.array(
[float(msg.point.x), float(msg.point.y), float(msg.point.z)], dtype=float
)
@@ -417,7 +417,7 @@ class ServoNode(Node):
self._last_seen_time = now_m
self._have_target = True
self._target_lost_flag = False
self._seek_t0 = 0.0 # target acquired — reset seek for next time
self._seek_t0 = 0.0 # 已捕获目标 —— 重置 seek，供下次使用
self._seek_warned = False
if msg.header.frame_id:
self.camera_frame = msg.header.frame_id
@@ -432,12 +432,12 @@ class ServoNode(Node):
if was_lost:
self._reacq_mono = time.monotonic()
self.get_logger().info(
"target reacquired — resume servo",
"已重新捕获目标 —— 继续伺服",
throttle_duration_sec=0.5,
)
def _reset_motion_state(self) -> None:
"""Hard stop internals: I=0, filt vel=0, dq window clear."""
"""硬停内部状态：I=0、滤波速度=0、dq 窗口清空。"""
self.pid_x.reset()
self.pid_y.reset()
self.pid_z.reset()
@@ -455,9 +455,9 @@ class ServoNode(Node):
self._twist_pub.publish(tw)
def _watchdog_trip(self, reason: str) -> None:
"""Priority-1 hard stop: no extrapolate, zero cmd, zero traj delta."""
"""优先级 1 的硬停：不外推、指令置零、轨迹增量为零。"""
if not self._target_lost_flag:
self.get_logger().warn(f"WATCHDOG STOP: {reason}")
self.get_logger().warn(f"看门狗急停：{reason}")
self._target_lost_flag = True
self._reset_motion_state()
self._zeroed = True
@@ -475,7 +475,7 @@ class ServoNode(Node):
if self._ctrl_count >= int(round(1.0 / self._ctrl_dt)):
elapsed = (now_msg - self._ctrl_t0).nanoseconds * 1e-9
hz = self._ctrl_count / max(elapsed, 1e-6)
self.get_logger().info(f"cmd_vel rate ~{hz:.1f} Hz", throttle_duration_sec=1.0)
self.get_logger().info(f"cmd_vel 速率约 {hz:.1f} Hz", throttle_duration_sec=1.0)
self._ctrl_count = 0
self._ctrl_t0 = now_msg
@@ -486,9 +486,9 @@ class ServoNode(Node):
p_meas = self._p_meas.copy()
lost = self._target_lost_flag
# ----- Startup: descend to lock height FIRST, even before any detection.
# have=False only until the first PointStamped ever arrives, so this is
# a pure startup phase: Z-only descent, XY pinned, vertical-down.
# ----- 启动阶段：**先**下降到锁定高度，甚至在任何检测到来之前。
# 在第一个 PointStamped 到达之前 have 一直是 False，所以这是一个纯启动
# 阶段：只动 Z 下降，XY 锁死，姿态朝下。
if (
self.enable_control
and self.ee_z_m > 0.0
@@ -497,22 +497,21 @@ class ServoNode(Node):
):
T_ee0 = self._lookup_T(self.base_frame, self.ee_frame)
if T_ee0 is None:
return # TF not ready yet — wait, do not hold-trip
return # TF 还没就绪 —— 等待，不要触发保持急停
self._update_xy_arm(float(T_ee0[2, 3]))
if not self._xy_armed:
self.get_logger().info(
f"startup descend to lock_z {self.ee_z_m:.3f}m "
f"now={T_ee0[2, 3]:.3f}m (no target yet, XY pinned)",
f"启动下降至 lock_z {self.ee_z_m:.3f}m "
f"当前={T_ee0[2, 3]:.3f}m（还没有目标，XY 锁死）",
throttle_duration_sec=1.0,
)
self._target_lost_flag = False # allow execute during blind descent
self._target_lost_flag = False # 盲降期间允许执行
self._execute_twist(np.zeros(3, dtype=float), p_cam=None, hold_xy=True)
self._lock_feedback(False)
return
# ----- Seek: at lock height but no valid target yet. The eye-in-hand
# camera only sees a small patch, so scan the work area (drift to the
# nominal work XY, then spiral out) until the workpiece enters view.
# ----- Seek：已在锁定高度但还没有有效目标。eye-in-hand 相机只能看到
# 一小块区域，所以扫描工作区（先漂到标称工作 XY，再向外螺旋）直到工件进入视野。
if (
self.enable_control
and self.seek_enabled
@@ -522,53 +521,53 @@ class ServoNode(Node):
):
T_ee0 = self._lookup_T(self.base_frame, self.ee_frame)
if T_ee0 is None:
return # TF not ready — wait, do not hold-trip
return # TF 还没就绪 —— 等待，不要触发保持急停
v_seek = self._seek_velocity(T_ee0[:3, 3])
if v_seek is None:
# timed out — give up searching, hold in place
# 超时了 —— 放弃搜索，原地保持
if not self._seek_warned:
self._seek_warned = True
self.get_logger().warn(
f"seek timeout {self.seek_timeout:.0f}s — workpiece not found. "
"Hold position. Put the workpiece on the table in view, or widen "
"seek_radius_max_m / check camera mount & handeye."
f"seek 超时 {self.seek_timeout:.0f}s —— 没找到工件。"
"保持当前位置。请把工件放到桌面视野内，或调大 "
"seek_radius_max_m / 检查相机安装与手眼标定。"
)
self._lock_feedback(False)
self._pub_zero_twist()
self._send_hold()
return
self.get_logger().info(
f"seek workpiece… ee_xy=({T_ee0[0,3]:.3f},{T_ee0[1,3]:.3f}) "
f"寻找工件…… ee_xy=({T_ee0[0,3]:.3f},{T_ee0[1,3]:.3f}) "
f"v=({v_seek[0]:+.3f},{v_seek[1]:+.3f})",
throttle_duration_sec=1.0,
)
self._target_lost_flag = False # allow execute during seek
self._target_lost_flag = False # seek 期间允许执行
self._execute_twist(
np.zeros(3, dtype=float), p_cam=None, v_base_override=v_seek
)
self._lock_feedback(False)
return
# ----- Item 1: Watchdog (receive-time) — freeze ALL motion if stale -----
# ----- 第 1 项：看门狗（按接收时间）—— 数据过期就冻结所有运动 -----
if (not have) or last_seen < 0.0:
self._lock_feedback(False)
self._watchdog_trip("no PointStamped yet")
self._watchdog_trip("还没有收到 PointStamped")
return
seen_age = now_mono - last_seen
stamp_age = now_sec - stamp if stamp >= 0.0 else 1e9
if seen_age > self.watchdog_timeout or stamp_age > self.lost_timeout:
self._watchdog_trip(
f"target lost seen_age={seen_age:.3f}s stamp_age={stamp_age:.3f}s"
f"目标丢失 seen_age={seen_age:.3f}s stamp_age={stamp_age:.3f}s"
)
self._lock_feedback(False)
return
# Frozen box / USB stall: pin joints, do not keep driving last error.
# 检测框冻结 / USB 卡住：锁住关节，不要继续按上一个误差驱动。
stale = max(0.15, float(self.stale_hold_sec))
if seen_age > stale or stamp_age > stale:
self.get_logger().info(
f"stale hold seen_age={seen_age:.2f}s — not tracking a frozen box",
f"过期保持 seen_age={seen_age:.2f}s —— 不跟踪冻结的检测框",
throttle_duration_sec=1.0,
)
self._v_raw[:] = 0.0
@@ -580,13 +579,13 @@ class ServoNode(Node):
return
if lost:
# Should have been cleared on _on_point; belt-and-suspenders
# 本应在 _on_point 里清掉；这里再加一道保险
self._lock_feedback(False)
self._pub_zero_twist()
return
# Hover uses the vision 3D point as-is. Do not add cmd_vel (wrong frame)
# onto p_cam — that walks the target off the workpiece.
# Hover 直接使用视觉三维点。不要把 cmd_vel（坐标系不对）
# 叠加到 p_cam 上 —— 那样会把目标从工件上带跑。
p = p_meas.copy()
if self.xy_track == "lookat":
dt_lat = float(np.clip(stamp_age, 0.0, self.max_extrap))
@@ -618,7 +617,7 @@ class ServoNode(Node):
ex *= s
ey *= s
r_xy = clip_xy
# Camera-depth Z is unused when holding flange height in base frame
# 在 base 系里保持法兰高度时，相机深度 Z 就不再使用
z_dead = True if self.ee_z_m > 0.0 else (abs(ez) <= self.deadzone_z_m)
if xy_dead:
ex = ey = 0.0
@@ -628,7 +627,7 @@ class ServoNode(Node):
ez = 0.0
self.pid_z.reset()
# ----- Item 2: PID with integral separation (inside PidController) -----
# ----- 第 2 项：带积分分离的 PID（分离逻辑在 PidController 内部） -----
v_des = np.array(
[
0.0 if self.ee_hold_xy else self.pid_x.update(ex, float(p[0]), dt),
@@ -644,7 +643,7 @@ class ServoNode(Node):
v_des[:2] = _saturate_vec(v_des[:2].copy(), self.v_max_xy)
v_des[2] = float(np.clip(v_des[2], -self.v_max_z, self.v_max_z))
# Base-plane mode: never command camera +Z (that dives into the part)
# base 平面模式：绝不指令相机 +Z（那会直接扎进工件里）
if self.ee_z_m > 0.0:
v_des[2] = 0.0
else:
@@ -661,7 +660,7 @@ class ServoNode(Node):
if z_meas < (self.z_min_m - 0.02):
v_des[2] = min(v_des[2], -0.03)
# Accel limit on raw desire, then LPF
# 先对原始期望速度做加速度限制，再做低通滤波
dv = v_des - self._v_raw
n_dv = float(np.linalg.norm(dv))
dv_max = self.a_max * dt
@@ -669,10 +668,10 @@ class ServoNode(Node):
dv *= dv_max / n_dv
self._v_raw = self._v_raw + dv
# ----- Item 3: first-order LPF on linear (+ reserved wz) -----
# ----- 第 3 项：对线速度做一阶低通（+ 预留的 wz） -----
a = self.vel_lpf_alpha
self._v_filt = a * self._v_raw + (1.0 - a) * self._v_filt
# Angular reserved (PBVS currently linear-only); still filter for API completeness
# 角速度目前预留（PBVS 当前只做线速度）；仍做滤波以保持接口完整
self._wz_raw = 0.0
aw = self.wz_lpf_alpha
self._wz_filt = aw * self._wz_raw + (1.0 - aw) * self._wz_filt
@@ -706,11 +705,11 @@ class ServoNode(Node):
if xy_dead and z_dead:
self._lock_feedback(True) # 串联信号：活目标 + 死区驻留 = 收敛
self.get_logger().info(
f"deadzone hold |e_xy|={r_xy_raw*1000:.1f}mm ez={ez*1000:.1f}mm",
f"死区保持 |e_xy|={r_xy_raw*1000:.1f}mm ez={ez*1000:.1f}mm",
throttle_duration_sec=1.0,
)
if self.enable_control:
# Still pin height / vertical-down if slightly off, else freeze
# 若高度 / 垂直朝下稍有偏差仍要修正，否则彻底冻结
if need_base:
self._execute_twist(np.zeros(3, dtype=float), p_cam=p, hold_xy=True)
elif not self._zeroed:
@@ -728,7 +727,7 @@ class ServoNode(Node):
f" feat=({look[1][0]:.3f},{look[1][1]:.3f})"
)
self.get_logger().info(
f"twist cam v=({v_cam[0]:+.3f},{v_cam[1]:+.3f},{v_cam[2]:+.3f}) "
f"twist 相机系 v=({v_cam[0]:+.3f},{v_cam[1]:+.3f},{v_cam[2]:+.3f}) "
f"|e_xy|={r_xy_raw*1000:.0f}mm ez={ez*1000:.0f}mm age={stamp_age*1000:.0f}ms"
f"{extra}",
throttle_duration_sec=0.5,
@@ -739,14 +738,14 @@ class ServoNode(Node):
self._execute_twist(v_cam, p_cam=p)
def _e_stop(self, reason: str) -> None:
self.get_logger().warn(f"E-STOP: {reason}")
self.get_logger().warn(f"急停：{reason}")
self._watchdog_trip(reason)
def _R_cam_to_base(self, T_ee: np.ndarray) -> np.ndarray:
return (T_ee @ self._T_ee_cam())[:3, :3]
def _T_ee_cam(self) -> np.ndarray:
# Always the calibrated yaml. TF is only for RViz / a sanity print.
# 始终使用标定出的 yaml。TF 只用于 RViz / 打印做核对。
return np.array(self.T_cam_to_gripper, dtype=float)
def _log_handeye_tf_vs_yaml(self) -> None:
@@ -754,7 +753,7 @@ class ServoNode(Node):
T_tf = self._lookup_T(self.ee_frame, self.handeye_child)
if T_tf is None:
self.get_logger().warn(
"HANDEYE TF lookup failed (ok) — servo uses yaml "
"HANDEYE TF 查询失败（无妨）—— 伺服使用 yaml "
f"t=({t_yaml[0]*1000:.1f},{t_yaml[1]*1000:.1f},{t_yaml[2]*1000:.1f})mm"
)
return
@@ -767,8 +766,8 @@ class ServoNode(Node):
)
if d > 0.015:
self.get_logger().error(
"HANDEYE TF ≠ yaml by >15mm — duplicate parent on camera frame? "
"Servo still uses yaml."
"HANDEYE TF 与 yaml 的偏差 >15mm —— 相机坐标系上挂了重复的父节点？ "
"伺服仍然使用 yaml。"
)
def _intersect_z(
@@ -789,11 +788,11 @@ class ServoNode(Node):
def _lookat_vs_feature(
self, T_ee: np.ndarray, p_cam: np.ndarray
) -> Optional[tuple]:
"""Base XY error that centers the feature in the image.
"""让特征点居中到图像里所需的 base 系 XY 误差。
30° eye-in-hand: image center is the optical-axis hit on the work
plane, not the flange XY. Translating the flange in XY at fixed
height/attitude moves that look-at 1:1.
30° 的 eye-in-hand 安装：图像中心是光轴打到工作平面上的点，
而不是法兰的 XY。在固定高度 / 姿态下平移法兰 XY，会把该 look-at 点
按 1:1 一起带走。
"""
T_bc = T_ee @ self._T_ee_cam()
R = T_bc[:3, :3]
@@ -812,7 +811,7 @@ class ServoNode(Node):
return err, p_feat, p_look
def _feature_in_base(self, T_ee: np.ndarray, p_cam: np.ndarray) -> np.ndarray:
"""Workpiece 3D point (camera frame) → base XYZ. Keep XYZ consistent."""
"""工件三维点（相机系）→ base 系 XYZ。保持 XYZ 一致。"""
T_bc = T_ee @ self._T_ee_cam()
ph = np.array(
[float(p_cam[0]), float(p_cam[1]), float(p_cam[2]), 1.0], dtype=float
@@ -822,11 +821,11 @@ class ServoNode(Node):
def _hover_vs_feature(
self, T_ee: np.ndarray, p_cam: np.ndarray
) -> Optional[tuple]:
"""Park flange (and coaxial gripper center) XY on the workpiece XY in base.
"""把法兰（以及同轴的夹爪中心）的 XY 停在 base 系里的工件 XY 上。
hover_offset_ee_xy_m shifts the tracked point in the flange frame
(jaw center if it is not on the flange origin). RGI is coaxial: leave [0,0].
Must NOT re-multiply a stale p_cam by the live T_ee.
hover_offset_ee_xy_m 会在法兰坐标系里平移被跟踪点
（当爪中心不在法兰原点时用它）。RGI 是同轴的：保持 [0,0] 即可。
绝不能拿实时的 T_ee 去重复乘一个过期的 p_cam。
"""
p_ee = np.asarray(T_ee[:3, 3], dtype=float)
off = self.hover_offset_ee_xy
@@ -887,7 +886,7 @@ class ServoNode(Node):
return None
def _xy_deadzone(self, r_xy: float, *, update: bool = True) -> bool:
"""Hysteresis: enter at deadzone_m, leave at 1.6× so noise does not chatter."""
"""滞回：在 deadzone_m 处进入，在 1.6 倍处退出，避免噪声来回抖。"""
enter = max(0.002, float(self.deadzone_m))
leave = enter * 1.6
if self._xy_in_dead:
@@ -925,7 +924,7 @@ class ServoNode(Node):
self._lock_since = None
def _w_align_flange_down(self, R_ee: np.ndarray) -> np.ndarray:
"""Angular vel in base to put flange +Z along world −Z (tool down)."""
"""base 系下的角速度，用来把法兰 +Z 转到世界 −Z 方向（工具朝下）。"""
z_now = np.asarray(R_ee, 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))
@@ -942,7 +941,7 @@ class ServoNode(Node):
return axis * mag
def _shape_xy_vel(self, v_des_xy: np.ndarray, dt: float) -> np.ndarray:
"""Slew-rate + LPF on the hover command that actually moves the arm."""
"""真正驱动机械臂的那条悬停指令，外加变化率限制 + 低通滤波。"""
v_des = np.asarray(v_des_xy, dtype=float).reshape(2)
prev = self._v_xy_cmd
dv = v_des - prev
@@ -961,7 +960,7 @@ class ServoNode(Node):
return out
def _update_xy_arm(self, ee_z: float) -> bool:
"""XY hover only after flange is on lock_z (real depth, adaptive hover)."""
"""只有当法兰到达 lock_z（真实深度、自适应悬停）后才做 XY 悬停。"""
if self.ee_z_m <= 0.0:
self._xy_armed = True
return True
@@ -977,8 +976,8 @@ class ServoNode(Node):
self.pid_x.reset()
self.pid_y.reset()
self.get_logger().warn(
f"lock-z ready ee_z={z:.3f}m (want {self.ee_z_m:.3f}, no climb) "
"— start XY hover on live handeye"
f"lock-z 就绪 ee_z={z:.3f}m（期望 {self.ee_z_m:.3f}，不再回抬） "
"—— 在实时 handeye 上开始 XY 悬停"
)
return True
err = abs(z - float(self.ee_z_m))
@@ -991,8 +990,8 @@ class ServoNode(Node):
self.pid_x.reset()
self.pid_y.reset()
self.get_logger().warn(
f"lock-z reached ee_z={z:.3f}m (want {self.ee_z_m:.3f}) "
"— start XY hover on live handeye"
f"已到达 lock-z ee_z={z:.3f}m（期望 {self.ee_z_m:.3f}）"
" —— 在实时 handeye 上开始 XY 悬停"
)
elif err > gate + 0.05:
self._xy_armed = False
@@ -1000,20 +999,19 @@ class ServoNode(Node):
self._feat_kf.initialized = False
self._v_xy_cmd[:] = 0.0
self.get_logger().info(
f"left lock-z band ee_z={z:.3f}m — XY hold, re-descend",
f"已离开 lock-z 区间 ee_z={z:.3f}m —— XY 保持，重新下降",
throttle_duration_sec=1.0,
)
return self._xy_armed
def _seek_velocity(self, p_ee: np.ndarray) -> Optional[np.ndarray]:
"""Base-frame XY velocity to scan for the workpiece: spiral outward around
the point where the arm reached lock height, so the eye-in-hand camera
sweeps the table. Returns None when the seek has timed out (caller holds)."""
"""base 系下的 XY 速度，用来扫描寻找工件：以「机械臂到达锁定高度时的位置」
为中心向外螺旋，让 eye-in-hand 相机扫过桌面。超时后返回 None（由调用方保持）。"""
now = time.monotonic()
if self._seek_t0 <= 0.0:
self._seek_t0 = now
self._seek_theta = 0.0
self._seek_origin = (float(p_ee[0]), float(p_ee[1])) # search near descent XY
self._seek_origin = (float(p_ee[0]), float(p_ee[1])) # 在下降点附近搜索 XY
if (now - self._seek_t0) > self.seek_timeout:
return None
ox, oy = self._seek_origin
@@ -1037,7 +1035,7 @@ class ServoNode(Node):
def _clip_workspace_vel(
self, p_ee: np.ndarray, v_base: np.ndarray, T: float
) -> np.ndarray:
"""Keep commands inside the box; if already outside, allow recovery in."""
"""把指令限制在盒子里；如果本来就在盒子外，允许它往回恢复。"""
v = np.asarray(v_base, dtype=float).copy()
dt = max(float(T), 1e-3)
for i in range(3):
@@ -1063,7 +1061,7 @@ class ServoNode(Node):
hold_xy: bool = False,
v_base_override: Optional[np.ndarray] = None,
) -> None:
# Hard freeze if watchdog already tripped mid-cycle
# 如果看门狗在本周期中途已经触发，就硬冻结
if self._target_lost_flag:
self._dq_hist.clear()
return
@@ -1078,7 +1076,7 @@ class ServoNode(Node):
T_ee = self._lookup_T(self.base_frame, self.ee_frame)
if q is None or T_ee is None:
self.get_logger().warn(
"execute skip: missing joint_states or TF base→ee",
"execute 跳过：缺少 joint_states 或 base→ee 的 TF",
throttle_duration_sec=1.0,
)
return
@@ -1097,8 +1095,8 @@ class ServoNode(Node):
if self._reacq_mono > 0.0 and (now_m - self._reacq_mono) < 0.12:
v_xy *= 0.85
# hover: park flange XY on the 3D feature at any lock height.
# lookat: drive optical-axis hit (image center) onto the feature.
# hover：在任意锁定高度上把法兰 XY 停在三维特征点上。
# lookat：把光轴命中点（图像中心）驱到特征点上。
xy_ok = self._update_xy_arm(float(p_ee[2]))
if (
xy_ok
@@ -1120,11 +1118,11 @@ class ServoNode(Node):
v_base[1] = 0.0
self._hover_ixy[:] = 0.0
else:
# Taper near the deadzone so P-control does not bang-bang.
# 在死区附近做渐变，避免 P 控制来回 bang-bang。
enter = max(0.002, float(self.deadzone_m))
taper = float(np.clip((nerr - enter) / max(enter, 1e-3), 0.0, 1.0))
# Integral (anti-windup ±4cm) breaks the joint-command deadband
# that otherwise stalls the last few mm of XY convergence.
# 积分项（抗积分饱和 ±4cm）用来突破关节指令死区，
# 否则 XY 收敛的最后几毫米就会停滞。
self._hover_ixy[0] = float(np.clip(
self._hover_ixy[0] + float(err[0]) * dt_use, -0.04, 0.04))
self._hover_ixy[1] = float(np.clip(
@@ -1149,8 +1147,8 @@ class ServoNode(Node):
v_base[0] *= v_xy / nxy
v_base[1] *= v_xy / nxy
# Hold flange XYZ in base (config, default ~250 mm). Visual XY is
# overwritten when ee_hold_xy is true.
# 在 base 系里保持法兰 XYZ（可配置，默认约 250 mm）。当 ee_hold_xy
# 为真时，视觉算出的 XY 会被覆盖。
if self.ee_hold_xy:
v_base[0] = float(np.clip(kp * (self.ee_x_m - p_ee[0]), -self.v_max_xy, self.v_max_xy))
v_base[1] = float(np.clip(kp * (self.ee_y_m - p_ee[1]), -self.v_max_xy, self.v_max_xy))
@@ -1164,7 +1162,7 @@ class ServoNode(Node):
self._v_xy_cmd[:] = 0.0
if self.ee_z_m > 0.0:
self.get_logger().info(
f"descend to lock_z {self.ee_z_m:.3f}m now={p_ee[2]:.3f}m XY hold",
f"下降至 lock_z {self.ee_z_m:.3f}m 当前={p_ee[2]:.3f}m XY 保持",
throttle_duration_sec=1.0,
)
shaped = self._shape_xy_vel(v_base[:2], dt_use)
@@ -1189,7 +1187,7 @@ class ServoNode(Node):
p_next = p_ee + v_base * T
if self.ee_z_m > 0.0:
# Never command below the configured plane
# 绝不指令到配置平面以下
floor = max(self.min_ee_z, self.ee_z_m - 0.02)
if p_next[2] < floor and v_base[2] < 0.0:
v_base[2] = 0.0
@@ -1205,7 +1203,7 @@ class ServoNode(Node):
Tl = self._lookup_T(self.base_frame, fr)
if Tl is None:
self.get_logger().warn(
f"execute skip: TF miss {fr}",
f"execute 跳过：TF 缺 {fr}",
throttle_duration_sec=1.0,
)
return
@@ -1217,10 +1215,10 @@ class ServoNode(Node):
)
dq = qdot * T
# ----- Item 5: sliding-window average on joint displacement -----
# ----- 第 5 项：对关节位移做滑动窗口平均 -----
self._dq_hist.append(dq.copy())
dq_avg = np.mean(np.stack(list(self._dq_hist), axis=0), axis=0)
# Keep end-velocity consistent with averaged step
# 让末端速度与平均后的步长保持一致
qdot_avg = dq_avg / T
mag = float(np.max(np.abs(dq_avg)))
@@ -1281,7 +1279,7 @@ def main(args=None) -> None:
except KeyboardInterrupt:
pass
finally:
node._e_stop("node shutdown")
node._e_stop("节点关闭")
executor.shutdown()
node.destroy_node()
if rclpy.ok():
src/arm_application/arm_application/site_io.py
+1 -1
View File
@@ -1,4 +1,4 @@
"""Load ~/ros2/config/site.yaml from application nodes."""
"""供应用节点加载 ``~/ros2/config/site.yaml``。"""
from __future__ import annotations
src/arm_application/arm_application/stage2_locate_node.py
+2 -2
View File
@@ -149,8 +149,8 @@ class Stage2Locate(Node):
self.declare_parameter("robot_sn", os.environ.get("SITE_ROBOT_SN") or "")
self.declare_parameter("hover_only", False)
self.declare_parameter("target_hole", "") # "" → site stage2.target_hole
self.declare_parameter("hover_z_m", -1.0) # >0 → 覆盖 site
self.declare_parameter("target_hole", "") # 空字符串 → 用 site 的 stage2.target_hole
self.declare_parameter("hover_z_m", -1.0) # 大于 0 → 覆盖 site 里的值
self.declare_parameter("preview", True)
self.declare_parameter("action_name", "rizon_arm_controller/follow_joint_trajectory")
src/arm_application/arm_application/tactile_cli.py
+5 -5
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""Orisys visuo-tactile CLI. Ports / config from site.yaml tactile:."""
"""Orisys 视触觉命令行工具。端口 / 配置来自 ``site.yaml`` 的 ``tactile`` 段。"""
from __future__ import annotations
@@ -12,7 +12,7 @@ from arm_application.site_io import load_site_dict, site_path
def main() -> int:
parser = argparse.ArgumentParser(description="Orisys tactile CLI (site.yaml)")
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()
@@ -29,7 +29,7 @@ def main() -> int:
try:
tactile.connect()
except Exception as exc:
print(f"ERROR: {exc}", file=sys.stderr)
print(f"错误：{exc}", file=sys.stderr)
return 1
t0 = time.time()
n = 0
@@ -49,10 +49,10 @@ def main() -> int:
)
time.sleep(0.01)
except KeyboardInterrupt:
print("interrupt")
print("已中断")
finally:
tactile.disconnect()
print(f"done frames={n}")
print(f"完成 frames={n}")
return 0
src/arm_application/arm_application/tactile_interface.py
+1 -1
View File
@@ -1,4 +1,4 @@
"""Tactile sample / command types. Backend (Orisys / mock) is swappable."""
"""触觉采样 / 指令类型。后端（Orisys / mock）可替换。"""
from __future__ import annotations
src/arm_application/arm_application/yolo_detector.py
+38 -38
View File
@@ -1,9 +1,9 @@
#!/usr/bin/env python3
"""Ultralytics YOLO / TensorRT wrapper.
"""Ultralytics YOLO / TensorRT 封装。
Public: ``resolve_weights``, ``YoloDetector``, ``Detection``, ``pick_best``,
``MpYoloDetector``. TensorRT loads the sibling ``.engine`` next to the
``.onnx`` / ``.pt`` path when ``backend='tensorrt'``.
对外接口：``resolve_weights``、``YoloDetector``、``Detection``、``pick_best``、
``MpYoloDetector``。当 ``backend='tensorrt'`` 时，TensorRT 会加载与
``.onnx`` / ``.pt`` 路径同级的 ``.engine`` 文件。
类别名（标签）由 ``model_labels`` 统一提供：优先读权重旁边的
``<模型名>.labels.yaml``（用户可随时手改），其次读模型内嵌 metadata。
@@ -33,13 +33,13 @@ from .model_labels import (
logger = logging.getLogger(__name__)
# Sentinel: worker still on the previous frame — caller must NOT count a miss.
# 哨兵值：worker 还停在上一次的那帧 —— 调用方不能把它算作漏检。
INFER_BUSY = object()
@dataclass
class Detection:
xyxy: np.ndarray # (4,) float: x1,y1,x2,y2
xyxy: np.ndarray # (4,) float：x1,y1,x2,y2
conf: float
cls_id: int
cls_name: str
@@ -69,7 +69,7 @@ def _resolve_device(device: str) -> str:
def resolve_weights(weights: Union[str, Path], backend: str = "auto") -> Path:
"""Prefer TensorRT .engine next to the .pt when backend=auto/tensorrt."""
"""当 backend=auto/tensorrt 时，优先使用与 .pt 同级的 TensorRT .engine。"""
p = Path(weights).expanduser()
b = (backend or "auto").strip().lower()
engine = p.with_suffix(".engine")
@@ -78,13 +78,13 @@ def resolve_weights(weights: Union[str, Path], backend: str = "auto") -> Path:
if engine.is_file():
return engine
raise FileNotFoundError(
f"TensorRT engine missing: {engine}. "
f"Export: bash ~/ros2/scripts/weights.sh convert {p} --name {p.stem}"
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 missing: {onnx}")
raise FileNotFoundError(f"缺少 ONNX 文件：{onnx}")
if b in ("", "auto"):
if engine.is_file():
return engine
@@ -155,19 +155,19 @@ class YoloDetector:
from ultralytics import YOLO
except Exception as exc: # noqa: BLE001
raise RuntimeError(
"ultralytics not installed. In ros_flexiv: pip install ultralytics"
"未安装 ultralytics。在 ros_flexiv 环境里执行：pip install ultralytics"
) from exc
path = Path(weights).expanduser()
if not path.is_file():
raise FileNotFoundError(
f"YOLO weights not found: {path}. "
f"Place your .pt at ~/ros2/models/yolo/workpiece.pt"
f"找不到 YOLO 权重：{path}。"
f"请把你的 .pt 放到 ~/ros2/models/yolo/workpiece.pt"
)
try:
import torch
# Do not let PyTorch occupy all CPU cores (camera + ROS need them).
# 别让 PyTorch 占满所有 CPU 核心（相机 + ROS 都要用）。
torch.set_num_threads(1)
torch.set_num_interop_threads(1)
if torch.cuda.is_available():
@@ -243,7 +243,7 @@ class YoloDetector:
imgsz: int | None = None,
**_kwargs,
) -> List[Detection]:
"""Alias used by hole detectors (xyxy + conf)."""
"""给孔检测器用的别名接口（xyxy + conf）。"""
if conf is not None:
self.conf = float(conf)
if iou is not None:
@@ -302,7 +302,7 @@ class YoloDetector:
def _yolo_worker_main(q_in, q_out, cfg: dict) -> None:
"""Spawned process: GPU YOLO only. Never import rclpy here."""
"""spawn 出来的子进程：只跑 GPU 上的 YOLO。这里绝不能 import rclpy。"""
try:
path = resolve_weights(cfg["weights"], cfg.get("backend") or "auto")
det = YoloDetector(
@@ -321,7 +321,7 @@ def _yolo_worker_main(q_in, q_out, cfg: dict) -> None:
item = q_in.get()
if item is None:
return
# Drain to the newest queued frame — never spend GPU on stale images.
# 一直排空到队列里最新的帧 —— 绝不把 GPU 浪费在过期图像上。
try:
while True:
newer = q_in.get_nowait()
@@ -345,7 +345,7 @@ def _yolo_worker_main(q_in, q_out, cfg: dict) -> None:
def _put_drop_old(q, item) -> None:
"""Never block the GPU process on a full result queue."""
"""绝不让 GPU 进程因为结果队列满了而阻塞。"""
try:
q.put_nowait(item)
return
@@ -362,7 +362,7 @@ def _put_drop_old(q, item) -> None:
class YoloInferProcess:
"""YOLO in a spawn() child so ROS callbacks never share the PyTorch GIL."""
"""把 YOLO 放进 spawn() 子进程，让 ROS 回调永远不用和 PyTorch 抢 GIL。"""
def __init__(self, cfg: dict) -> None:
self._cfg = dict(cfg)
@@ -371,9 +371,9 @@ class YoloInferProcess:
def _start(self) -> None:
ctx = mp.get_context("spawn")
# maxsize=2 + worker drains to newest: parent never has to drop frames
# (Queue put_nowait/get_nowait races were the source of multi-second
# "silent" freezes with the worker alive).
# 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(
@@ -386,12 +386,12 @@ class YoloInferProcess:
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 worker: {err}")
raise RuntimeError(f"YOLO 工作进程：{err}")
self.weights_path = str(msg[1])
self.imgsz = int(self._cfg.get("imgsz") or 416)
def restart(self) -> None:
"""Kill a hung/dead worker and spawn a fresh one (engine reload ~2-5 s)."""
"""杀掉卡死 / 挂掉的 worker，再起一个新的（重新加载 engine 约 2-5 s）。"""
self.close()
self._start()
@@ -400,19 +400,19 @@ class YoloInferProcess:
return bool(self._p is not None and self._p.is_alive())
def submit(self, image_bgr: np.ndarray, seq: int = 0) -> None:
"""Queue the newest frame. If the worker is busy, skip — it drains to
the newest queued frame after every infer, so nothing stale runs."""
"""把最新一帧排进队列。如果 worker 正忙就跳过 —— 它在每次推理后都会
排空到队列里最新的帧，所以不会跑任何过期数据。"""
if not self.alive:
raise RuntimeError("YOLO worker died")
raise RuntimeError("YOLO 工作进程已死亡")
try:
self._q_in.put((int(seq), image_bgr), timeout=0.010)
except Exception:
pass
def wait_result(self, timeout: float = 0.045):
"""Return ``(seq, list[Detection], gpu_ms)`` or ``None``."""
"""返回 ``(seq, list[Detection], gpu_ms)`` 或 ``None``。"""
if not self.alive:
raise RuntimeError("YOLO worker died")
raise RuntimeError("YOLO 工作进程已死亡")
deadline = time.monotonic() + max(0.004, float(timeout))
latest = None
while time.monotonic() < deadline:
@@ -456,7 +456,7 @@ class YoloInferProcess:
return seq, dets, gpu_ms
def infer(self, image_bgr: np.ndarray, *, half: bool = False, seq: int = 0):
"""Compat: submit + wait. Prefer submit/wait_result for pipelining."""
"""兼容接口：提交 + 等待。要做流水线的话优先用 submit/wait_result。"""
del half
self.submit(image_bgr, seq)
got = self.wait_result(timeout=0.08)
@@ -476,7 +476,7 @@ class YoloInferProcess:
self._p.terminate()
self._p.join(timeout=1.0)
if self._p.is_alive():
self._p.kill() # stuck in a CUDA call — SIGKILL
self._p.kill() # 卡在 CUDA 调用里出不来 —— 直接 SIGKILL
self._p.join(timeout=1.0)
self._p = None
@@ -507,16 +507,16 @@ def pick_best(
min_conf: float = 0.12,
min_area: float = 4000.0,
) -> Optional[Detection]:
"""Keep the current workpiece under fast motion.
"""让当前工件在快速运动下也能被持续跟踪。
Match against a *predicted* previous box with a wide gate. If IoU fails
but YOLO still fired above min_conf, keep that detection — do not return
None (that was the main source of dropouts while moving).
Tiny boxes (logo / 通止规尖) are rejected so the arm does not lock on the mat.
用一个较宽的匹配门限去和**预测出的**上一帧检测框做匹配。如果 IoU 不达标、
但 YOLO 仍以高于 min_conf 的置信度报出了目标，就保留该检测 —— 不要返回
None（那正是运动过程中目标丢失的主要来源）。
过小的框（logo / 通止规尖）会被拒掉，避免机械臂锁到垫子上。
"""
if not dets:
return None
# Locked track may dip below min_conf while moving (blur). New locks cannot.
# 已锁定的跟踪在运动（模糊）时可能掉到 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:
@@ -553,7 +553,7 @@ def pick_best(
return s
best = max(usable, key=score_new)
# Fast move: keep a live YOLO even if it jumped far from the last box.
# 快速移动：即使 YOLO 报出的框离上一帧很远，也保留这个实时检测。
need = max(float(min_conf), float(reacquire_conf))
if float(best.conf) >= need:
return best
src/arm_application/config/dual_grippers.yaml
+2 -2
View File
@@ -1,5 +1,5 @@
# Optional fallback for DualArmGripperManager(load_dual_arm_gripper_config(path)).
# Daily use reads ~/ros2/config/site.yaml via load_from_site().
# DualArmGripperManager(load_dual_arm_gripper_config(path)) 的可选兜底配置。
# 日常使用通过 load_from_site() 读取 ~/ros2/config/site.yaml。
left_arm:
robot_sn: "Rizon4-063989"
gripper_port: "/dev/ttyUSB0"
src/arm_application/config/perception.yaml
+5 -5
View File
@@ -21,26 +21,26 @@ perception_node:
min_conf: 0.12
vision_hz: 20.0
z_ref_m: 0.30
# Large gate + reacquire: fast motion must NOT freeze track
# 大跳变门限 + 重新捕获：快速运动绝不能把跟踪卡死
jump_px: 280.0
jump_z_m: 0.20
min_depth_m: 0.05
max_depth_m: 2.50
object_frame: "tracked_object"
camera_frame: "d405_color_optical_frame"
# Snappy KF + blend toward raw measurement
# 灵敏的 KF + 向原始观测融合
kf_q_pos: 0.002
kf_q_vel: 0.05
kf_r_pos: 0.0008
meas_blend: 0.65
# Faster infer + tight box follow
# 更快的推理 + 更紧的框跟随
infer_max_width: 640
box_ema_alpha: 0.85
miss_clear_frames: 2
# Item 4 — adaptive Z depth LPF
# 第 4 项 — 自适应 Z 深度低通滤波
z_alpha_min: 0.10
z_alpha_max: 0.60
z_alpha_gain: 2.0
feature_mode: "plane" # stereo 3D centroid; box = monocular box-center ray
feature_mode: "plane" # stereo 为 3D 质心；box = 单目框中心的射线
plane_inlier_m: 0.008
plane_band_m: 0.06
src/arm_application/config/servo.yaml
+6 -6
View File
@@ -1,5 +1,5 @@
# servo_node defaults. Launch overlays ~/ros2/config/site.yaml → servo:
# Do not edit this file for day-to-day tuning.
# servo_node 的默认值。launch 会用 ~/ros2/config/site.yaml 的 servo: 覆盖。
# 日常调参不要改这个文件。
servo_node:
ros__parameters:
@@ -19,7 +19,7 @@ servo_node:
enable_control: false
max_extrapolate_sec: 0.08
# Item 1 — lost-target hard stop
# 第 1 项 — 丢失目标硬停
watchdog_timeout_sec: 0.3
lost_timeout_sec: 0.3
@@ -31,7 +31,7 @@ servo_node:
kd_z: 0.45
i_limit_xy: 0.03
i_limit_z: 0.025
# Item 2 — integral separation
# 第 2 项 — 积分分离
i_sep_pos_m: 0.005
i_sep_ang_rad: 0.02
@@ -41,11 +41,11 @@ servo_node:
v_max_z: 0.07
a_max: 0.70
# Item 3 — velocity LPF
# 第 3 项 — 速度低通滤波
vel_lpf_alpha: 0.40
wz_lpf_alpha: 0.40
# Item 5 — dq sliding window
# 第 5 项 — dq 滑动窗口
dq_window_size: 5
min_ee_z_m: 0.20
src/arm_application/launch/dh_pgea.launch.py
+3 -3
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""Launch DH PGEA Modbus node. Config from ~/ros2/config/site.yaml gripper:."""
"""启动 DH PGEA Modbus 节点。配置来自 ``~/ros2/config/site.yaml`` 的 ``gripper`` 段。"""
from __future__ import annotations
@@ -44,8 +44,8 @@ def _setup(context, *args, **kwargs):
),
LogInfo(
msg=(
"[dh_pgea] Flexiv load_gripper stays false — this is DH Modbus, "
"not a Flexiv Studio gripper."
"[dh_pgea] Flexiv 的 load_gripper 保持 false —— 这里是 DH Modbus，"
"不是 Flexiv Studio 夹爪。"
)
),
Node(
src/arm_application/launch/dual_gripper.launch.py
+2 -2
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""Launch dual DH grippers. Ports come from site.yaml robots[].gripper."""
"""启动双路 DH 夹爪。端口来自 ``site.yaml`` 的 ``robots[].gripper``。"""
from __future__ import annotations
@@ -20,7 +20,7 @@ def generate_launch_description() -> LaunchDescription:
[
DeclareLaunchArgument("sides", default_value="left,right"),
DeclareLaunchArgument("allow_offline", default_value="true"),
LogInfo(msg=f"[dual_gripper] site={site} (arm_msgs, IPC Modbus, not Flexiv gripper)"),
LogInfo(msg=f"[dual_gripper] site={site}（arm_msgs，IPC Modbus，不是 Flexiv 夹爪）"),
Node(
package="arm_application",
executable="dual_gripper",
src/arm_application/launch/hole_detect.launch.py
+1 -1
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""Launch hole_detect on the current-arm color topic."""
"""在当前机械臂的彩色话题上启动 hole_detect。"""
from __future__ import annotations
src/arm_application/launch/visual_servo.launch.py
+11 -11
View File
@@ -1,7 +1,7 @@
#!/usr/bin/env python3
"""Launch: perception_node + servo_node + static hand-eye TF.
"""启动：perception_node + servo_node + 静态手眼 TF。
All tunables come from ~/ros2/config/site.yaml (override via launch args if needed).
所有可调参数都来自 ``~/ros2/config/site.yaml``（必要时可用 launch 参数覆盖）。
"""
from __future__ import annotations
@@ -20,12 +20,12 @@ from launch_ros.actions import Node
def _ws_root() -> Path:
# Prefer env from env.sh / load_site_config
# 优先使用 env.sh / load_site_config 提供的环境变量
for key in ("ROS2_WS", "SITE_ROS2_ROOT"):
v = os.environ.get(key, "").strip()
if v:
return Path(v)
# launch file lives in install/.../share/arm_application/launch or src/...
# launch 文件可能位于 install/.../share/arm_application/launch 或 src/...
here = Path(__file__).resolve()
for p in [here.parents[i] for i in range(2, 8)]:
if (p / "config" / "site.yaml").is_file():
@@ -46,7 +46,7 @@ def _load_site():
cfg = os.environ.get("SITE_CONFIG", "").strip()
path = Path(cfg).expanduser() if cfg else site_path()
# site_path() uses script location; force workspace root file
# site_path() 依赖脚本所在位置；这里强制使用工作区根目录下的文件
if not path.is_file():
path = root / "config" / "site.yaml"
data = load_site(path)
@@ -97,7 +97,7 @@ def _launch_setup(context, *args, **kwargs):
share = Path(get_package_share_directory("arm_application"))
site, site_path, perc_over, servo_over, robot = _load_site()
# Launch args override site when explicitly passed (non-empty / non-default handled below)
# launch 参数显式传入时覆盖 site 配置（空值 / 默认值的处理见下）
handeye = LaunchConfiguration("handeye_file").perform(context).strip()
site_he = str(servo_over.get("handeye_file") or (site.get("paths") or {}).get("handeye_file") or "")
if not handeye or handeye in (
@@ -107,8 +107,8 @@ def _launch_setup(context, *args, **kwargs):
handeye = site_he or handeye
if not Path(handeye).is_file():
raise FileNotFoundError(
f"hand-eye result missing: {handeye}. "
"Re-run: bash ~/ros2/scripts/run_handeye.sh calibrate"
f"手眼标定结果缺失：{handeye}。"
"请重新运行：bash ~/ros2/scripts/run_handeye.sh calibrate"
)
enable = LaunchConfiguration("enable_control").perform(context).lower() in (
@@ -197,8 +197,8 @@ def _launch_setup(context, *args, **kwargs):
),
LogInfo(msg=f"[site] YOLO weights: {weights}"),
LogInfo(
msg="nodes: perception_node (YOLO) | servo_node (100Hz) — "
"separate processes, topic /visual_servo/filtered_point"
msg="节点：perception_node（YOLO）| servo_node（100Hz）—— "
"各自独立进程，话题 /visual_servo/filtered_point"
),
]
pub_tf = LaunchConfiguration("publish_handeye_tf").perform(context).lower() in (
@@ -206,7 +206,7 @@ def _launch_setup(context, *args, **kwargs):
)
if pub_tf:
nodes.append(
LogInfo(msg=f"static hand-eye {ee_frame} → {cam_frame} t=[{tx:.3f},{ty:.3f},{tz:.3f}]")
LogInfo(msg=f"静态手眼 {ee_frame} → {cam_frame} t=[{tx:.3f},{ty:.3f},{tz:.3f}]")
)
nodes.append(static_tf)
nodes.extend([perception, servo])
src/arm_application/setup.py
+8 -8
View File
@@ -55,28 +55,28 @@ setup(
'move_to_pose_client = arm_application.move_to_pose_client:main',
'run_joint_trajectory = arm_application.run_joint_trajectory:main',
'goto_ee_plane = arm_application.goto_ee_plane:main',
# Camera
# 相机
'realsense_color_pub = arm_application.realsense_color_pub:main',
'realsense_view = arm_application.realsense_view:main',
# Hand-eye
# 手眼标定
'handeye_teach = arm_application.handeye_teach:main',
'handeye_waypoints = arm_application.handeye_waypoints:main',
'handeye_calibrate = arm_application.handeye_calibrate:main',
# Visual servo (current stack)
# 视觉伺服（当前主链路）
'perception_node = arm_application.perception_node:main',
'servo_node = arm_application.servo_node:main',
# Gripper
# 夹爪
'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',
# Hole detect (no insert motion)
# 入孔检测（只检测，不做插入动作）
'hole_detect = arm_application.hole_detect_node:main',
# Stage-2 one-shot hole localization (k1/k2) + gauge alignment
# 第二阶段：一次性孔定位（k1/k2）+ 对规
'stage2_locate = arm_application.stage2_locate_node:main',
# Stage-3 compliant hole search (Orisys tactile spiral + insert)
# 第三阶段：柔顺找孔（Orisys 触觉螺旋 + 插入）
'stage3_search = arm_application.stage3_search_node:main',
# Three-stage assembly pipeline (track → locate → search, one process)
# 三段式装配流水线（跟踪 → 定位 → 找孔，单进程）
'assembly_pipeline = arm_application.pipeline_node:main',
'tactile = arm_application.tactile_node:main',
'tactile_cli = arm_application.tactile_cli:main',
src/arm_bringup/config/bringup.yaml
+3 -3
View File
@@ -1,12 +1,12 @@
# Site defaults for Flexiv Rizon 4 (SN: Rizon4-063989)
# Flexiv Rizon 4 的站点默认值（SN: Rizon4-063989）
robot_sn: Rizon4-063989
rizon_type: Rizon4
# Flexiv Studio gripper (GN01) — keep false. DH PGEA uses run_gripper.sh.
# Flexiv Studio 夹爪（GN01）—— 保持 false。DH PGEA 使用 run_gripper.sh。
load_gripper: false
use_fake_hardware: false
start_rviz: true
# Official Flexiv joint naming: Rizon4-063989_joint1 ... joint7
# Flexiv 官方关节命名：Rizon4-063989_joint1 ... joint7
joint_prefix: Rizon4-063989_
base_frame: base_link
src/arm_bringup/launch/real.launch.py
+9 -9
View File
@@ -7,8 +7,8 @@ from ament_index_python.packages import get_package_share_directory, PackageNotF
import os
# Site robot — DH PGEA is a separate Modbus node (run_gripper.sh).
# Keep Flexiv load_gripper false (that flag is Flexiv-GN01 in Studio).
# 现场机械臂 —— DH PGEA 是独立的 Modbus 节点（run_gripper.sh）。
# 保持 Flexiv 的 load_gripper 为 false（该开关在 Studio 里对应 Flexiv-GN01）。
SITE_ROBOT_SN = os.environ.get('SITE_ROBOT_SN', 'Rizon4-063989')
@@ -21,7 +21,7 @@ def _launch_setup(context, *args, **kwargs):
if not robot_sn:
return [
LogInfo(msg=f'[arm_bringup] ERROR: robot_sn is required, e.g. robot_sn:={SITE_ROBOT_SN}')
LogInfo(msg=f'[arm_bringup] 错误：必须提供 robot_sn，例如 robot_sn:={SITE_ROBOT_SN}')
]
try:
@@ -30,8 +30,8 @@ def _launch_setup(context, *args, **kwargs):
return [
LogInfo(
msg=(
'[arm_bringup] flexiv_bringup not found. '
'Run scripts/fetch_flexiv.sh and build, or use sim.launch.py for mock.'
'[arm_bringup] 找不到 flexiv_bringup。'
'请运行 scripts/fetch_flexiv.sh 并编译，或改用 sim.launch.py 跑 mock。'
)
)
]
@@ -39,7 +39,7 @@ def _launch_setup(context, *args, **kwargs):
return [
LogInfo(
msg=(
f'[arm_bringup] Starting Flexiv {rizon_type} ({robot_sn}), '
f'[arm_bringup] 启动 Flexiv {rizon_type}（{robot_sn}），'
f'load_gripper={load_gripper}'
)
),
@@ -61,16 +61,16 @@ def _launch_setup(context, *args, **kwargs):
def generate_launch_description():
"""Real Rizon4 bringup via official flexiv_bringup (RDK v1 / humble-v1)."""
"""通过官方 flexiv_bringup 启动真机 Rizon4（RDK v1 / humble-v1）。"""
return LaunchDescription([
DeclareLaunchArgument(
'robot_sn',
default_value=SITE_ROBOT_SN,
description='Site robot serial number (no spaces)',
description='现场机械臂序列号（不能有空格）',
),
DeclareLaunchArgument('rizon_type', default_value='Rizon4'),
DeclareLaunchArgument('use_fake_hardware', default_value='false'),
# Flexiv-GN01 only. DH PGEA: bash ~/ros2/scripts/run_gripper.sh
# 仅适用于 Flexiv-GN01。DH PGEA 用: bash ~/ros2/scripts/run_gripper.sh
DeclareLaunchArgument('load_gripper', default_value='false'),
DeclareLaunchArgument('start_rviz', default_value='true'),
OpaqueFunction(function=_launch_setup),
src/arm_bringup/launch/sim.launch.py
+2 -2
View File
@@ -7,7 +7,7 @@ from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
"""Offline mock bringup for Flexiv Rizon 4 (7-DOF), no real robot required."""
"""Flexiv Rizon 4（7 自由度）的离线 mock 启动，无需真机。"""
use_rviz = LaunchConfiguration('use_rviz')
use_moveit = LaunchConfiguration('use_moveit')
robot_sn = LaunchConfiguration('robot_sn')
@@ -39,7 +39,7 @@ def generate_launch_description():
DeclareLaunchArgument(
'robot_sn',
default_value='Rizon4-063989',
description='Site SN; joints become Rizon4-063989_joint1..joint7',
description='现场 SN；关节名会变成 Rizon4-063989_joint1..joint7',
),
mock,
moveit,
src/arm_bringup/launch/stack.launch.py
+17 -17
View File
@@ -1,6 +1,6 @@
"""Unified stack launch: backend:=mock|moveit|real|isaac
"""统一的功能栈启动：backend:=mock|moveit|real|isaac
Application interfaces stay the same across backends:
各后端下应用接口保持不变：
/set_named_pose, /get_arm_state, /arm_status, /follow_joint_trajectory
"""
@@ -82,9 +82,9 @@ def _setup(context, *args, **kwargs):
actions.append(
LogInfo(
msg=(
'[arm_bringup] MoveIt on top of mock. '
'Named poses still via /set_named_pose; '
'MoveIt plans execute through trajectory_bridge.'
'[arm_bringup] 在 mock 之上叠加 MoveIt。'
'命名位姿仍走 /set_named_pose；'
'MoveIt 的规划通过 trajectory_bridge 执行。'
)
)
)
@@ -96,9 +96,9 @@ def _setup(context, *args, **kwargs):
actions.append(
LogInfo(
msg=(
'[arm_bringup] flexiv_bringup missing. '
'Run: ./scripts/fetch_flexiv.sh && rebuild. '
'Falling back tip: use backend:=mock for now.'
'[arm_bringup] 缺少 flexiv_bringup。'
'请运行: ./scripts/fetch_flexiv.sh && 重新编译。'
'临时退回建议：先用 backend:=mock。'
)
)
)
@@ -119,7 +119,7 @@ def _setup(context, *args, **kwargs):
}.items(),
)
)
# Keep application services on top of real HW when possible
# 尽量把应用服务叠在真机之上
actions.append(
Node(
package='arm_control',
@@ -167,9 +167,9 @@ def _setup(context, *args, **kwargs):
actions.append(
LogInfo(
msg=(
'[arm_bringup] REAL mode: flexiv_bringup + named_pose_server '
'+ arm_status_node. '
'Ensure RDK is enabled and network reaches the robot.'
'[arm_bringup] REAL 模式：flexiv_bringup + named_pose_server '
'+ arm_status_node。'
'请确保 RDK 已使能，且网络能连到机械臂。'
)
)
)
@@ -178,17 +178,17 @@ def _setup(context, *args, **kwargs):
actions.append(
LogInfo(
msg=(
'[arm_bringup] ISAAC mode: mock ROS stack is up for bridge testing. '
'Start Isaac Sim + flexiv_sim_plugin separately '
'(see docs/INTERFACES.md). '
'Sync joints via /joint_states or RDK bridge topics.'
'[arm_bringup] ISAAC 模式：已起 mock ROS 栈用于桥接测试。'
'请另行启动 Isaac Sim + flexiv_sim_plugin '
'（见 docs/INTERFACES.md）。'
'通过 /joint_states 或 RDK 桥接话题同步关节。'
)
)
)
if backend not in ('mock', 'moveit', 'real', 'isaac'):
actions.append(
LogInfo(msg=f'[arm_bringup] Unknown backend "{backend}". Use mock|moveit|real|isaac')
LogInfo(msg=f'[arm_bringup] 未知 backend "{backend}"。可用 mock|moveit|real|isaac')
)
return actions
src/arm_control/arm_control/__init__.py
+1 -1
View File
@@ -1 +1 @@
"""Package init for arm_control."""
"""arm_control 的包初始化。"""
src/arm_control/arm_control/arm_status_node.py
+4 -4
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""Publish ArmStatus + GetArmState from joint_states, TF, and Flexiv tcp_pose."""
"""根据 joint_states、TF 与 Flexiv 的 tcp_pose 发布 ArmStatus，并提供 GetArmState 服务。"""
from __future__ import annotations
@@ -43,7 +43,7 @@ class ArmStatusNode(Node):
self.create_timer(1.0 / rate, self._tick)
self.get_logger().info(
f'ArmStatus: base={self.base_frame} ee={self.ee_frame} '
f'tcp_topic={self.tcp_pose_topic or "(none)"}'
f'tcp_topic={self.tcp_pose_topic or "（无）"}'
)
def _on_js(self, msg: JointState) -> None:
@@ -55,7 +55,7 @@ class ArmStatusNode(Node):
self._last_tcp = msg
def _ee(self) -> Optional[PoseStamped]:
# Prefer Flexiv-published TCP pose (reliable on real stack)
# 优先用 Flexiv 发布的 TCP 位姿（真机栈上更可靠）
if self._last_tcp is not None:
return self._last_tcp
try:
@@ -101,7 +101,7 @@ class ArmStatusNode(Node):
and abs(s.ee_pose.pose.position.y) < 1e-9
and abs(s.ee_pose.pose.position.z) < 1e-9
):
response.message = 'ok (ee pose unavailable — check TF/tcp_pose)'
response.message = 'ok（末端位姿不可用 —— 请检查 TF/tcp_pose）'
else:
response.message = 'ok'
return response
src/arm_control/arm_control/named_pose_server.py
+10 -10
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""Service server: SetNamedPose -> FollowJointTrajectory (backend-agnostic)."""
"""服务端：SetNamedPose -> FollowJointTrajectory（与底层后端无关）。"""
from __future__ import annotations
@@ -65,18 +65,18 @@ class NamedPoseServer(Node):
SetNamedPose, 'set_named_pose', self._on_set, callback_group=self._cb
)
self.get_logger().info(
f'Named poses {list(self.poses)} joints={self.joint_names}'
f'命名位姿 {list(self.poses)} joints={self.joint_names}'
)
def _on_set(self, request: SetNamedPose.Request, response: SetNamedPose.Response):
name = request.pose_name
if name not in self.poses:
response.success = False
response.message = f'Unknown pose "{name}". Available: {list(self.poses)}'
response.message = f'未知位姿"{name}"。可用: {list(self.poses)}'
return response
if not self._client.wait_for_server(timeout_sec=3.0):
response.success = False
response.message = 'follow_joint_trajectory action not available'
response.message = 'follow_joint_trajectory action 不可用'
return response
goal = FollowJointTrajectory.Goal()
@@ -87,30 +87,30 @@ class NamedPoseServer(Node):
goal.time_from_start = [duration]
goal.velocity_scale = float(request.velocity_scale)
self.get_logger().info(f'-> pose [{name}] T={duration:.2f}s')
self.get_logger().info(f'-> 位姿 [{name}] T={duration:.2f}s')
send_fut = self._client.send_goal_async(goal)
if not _wait_future(send_fut, 5.0):
response.success = False
response.message = 'Timed out waiting for goal accept'
response.message = '等待目标被接受超时'
return response
handle = send_fut.result()
if handle is None or not handle.accepted:
response.success = False
response.message = 'Goal rejected'
response.message = '目标被拒绝'
return response
res_fut = handle.get_result_async()
if not _wait_future(res_fut, 60.0):
response.success = False
response.message = 'Timed out waiting for trajectory result'
response.message = '等待轨迹结果超时'
return response
wrapped = res_fut.result()
if wrapped is None:
response.success = False
response.message = 'Empty result'
response.message = '结果为空'
return response
result = wrapped.result
response.success = bool(result.success)
response.message = result.message or f'Moved to {name}'
response.message = result.message or f'已移动到位姿 {name}'
return response
src/arm_control/arm_control/real_trajectory_adapter.py
+10 -10
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""Expose arm_msgs/FollowJointTrajectory for apps; drive flexiv JTC (control_msgs)."""
"""对应用侧暴露 arm_msgs/FollowJointTrajectory；实际驱动 Flexiv JTC（control_msgs）。"""
from __future__ import annotations
@@ -45,19 +45,19 @@ class RealTrajectoryAdapter(Node):
cancel_callback=lambda _g: CancelResponse.ACCEPT,
callback_group=self._cb,
)
self.get_logger().info(f'Real adapter {arm} -> {ctrl}')
self.get_logger().info(f'真机适配器 {arm} -> {ctrl}')
def _execute(self, goal_handle):
result = ArmFJT.Result()
req = goal_handle.request
if not req.joint_names or req.num_points < 1:
result.success = False
result.message = 'Invalid goal'
result.message = '目标非法'
goal_handle.abort()
return result
if not self._ctrl.wait_for_server(timeout_sec=5.0):
result.success = False
result.message = f'Controller action not available'
result.message = f'控制器 action 不可用'
goal_handle.abort()
return result
@@ -72,8 +72,8 @@ class RealTrajectoryAdapter(Node):
pt = JointTrajectoryPoint()
pt.positions = pos[idx : idx + n_j]
idx += n_j
# Point-to-point named poses must end at rest — nonzero end vel
# + open_loop JTC makes the arm coast / shake after arrival.
# 点到点的命名位姿必须在静止状态下结束 —— 末速度非零
# 再叠加开环 JTC，会让机械臂到位后滑行/抖动。
pt.velocities = [0.0] * n_j
pt.accelerations = [0.0] * n_j
sec = int(t)
@@ -86,13 +86,13 @@ class RealTrajectoryAdapter(Node):
send = self._ctrl.send_goal_async(ctrl_goal)
if not _wait(send, 5.0):
result.success = False
result.message = 'Timed out sending to controller'
result.message = '发送给控制器超时'
goal_handle.abort()
return result
handle = send.result()
if handle is None or not handle.accepted:
result.success = False
result.message = 'Controller rejected goal'
result.message = '控制器拒绝了目标'
goal_handle.abort()
return result
traj_duration = 0.0
@@ -102,13 +102,13 @@ class RealTrajectoryAdapter(Node):
res_fut = handle.get_result_async()
if not _wait(res_fut, max(120.0, traj_duration * 3.0)):
result.success = False
result.message = 'Timed out waiting for controller'
result.message = '等待控制器超时'
goal_handle.abort()
return result
ctrl_res = res_fut.result().result
ok = ctrl_res.error_code == CtrlFJT.Result.SUCCESSFUL
result.success = ok
result.message = 'OK' if ok else f'controller error_code={ctrl_res.error_code}'
result.message = 'OK' if ok else f'控制器 error_code={ctrl_res.error_code}'
if ok:
goal_handle.succeed()
else:
src/arm_control/arm_control/trajectory_bridge.py
+2 -2
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python3
"""Bridge control_msgs/FollowJointTrajectory -> arm_msgs/FollowJointTrajectory."""
"""把 control_msgs/FollowJointTrajectory 桥接到 arm_msgs/FollowJointTrajectory。"""
from __future__ import annotations
@@ -44,7 +44,7 @@ class TrajectoryBridge(Node):
cancel_callback=lambda _g: CancelResponse.ACCEPT,
callback_group=self._cb,
)
self.get_logger().info(f'Bridge {ctrl} -> {arm}')
self.get_logger().info(f'桥接 {ctrl} -> {arm}')
def _execute(self, goal_handle):
result = CtrlFJT.Result()
src/arm_control/config/controllers.yaml
+4 -4
View File
@@ -5,13 +5,13 @@ controller_manager:
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
# Name aligned with Flexiv official bringup
# 名字与 Flexiv 官方 bringup 对齐
rizon_arm_controller:
type: joint_trajectory_controller/JointTrajectoryController
rizon_arm_controller:
ros__parameters:
# Site robot Rizon4-063989 (official flexiv_description keeps hyphen)
# 站点机器人 Rizon4-063989（官方 flexiv_description 保留连字符）
joints:
- Rizon4-063989_joint1
- Rizon4-063989_joint2
@@ -26,8 +26,8 @@ rizon_arm_controller:
- position
- velocity
allow_partial_joints_goal: false
# Must be false: true interpolates from the last *command* (already at target
# after a "success" with no hardware motion) so the arm never moves.
# 必须为 false：若为 true，会从最后一次 *命令* 开始插值（在硬件没动
# 却报"成功"之后，命令已经落在目标点上），于是机械臂永远不动。
open_loop_control: false
constraints:
stopped_velocity_tolerance: 0.05
src/arm_control/config/named_poses.yaml
+2 -2
View File
@@ -1,5 +1,5 @@
# Named poses — site robot Rizon4-063989 (7-DOF), radians
# Joint base names match flexiv_description: joint1..joint7
# 命名位姿 — 站点机器人 Rizon4-063989（7 自由度），弧度
# 关节基础名与 flexiv_description 一致：joint1..joint7
named_pose_server:
ros__parameters:
joint_prefix: "Rizon4-063989_"
src/arm_control/config/named_poses_91.yaml
+2 -2
View File
@@ -1,5 +1,5 @@
# Right arm 91 (Rizon4-063991) named poses (radians).
# Starting values match the generic home; teach cell-specific poses on 91.
# 右臂 91（Rizon4-063991）的命名位姿（弧度）。
# 起始值与通用 home 一致；在 91 上示教该工位专用的位姿。
named_pose_server:
ros__parameters:
joint_prefix: "Rizon4-063991_"
src/arm_control/launch/mock_control.launch.py
+1 -1
View File
@@ -8,7 +8,7 @@ from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from launch_ros.substitutions import FindPackageShare
# Site robot. Official flexiv_description keeps the hyphen in the SN prefix.
# 现场机械臂。官方 flexiv_description 要求 SN 前缀里**保留连字符**。
SITE_ROBOT_SN = os.environ.get('SITE_ROBOT_SN', 'Rizon4-063989')
SITE_JOINT_PREFIX = os.environ.get('SITE_JOINT_PREFIX', f'{SITE_ROBOT_SN}_')
SITE_NAMED_POSES = os.environ.get('SITE_NAMED_POSES_FILE', '').strip()

Some files were not shown because too many files have changed in this diff Show More