旋转操作加速版本
This commit is contained in:
arm_dev
+1
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/resource/arm_application
|
||||
@@ -1,5 +0,0 @@
|
||||
# Application-level waypoints / task parameters
|
||||
demo:
|
||||
poses: [home, ready, inspect, home]
|
||||
velocity_scale: 0.5
|
||||
dwell_sec: 1.0
|
||||
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/config/demo.yaml
|
||||
@@ -1,25 +0,0 @@
|
||||
# DualArmGripperManager(load_dual_arm_gripper_config(path)) 的可选兜底配置。
|
||||
# 日常使用通过 load_from_site() 读取 ~/ros2/config/site.yaml。
|
||||
left_arm:
|
||||
robot_sn: "Rizon4-063989"
|
||||
gripper_port: "/dev/ttyUSB0"
|
||||
gripper_slave_id: 1
|
||||
gripper_baudrate: 115200
|
||||
right_arm:
|
||||
robot_sn: "Rizon4-063991"
|
||||
gripper_port: "/dev/ttyUSB1"
|
||||
gripper_slave_id: 1
|
||||
gripper_baudrate: 115200
|
||||
gripper:
|
||||
parallel:
|
||||
stroke_min: 0.0
|
||||
stroke_max: 20.0
|
||||
default_speed: 50
|
||||
default_force: 20
|
||||
auto_initialize: true
|
||||
rotary:
|
||||
auto_initialize: false
|
||||
default_speed: 50
|
||||
default_torque: 50
|
||||
finger_open_per_mille: 1000
|
||||
finger_close_per_mille: 0
|
||||
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/config/dual_grippers.yaml
|
||||
@@ -1,47 +0,0 @@
|
||||
# 工件二阶段检测配置：YOLO 定位 + HoughCircles 螺纹孔
|
||||
# 供 hole_detect_hough.py / hole_detect_node（backend=two_stage）使用
|
||||
# 操作入口：bash ~/ros2/scripts/run_hole_detect.sh --arm 89|91 --preview
|
||||
|
||||
# ----- YOLO -----
|
||||
conf: 0.25
|
||||
iou: 0.7
|
||||
imgsz: 640
|
||||
|
||||
# ----- 居中门控：bbox 中心接近画面中心才做二阶圆孔检测 -----
|
||||
# 精对准 k1/k2 时 stage2 会 skip 此门控（对准目标孔后工件必然离开画面中心）
|
||||
center_gate_enable: true
|
||||
center_max_offset_ratio_x: 0.15 # |bbox_cx - 图像中心x| <= 该比例 * 宽
|
||||
center_max_offset_ratio_y: 0.18 # |bbox_cy - 图像中心y| <= 该比例 * 高
|
||||
center_gate_draw: true # 可视化画出允许居中区域
|
||||
refine_hole_centers: true # Hough 后再用暗孔矩细化亚像素圆心
|
||||
|
||||
# ----- ROI：bbox 向外膨胀像素，裁剪时做边界保护 -----
|
||||
bbox_padding: 25
|
||||
|
||||
# ----- ROI 预处理 -----
|
||||
gaussian_ksize: 5 # 必须为奇数
|
||||
gaussian_sigma: 1.5
|
||||
|
||||
# ----- HoughCircles（全部由此读取）-----
|
||||
hough_dp: 1.2
|
||||
hough_min_dist: 50
|
||||
hough_param1: 100
|
||||
hough_param2: 26
|
||||
hough_min_radius: 10
|
||||
hough_max_radius: 30
|
||||
|
||||
# ----- 工业校验 -----
|
||||
min_radius: 10 # 有效圆孔半径下限（滤掉噪点）
|
||||
max_radius: 30 # 有效圆孔半径上限（滤掉中间大孔）
|
||||
hole_distance_min: 150 # 两孔像素距离下限
|
||||
hole_distance_max: 700 # 两孔像素距离上限
|
||||
|
||||
# ----- 时序稳帧（抑制闪烁）-----
|
||||
stabilize_hold_frames: 15 # 连续失败多少帧后才清空结果
|
||||
stabilize_ema_alpha: 0.35 # 中心点/半径 EMA，越小越稳
|
||||
stabilize_max_jump: 60 # 单帧跳变超过该像素则忽略（防闪）
|
||||
|
||||
# ----- 螺纹嵌件对（亮环签名 + 关于中心对称，深度自适应）-----
|
||||
insert_pair_enable: true
|
||||
insert_percentile: 96
|
||||
insert_bolt_radius_mm: 29.0 # 两目标孔物理间距~58mm → 螺栓圆半径29mm；按深度换算像素，尺度不变
|
||||
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/config/hole_detect.yaml
|
||||
@@ -1,37 +0,0 @@
|
||||
# 孔定位检测配置（改 conf / 内参 / 双孔间距即可，不必改代码）
|
||||
# 权重来源：优先 site.yaml（hole_detect.weights / paths.hole_weights）；
|
||||
# 两者都缺时兜底到 models/yolo/gongjian0929.onnx。
|
||||
# 原先这里注释写的是 best.pt —— 该文件不存在，已修正。
|
||||
# backend: yolo | hough | two_stage（two_stage 请用 hole_detect.yaml）
|
||||
|
||||
model_path: ""
|
||||
backend: yolo # yolo | faster_rcnn | hough
|
||||
num_classes: 1 # 单类孔=1；左右孔分两类=2
|
||||
expected_holes: 2
|
||||
min_hole_sep_px: 20
|
||||
max_hole_sep_px: 2000
|
||||
|
||||
conf: 0.25
|
||||
iou: 0.7
|
||||
imgsz: 640
|
||||
|
||||
gaussian_ksize: 5
|
||||
gaussian_sigma: 1.5
|
||||
hough_dp: 1.2
|
||||
hough_min_dist: 40
|
||||
hough_param1: 100
|
||||
hough_param2: 28
|
||||
hough_min_radius: 6
|
||||
hough_max_radius: 45
|
||||
|
||||
stabilize_window: 7
|
||||
stabilize_ema: 0.35
|
||||
stabilize_max_jump_px: 60
|
||||
|
||||
camera_intrinsics:
|
||||
fx: 425.0
|
||||
fy: 425.0
|
||||
cx: 640.0
|
||||
cy: 360.0
|
||||
|
||||
default_depth_m: 0.10
|
||||
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/config/hole_insert.yaml
|
||||
@@ -1,46 +0,0 @@
|
||||
# perception_node 的默认值。launch 会用 ~/ros2/config/site.yaml 的 yolo: / paths: 覆盖。
|
||||
# 日常调参**不要改这里**，改 site.yaml。
|
||||
#
|
||||
# 注意：下面 weights / conf / iou / imgsz / device / half_precision / min_conf 这几项
|
||||
# 一定被 site.yaml 覆盖，这里只是"没有 site.yaml 时的兜底"。所以它们必须和 site.yaml
|
||||
# 保持一致 —— 曾经这里写死 imgsz=416 而实际模型是 640，一旦覆盖失效就会用错尺寸。
|
||||
|
||||
perception_node:
|
||||
ros__parameters:
|
||||
camera_source: "d405"
|
||||
camera_namespace: ""
|
||||
image_topic: ""
|
||||
camera_info_topic: ""
|
||||
depth_topic: "/d405/aligned_depth_to_color/image_raw"
|
||||
weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
|
||||
conf: 0.10
|
||||
iou: 0.45
|
||||
imgsz: 640
|
||||
device: "0"
|
||||
half_precision: true
|
||||
min_conf: 0.12
|
||||
vision_hz: 20.0
|
||||
z_ref_m: 0.30
|
||||
# 大跳变门限 + 重新捕获：快速运动绝不能把跟踪卡死
|
||||
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"
|
||||
# 灵敏的 KF + 向原始观测融合
|
||||
kf_q_pos: 0.002
|
||||
kf_q_vel: 0.05
|
||||
kf_r_pos: 0.0008
|
||||
meas_blend: 0.65
|
||||
# 更快的推理 + 更紧的框跟随
|
||||
infer_max_width: 640
|
||||
box_ema_alpha: 0.85
|
||||
miss_clear_frames: 2
|
||||
# 第 4 项 — 自适应 Z 深度低通滤波
|
||||
z_alpha_min: 0.10
|
||||
z_alpha_max: 0.60
|
||||
z_alpha_gain: 2.0
|
||||
feature_mode: "plane" # stereo 为 3D 质心；box = 单目框中心的射线
|
||||
plane_inlier_m: 0.008
|
||||
plane_band_m: 0.06
|
||||
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/config/perception.yaml
|
||||
@@ -1,69 +0,0 @@
|
||||
# servo_node 的默认值。launch 会用 ~/ros2/config/site.yaml 的 servo: 覆盖。
|
||||
# 日常调参不要改这个文件。
|
||||
|
||||
servo_node:
|
||||
ros__parameters:
|
||||
joint_prefix: "Rizon4-063989_"
|
||||
joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]
|
||||
base_frame: "Rizon4-063989_base_link"
|
||||
ee_frame: "Rizon4-063989_flange"
|
||||
camera_frame: "d405_color_optical_frame"
|
||||
traj_topic: "rizon_arm_controller/joint_trajectory"
|
||||
point_topic: "visual_servo/filtered_point"
|
||||
handeye_file: "src/arm_application/config/handeye_result.yaml"
|
||||
use_static_handeye_tf: false
|
||||
handeye_child_frame: "d405_color_optical_frame"
|
||||
|
||||
control_hz: 100.0
|
||||
z_ref_m: 0.30
|
||||
enable_control: false
|
||||
max_extrapolate_sec: 0.08
|
||||
|
||||
# 第 1 项 — 丢失目标硬停
|
||||
watchdog_timeout_sec: 0.3
|
||||
lost_timeout_sec: 0.3
|
||||
|
||||
kp_xy: 2.00
|
||||
ki_xy: 0.02
|
||||
kd_xy: 0.50
|
||||
kp_z: 1.20
|
||||
ki_z: 0.02
|
||||
kd_z: 0.45
|
||||
i_limit_xy: 0.03
|
||||
i_limit_z: 0.025
|
||||
# 第 2 项 — 积分分离
|
||||
i_sep_pos_m: 0.005
|
||||
i_sep_ang_rad: 0.02
|
||||
|
||||
deadzone_m: 0.0015
|
||||
deadzone_z_m: 0.006
|
||||
v_max_xy: 0.15
|
||||
v_max_z: 0.07
|
||||
a_max: 0.70
|
||||
|
||||
# 第 3 项 — 速度低通滤波
|
||||
vel_lpf_alpha: 0.40
|
||||
wz_lpf_alpha: 0.40
|
||||
|
||||
# 第 5 项 — dq 滑动窗口
|
||||
dq_window_size: 5
|
||||
|
||||
min_ee_z_m: 0.20
|
||||
workspace_xyz_min: [0.08, -0.55, 0.18]
|
||||
workspace_xyz_max: [0.85, 0.55, 0.65]
|
||||
ee_x_m: 0.25
|
||||
ee_y_m: 0.00
|
||||
ee_z_m: 0.25
|
||||
ee_hold_xy: false
|
||||
ee_vertical_down: true
|
||||
ee_hold_kp: 2.0
|
||||
ee_w_max: 0.60
|
||||
damping: 0.08
|
||||
max_qdot: 0.85
|
||||
exec_horizon_sec: 0.20
|
||||
traj_stream_hz: 15.0
|
||||
min_cmd_delta_rad: 0.004
|
||||
camera_fx: 390.0
|
||||
camera_fy: 390.0
|
||||
camera_cx: 320.0
|
||||
camera_cy: 240.0
|
||||
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/config/servo.yaml
|
||||
@@ -1,15 +0,0 @@
|
||||
# Auto-converted from Flexiv Elements .traj (joints: deg -> rad)
|
||||
# ROS params: flat waypoint_positions + waypoint_times (no nested dicts)
|
||||
run_joint_trajectory:
|
||||
ros__parameters:
|
||||
joint_prefix: "Rizon4-063989_"
|
||||
joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]
|
||||
velocity_scale: 0.2
|
||||
waypoint_times: [3.0, 6.0, 9.92, 12.92, 20.54]
|
||||
waypoint_positions: [
|
||||
-0.121649, 0.522726, 0.030020, 1.610764, -0.113097, -0.218864, -0.006458, # WPT0
|
||||
-0.006889, 0.050452, 0.128620, 1.823614, -0.044784, 0.338795, -0.091060, # WPT1
|
||||
-0.009020, -0.311032, 0.106302, 2.548074, -0.066892, 1.318222, -0.091348, # WPT2
|
||||
0.014012, -0.847726, 0.074948, 2.395150, -0.251875, 1.738244, -0.091738, # WPT3
|
||||
0.089637, 0.502090, 0.299444, 1.749088, 0.070761, -0.168363, -0.091845, # WPT4
|
||||
]
|
||||
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/config/xunhuan_trajectory.yaml
|
||||
@@ -1,19 +0,0 @@
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
Node(
|
||||
package='arm_application',
|
||||
executable='demo_named_poses',
|
||||
name='demo_named_poses',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'poses': ['home', 'ready', 'inspect', 'home'],
|
||||
'velocity_scale': 0.5,
|
||||
'dwell_sec': 1.0,
|
||||
'loops': 1,
|
||||
}],
|
||||
),
|
||||
])
|
||||
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/launch/demo.launch.py
|
||||
@@ -1,62 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""启动 DH PGEA Modbus 节点。配置来自 ``~/ros2/config/site.yaml`` 的 ``gripper`` 段。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import LogInfo, OpaqueFunction
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def _ws_root() -> Path:
|
||||
for key in ("ROS2_WS", "SITE_ROS2_ROOT"):
|
||||
v = os.environ.get(key, "").strip()
|
||||
if v:
|
||||
return Path(v)
|
||||
here = Path(__file__).resolve()
|
||||
for p in [here.parents[i] for i in range(2, 8)]:
|
||||
if (p / "config" / "site.yaml").is_file():
|
||||
return p
|
||||
return Path.home() / "ros2"
|
||||
|
||||
|
||||
def _setup(context, *args, **kwargs):
|
||||
root = _ws_root()
|
||||
sys.path.insert(0, str(root / "scripts" / "lib"))
|
||||
from export_site_env import gripper_overrides, load_site, site_path # type: ignore
|
||||
|
||||
cfg = os.environ.get("SITE_CONFIG", "").strip()
|
||||
path = Path(cfg).expanduser() if cfg else site_path()
|
||||
if not path.is_file():
|
||||
path = root / "config" / "site.yaml"
|
||||
data = load_site(path)
|
||||
params = gripper_overrides(data)
|
||||
return [
|
||||
LogInfo(
|
||||
msg=(
|
||||
f"[dh_pgea] {params.get('model')} SN={params.get('serial_number')} "
|
||||
f"{params.get('port')} @{params.get('baud')} site={path}"
|
||||
)
|
||||
),
|
||||
LogInfo(
|
||||
msg=(
|
||||
"[dh_pgea] Flexiv 的 load_gripper 保持 false —— 这里是 DH Modbus，"
|
||||
"不是 Flexiv Studio 夹爪。"
|
||||
)
|
||||
),
|
||||
Node(
|
||||
package="arm_application",
|
||||
executable="dh_pgea",
|
||||
name="dh_pgea",
|
||||
output="screen",
|
||||
parameters=[params],
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
return LaunchDescription([OpaqueFunction(function=_setup)])
|
||||
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/launch/dh_pgea.launch.py
|
||||
@@ -1,40 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""启动双路 DH 夹爪。端口来自 ``site.yaml`` 的 ``robots[].gripper``。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
from pathlib import Path
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, LogInfo
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
sides = LaunchConfiguration("sides")
|
||||
allow_offline = LaunchConfiguration("allow_offline")
|
||||
site = os.environ.get("SITE_CONFIG") or str(Path.home() / "ros2" / "config" / "site.yaml")
|
||||
return LaunchDescription(
|
||||
[
|
||||
DeclareLaunchArgument("sides", default_value="left,right"),
|
||||
DeclareLaunchArgument("allow_offline", default_value="true"),
|
||||
LogInfo(msg=f"[dual_gripper] site={site}（arm_msgs，IPC Modbus，不是 Flexiv 夹爪）"),
|
||||
Node(
|
||||
package="arm_application",
|
||||
executable="dual_gripper",
|
||||
name="dual_gripper_driver",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{
|
||||
"config_file": "",
|
||||
"auto_connect": True,
|
||||
"allow_offline": allow_offline,
|
||||
"publish_hz": 10.0,
|
||||
"sides": sides,
|
||||
}
|
||||
],
|
||||
),
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/launch/dual_gripper.launch.py
|
||||
@@ -1,270 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""启动通止规装配主流程（看板 S00–S11）。
|
||||
|
||||
与 stage2/stage3 的 launch 差别：**这里不检查任何服务**。
|
||||
"所有服务是否就绪"是 S00 的职责（``gauge_boot`` + 节点里的探测），
|
||||
在 launch/shell 里再查一遍就会出现两处判据 —— 一处改了另一处没改，
|
||||
现场表现为"脚本说没问题、节点说没数据"。
|
||||
|
||||
唯一在这里做的是**发布手眼静态 TF**：腕部相机的 eye-in-hand 外参是主流程
|
||||
（S05 精拍定位）算坐标的前提，而它只能由标定产物提供（site.yaml →
|
||||
paths.handeye_file）。文件不存在时不发 TF，节点会在 S05 报"查不到 TF"。
|
||||
|
||||
用法（一般由 scripts/run_gauge_assembly.sh 调用）::
|
||||
|
||||
ros2 launch arm_application gauge_assembly.launch.py
|
||||
ros2 launch arm_application gauge_assembly.launch.py dry_run:=true holes_total:=1
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
LogInfo,
|
||||
OpaqueFunction,
|
||||
RegisterEventHandler,
|
||||
Shutdown,
|
||||
)
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def _ws_root() -> Path:
|
||||
"""工作区根目录：优先环境变量，其次向上找 config/site.yaml。"""
|
||||
for key in ("ROS2_WS", "SITE_ROS2_ROOT"):
|
||||
v = os.environ.get(key, "").strip()
|
||||
if v:
|
||||
return Path(v)
|
||||
here = Path(__file__).resolve()
|
||||
for i in range(2, 8):
|
||||
p = here.parents[i]
|
||||
if (p / "config" / "site.yaml").is_file():
|
||||
return p
|
||||
return Path.home() / "ros2"
|
||||
|
||||
|
||||
def _handeye_xyz_xyzw(path: str):
|
||||
"""读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。
|
||||
|
||||
**不要在这里再写一遍四元数换算**：这段换算在别的 launch 里已各抄了一遍，
|
||||
抄错过的症状是"手眼 TF 差了 30°"（定位总能到附近，永远差一点）。
|
||||
实现只有一份在 ``arm_application.handeye_lib``，且被回环测试钉住。
|
||||
"""
|
||||
from arm_application.handeye_lib import read_transform_yaml
|
||||
|
||||
xyz, quat = read_transform_yaml(path)
|
||||
return (
|
||||
float(xyz[0]), float(xyz[1]), float(xyz[2]),
|
||||
float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]),
|
||||
)
|
||||
|
||||
|
||||
def _launch_setup(context, *args, **kwargs):
|
||||
root = _ws_root()
|
||||
sys.path.insert(0, str(root / "scripts" / "lib"))
|
||||
from export_site_env import apply_active_arm, load_site, site_path # type: ignore
|
||||
|
||||
cfg = os.environ.get("SITE_CONFIG", "").strip()
|
||||
path = Path(cfg).expanduser() if cfg else site_path()
|
||||
if not path.is_file():
|
||||
path = root / "config" / "site.yaml"
|
||||
data = load_site(path)
|
||||
merged, robot = apply_active_arm(data)
|
||||
|
||||
ee_frame = str(robot.get("ee_frame") or f"{robot.get('sn')}_flange")
|
||||
base_frame = str(
|
||||
robot.get("base_frame")
|
||||
or os.environ.get("SITE_BASE_FRAME")
|
||||
or f"{robot.get('sn')}_base_link"
|
||||
)
|
||||
handeye = str((merged.get("paths") or {}).get("handeye_file") or "").strip()
|
||||
if not handeye:
|
||||
handeye = str((robot.get("paths") or {}).get("handeye_file") or "").strip()
|
||||
cam_cfg = merged.get("camera") or {}
|
||||
cam = str(cam_cfg.get("name") or "d405").strip().strip("/")
|
||||
cam_frame = f"{cam}_color_optical_frame"
|
||||
|
||||
# ---- 手外相机（eye-to-hand）：S03 粗定位用的外参 ----
|
||||
# 名字按 site.yaml 的 cameras 注册表（role=overhead*）由**唯一实现**解析
|
||||
# （site_io.overhead_camera，节点自己取命名空间也走它）。
|
||||
sys.path.insert(0, str(root / "src" / "arm_application"))
|
||||
from arm_application.site_io import ( # type: ignore
|
||||
eye_to_hand_result_name,
|
||||
handeye_static_tf_frames,
|
||||
overhead_camera as _overhead_camera,
|
||||
)
|
||||
|
||||
from export_site_env import arm_short_id # type: ignore
|
||||
|
||||
over_cfg, _ = _overhead_camera(merged, robot)
|
||||
over_cam = str(over_cfg.get("name") or "").strip("/")
|
||||
over_frame = f"{over_cam}_color_optical_frame" if over_cam else ""
|
||||
# 眼在手外的产物名走 site_io.eye_to_hand_result_name（唯一口径），
|
||||
# 不在这里自己拼（拼错/漏拼会让 S03 找不到刚标完的文件）。
|
||||
short = arm_short_id(str(robot.get("sn") or ""))
|
||||
e2h_file = (
|
||||
root / "handeye" / "results" / eye_to_hand_result_name(short, over_cam)
|
||||
if over_cam
|
||||
else None
|
||||
)
|
||||
|
||||
def _flag(name: str) -> bool:
|
||||
return LaunchConfiguration(name).perform(context).strip().lower() in (
|
||||
"1", "true", "yes", "on",
|
||||
)
|
||||
|
||||
dry_run = _flag("dry_run")
|
||||
holes = LaunchConfiguration("holes_total").perform(context).strip()
|
||||
# 仿真里必须 true：本节点所有 header.stamp 比对和 TF 查询都按这个时钟走。
|
||||
# 忘了开的话症状是"相机没数据/TF 查不到"，看不出真正原因（脚本 --sim 会带）。
|
||||
use_sim_time = _flag("use_sim_time")
|
||||
backend = LaunchConfiguration("gripper_backend").perform(context).strip() or "auto"
|
||||
result_file = LaunchConfiguration("result_file").perform(context).strip()
|
||||
|
||||
# 是否由本 launch 发手眼静态 TF。真机必须发（相机系与法兰之间除了标定值
|
||||
# 没有别的来源）；仿真**不能**发 —— 仿真里相机在 URDF 里就挂在法兰上，
|
||||
# 再发一条同父子关系的静态 TF 会出现两个父节点（TF_MULTIPLE_AUTHORITIES），
|
||||
# 查出来的位姿在两条之间跳。脚本 --sim 会传 false。
|
||||
publish_handeye_tf = _flag("publish_handeye_tf")
|
||||
nodes = []
|
||||
frames = []
|
||||
if publish_handeye_tf and os.path.isfile(handeye):
|
||||
try:
|
||||
tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(handeye)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
frames.append(f"[gauge] 手眼文件读不了（{handeye}）：{exc}")
|
||||
else:
|
||||
frames.append(f"[gauge] 手眼 TF {ee_frame} ← {cam_frame}（{Path(handeye).name}）")
|
||||
nodes.append(
|
||||
Node(
|
||||
package="tf2_ros",
|
||||
executable="static_transform_publisher",
|
||||
name="gauge_handeye_static_tf",
|
||||
arguments=[
|
||||
"--x", str(tx), "--y", str(ty), "--z", str(tz),
|
||||
"--qx", str(qx), "--qy", str(qy), "--qz", str(qz), "--qw", str(qw),
|
||||
"--frame-id", ee_frame,
|
||||
"--child-frame-id", cam_frame,
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
)
|
||||
elif not publish_handeye_tf:
|
||||
frames.append(
|
||||
f"[gauge] 按参数不发手眼静态 TF（publish_handeye_tf=false）："
|
||||
f"相机系 {cam_frame} 由 URDF/驱动给出（仿真就是这条路径）"
|
||||
)
|
||||
else:
|
||||
frames.append(
|
||||
f"[gauge] 没有手眼文件（{handeye or 'paths.handeye_file 为空'}）"
|
||||
"—— 不发布静态 TF，S05 会在查 TF 时报错。先跑 scripts/run_handeye.sh"
|
||||
)
|
||||
|
||||
# ---- 手外相机外参：**基座 ← 相机**（眼在手外）----
|
||||
# 与上面那条的父子关系**不同**，两者不能混：
|
||||
# 腕部（eye-in-hand）：法兰 ← 相机（相机跟着臂动）
|
||||
# 手外（eye-to-hand）：基座 ← 相机（相机固定，不随臂动）
|
||||
# S03 反投影查的正是 `{prefix}base_link ← {相机}_color_optical_frame`
|
||||
#（gauge_assembly_node._coarse_workpiece_xy）。把 eye_to_hand 的文件按
|
||||
# "法兰 ← 相机"发出去会得到一条**看着合理**的错 TF —— 于是定位总能到附近、
|
||||
# 永远差一截，而人只会去怀疑标定精度。
|
||||
if over_cam and e2h_file and publish_handeye_tf:
|
||||
if e2h_file.is_file():
|
||||
try:
|
||||
tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(str(e2h_file))
|
||||
except Exception as exc: # noqa: BLE001
|
||||
frames.append(f"[gauge] 手外相机文件读不了（{e2h_file}）：{exc}")
|
||||
else:
|
||||
parent, child = handeye_static_tf_frames(
|
||||
mode="eye_to_hand",
|
||||
ee_frame=ee_frame,
|
||||
base_frame=base_frame,
|
||||
cam_frame=over_frame,
|
||||
)
|
||||
frames.append(
|
||||
f"[gauge] 手外 TF {parent} ← {child}（{e2h_file.name}）"
|
||||
)
|
||||
nodes.append(
|
||||
Node(
|
||||
package="tf2_ros",
|
||||
executable="static_transform_publisher",
|
||||
name="gauge_coarse_handeye_static_tf",
|
||||
arguments=[
|
||||
"--x", str(tx), "--y", str(ty), "--z", str(tz),
|
||||
"--qx", str(qx), "--qy", str(qy), "--qz", str(qz),
|
||||
"--qw", str(qw),
|
||||
"--frame-id", parent,
|
||||
"--child-frame-id", child,
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
)
|
||||
else:
|
||||
frames.append(
|
||||
f"[gauge] 手外相机 {over_cam} 没有 eye-to-hand 标定（{e2h_file.name}）"
|
||||
"—— 不发它的静态 TF，S03 会明确报缺文件并回 S04 跟踪（不假装成功）"
|
||||
)
|
||||
|
||||
gauge_node = Node(
|
||||
package="arm_application",
|
||||
executable="gauge_assembly",
|
||||
name="gauge_assembly",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{
|
||||
"robot_sn": str(
|
||||
robot.get("sn") or os.environ.get("SITE_ROBOT_SN") or ""
|
||||
),
|
||||
"dry_run": dry_run,
|
||||
"holes_total": int(holes) if holes else -1,
|
||||
"gripper_backend": backend,
|
||||
"result_file": result_file,
|
||||
"use_sim_time": use_sim_time,
|
||||
# S03 找手外外参用的确切路径（名字口径由 export_site_env 一处实现）。
|
||||
"coarse_eye_to_hand_file": str(e2h_file) if e2h_file else "",
|
||||
}
|
||||
],
|
||||
)
|
||||
nodes.append(gauge_node)
|
||||
|
||||
return [
|
||||
LogInfo(msg=f"[gauge] site={path}"),
|
||||
LogInfo(msg=f"[gauge] arm={robot.get('label') or robot.get('sn')} cam={cam}"),
|
||||
LogInfo(
|
||||
msg=f"[gauge] dry_run={dry_run} holes={holes or 'site'} "
|
||||
f"backend={backend} use_sim_time={use_sim_time}"
|
||||
),
|
||||
LogInfo(msg=f"[gauge] 结论文件 {result_file}"),
|
||||
*[LogInfo(msg=m) for m in frames],
|
||||
*nodes,
|
||||
# 流程节点退出 → 整个 launch 退出。
|
||||
# 为什么必须显式写：静态 TF 发布者是**常驻**的（它不发完就退，
|
||||
# 要一直挂着供 tf2 查询），所以主流程跑完 DONE 之后 launch 不会自己结束 ——
|
||||
# 表现为"流程明明打完了，终端一直不回到提示符"（实测会一直挂到 timeout 杀掉）。
|
||||
RegisterEventHandler(
|
||||
OnProcessExit(target_action=gauge_node, on_exit=Shutdown())
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
return LaunchDescription(
|
||||
[
|
||||
DeclareLaunchArgument("dry_run", default_value="false"),
|
||||
DeclareLaunchArgument("holes_total", default_value=""),
|
||||
DeclareLaunchArgument("gripper_backend", default_value="auto"),
|
||||
# 结论落盘路径（脚本读它判成败：ros2 launch 的退出码恒为 0）
|
||||
DeclareLaunchArgument("result_file", default_value="/tmp/gauge_assembly_result.json"),
|
||||
# 仿真必须 true（真机 false）。理由见 _launch_setup 里的注释。
|
||||
DeclareLaunchArgument("use_sim_time", default_value="false"),
|
||||
# 真机 true（相机系只能由标定值给出）；仿真 false（URDF 已给出）。
|
||||
DeclareLaunchArgument("publish_handeye_tf", default_value="true"),
|
||||
OpaqueFunction(function=_launch_setup),
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/launch/gauge_assembly.launch.py
|
||||
@@ -1,70 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""在当前机械臂的彩色话题上启动 hole_detect。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
from pathlib import Path
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def _ws_root() -> Path:
|
||||
for key in ("ROS2_WS", "SITE_ROS2_ROOT"):
|
||||
v = os.environ.get(key, "").strip()
|
||||
if v:
|
||||
return Path(v)
|
||||
return Path.home() / "ros2"
|
||||
|
||||
|
||||
def _setup(context, *args, **kwargs):
|
||||
root = _ws_root()
|
||||
backend = LaunchConfiguration("backend").perform(context) or os.environ.get("SITE_HOLE_BACKEND") or "two_stage"
|
||||
weights = LaunchConfiguration("weights").perform(context) or os.environ.get("SITE_HOLE_WEIGHTS") or ""
|
||||
cfg = LaunchConfiguration("config_file").perform(context) or os.environ.get("SITE_HOLE_CONFIG") or ""
|
||||
cam = os.environ.get("SITE_CAM_NAME") or "d405"
|
||||
preview = LaunchConfiguration("preview").perform(context).lower() in ("1", "true", "yes")
|
||||
if not cfg:
|
||||
if backend in ("two_stage", "hough_roi"):
|
||||
cfg = str(root / "src" / "arm_application" / "config" / "hole_detect.yaml")
|
||||
else:
|
||||
cfg = str(root / "src" / "arm_application" / "config" / "hole_insert.yaml")
|
||||
if not weights:
|
||||
if backend in ("two_stage", "hough_roi"):
|
||||
weights = os.environ.get("SITE_HOLE_WORKPIECE_WEIGHTS") or os.environ.get("SITE_YOLO_WEIGHTS") or ""
|
||||
else:
|
||||
weights = os.environ.get("SITE_HOLE_WEIGHTS") or str(root / "models" / "yolo" / "gongjian0929.onnx")
|
||||
return [
|
||||
LogInfo(msg=f"[hole_detect] cam={cam} backend={backend} weights={weights} cfg={cfg}"),
|
||||
Node(
|
||||
package="arm_application",
|
||||
executable="hole_detect",
|
||||
name="hole_detect",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{
|
||||
"camera_source": cam,
|
||||
"weights": weights,
|
||||
"config_file": cfg,
|
||||
"backend": backend,
|
||||
"device": os.environ.get("SITE_YOLO_DEVICE") or "0",
|
||||
"preview": preview,
|
||||
}
|
||||
],
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
return LaunchDescription(
|
||||
[
|
||||
DeclareLaunchArgument("backend", default_value=""),
|
||||
DeclareLaunchArgument("weights", default_value=""),
|
||||
DeclareLaunchArgument("config_file", default_value=""),
|
||||
DeclareLaunchArgument("preview", default_value="false"),
|
||||
OpaqueFunction(function=_setup),
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/launch/hole_detect.launch.py
|
||||
@@ -1,176 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Launch the three-stage assembly pipeline + static hand-eye TF.
|
||||
|
||||
相机 / 触觉由 scripts/run_all.sh 负责拉起一次。本 launch 只起手眼 TF 和串联节点。
|
||||
一阶段跟踪由 pipeline 节点在进程内再拉 visual_servo.launch（不重复发手眼 TF）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
|
||||
from launch.actions import EmitEvent, RegisterEventHandler
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.events import Shutdown
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def _ws_root() -> Path:
|
||||
for key in ("ROS2_WS", "SITE_ROS2_ROOT"):
|
||||
v = os.environ.get(key, "").strip()
|
||||
if v:
|
||||
return Path(v)
|
||||
here = Path(__file__).resolve()
|
||||
for p in [here.parents[i] for i in range(2, 8)]:
|
||||
if (p / "config" / "site.yaml").is_file():
|
||||
return p
|
||||
return Path.home() / "ros2"
|
||||
|
||||
|
||||
def _handeye_xyz_xyzw(path: str):
|
||||
"""读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。
|
||||
|
||||
实现只有一份，在 ``arm_application.handeye_lib``（``read_transform_yaml``），
|
||||
并由回环测试钉住：``quat → R`` 必须能还原文件里的 ``T_4x4``。
|
||||
这段换算曾经在 4 个 launch 文件里各抄了一遍，且抄错了 3 个分支 ——
|
||||
手眼姿态差几十度，定位"总能到附近、永远差一点"，看起来像相机标定不准。
|
||||
"""
|
||||
from arm_application.handeye_lib import read_transform_yaml
|
||||
|
||||
xyz, quat = read_transform_yaml(path)
|
||||
return (
|
||||
float(xyz[0]), float(xyz[1]), float(xyz[2]),
|
||||
float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]),
|
||||
)
|
||||
|
||||
|
||||
def _as_bool(s: str) -> bool:
|
||||
return s.strip().lower() in ("1", "true", "yes")
|
||||
|
||||
|
||||
def _launch_setup(context, *args, **kwargs):
|
||||
root = _ws_root()
|
||||
sys.path.insert(0, str(root / "scripts" / "lib"))
|
||||
from export_site_env import apply_active_arm, load_site, site_path # type: ignore
|
||||
|
||||
cfg = os.environ.get("SITE_CONFIG", "").strip()
|
||||
path = Path(cfg).expanduser() if cfg else site_path()
|
||||
if not path.is_file():
|
||||
path = root / "config" / "site.yaml"
|
||||
data = load_site(path)
|
||||
merged, robot = apply_active_arm(data)
|
||||
|
||||
ee_frame = str(robot.get("ee_frame") or f"{robot.get('sn')}_flange")
|
||||
paths = merged.get("paths") or {}
|
||||
handeye = str(paths.get("handeye_file") or "").strip()
|
||||
cam_cfg = merged.get("camera") or {}
|
||||
cam = str(cam_cfg.get("name") or "d405").strip().strip("/")
|
||||
cam_frame = f"{cam}_color_optical_frame"
|
||||
|
||||
skip1 = _as_bool(LaunchConfiguration("skip_stage1").perform(context))
|
||||
skip2 = _as_bool(LaunchConfiguration("skip_stage2").perform(context))
|
||||
skip3 = _as_bool(LaunchConfiguration("skip_stage3").perform(context))
|
||||
preview = _as_bool(LaunchConfiguration("preview").perform(context))
|
||||
resnap = _as_bool(LaunchConfiguration("resnap").perform(context))
|
||||
# 调试开关：只验证精对准、不轻降。用途是"对准逻辑刚改完、还没确认它真能把规
|
||||
# 对到孔心"时先别让规往下走 —— 对不准就下压会把规顶在孔沿上（2026-10-08 真机
|
||||
# 顶到 63N）。默认关；验证对准时用 --no-descent 打开。
|
||||
no_descent = _as_bool(LaunchConfiguration("no_descent").perform(context))
|
||||
d435_only = _as_bool(LaunchConfiguration("d435_move_only").perform(context))
|
||||
init_z = LaunchConfiguration("init_z_m").perform(context).strip()
|
||||
hover_z = LaunchConfiguration("hover_z_m").perform(context).strip()
|
||||
final_gap = LaunchConfiguration("final_gap_mm").perform(context).strip()
|
||||
target_hole = LaunchConfiguration("target_hole").perform(context).strip()
|
||||
depth = LaunchConfiguration("insert_depth_mm").perform(context).strip()
|
||||
pitch = LaunchConfiguration("screw_pitch_mm").perform(context).strip()
|
||||
|
||||
nodes = []
|
||||
if handeye and Path(handeye).is_file():
|
||||
tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(handeye)
|
||||
nodes.append(
|
||||
Node(
|
||||
package="tf2_ros",
|
||||
executable="static_transform_publisher",
|
||||
name="handeye_static_tf",
|
||||
arguments=[
|
||||
"--x", str(tx), "--y", str(ty), "--z", str(tz),
|
||||
"--qx", str(qx), "--qy", str(qy), "--qz", str(qz), "--qw", str(qw),
|
||||
"--frame-id", ee_frame,
|
||||
"--child-frame-id", cam_frame,
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
)
|
||||
|
||||
node = Node(
|
||||
package="arm_application",
|
||||
executable="assembly_pipeline",
|
||||
name="assembly_pipeline",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{
|
||||
"robot_sn": str(robot.get("sn") or os.environ.get("SITE_ROBOT_SN") or ""),
|
||||
"preview": preview,
|
||||
"resnap": resnap,
|
||||
"target_hole": target_hole,
|
||||
"insert_depth_mm": float(depth) if depth else -1.0,
|
||||
"screw_pitch_mm": float(pitch) if pitch else -999.0,
|
||||
"skip_stage1": skip1,
|
||||
"skip_stage2": skip2 or d435_only,
|
||||
"skip_stage3": skip3 or d435_only,
|
||||
"d435_move_only": d435_only,
|
||||
"init_z_m": float(init_z) if init_z else -1.0,
|
||||
"hover_z_m": float(hover_z) if hover_z else -1.0,
|
||||
"final_gap_mm": float(final_gap) if final_gap else -1.0,
|
||||
# -1=按 site 的 stage2.final_descent_enable；0=强制不降（调试）
|
||||
"final_descent_enable": 0 if no_descent else -1,
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
return [
|
||||
LogInfo(msg=f"[pipeline] site={path}"),
|
||||
LogInfo(msg=f"[pipeline] arm={robot.get('label') or robot.get('sn')} cam={cam}"),
|
||||
LogInfo(
|
||||
msg=f"[pipeline] hole={target_hole or 'site'} depth={depth or 'site'} "
|
||||
f"pitch={pitch or 'site'} final_gap={final_gap or 'site'} "
|
||||
f"skip1={skip1} skip2={skip2} skip3={skip3}"
|
||||
+ (" [d435_only：只测 D435 位移]" if d435_only else "")
|
||||
+ (" [no_descent：只对准，不轻降]" if no_descent else "")
|
||||
),
|
||||
*nodes,
|
||||
node,
|
||||
RegisterEventHandler(
|
||||
OnProcessExit(
|
||||
target_action=node,
|
||||
on_exit=[EmitEvent(event=Shutdown(reason="pipeline 完成"))],
|
||||
)
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
return LaunchDescription(
|
||||
[
|
||||
DeclareLaunchArgument("skip_stage1", default_value="false"),
|
||||
DeclareLaunchArgument("skip_stage2", default_value="false"),
|
||||
DeclareLaunchArgument("skip_stage3", default_value="false"),
|
||||
DeclareLaunchArgument("target_hole", default_value=""),
|
||||
DeclareLaunchArgument("insert_depth_mm", default_value=""),
|
||||
DeclareLaunchArgument("screw_pitch_mm", default_value=""),
|
||||
DeclareLaunchArgument("resnap", default_value="false"),
|
||||
# 只验证精对准、不轻降（见 _launch_setup 里的说明）
|
||||
DeclareLaunchArgument("no_descent", default_value="false"),
|
||||
DeclareLaunchArgument("d435_move_only", default_value="false"),
|
||||
DeclareLaunchArgument("init_z_m", default_value=""),
|
||||
DeclareLaunchArgument("hover_z_m", default_value=""),
|
||||
DeclareLaunchArgument("final_gap_mm", default_value=""),
|
||||
DeclareLaunchArgument("preview", default_value="true"),
|
||||
OpaqueFunction(function=_launch_setup),
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/launch/pipeline.launch.py
|
||||
@@ -1,132 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Launch stage-2 one-shot hole localization (k1/k2) + static hand-eye TF.
|
||||
|
||||
所有参数来自 ~/ros2/config/site.yaml（launch 参数仅做少量覆盖）。
|
||||
相机与触觉服务由 scripts/run_stage2_locate.sh 负责拉起/检查。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def _ws_root() -> Path:
|
||||
for key in ("ROS2_WS", "SITE_ROS2_ROOT"):
|
||||
v = os.environ.get(key, "").strip()
|
||||
if v:
|
||||
return Path(v)
|
||||
here = Path(__file__).resolve()
|
||||
for p in [here.parents[i] for i in range(2, 8)]:
|
||||
if (p / "config" / "site.yaml").is_file():
|
||||
return p
|
||||
return Path.home() / "ros2"
|
||||
|
||||
|
||||
def _handeye_xyz_xyzw(path: str):
|
||||
"""读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。
|
||||
|
||||
实现只有一份，在 ``arm_application.handeye_lib``（``read_transform_yaml``），
|
||||
并由回环测试钉住：``quat → R`` 必须能还原文件里的 ``T_4x4``。
|
||||
这段换算曾经在 4 个 launch 文件里各抄了一遍，且抄错了 3 个分支 ——
|
||||
手眼姿态差几十度，定位"总能到附近、永远差一点"，看起来像相机标定不准。
|
||||
"""
|
||||
from arm_application.handeye_lib import read_transform_yaml
|
||||
|
||||
xyz, quat = read_transform_yaml(path)
|
||||
return (
|
||||
float(xyz[0]), float(xyz[1]), float(xyz[2]),
|
||||
float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]),
|
||||
)
|
||||
|
||||
|
||||
def _launch_setup(context, *args, **kwargs):
|
||||
root = _ws_root()
|
||||
sys.path.insert(0, str(root / "scripts" / "lib"))
|
||||
from export_site_env import apply_active_arm, load_site, site_path # type: ignore
|
||||
|
||||
cfg = os.environ.get("SITE_CONFIG", "").strip()
|
||||
path = Path(cfg).expanduser() if cfg else site_path()
|
||||
if not path.is_file():
|
||||
path = root / "config" / "site.yaml"
|
||||
data = load_site(path)
|
||||
merged, robot = apply_active_arm(data)
|
||||
|
||||
ee_frame = str(robot.get("ee_frame") or f"{robot.get('sn')}_flange")
|
||||
paths = merged.get("paths") or {}
|
||||
handeye = str(paths.get("handeye_file") or "").strip()
|
||||
if not handeye or not Path(handeye).is_file():
|
||||
raise FileNotFoundError(
|
||||
f"hand-eye result missing: {handeye}. "
|
||||
"先标定: bash ~/ros2/scripts/run_handeye.sh calibrate"
|
||||
)
|
||||
cam_cfg = merged.get("camera") or {}
|
||||
cam = str(cam_cfg.get("name") or "d405").strip().strip("/")
|
||||
cam_frame = f"{cam}_color_optical_frame"
|
||||
|
||||
hover_only = LaunchConfiguration("hover_only").perform(context).lower() in ("1", "true", "yes")
|
||||
preview = LaunchConfiguration("preview").perform(context).lower() in ("1", "true", "yes")
|
||||
target_hole = LaunchConfiguration("target_hole").perform(context).strip()
|
||||
hover_z = LaunchConfiguration("hover_z_m").perform(context).strip()
|
||||
final_gap = LaunchConfiguration("final_gap_mm").perform(context).strip()
|
||||
|
||||
tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(handeye)
|
||||
static_tf = Node(
|
||||
package="tf2_ros",
|
||||
executable="static_transform_publisher",
|
||||
name="handeye_static_tf",
|
||||
arguments=[
|
||||
"--x", str(tx), "--y", str(ty), "--z", str(tz),
|
||||
"--qx", str(qx), "--qy", str(qy), "--qz", str(qz), "--qw", str(qw),
|
||||
"--frame-id", ee_frame,
|
||||
"--child-frame-id", cam_frame,
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
node = Node(
|
||||
package="arm_application",
|
||||
executable="stage2_locate",
|
||||
name="stage2_locate",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{
|
||||
"robot_sn": str(robot.get("sn") or os.environ.get("SITE_ROBOT_SN") or ""),
|
||||
"hover_only": hover_only,
|
||||
"target_hole": target_hole,
|
||||
"hover_z_m": float(hover_z) if hover_z else -1.0,
|
||||
"final_gap_mm": float(final_gap) if final_gap else -1.0,
|
||||
"preview": preview,
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
return [
|
||||
LogInfo(msg=f"[stage2] site={path}"),
|
||||
LogInfo(
|
||||
msg=f"[stage2] arm={robot.get('label') or robot.get('sn')} cam={cam} handeye={handeye}"
|
||||
),
|
||||
LogInfo(msg=f"[stage2] hover_only={hover_only} target={target_hole or 'site'}"),
|
||||
static_tf,
|
||||
node,
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
return LaunchDescription(
|
||||
[
|
||||
DeclareLaunchArgument("hover_only", default_value="false"),
|
||||
DeclareLaunchArgument("target_hole", default_value=""),
|
||||
DeclareLaunchArgument("hover_z_m", default_value=""),
|
||||
DeclareLaunchArgument("final_gap_mm", default_value=""),
|
||||
DeclareLaunchArgument("preview", default_value="true"),
|
||||
OpaqueFunction(function=_launch_setup),
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/launch/stage2_locate.launch.py
|
||||
@@ -1,135 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Launch stage-3 compliant hole search (Orisys tactile + optional D405 resnap).
|
||||
|
||||
所有阈值来自 ~/ros2/config/site.yaml → hole_detect.stage3（launch 参数仅做少量覆盖）。
|
||||
触觉服务由 scripts/run_stage3_search.sh 负责拉起/检查。
|
||||
前置：二阶段已完成（臂悬停在目标孔上方、垂直、夹持通止规）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def _ws_root() -> Path:
|
||||
for key in ("ROS2_WS", "SITE_ROS2_ROOT"):
|
||||
v = os.environ.get(key, "").strip()
|
||||
if v:
|
||||
return Path(v)
|
||||
here = Path(__file__).resolve()
|
||||
for p in [here.parents[i] for i in range(2, 8)]:
|
||||
if (p / "config" / "site.yaml").is_file():
|
||||
return p
|
||||
return Path.home() / "ros2"
|
||||
|
||||
|
||||
def _handeye_xyz_xyzw(path: str):
|
||||
"""读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。
|
||||
|
||||
实现只有一份，在 ``arm_application.handeye_lib``（``read_transform_yaml``），
|
||||
并由回环测试钉住：``quat → R`` 必须能还原文件里的 ``T_4x4``。
|
||||
这段换算曾经在 4 个 launch 文件里各抄了一遍，且抄错了 3 个分支 ——
|
||||
手眼姿态差几十度，定位"总能到附近、永远差一点"，看起来像相机标定不准。
|
||||
"""
|
||||
from arm_application.handeye_lib import read_transform_yaml
|
||||
|
||||
xyz, quat = read_transform_yaml(path)
|
||||
return (
|
||||
float(xyz[0]), float(xyz[1]), float(xyz[2]),
|
||||
float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]),
|
||||
)
|
||||
|
||||
|
||||
def _launch_setup(context, *args, **kwargs):
|
||||
root = _ws_root()
|
||||
sys.path.insert(0, str(root / "scripts" / "lib"))
|
||||
from export_site_env import apply_active_arm, load_site, site_path # type: ignore
|
||||
|
||||
cfg = os.environ.get("SITE_CONFIG", "").strip()
|
||||
path = Path(cfg).expanduser() if cfg else site_path()
|
||||
if not path.is_file():
|
||||
path = root / "config" / "site.yaml"
|
||||
data = load_site(path)
|
||||
merged, robot = apply_active_arm(data)
|
||||
|
||||
ee_frame = str(robot.get("ee_frame") or f"{robot.get('sn')}_flange")
|
||||
paths = merged.get("paths") or {}
|
||||
handeye = str(paths.get("handeye_file") or "").strip()
|
||||
cam_cfg = merged.get("camera") or {}
|
||||
cam = str(cam_cfg.get("name") or "d405").strip().strip("/")
|
||||
cam_frame = f"{cam}_color_optical_frame"
|
||||
|
||||
preview = LaunchConfiguration("preview").perform(context).lower() in ("1", "true", "yes")
|
||||
resnap = LaunchConfiguration("resnap").perform(context).lower() in ("1", "true", "yes")
|
||||
target_hole = LaunchConfiguration("target_hole").perform(context).strip()
|
||||
depth = LaunchConfiguration("insert_depth_mm").perform(context).strip()
|
||||
pitch = LaunchConfiguration("screw_pitch_mm").perform(context).strip()
|
||||
rmax = LaunchConfiguration("spiral_r_max_mm").perform(context).strip()
|
||||
|
||||
nodes = []
|
||||
if handeye and Path(handeye).is_file():
|
||||
tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(handeye)
|
||||
nodes.append(
|
||||
Node(
|
||||
package="tf2_ros",
|
||||
executable="static_transform_publisher",
|
||||
name="handeye_static_tf",
|
||||
arguments=[
|
||||
"--x", str(tx), "--y", str(ty), "--z", str(tz),
|
||||
"--qx", str(qx), "--qy", str(qy), "--qz", str(qz), "--qw", str(qw),
|
||||
"--frame-id", ee_frame,
|
||||
"--child-frame-id", cam_frame,
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
)
|
||||
|
||||
node = Node(
|
||||
package="arm_application",
|
||||
executable="stage3_search",
|
||||
name="stage3_search",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{
|
||||
"robot_sn": str(robot.get("sn") or os.environ.get("SITE_ROBOT_SN") or ""),
|
||||
"preview": preview,
|
||||
"resnap": resnap,
|
||||
"target_hole": target_hole,
|
||||
"insert_depth_mm": float(depth) if depth else -1.0,
|
||||
"screw_pitch_mm": float(pitch) if pitch else -999.0,
|
||||
"spiral_r_max_mm": float(rmax) if rmax else -1.0,
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
return [
|
||||
LogInfo(msg=f"[stage3] site={path}"),
|
||||
LogInfo(msg=f"[stage3] arm={robot.get('label') or robot.get('sn')} cam={cam}"),
|
||||
LogInfo(
|
||||
msg=f"[stage3] target={target_hole or 'site'} depth={depth or 'site'} "
|
||||
f"pitch={pitch or 'site'} r_max={rmax or 'site'} resnap={resnap}"
|
||||
),
|
||||
*nodes,
|
||||
node,
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
return LaunchDescription(
|
||||
[
|
||||
DeclareLaunchArgument("target_hole", default_value=""),
|
||||
DeclareLaunchArgument("insert_depth_mm", default_value=""),
|
||||
DeclareLaunchArgument("screw_pitch_mm", default_value=""),
|
||||
DeclareLaunchArgument("spiral_r_max_mm", default_value=""),
|
||||
DeclareLaunchArgument("resnap", default_value="false"),
|
||||
DeclareLaunchArgument("preview", default_value="true"),
|
||||
OpaqueFunction(function=_launch_setup),
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/launch/stage3_search.launch.py
|
||||
@@ -1,28 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Launch Orisys tactile node（双指，多进程架构）。
|
||||
|
||||
tactile 节点内部为每侧手指各起一个独立进程（厂家推荐：独立 CUDA 上下文），
|
||||
发布 /tactile/left|right/status。Device ids come from site.yaml tactile:.
|
||||
"""
|
||||
|
||||
from pathlib import Path
|
||||
import os
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import LogInfo
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
site = os.environ.get("SITE_CONFIG") or str(Path.home() / "ros2" / "config" / "site.yaml")
|
||||
return LaunchDescription(
|
||||
[
|
||||
LogInfo(msg=f"[tactile] site={site} (mp workers: left + right)"),
|
||||
Node(
|
||||
package="arm_application",
|
||||
executable="tactile",
|
||||
name="tactile",
|
||||
output="screen",
|
||||
),
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/launch/tactile.launch.py
|
||||
@@ -1,365 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""启动：perception_node + servo_node + 静态手眼 TF。
|
||||
|
||||
所有可调参数都来自 ``~/ros2/config/site.yaml``（必要时可用 launch 参数覆盖）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
EmitEvent,
|
||||
LogInfo,
|
||||
OpaqueFunction,
|
||||
RegisterEventHandler,
|
||||
)
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.events import Shutdown
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def _ws_root() -> Path:
|
||||
# 优先使用 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 文件可能位于 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():
|
||||
return p
|
||||
return Path.home() / "ros2"
|
||||
|
||||
|
||||
def _load_site():
|
||||
root = _ws_root()
|
||||
sys.path.insert(0, str(root / "scripts" / "lib"))
|
||||
from export_site_env import ( # type: ignore
|
||||
apply_active_arm,
|
||||
load_site,
|
||||
perception_overrides,
|
||||
servo_overrides,
|
||||
site_path,
|
||||
)
|
||||
|
||||
cfg = os.environ.get("SITE_CONFIG", "").strip()
|
||||
path = Path(cfg).expanduser() if cfg else site_path()
|
||||
# site_path() 依赖脚本所在位置；这里强制使用工作区根目录下的文件
|
||||
if not path.is_file():
|
||||
path = root / "config" / "site.yaml"
|
||||
data = load_site(path)
|
||||
merged, robot = apply_active_arm(data)
|
||||
return merged, path, perception_overrides(data), servo_overrides(data), robot
|
||||
|
||||
|
||||
def _as_bool(s: str) -> bool:
|
||||
return s.strip().lower() in ("1", "true", "yes")
|
||||
|
||||
|
||||
def _handeye_xyz_xyzw(path: str):
|
||||
"""读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。
|
||||
|
||||
实现只有一份，在 ``arm_application.handeye_lib``（``read_transform_yaml``），
|
||||
并由回环测试钉住：``quat → R`` 必须能还原文件里的 ``T_4x4``。
|
||||
这段换算曾经在 4 个 launch 文件里各抄了一遍，且抄错了 3 个分支 ——
|
||||
手眼姿态差几十度，定位"总能到附近、永远差一点"，看起来像相机标定不准。
|
||||
"""
|
||||
from arm_application.handeye_lib import read_transform_yaml
|
||||
|
||||
xyz, quat = read_transform_yaml(path)
|
||||
return (
|
||||
float(xyz[0]), float(xyz[1]), float(xyz[2]),
|
||||
float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]),
|
||||
)
|
||||
|
||||
|
||||
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 参数显式传入时覆盖；空则用 servo_overrides（已按 track_camera 选文件）。
|
||||
# 不要再拿某个臂的腕部结果路径当"空"哨兵：换臂或切到 D435 后那个判断失效，
|
||||
# 会把眼在手外的跟踪悄悄接上腕部 T。
|
||||
handeye = LaunchConfiguration("handeye_file").perform(context).strip()
|
||||
site_he = str(servo_over.get("handeye_file") or "")
|
||||
if not handeye:
|
||||
handeye = site_he
|
||||
if not Path(handeye).is_file():
|
||||
raise FileNotFoundError(
|
||||
f"手眼标定结果缺失：{handeye}。"
|
||||
"请重新运行：bash ~/ros2/scripts/run_handeye.sh calibrate"
|
||||
)
|
||||
|
||||
enable = LaunchConfiguration("enable_control").perform(context).lower() in (
|
||||
"1", "true", "yes",
|
||||
)
|
||||
ee_frame = LaunchConfiguration("ee_frame").perform(context).strip()
|
||||
cam_frame = LaunchConfiguration("camera_frame").perform(context).strip()
|
||||
yolo = site.get("yolo") or {}
|
||||
# "没指定"用**空串**表示，不再拿一个具体坐标系名当哨兵：
|
||||
# 之前写的是 `ee_frame == "Rizon4-063989_flange"`，换个站点以后
|
||||
# 这个判断永远不成立，于是 site.yaml 里的 ee_frame 再也不会被用上
|
||||
# （症状是静态 TF 挂到了错的坐标系上，而点还是能查到）。
|
||||
if not ee_frame:
|
||||
ee_frame = str(robot.get("ee_frame") or ee_frame)
|
||||
# 同 ee_frame：用空串表示"没指定"，不拿具体坐标系名当哨兵。
|
||||
if not cam_frame:
|
||||
cam_frame = str(
|
||||
perc_over.get("camera_frame")
|
||||
or yolo.get("camera_frame")
|
||||
or cam_frame
|
||||
)
|
||||
|
||||
detect_yaml = str(share / "config" / "perception.yaml")
|
||||
servo_yaml = str(share / "config" / "servo.yaml")
|
||||
|
||||
tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(handeye)
|
||||
|
||||
# 跟踪相机的手眼父子：腕部=法兰←相机，顶视=基座←相机。不许写死法兰。
|
||||
from arm_application.site_io import handeye_static_tf_frames # noqa: PLC0415
|
||||
|
||||
he_mode = str(servo_over.get("handeye_mode") or "eye_in_hand")
|
||||
base_frame = str(robot.get("base_frame") or "")
|
||||
tf_parent, tf_child = handeye_static_tf_frames(
|
||||
mode=he_mode,
|
||||
ee_frame=ee_frame,
|
||||
base_frame=base_frame,
|
||||
cam_frame=cam_frame,
|
||||
)
|
||||
|
||||
static_tf = Node(
|
||||
package="tf2_ros",
|
||||
executable="static_transform_publisher",
|
||||
name="handeye_static_tf",
|
||||
arguments=[
|
||||
"--x", str(tx), "--y", str(ty), "--z", str(tz),
|
||||
"--qx", str(qx), "--qy", str(qy), "--qz", str(qz), "--qw", str(qw),
|
||||
"--frame-id", tf_parent,
|
||||
"--child-frame-id", tf_child,
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
perc_params = dict(perc_over)
|
||||
perc_params["camera_frame"] = cam_frame
|
||||
|
||||
servo_params = dict(servo_over)
|
||||
servo_params.update(
|
||||
{
|
||||
"enable_control": enable,
|
||||
"ee_frame": ee_frame,
|
||||
"camera_frame": cam_frame,
|
||||
"handeye_child_frame": cam_frame,
|
||||
"handeye_file": handeye,
|
||||
}
|
||||
)
|
||||
ee_z_arg = LaunchConfiguration("ee_z_m").perform(context).strip()
|
||||
if ee_z_arg:
|
||||
try:
|
||||
z = float(ee_z_arg)
|
||||
if z > 0.05:
|
||||
servo_params["ee_z_m"] = z
|
||||
except ValueError:
|
||||
pass
|
||||
lock_hold_arg = LaunchConfiguration("lock_hold_sec").perform(context).strip()
|
||||
if lock_hold_arg:
|
||||
try:
|
||||
servo_params["lock_hold_sec"] = float(lock_hold_arg)
|
||||
except ValueError:
|
||||
pass
|
||||
d435_only = _as_bool(LaunchConfiguration("d435_move_only").perform(context))
|
||||
perc_params["d435_move_only"] = d435_only
|
||||
servo_params["d435_move_only"] = d435_only
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# 感知来源：谁来产生"视觉测量"（site.yaml 的 perception.source）
|
||||
#
|
||||
# yolo 本体 perception_node（要 torch/ultralytics）
|
||||
# truth 仿真真值节点（按场景已知位姿直接算目标点，不跑推理）
|
||||
# external 外部节点按同一话题提供，本 launch 不起感知进程
|
||||
#
|
||||
# 为什么要有这个开关：仿真环境（ros_gz_sim conda）没有 torch/ultralytics
|
||||
# 且离线装不上，perception_node 一启动就死；而仿真里工件位姿本来就是
|
||||
# 已知的。切到 truth 后，流程能在仿真里验"控制与接口"那一半。
|
||||
# 值写错**直接报错**，不要默默当 yolo —— 那会退回到"一阶段永远不锁定"。
|
||||
# ------------------------------------------------------------------
|
||||
# 允许值与校验只有一处实现（gauge_boot.perception_source_of），S00 自检用的
|
||||
# 也是它 —— 两边各写一份的话，会出现"起了真值节点、S00 却去查 ultralytics"。
|
||||
from arm_application.gauge_boot import perception_source_of
|
||||
|
||||
perc_src = perception_source_of(site.get("perception"))
|
||||
# 仿真时间必须**一路传下去**：这个 launch 由装配流程（pipeline_node）在
|
||||
# S04 里以子进程拉起，父节点在仿真里带 use_sim_time=true，这里若漏传，
|
||||
# perception/servo 会跑在系统时间上 —— TF 全是仿真时刻的戳，于是
|
||||
# "看得见图、算得出点，就是查不到变换"，表现为一阶段永远不收敛。
|
||||
use_sim = LaunchConfiguration("use_sim_time").perform(context).lower() in (
|
||||
"1", "true", "yes",
|
||||
)
|
||||
|
||||
perception = None
|
||||
if perc_src == "yolo":
|
||||
perception = Node(
|
||||
package="arm_application",
|
||||
executable="perception_node",
|
||||
name="perception_node",
|
||||
output="screen",
|
||||
parameters=[detect_yaml, perc_params, {"use_sim_time": use_sim}],
|
||||
)
|
||||
elif perc_src == "truth":
|
||||
truth_cfg = (site.get("perception") or {}).get("truth") or {}
|
||||
perception = Node(
|
||||
package="arm_sim_gazebo",
|
||||
executable="sim_truth_perception",
|
||||
name="sim_truth_perception",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{
|
||||
"camera_frame": cam_frame,
|
||||
"target_object": str(truth_cfg.get("object") or ""),
|
||||
"offset_m": [float(v) for v in (truth_cfg.get("offset_m") or [0.0, 0.0, 0.0])],
|
||||
"use_sim_time": use_sim,
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
servo = Node(
|
||||
package="arm_application",
|
||||
executable="servo_node",
|
||||
name="servo_node",
|
||||
output="screen",
|
||||
parameters=[servo_yaml, servo_params, {"use_sim_time": use_sim}],
|
||||
)
|
||||
|
||||
weights = perc_params.get("weights", "")
|
||||
nodes = [
|
||||
LogInfo(msg=f"[site] {site_path}"),
|
||||
LogInfo(
|
||||
msg=f"[arm] {robot.get('label') or robot.get('sn')} "
|
||||
f"handeye={handeye} mode={he_mode} track_cam={cam_frame}"
|
||||
),
|
||||
LogInfo(msg=f"[site] 感知来源 perception.source = {perc_src}"),
|
||||
]
|
||||
if d435_only:
|
||||
nodes.append(
|
||||
LogInfo(msg="[d435_only] 只测 D435 单次位移：不交 D405、不进二阶段")
|
||||
)
|
||||
if perc_src == "yolo":
|
||||
nodes.append(LogInfo(msg=f"[site] YOLO weights: {weights}"))
|
||||
nodes.append(
|
||||
LogInfo(
|
||||
msg="节点：perception_node（YOLO）| servo_node（100Hz）—— "
|
||||
"各自独立进程，话题 /visual_servo/filtered_point"
|
||||
)
|
||||
)
|
||||
elif perc_src == "truth":
|
||||
nodes.append(
|
||||
LogInfo(
|
||||
msg="节点：sim_truth_perception（仿真真值，不跑推理）| servo_node（100Hz）\n"
|
||||
" 真值只验**控制与接口**：像素噪声/遮挡/算法错判它都没有。"
|
||||
"能不能在真机上看到孔，要看真机那一次（见 devices/sim/gazebo/README.md）"
|
||||
)
|
||||
)
|
||||
else:
|
||||
nodes.append(
|
||||
LogInfo(
|
||||
msg="节点：servo_node（100Hz）。感知由**外部**节点按 "
|
||||
"visual_servo/filtered_point 提供（本 launch 不起感知进程）"
|
||||
)
|
||||
)
|
||||
pub_tf = LaunchConfiguration("publish_handeye_tf").perform(context).lower() in (
|
||||
"1", "true", "yes",
|
||||
)
|
||||
if pub_tf:
|
||||
nodes.append(
|
||||
LogInfo(
|
||||
msg=f"静态手眼 {tf_parent} → {tf_child} t=[{tx:.3f},{ty:.3f},{tz:.3f}] "
|
||||
f"（{he_mode}）"
|
||||
)
|
||||
)
|
||||
nodes.append(static_tf)
|
||||
if perception is not None:
|
||||
nodes.append(perception)
|
||||
nodes.append(servo)
|
||||
# 感知进程一死就整段退出（truth 后端同样适用 —— 它可能因为配置/场景读
|
||||
# 不出来而退出，那时 servo 还活着，调用方只能等满 stage1_timeout_sec）。
|
||||
# 为什么必须显式写：yolo 后端是最需要 GPU/torch 的那个（缺 ultralytics/torch
|
||||
# 时**启动即死**，而 servo_node 还活着继续跑）。没有这个 handler，launch 不会
|
||||
# 结束，调用方（装配流程的 S04）只能等满 stage1_timeout_sec 才报"没锁定"——
|
||||
# 现场看到的是"算法不收敛"，真正原因是感知进程早就不在了。
|
||||
if perception is not None:
|
||||
nodes.append(
|
||||
RegisterEventHandler(
|
||||
OnProcessExit(
|
||||
target_action=perception,
|
||||
on_exit=[
|
||||
LogInfo(
|
||||
msg=f"[visual_servo] ✘ 感知进程（{perc_src}）退出了 —— 停止本段（"
|
||||
"看它退出前最后几行：yolo 多为缺推理依赖，"
|
||||
"truth 多为场景/参数读不出来）"
|
||||
),
|
||||
EmitEvent(event=Shutdown(reason="perception exited")),
|
||||
],
|
||||
)
|
||||
)
|
||||
)
|
||||
return nodes
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
return LaunchDescription(
|
||||
[
|
||||
# 默认值从环境取（scripts/env.sh → load_site_config.sh 导出的
|
||||
# SITE_HANDEYE_FILE / SITE_EE_FRAME 已经是**当前臂**的值）。
|
||||
# 不写死某个臂的文件名/坐标系：写死的话换臂后这里就悄悄指向上一台。
|
||||
DeclareLaunchArgument(
|
||||
"handeye_file",
|
||||
default_value=os.environ.get("SITE_HANDEYE_FILE", ""),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"ee_frame",
|
||||
default_value=os.environ.get("SITE_EE_FRAME", ""),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"camera_frame",
|
||||
default_value=os.environ.get("SITE_CAMERA_FRAME", ""),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"enable_control",
|
||||
default_value="false",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"publish_handeye_tf",
|
||||
default_value="true",
|
||||
),
|
||||
# 仿真时间开关。**仿真里必须传 true**：/clock 由 Gazebo 发，
|
||||
# 节点若跑在系统时间上，TF/图像时间戳全对不上（症状是"看得见图、
|
||||
# 就是查不到变换，一阶段永远不锁定"）。
|
||||
# 装配流程在仿真里跑时由 pipeline_node 自动传 true（见
|
||||
# pipeline_node._run_stage1），手工跑时自己加上。
|
||||
DeclareLaunchArgument(
|
||||
"use_sim_time",
|
||||
default_value="false",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"ee_z_m",
|
||||
default_value="",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"lock_hold_sec",
|
||||
default_value="",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"d435_move_only",
|
||||
default_value="false",
|
||||
),
|
||||
OpaqueFunction(function=_launch_setup),
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/launch/visual_servo.launch.py
|
||||
@@ -4,3 +4,6 @@ source;share/arm_application/hook/pythonpath.sh
|
||||
source;share/arm_application/hook/ament_prefix_path.ps1
|
||||
source;share/arm_application/hook/ament_prefix_path.dsv
|
||||
source;share/arm_application/hook/ament_prefix_path.sh
|
||||
source;../../build/arm_application/share/arm_application/hook/pythonpath_develop.ps1
|
||||
source;../../build/arm_application/share/arm_application/hook/pythonpath_develop.dsv
|
||||
source;../../build/arm_application/share/arm_application/hook/pythonpath_develop.sh
|
||||
|
||||
@@ -112,5 +112,6 @@ $env:COLCON_CURRENT_PREFIX=(Get-Item $PSCommandPath).Directory.Parent.Parent.Ful
|
||||
|
||||
colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/share/arm_application/hook/pythonpath.ps1"
|
||||
colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/share/arm_application/hook/ament_prefix_path.ps1"
|
||||
colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/../../build/arm_application/share/arm_application/hook/pythonpath_develop.ps1"
|
||||
|
||||
Remove-Item Env:\COLCON_CURRENT_PREFIX
|
||||
|
||||
@@ -80,6 +80,7 @@ _colcon_package_sh_source_script() {
|
||||
# source sh hooks
|
||||
_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/arm_application/hook/pythonpath.sh"
|
||||
_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/arm_application/hook/ament_prefix_path.sh"
|
||||
_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/../../build/arm_application/share/arm_application/hook/pythonpath_develop.sh"
|
||||
|
||||
unset _colcon_package_sh_source_script
|
||||
unset COLCON_CURRENT_PREFIX
|
||||
|
||||
@@ -1,30 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>arm_application</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Field nodes: D405, visual servo, hand-eye, DH grippers, hole detect.</description>
|
||||
<maintainer email="dev@example.com">arm_dev</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>arm_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>std_srvs</depend>
|
||||
<depend>trajectory_msgs</depend>
|
||||
<depend>control_msgs</depend>
|
||||
<exec_depend>tf2_ros</exec_depend>
|
||||
<exec_depend>python3-yaml</exec_depend>
|
||||
<exec_depend>python3-serial</exec_depend>
|
||||
<!-- geom_ik 的实现已移到 arm_kinematics（驱动层也要用，避免层次倒置）。
|
||||
本包保留 arm_application/geom_ik.py 作为 re-export shim。 -->
|
||||
<exec_depend>arm_kinematics</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/package.xml
|
||||
@@ -1,247 +0,0 @@
|
||||
traj_coord_type: ""
|
||||
traj_coord_ref: ""
|
||||
pattern_params {
|
||||
pattern_name: "WPT0"
|
||||
pattern_type: "Scatter"
|
||||
scatterpoint_pattern {
|
||||
pose_params {
|
||||
x: "0.26072"
|
||||
y: "-0.14604"
|
||||
z: "0.85707"
|
||||
rx: "6.58"
|
||||
ry: "165.63"
|
||||
rz: "-6.63"
|
||||
coord_type: "WORLD"
|
||||
coord_ref: "WORLD_ORIGIN"
|
||||
use_quaternion: false
|
||||
preferJntPos {
|
||||
joint1: "-6.97"
|
||||
joint2: "29.95"
|
||||
joint3: "1.72"
|
||||
joint4: "92.29"
|
||||
joint5: "-6.48"
|
||||
joint6: "-12.54"
|
||||
joint7: "-0.37"
|
||||
}
|
||||
externalAxes {
|
||||
axis1: "0"
|
||||
axis2: "0"
|
||||
axis3: "0"
|
||||
axis4: "0"
|
||||
axis5: "0"
|
||||
axis6: "0"
|
||||
}
|
||||
}
|
||||
force_params {
|
||||
fx: "0"
|
||||
fy: "0"
|
||||
fz: "0"
|
||||
mx: "0"
|
||||
my: "0"
|
||||
mz: "0"
|
||||
}
|
||||
path_profile: "line"
|
||||
blend_zone: "Z10"
|
||||
velocity: "0.2"
|
||||
acceleration: "1"
|
||||
vel_var_name: ""
|
||||
wrench_var_name: ""
|
||||
}
|
||||
}
|
||||
pattern_params {
|
||||
pattern_name: "WPT1"
|
||||
pattern_type: "Scatter"
|
||||
scatterpoint_pattern {
|
||||
pose_params {
|
||||
x: "0.50244"
|
||||
y: "-0.05458"
|
||||
z: "0.62368"
|
||||
rx: "3.30681"
|
||||
ry: "173.28739"
|
||||
rz: "12.76833"
|
||||
coord_type: "WORLD"
|
||||
coord_ref: "WORLD_ORIGIN"
|
||||
use_quaternion: false
|
||||
preferJntPos {
|
||||
joint1: "-0.39471"
|
||||
joint2: "2.89069"
|
||||
joint3: "7.36936"
|
||||
joint4: "104.4854"
|
||||
joint5: "-2.56594"
|
||||
joint6: "19.41153"
|
||||
joint7: "-5.21738"
|
||||
}
|
||||
externalAxes {
|
||||
axis1: "0"
|
||||
axis2: "0"
|
||||
axis3: "0"
|
||||
axis4: "0"
|
||||
axis5: "0"
|
||||
axis6: "0"
|
||||
}
|
||||
}
|
||||
force_params {
|
||||
fx: "0"
|
||||
fy: "0"
|
||||
fz: "0"
|
||||
mx: "0"
|
||||
my: "0"
|
||||
mz: "0"
|
||||
}
|
||||
path_profile: "line"
|
||||
blend_zone: "Z10"
|
||||
velocity: "0.2"
|
||||
acceleration: "1"
|
||||
vel_var_name: ""
|
||||
wrench_var_name: ""
|
||||
}
|
||||
}
|
||||
pattern_params {
|
||||
pattern_name: "WPT2"
|
||||
pattern_type: "Scatter"
|
||||
scatterpoint_pattern {
|
||||
pose_params {
|
||||
x: "0.38216"
|
||||
y: "-0.07015"
|
||||
z: "0.2772"
|
||||
rx: "-0.76189"
|
||||
ry: "178.83063"
|
||||
rz: "13.77851"
|
||||
coord_type: "WORLD"
|
||||
coord_ref: "WORLD_ORIGIN"
|
||||
use_quaternion: false
|
||||
preferJntPos {
|
||||
joint1: "-0.51679"
|
||||
joint2: "-17.82083"
|
||||
joint3: "6.09068"
|
||||
joint4: "145.99388"
|
||||
joint5: "-3.83264"
|
||||
joint6: "75.52854"
|
||||
joint7: "-5.23384"
|
||||
}
|
||||
externalAxes {
|
||||
axis1: "0"
|
||||
axis2: "0"
|
||||
axis3: "0"
|
||||
axis4: "0"
|
||||
axis5: "0"
|
||||
axis6: "0"
|
||||
}
|
||||
}
|
||||
force_params {
|
||||
fx: "0"
|
||||
fy: "0"
|
||||
fz: "0"
|
||||
mx: "0"
|
||||
my: "0"
|
||||
mz: "0"
|
||||
}
|
||||
path_profile: "line"
|
||||
blend_zone: "Z10"
|
||||
velocity: "0.2"
|
||||
acceleration: "1"
|
||||
vel_var_name: ""
|
||||
wrench_var_name: ""
|
||||
}
|
||||
}
|
||||
pattern_params {
|
||||
pattern_name: "WPT3"
|
||||
pattern_type: "Scatter"
|
||||
scatterpoint_pattern {
|
||||
pose_params {
|
||||
x: "0.42395"
|
||||
y: "-0.04059"
|
||||
z: "0.135"
|
||||
rx: "-4.09647"
|
||||
ry: "175.59924"
|
||||
rz: "22.59976"
|
||||
coord_type: "WORLD"
|
||||
coord_ref: "WORLD_ORIGIN"
|
||||
use_quaternion: false
|
||||
preferJntPos {
|
||||
joint1: "0.80285"
|
||||
joint2: "-48.57114"
|
||||
joint3: "4.29418"
|
||||
joint4: "137.232"
|
||||
joint5: "-14.43138"
|
||||
joint6: "99.59405"
|
||||
joint7: "-5.25622"
|
||||
}
|
||||
externalAxes {
|
||||
axis1: "0"
|
||||
axis2: "0"
|
||||
axis3: "0"
|
||||
axis4: "0"
|
||||
axis5: "0"
|
||||
axis6: "0"
|
||||
}
|
||||
}
|
||||
force_params {
|
||||
fx: "0"
|
||||
fy: "0"
|
||||
fz: "0"
|
||||
mx: "0"
|
||||
my: "0"
|
||||
mz: "0"
|
||||
}
|
||||
path_profile: "line"
|
||||
blend_zone: "Z10"
|
||||
velocity: "0.2"
|
||||
acceleration: "1"
|
||||
vel_var_name: ""
|
||||
wrench_var_name: ""
|
||||
}
|
||||
}
|
||||
pattern_params {
|
||||
pattern_name: "WPT4"
|
||||
pattern_type: "Scatter"
|
||||
scatterpoint_pattern {
|
||||
pose_params {
|
||||
x: "0.2971"
|
||||
y: "0.06042"
|
||||
z: "0.76959"
|
||||
rx: "4.64468"
|
||||
ry: "173.46657"
|
||||
rz: "27.06819"
|
||||
coord_type: "WORLD"
|
||||
coord_ref: "WORLD_ORIGIN"
|
||||
use_quaternion: false
|
||||
preferJntPos {
|
||||
joint1: "5.13584"
|
||||
joint2: "28.76765"
|
||||
joint3: "17.15688"
|
||||
joint4: "100.21538"
|
||||
joint5: "4.05433"
|
||||
joint6: "-9.64651"
|
||||
joint7: "-5.26232"
|
||||
}
|
||||
externalAxes {
|
||||
axis1: "0"
|
||||
axis2: "0"
|
||||
axis3: "0"
|
||||
axis4: "0"
|
||||
axis5: "0"
|
||||
axis6: "0"
|
||||
}
|
||||
}
|
||||
force_params {
|
||||
fx: "0"
|
||||
fy: "0"
|
||||
fz: "0"
|
||||
mx: "0"
|
||||
my: "0"
|
||||
mz: "0"
|
||||
}
|
||||
path_profile: "line"
|
||||
blend_zone: "Z10"
|
||||
velocity: "0.2"
|
||||
acceleration: "1"
|
||||
vel_var_name: ""
|
||||
wrench_var_name: ""
|
||||
}
|
||||
}
|
||||
group_params {
|
||||
group_name: "RecordGroup"
|
||||
pattern_start_idx: 0
|
||||
pattern_end_idx: 4
|
||||
}
|
||||
@@ -0,0 +1 @@
|
||||
/home/cc/ros2/build/arm_application/trajectories/xunhuan.traj
|
||||
Reference in New Issue
Block a user