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

P5c-4 相机出图联调：桥接 d405/d435 到 ROS，并修掉两处静默的几何错误

Browse Source 
相机链路（devices/sim/gazebo/config/cameras.yaml 是唯一真相）
  - sim_cameras.py        纯逻辑：话题映射 / 深度换算 / 内参 / 视场一致性比对
  - sim_camera_adapter.py 改光学系 frame_id、32FC1 米 → 16UC1 毫米、覆盖内参
  - gazebo_sim.launch.py  桥接 + 适配器 + 世界相机的静态 TF（位姿从 world 解析）

本轮修掉两个**静默**的几何错误（都能出图、都能查到 TF，但点位是错的）

  1. gz 发的 camera_info 是错的：恒为 fx=fy=277, cx=160, cy=120
     （等于它的默认 60° 视场 + 半分辨率主点），而渲染用的是配置的 87° 视场。
     照抄它做三维反投影，点会偏半米量级；单目预览完全正常。
     用已知地面平面做最小二乘反解，得 fy=337.2 / cy=239.5，与
     640/(2·tan(87°/2))=337.2、480/2=240 吻合（残差中位 0.2mm）。
     改为由 sim_cameras.intrinsics() 按 cameras.yaml 的视场重算并覆盖。
     代价：cameras.yaml 的 horizontal_fov 必须与 SDF 一致，启动时自动比对告警。

  2. assembly.world 里 D435 的 rpy 写成 (0, 50°, 180°)，
     而 URDF 相机的视线是**本体 X 轴**、rpy 复合顺序又是 Rz·Ry·Rx，
     于是"先俯 50° 再偏航 180°"把视线翻到了背面：相机背对台面、只看地面。
     它不报错，而且几何自洽（点确实落在真实地面上），只查数值发现不了。
     改为 (0, 50°, 0)，并加回归测试锁住。

新增验收工具 + 测试
  - scripts/check_sim_cameras.py  反投影深度、要求命中地面/台面才算通过。
    "有图像"不是验收标准；实测 d435 台面 11.9%/地面 75.3%、d405 台面 37.8%。
  - 97 项单测（新增 sim_cameras 22 项、sim_site 16 项）

顺带修掉一处"改了配置静默无效"
  - sim.world 此前硬编码在 robot_description._WORLD_REL，而 site.yaml 与
    manifest.yaml 都宣称改它就生效。现在真的读了：
    优先级 --world > site.yaml:sim.world > 内置默认（相对路径按仓库根解析）。
  - physics_enable / joint_prefix 仍未接线，但若其值与实际状态冲突，
    启动时会告警（sim_site.unwired_warnings），不再静默忽略。

文档
  - devices/sim/gazebo/README.md 重写：原文还写着"仿真不是 Gazebo"、
    相机插件用 libgazebo_ros_camera.so，与实现早已不符。现分三部分：
    哪个"仿真"是真的 / 现状与缺什么 / 五条静默失效的坑及怎么验。

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
2026-09-23 15:19:16 +08:00
parent f67f59f383
commit 47ad9d9a8b
17 changed files with 2690 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/arm_sim_gazebo/arm_sim_gazebo/gazebo_sim_launcher.py
+34 -3
View File
@@ -33,10 +33,23 @@ from .robot_description import (
repo_root,
world_name,
)
from .sim_site import (
SimSiteError,
load_sim_site,
physics_enabled_in_world,
resolve_world,
unwired_warnings,
)
LAUNCH_PKG = "arm_sim_gazebo"
LAUNCH_FILE = "gazebo_sim.launch.py"
# 仿真实际使用的关节前缀，用来跟 site.yaml 的 sim.joint_prefix 比对。
# 它由 arm.sim.urdf.xacro 的 arm_prefix 默认值决定，并且 controllers.yaml
# 里硬编码了 sim_joint1..7，所以暂时**不可配置** —— 改了这里不改那两处会散架。
# site.yaml 若写了一个不一样的值，unwired_warnings() 会告警，而不是静默忽略。
ACTUAL_JOINT_PREFIX = "sim_"
def _build_launch_cmd(extra: List[str]) -> List[str]:
# 用 python -m 调 ros2 而不是 exec "ros2"：这样在 ros2 CLI 不在 PATH 的
@@ -95,8 +108,12 @@ def main(argv: Optional[List[str]] = None) -> int:
root = repo_root()
xacro_path = default_xacro(root)
controllers = default_controllers(root)
world = args.world or str(default_world(root))
# world 的优先级：--world > site.yaml 的 sim.world > 内置默认。
# 以前这里只认 --world 和硬编码默认值，而 site.yaml / manifest.yaml
# 都宣称改 sim.world 就生效 —— 结果改了毫无反应且不报错。
world, world_origin = resolve_world(args.world, root, fallback=default_world(root))
world = str(world)
site = load_sim_site(root)
urdf, report = prepare_urdf(
xacro_path=xacro_path,
controllers_file=controllers,
@@ -105,10 +122,19 @@ def main(argv: Optional[List[str]] = None) -> int:
)
wname = world_name(world)
missing = check_base_plugins(Path(world))
except SimDescriptionError as exc:
except (SimDescriptionError, SimSiteError) as exc:
print(f"[gazebo_sim_launcher] 失败：\n{exc}", file=sys.stderr)
return 2
# site.yaml 里"声明了但没接线"的键：与其静默无效，不如明说。
# 见 sim_site.unwired_warnings() —— 那里逐条说明了为什么还没接。
for _w in unwired_warnings(
site,
actual_joint_prefix=ACTUAL_JOINT_PREFIX,
physics_in_world=physics_enabled_in_world(Path(world)),
):
print(f"[gazebo_sim_launcher] ⚠ {_w}", file=sys.stderr)
if missing:
# 这一条能省掉一整个下午：world 漏声明基础系统时，仿真看起来完全正常
# （/clock 在走、create 打印成功），但机械臂从未进入 Gazebo。
@@ -135,6 +161,11 @@ def main(argv: Optional[List[str]] = None) -> int:
print(f" 描述 : {xacro_path}")
print(f" 控制器配置 : {controllers}")
print(f" 世界 : {world} (world 名: {wname})")
print(f" 世界来源 : {world_origin}")
if site.source is not None:
print(f" site.yaml : {site.source}")
else:
print(" site.yaml : 不存在（用默认值；这不会影响仿真）")
print(f" URDF 归一化 : {report.summary()}")
if missing:
print(
src/arm_sim_gazebo/arm_sim_gazebo/robot_description.py
+178
View File
@@ -20,6 +20,7 @@ devices/ **不是 ROS 包**，所以 $(find ...) 找不到它，必须用文件
"""
from __future__ import annotations
import math
import os
import re
import subprocess
@@ -107,6 +108,183 @@ def world_name(world_file: Path) -> str:
return m.group(1)
# =============================================================================
# 从 world 文件里读模型位姿
# =============================================================================
# 为什么需要：Gazebo 里的 <model>（尤其是 static 的工装/相机支架）**不会**产生
# ROS TF —— TF 只来自 robot_state_publisher。但应用要做 tf2 查询，比如
# "把 D435 看到的点从相机系转到 world"。少了这条 TF，查询直接报
# "frame does not exist"，而图像本身看起来完全正常。
#
# 为什么是"解析"而不是"再配置一遍"：位姿的唯一真相就在 world 文件里。
# 另配一份等于制造"改了 world 忘了改配置"的机会，而这种不一致不会报错，
# 只会让相机看到的东西和它自称的位置对不上 —— 极难查。
def _find_block(text: str, tag: str, name: str) -> Optional[str]:
"""取出 <tag name="name">...</tag> 的内容（考虑同名标签嵌套）。
SDF 里 <model> 可以嵌套 <model>（组合体），<link> 里也可能有 <link>（少见）。
只找第一个 </tag> 在嵌套时会截断到错误的位置，所以这里数嵌套深度。
"""
pat = re.compile(
r"<%(t)s\b[^>]*\bname\s*=\s*[\"']%(n)s[\"'][^>]*>" % {"t": tag, "n": re.escape(name)}
)
m = pat.search(text)
if not m:
return None
# 自闭合：<link name="l"/> 是完全合法的 SDF（没有子元素）。
# 不特判的话会走到下面的"找 </link>"分支、找不到 → 返回 None，
# 于是被误报成"这个 link 不存在"，而它明明在。
if m.group(0).rstrip().endswith("/>"):
return ""
inner_start = m.end()
depth = 1
pos = inner_start
open_re = re.compile(r"<%(t)s\b" % {"t": tag})
close_re = re.compile(r"</%(t)s\s*>" % {"t": tag})
while depth > 0:
o = open_re.search(text, pos)
c = close_re.search(text, pos)
if c is None:
return None # 标签没闭合 → 文件本身有问题，交给别的检查去报
if o is not None and o.start() < c.start():
depth += 1
pos = o.end()
else:
depth -= 1
if depth == 0:
return text[inner_start : c.start()]
pos = c.end()
return None
def _own_pose(block: str, stop_before: Tuple[str, ...]) -> Tuple[float, float, float, float, float, float]:
"""取一段块自己的 <pose>（默认全 0）。
只认**第一个** <pose>，且必须出现在 stop_before 里任一标签之前 ——
这样才不会把子元素的 pose 误当成父元素的。例如 <model> 的 pose 必须在
第一个 <link>/<model>/<joint> 之前；<link> 的 pose 必须在第一个
<visual>/<collision>/<sensor> 之前。
"""
cut = len(block)
for tag in stop_before:
i = block.find("<" + tag)
if 0 <= i < cut:
cut = i
m = re.search(r"<pose(?:\s[^>]*)?>([^<]*)</pose>", block[:cut])
if not m:
return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
vals = m.group(1).split()
if len(vals) < 6:
return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
try:
return tuple(float(v) for v in vals[:6]) # type: ignore[return-value]
except ValueError:
return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
def _rpy_to_quat(r: float, p: float, y: float) -> Tuple[float, float, float, float]:
cr, sr = math.cos(r / 2), math.sin(r / 2)
cp, sp = math.cos(p / 2), math.sin(p / 2)
cy, sy = math.cos(y / 2), math.sin(y / 2)
return (
sr * cp * cy - cr * sp * sy,
cr * sp * cy + sr * cp * sy,
cr * cp * sy - sr * sp * cy,
cr * cp * cy + sr * sp * sy,
)
def _quat_to_rpy(q: Tuple[float, float, float, float]) -> Tuple[float, float, float]:
x, y, z, w = q
# roll (x 轴)
sinr = 2 * (w * x + y * z)
cosr = 1 - 2 * (x * x + y * y)
roll = math.atan2(sinr, cosr)
# pitch (y 轴)：用 asin，并夹到 [-1,1] 免得浮点误差导致 domain error
sinp = 2 * (w * y - z * x)
sinp = max(-1.0, min(1.0, sinp))
pitch = math.asin(sinp)
# yaw (z 轴)
siny = 2 * (w * z + x * y)
cosy = 1 - 2 * (y * y + z * z)
yaw = math.atan2(siny, cosy)
return (roll, pitch, yaw)
def _quat_mul(a, b):
ax, ay, az, aw = a
bx, by, bz, bw = b
return (
aw * bx + ax * bw + ay * bz - az * by,
aw * by - ax * bz + ay * bw + az * bx,
aw * bz + ax * by - ay * bx + az * bw,
aw * bw - ax * bx - ay * by - az * bz,
)
def _rotate(q, v):
"""用四元数旋转向量 v（v 在子系里，返回在父系里的表达）。"""
x, y, z, w = q
vx, vy, vz = v
# t = 2 * (q_vec × v)
tx = 2 * (y * vz - z * vy)
ty = 2 * (z * vx - x * vz)
tz = 2 * (x * vy - y * vx)
# v' = v + w*t + q_vec × t
return (
vx + w * tx + (y * tz - z * ty),
vy + w * ty + (z * tx - x * tz),
vz + w * tz + (x * ty - y * tx),
)
def model_link_pose(
world_file: Path, model: str, link: str = ""
) -> Tuple[Tuple[float, float, float], Tuple[float, float, float]]:
"""返回 world 文件里 <model name=model> 的 <link name=link> 在**世界系**的位姿。
返回值是 ((x, y, z), (roll, pitch, yaw))。link 留空表示取模型本身的位姿。
两者都会做复合（模型位姿 ∘ link 位姿），因为 SDF 的 <pose> 是相对父元素的。
找不到模型/link 时抛 SimDescriptionError，并提示这里和 cameras.yaml 的
world_model 字段必须对应 —— 配错的话相机不会有 TF，而症状是
"tf 查询报 frame does not exist"，与相机本身无关，容易查错方向。
"""
text = Path(world_file).read_text(encoding="utf-8", errors="replace")
mblock = _find_block(text, "model", model)
if mblock is None:
raise SimDescriptionError(
f"{world_file} 里找不到 <model name=\"{model}\">。\n"
f" devices/sim/gazebo/config/cameras.yaml 里若有 world_model: {model}，"
f"就必须在 world 里真的有这个模型 —— 否则该相机没有 TF，"
f"应用做 tf2 查询会报 frame does not exist。"
)
mp = _own_pose(mblock, ("link", "model", "joint", "plugin"))
mq = _rpy_to_quat(mp[3], mp[4], mp[5])
if not link:
return ((mp[0], mp[1], mp[2]), (mp[3], mp[4], mp[5]))
lblock = _find_block(mblock, "link", link)
if lblock is None:
raise SimDescriptionError(
f"{world_file} 的 <model name=\"{model}\"> 里找不到 <link name=\"{link}\">。"
)
lp = _own_pose(lblock, ("visual", "collision", "sensor", "light"))
lq = _rpy_to_quat(lp[3], lp[4], lp[5])
# 复合：世界系下的位置 = t_model + R_model * t_link
t = _rotate(mq, (lp[0], lp[1], lp[2]))
pos = (mp[0] + t[0], mp[1] + t[1], mp[2] + t[2])
rpy = _quat_to_rpy(_quat_mul(mq, lq))
return (pos, rpy)
# 只要 world 里声明了任何一个 <plugin>，Gazebo 就不再加载 server.config 的默认
# 系统列表，这三个基础系统必须由 world 自己声明，否则整个仿真静默失效。
# 详细后果见 assembly.world 里那段注释，以及这里的 check_base_plugins()。
src/arm_sim_gazebo/arm_sim_gazebo/sim_camera_adapter.py
+309
View File
@@ -0,0 +1,309 @@
"""sim_camera_adapter —— 把桥接过来的 Gazebo 相机，变成应用能直接吃的话题。
=============================================================================
它到底解决什么问题
=============================================================================
ros_gz_bridge 只做"传输"：把 gz 的消息类型翻成 ROS 的消息类型，话题名照抄。
它**改不了**两样东西，而这两样恰恰是仿真里最容易错、也最不报错的：
1. frame_id
gz 盖的是内部路径（实测是 rizon_sim/sim_link7/d405_rgbd），
而 TF 里真实存在的坐标系叫 sim_d405_link —— 两者对不上。
更麻烦的是：即使改成 sim_d405_link 也还是错的，因为那是"相机机身"系
（X 前 / Y 左 / Z 上），而 ROS 的 image/camera_info 必须是光学系
（Z 前 / X 右 / Y 下）。tf2 查询两种都能查到、都不报错，
但用错的那个把深度反投影成三维点会整体转错。
所以这里盖的是 <相机>_color_optical_frame，并同时发布
link → optical 的静态 TF。
2. 深度单位
gz 是 32FC1 / 米，真机 realsense 的 aligned_depth 是 16UC1 / 毫米。
不转换的话应用按毫米解释会整体差 1000 倍，而且 dtype 不符时
numpy 读出来直接是垃圾。
=============================================================================
为什么是"桥接 + 适配器"两层，而不是合成一层
=============================================================================
桥接器改不了 frame_id，所以无论如何都要有第二层做语义转换。
既然必须有，就让第二层把"改名 + 坐标系 + 单位"全都集中处理，
而不是让桥接器改一半、适配器补一半 —— 那样出问题时不知道去哪找。
副产品：gz 原名的话题（/d405/image 等）在 ROS 图里仍然可见，
排查"是桥接断了还是适配器断了"时能一刀切开。
=============================================================================
发布的话题（与真机完全一致，应用无需为仿真改任何代码）
=============================================================================
/<ros_ns>/color/image_raw rgb8
/<ros_ns>/color/camera_info plumb_bob（畸变全 0，gz 不模拟畸变）
/<ros_ns>/aligned_depth_to_color/image_raw 16UC1 毫米，0 = 无读数
QoS 用 sensor data（BEST_EFFORT, keep_last 1），与真机 realsense 发布端、
以及应用的订阅端（qos_profile_sensor_data）都对齐。这一点不能想当然：
发布端 BEST_EFFORT、订阅端 RELIABLE 是不兼容的，表现是"话题在、echo 不到"。
"""
from __future__ import annotations
import math
from typing import Dict, List, Optional
import numpy as np
import rclpy
from geometry_msgs.msg import TransformStamped
from rclpy.node import Node
from rclpy.qos import (
DurabilityPolicy,
HistoryPolicy,
QoSProfile,
ReliabilityPolicy,
qos_profile_sensor_data,
)
from sensor_msgs.msg import CameraInfo, Image
from tf2_ros import StaticTransformBroadcaster
from .sim_cameras import (
DEFAULT_CAMERAS_REL,
CameraConfigError,
CameraSpec,
depth_m_to_mm16,
intrinsics,
load_cameras,
optical_rpy,
rpy_to_quat,
)
# 图像上盖的坐标系（光学系）相对相机本体系的固定旋转。
# 见 sim_cameras.OPTICAL_RPY 的说明 —— 这是"看起来正常但三维全错"的那一项。
class SimCameraAdapter(Node):
def __init__(self) -> None:
super().__init__("sim_camera_adapter")
self.declare_parameter("cameras_file", "")
path = str(self.get_parameter("cameras_file").value or "").strip()
try:
specs = load_cameras(path or None, root=self._repo_root())
except Exception as exc: # 配置错误应当直接说清怎么修，而不是抛栈
self.get_logger().error(f"\n仿真相机清单读取失败：\n{exc}")
raise
self._specs: List[CameraSpec] = specs
self._subs: List[object] = []
self._pubs: Dict[str, object] = {}
self._warned: set = set()
# 相机本体 → 光学系的静态 TF。必须在图像到达**之前**就发出去，
# 因为 TF 是 transient_local（迟到的订阅者也能拿到），但若从没发过，
# 应用查 tf 会直接报 "frame does not exist"。
self._tf = StaticTransformBroadcaster(self)
self._publish_optical_tfs()
for s in specs:
self._wire(s)
names = ", ".join(s.ros_ns for s in specs)
self.get_logger().info(
f"仿真相机适配器就绪: {names}"
+ f"（深度 {sum(1 for s in specs if s.depth)}/台 已做 米→毫米 转换）"
)
# ------------------------------------------------------------------ 内部
def _repo_root(self):
"""复用 robot_description 的仓库根定位（它已经处理了三种情形）。"""
from .robot_description import repo_root
try:
return repo_root()
except Exception:
return None
def _qos(self) -> QoSProfile:
# 深拷贝一份 sensor data QoS：共享的那个对象若被改动会影响其它节点。
return QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST,
depth=qos_profile_sensor_data.depth,
)
def _publish_optical_tfs(self) -> None:
"""发布 link → optical 的静态 TF（每台相机一条，平移为零）。"""
msgs = []
for s in self._specs:
t = TransformStamped()
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = s.link_frame
t.child_frame_id = s.optical_frame
t.transform.translation.x = 0.0
t.transform.translation.y = 0.0
t.transform.translation.z = 0.0
qx, qy, qz, qw = rpy_to_quat(optical_rpy())
t.transform.rotation.x = qx
t.transform.rotation.y = qy
t.transform.rotation.z = qz
t.transform.rotation.w = qw
msgs.append(t)
self._tf.sendTransform(msgs)
def _wire(self, s: CameraSpec) -> None:
qos = self._qos()
self._pubs[s.ros_color] = self.create_publisher(Image, s.ros_color, qos)
self._pubs[s.ros_info] = self.create_publisher(CameraInfo, s.ros_info, qos)
if s.depth:
self._pubs[s.ros_depth] = self.create_publisher(Image, s.ros_depth, qos)
self._subs.append(
self.create_subscription(
Image, s.gz_image, lambda m, sp=s: self._on_color(sp, m), qos
)
)
self._subs.append(
self.create_subscription(
CameraInfo, s.gz_info, lambda m, sp=s: self._on_info(sp, m), qos
)
)
if s.depth:
self._subs.append(
self.create_subscription(
Image, s.gz_depth, lambda m, sp=s: self._on_depth(sp, m), qos
)
)
def _restamp(self, msg, frame_id: str) -> None:
"""把坐标系换掉。就地改，避免每帧多一次消息构造与一次大数组拷贝。"""
if msg.header.frame_id != frame_id:
msg.header.frame_id = frame_id
def _on_color(self, s: CameraSpec, msg: Image) -> None:
self._note_encoding(s, msg)
self._restamp(msg, s.optical_frame)
self._pubs[s.ros_color].publish(msg)
def _on_info(self, s: CameraSpec, msg: CameraInfo) -> None:
"""重算并覆盖内参 —— gz 发的那份是错的，不能透传。
本机实测：gz 恒发 fx=fy=277, cx=160, cy=120（等于它的默认 60° 视场 +
半分辨率主点），而渲染用的是配置的 87° 视场与 640x480。
照抄它做三维反投影会错半米量级，且单目预览里完全看不出来。
详见 sim_cameras.intrinsics() 的说明。
"""
w = int(msg.width) or int(s.width)
h = int(msg.height) or int(s.height)
try:
k, p = intrinsics(s, w, h)
except CameraConfigError as exc:
key = ("info", s.ros_ns)
if key not in self._warned:
self._warned.add(key)
self.get_logger().error(f"{s.ros_ns}: 内参算不出来，{s.ros_info} 无输出。{exc}")
return
key = ("info", s.ros_ns)
if key not in self._warned:
self._warned.add(key)
gz_k = list(msg.k) if len(msg.k) == 9 else []
self.get_logger().info(
f"{s.ros_ns}: 内参已按视场 {s.horizontal_fov:.4f} rad "
f"({math.degrees(s.horizontal_fov):.1f}°) 与 {w}x{h} 重算 —— "
f"fx={k[0]:.1f} cx={k[2]:.1f} cy={k[5]:.1f}"
+ (
f"（覆盖 gz 的 fx={gz_k[0]:.1f} cx={gz_k[2]:.1f} cy={gz_k[5]:.1f}）"
if gz_k
else ""
)
)
out = CameraInfo()
out.header = msg.header
out.header.frame_id = s.optical_frame
out.height = h
out.width = w
out.distortion_model = "plumb_bob"
# gz 不模拟镜头畸变，所以畸变系数全 0、R 为单位阵。
# 这意味着"仿真里用畸变矫正调出来的参数"在真机上仍需重调 —— 已在
# devices/sim/gazebo/README.md 里说明。
out.d = [0.0] * 5
out.k = list(k)
out.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
out.p = list(p)
self._pubs[s.ros_info].publish(out)
def _on_depth(self, s: CameraSpec, msg: Image) -> None:
"""32FC1 米 → 16UC1 毫米。
用 frombuffer 而不是 array(...)：后者会把整个 buffer 再拷一遍。
这一步每帧都在跑（30Hz × 640×480×4B），能省则省。
"""
if msg.encoding != "32FC1":
# 只提醒一次，避免刷屏
key = (s.ros_ns, msg.encoding)
if key not in self._warned:
self._warned.add(key)
self.get_logger().warn(
f"{s.gz_depth} 的编码是 {msg.encoding}，不是预期的 32FC1。"
f"已跳过本次转换 —— 深度话题会没有输出。"
f"若 Gazebo 版本改动了深度编码，需要同步改 sim_cameras."
f"depth_m_to_mm16 的调用处。"
)
return
arr = np.frombuffer(msg.data, dtype=np.float32).reshape(msg.height, msg.width)
mm = depth_m_to_mm16(arr)
out = Image()
out.header = msg.header
out.header.frame_id = s.optical_frame
out.height = msg.height
out.width = msg.width
out.encoding = "16UC1"
# gz 给的是连续 buffer；转成 uint16 后每像素 2 字节，step 必须同步改，
# 否则下游按 RGB 的 stride 解读会错位。
out.is_bigendian = 0
out.step = msg.width * 2
out.data = mm.tobytes()
self._pubs[s.ros_depth].publish(out)
def _note_encoding(self, s: CameraSpec, msg: Image) -> None:
"""第一次收到彩色帧时把编码打出来。
为什么值得单独打印：gz 给的是 rgb8（实测），而真机 realsense 的
color/image_raw 也是 rgb8 —— 一致，所以不需要转换。但这一点是实测
结论，不是保证；换个 Gazebo 版本或传感器配置可能变成 bgr8。
打一行日志，使"颜色不对"时能立刻定位到是不是编码变了。
"""
key = ("enc", s.ros_ns, msg.encoding)
if key not in self._warned:
self._warned.add(key)
self.get_logger().info(
f"{s.ros_ns}: 彩色 {msg.width}x{msg.height} encoding={msg.encoding} "
f"→ {s.ros_color} (frame={s.optical_frame})"
)
if msg.encoding not in ("rgb8", "bgr8"):
self.get_logger().warn(
f"{s.ros_ns} 的彩色编码是 {msg.encoding}，既不是 rgb8 也不是 "
f"bgr8。应用按 rgb8 解释会得到错误的颜色。"
)
def main(argv: Optional[List[str]] = None) -> int:
rclpy.init(args=argv)
try:
node = SimCameraAdapter()
except Exception:
# 参数/配置错误时 rclpy 已经打印了原因；这里只需保证退出码非零，
# 让 launch 能把它当成失败（否则会留下一个没有相机的"成功"启动）。
rclpy.try_shutdown()
return 2
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.try_shutdown()
return 0
if __name__ == "__main__":
raise SystemExit(main())
src/arm_sim_gazebo/arm_sim_gazebo/sim_cameras.py
+394
View File
@@ -0,0 +1,394 @@
"""仿真相机的配置解析与格式换算。
=============================================================================
为什么单独一个模块
=============================================================================
"把 Gazebo 的相机输出变成应用能直接吃的话题"这件事里，真正有内容的是三件事：
1. 深度单位换算 32FC1 米 → 16UC1 毫米（且 0 表示无读数）
2. 坐标系盖章 图像要盖**光学系**，不能盖相机本体系
3. 话题改名 /d405/image → /d405/color/image_raw 等
这三件事全是纯数据变换，不需要 ROS 跑起来就能验。所以实现放在这里、
用普通函数表达，节点（sim_camera_adapter.py）只做订阅/发布这层壳。
好处是换算逻辑可以被单测钉死 —— 深度单位差 1000 倍这种错误，
在单目预览里肉眼看不出任何异常，只能靠测试兜住。
另外，启动文件里生成 ros_gz_bridge 参数也要用同一份相机清单，
所以清单的解析也放在这里，避免"桥接用的清单"和"适配器用的清单"各写一份。
"""
from __future__ import annotations
import math
import re
from dataclasses import dataclass
from pathlib import Path
from typing import Dict, List, Optional, Sequence, Tuple
import numpy as np
import yaml
# 默认配置位置（相对仓库根）。与其它 devices/ 下的配置一样，是**物料**而非代码。
DEFAULT_CAMERAS_REL = "devices/sim/gazebo/config/cameras.yaml"
# 相机本体系（X 前 / Y 左 / Z 上）→ 光学系（Z 前 / X 右 / Y 下）的固定旋转。
# 这就是真机 realsense 的 <相机>_color_optical_frame 相对其 link 用的一组角，
# 且是 ROS 的惯例（REP-103 / REP-105）。
#
# ⚠️ 这一步很容易被省掉，而且省掉之后**什么都不会报错**：
# 应用拿 image 的 frame_id 去做 tf2 查询，本体系是存在的，查询成功；
# 但那是"相机机身"的朝向，用它把深度反投影成三维点会整体转错。
# 单目预览看起来完全正常，只有把点投回世界坐标时才露馅 —— 所以
# 专门有一个端到端测试把"深度反投影后应落在已知平面"作为断言。
OPTICAL_RPY: Tuple[float, float, float] = (-1.5708, 0.0, -1.5708)
# uint16 能表示的最大毫米数（65535 mm = 65.535 m）。
# 更远的读数按"无读数"处理（0），而不是饱和到 65535 ——
# 饱和会让远处凭空出现一个 65.5m 的平面，比"无效"更有害。
MAX_MM = 65535
class CameraConfigError(RuntimeError):
"""配置有问题时抛出，消息面向使用者。"""
@dataclass(frozen=True)
class CameraSpec:
"""一台仿真相机的声明。"""
gz_ns: str
ros_ns: str
link_frame: str
optical_frame: str
depth: bool = True
horizontal_fov: float = 0.0
world_model: str = ""
world_link: str = "link"
width: int = 0
height: int = 0
# --- Gazebo 侧（ros_gz_bridge 的输入）-----------------------------------
@property
def gz_image(self) -> str:
return f"{self.gz_ns}/image"
@property
def gz_depth(self) -> str:
return f"{self.gz_ns}/depth_image"
@property
def gz_info(self) -> str:
return f"{self.gz_ns}/camera_info"
# --- ROS 侧（应用按这些名字订阅，与真机 realsense 一致）------------------
@property
def ros_color(self) -> str:
return f"/{self.ros_ns}/color/image_raw"
@property
def ros_info(self) -> str:
return f"/{self.ros_ns}/color/camera_info"
@property
def ros_depth(self) -> str:
return f"/{self.ros_ns}/aligned_depth_to_color/image_raw"
@property
def is_world_mounted(self) -> bool:
"""挂在世界模型上（而非机器人本体）→ 需要额外补一条静态 TF。"""
return bool(self.world_model)
def load_cameras(path: Optional[Path] = None, root: Optional[Path] = None) -> List[CameraSpec]:
"""读相机清单。找不到或写错时抛 CameraConfigError，消息里给出修法。"""
if path is None:
base = root if root is not None else Path.cwd()
path = Path(base) / DEFAULT_CAMERAS_REL
path = Path(path)
if not path.is_file():
raise CameraConfigError(
f"相机清单不存在: {path}\n"
f" 它是仿真相机的话题/坐标系唯一来源（桥接与适配器都从它派生）。\n"
f" 缺了它仿真里的相机就完全没输出。确认仓库根下存在 "
f"{DEFAULT_CAMERAS_REL}。"
)
try:
doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
except yaml.YAMLError as exc:
raise CameraConfigError(f"相机清单 YAML 解析失败: {path}\n 原因: {exc}") from exc
raw = doc.get("cameras")
if not isinstance(raw, list) or not raw:
raise CameraConfigError(
f"{path} 里没有 cameras 列表（或为空）。至少写一台相机，否则仿真里没有图像。"
)
specs: List[CameraSpec] = []
seen: Dict[str, str] = {}
for i, item in enumerate(raw):
if not isinstance(item, dict):
raise CameraConfigError(f"{path} 的 cameras[{i}] 不是映射（键值对）。")
def need(key: str) -> str:
v = str(item.get(key, "") or "").strip()
if not v:
raise CameraConfigError(
f"{path} 的 cameras[{i}] 缺少 {key}。"
f"（gz_ns / ros_ns / link_frame / optical_frame 都必填）"
)
return v
# 必填键先查完，再查取值是否合理 —— 否则一份"缺键且键值也不对"的配置
# 会先报一个后者的错，让人以为只是值填错，实际键都还没写全。
gz_ns = "/" + need("gz_ns").strip("/")
ros_ns = need("ros_ns").strip("/")
link_frame = need("link_frame")
optical_frame = need("optical_frame")
# 同一个 ROS 命名空间出现两次 → 两台相机会互相覆盖话题，
# 表现为"只有一台出图"或图像在两个相机之间跳，很难查。直接拒绝。
if ros_ns in seen:
raise CameraConfigError(
f"{path}: ros_ns '{ros_ns}' 重复（cameras[{i}] 与 {seen[ros_ns]}）。\n"
f" 两台相机用同一个 ros_ns 会互相覆盖话题，必须区分开。"
)
seen[ros_ns] = f"cameras[{i}]"
try:
fov = float(item.get("horizontal_fov", 0.0) or 0.0)
except (TypeError, ValueError):
raise CameraConfigError(
f"{path} 的 cameras[{i}].horizontal_fov 不是数字。"
) from None
# 视场是内参的唯一来源（gz 自己发的 camera_info 是错的，见文件头），
# 所以它必须存在且合理。填错的话三维点位会整体错，而且看不出来。
if not (0.0 < fov < math.pi):
raise CameraConfigError(
f"{path} 的 cameras[{i}].horizontal_fov = {fov} 不合理。\n"
f" 必须是 (0, π) 弧度之间的值，例如真机 D405 约 87° = 1.5184。\n"
f" 它决定内参 fx = fy = W/(2*tan(hfov/2))，填错会让反投影出的"
f"三维点整体偏移，而单目预览里完全看不出来。"
)
specs.append(
CameraSpec(
gz_ns=gz_ns,
ros_ns=ros_ns,
link_frame=link_frame,
optical_frame=optical_frame,
depth=bool(item.get("depth", True)),
horizontal_fov=fov,
world_model=str(item.get("world_model", "") or "").strip(),
world_link=str(item.get("world_link", "") or "link").strip() or "link",
width=int(item.get("width", 0) or 0),
height=int(item.get("height", 0) or 0),
)
)
return specs
def intrinsics(spec: CameraSpec, width: int, height: int) -> Tuple[List[float], List[float]]:
"""由视场与图像尺寸算出 (K, P)。返回扁平列表，可直接塞进 CameraInfo。
=========================================================================
为什么必须自己算，而不是用 gz 发的那份 camera_info
=========================================================================
本机实测（Fortress 6.16 / RoboStack）：gz 给 rgbd_camera 发的 camera_info
**恒为** fx=fy=277, cx=160, cy=120 —— 无论 SDF 里怎么配。这组值对应 gz 的
默认参数：60° 视场 + 320x240 图幅。
而它**渲染**用的却是 SDF 里配的 87° 视场与 640x480。证据是：用已知地面平面
对深度反投影做最小二乘，解出 fy=337.2、cy=239.5，与
640 / (2*tan(1.5184/2)) = 337.2 480/2 = 240
完全吻合（残差中位 0.2mm）。也就是说渲染是对的，只有 camera_info 是默认值。
后果有多严重：照抄那份 camera_info 反投影，D435 看到的地面会被算到
z ≈ -0.49m（真实是 0），偏差半米量级。而单目预览一切正常 ——
这类错误只有在用三维点位时才暴露，恰好是本项目装配任务的核心输入。
因此这里按"渲染实际用的视场"重算，并覆盖 gz 的 camera_info。
像素是方的（gz 由同一视场与宽高比推出两个方向），所以 fx = fy。
⚠️ 相应地，horizontal_fov 必须与 SDF 里真正渲染用的值一致。
它有两个来源，启动时会自动比对并告警（check_fov_consistency）。
"""
if not (0.0 < spec.horizontal_fov < math.pi):
raise CameraConfigError(
f"相机 {spec.ros_ns} 的 horizontal_fov={spec.horizontal_fov} 不合理，"
f"无法算内参。"
)
if width <= 0 or height <= 0:
raise CameraConfigError(
f"相机 {spec.ros_ns} 的图像尺寸非法（{width}x{height}），无法算内参。"
)
fx = width / (2.0 * math.tan(spec.horizontal_fov / 2.0))
fy = fx # 方形像素
cx = width / 2.0
cy = height / 2.0
k = [fx, 0.0, cx,
0.0, fy, cy,
0.0, 0.0, 1.0]
# P 是 3x4 的投影矩阵。gz 不模拟畸变，所以 R 是单位阵、最后一列为 0。
p = [fx, 0.0, cx, 0.0,
0.0, fy, cy, 0.0,
0.0, 0.0, 1.0, 0.0]
return k, p
def check_fov_consistency(
specs: Sequence[CameraSpec],
world_file: Optional[Path] = None,
sensors_xacro: Optional[Path] = None,
) -> List[str]:
"""比对 cameras.yaml 里声明的视场与 SDF 里真正渲染用的视场。
为什么值得做这个检查：内参由 cameras.yaml 的 horizontal_fov 算出，
而**渲染**用的是 SDF 里的值。两者一旦不一致，图像与内参就对不上了 ——
症状仍然是"三维点整体错"，而单目预览看不出来。这正是这个模块开头
那一长串说明所警告的那类错误，所以要主动比对。
返回人类可读的告警列表（空 = 没问题）。
精度说明（有意为之，不做过度解析）：
- 世界相机：按 <sensor> 里的 <topic> 精确匹配到对应相机，读到的是精确值。
- 腕上相机：它在 xacro 宏里、值由参数传入，展开前无法可靠地归属到具体
相机。所以退化成"文件里出现过这个值就算过"—— 偏宽松，但足以抓住
"改了 SDF 忘了改 cameras.yaml"这种最常见的漂移。
"""
warnings: List[str] = []
if world_file is not None and Path(world_file).is_file():
text = Path(world_file).read_text(encoding="utf-8", errors="replace")
for s in specs:
if not s.is_world_mounted:
continue
# 找到 <topic> 等于本相机 gz_ns 的那个 <sensor> 块
fov = None
for m in re.finditer(r"<sensor\b[^>]*>(.*?)</sensor>", text, re.S):
body = m.group(1)
tm = re.search(r"<topic>\s*([^<\s]+)\s*</topic>", body)
if not tm or "/" + tm.group(1).strip().lstrip("/") != s.gz_ns:
continue
fm = re.search(r"<horizontal_fov>\s*([0-9.eE+-]+)\s*</horizontal_fov>", body)
if fm:
fov = float(fm.group(1))
break
if fov is None:
warnings.append(
f"相机 {s.ros_ns}: 在 {Path(world_file).name} 里找不到 topic 为 "
f"{s.gz_ns} 的 <sensor> 的 <horizontal_fov>，无法比对。"
f"内参将按 cameras.yaml 的 {s.horizontal_fov} 计算 —— "
f"若与渲染实际用的视场不符，三维点位会整体错。"
)
elif abs(fov - s.horizontal_fov) > 1e-4:
warnings.append(
f"相机 {s.ros_ns}: 视场不一致！cameras.yaml 写 {s.horizontal_fov}，"
f"而 {Path(world_file).name} 里渲染用的是 {fov}。\n"
f" 内参按前者算、图像按后者渲染 → 三维点位会整体错，"
f"而单目预览看不出来。请把两处改成一致。"
)
if sensors_xacro is not None and Path(sensors_xacro).is_file():
text = Path(sensors_xacro).read_text(encoding="utf-8", errors="replace")
found = [float(v) for v in re.findall(r"<horizontal_fov>\s*([0-9.eE+-]+)", text)]
for s in specs:
if s.is_world_mounted:
continue
if not found:
warnings.append(
f"相机 {s.ros_ns}: 在 {Path(sensors_xacro).name} 里没找到 "
f"<horizontal_fov>，无法比对（内参按 cameras.yaml 的 "
f"{s.horizontal_fov} 计算）。"
)
elif not any(abs(v - s.horizontal_fov) <= 1e-4 for v in found):
warnings.append(
f"相机 {s.ros_ns}: 视场可能不一致。cameras.yaml 写 "
f"{s.horizontal_fov}，而 {Path(sensors_xacro).name} 里出现的值是 "
f"{found}。若渲染用的不是 {s.horizontal_fov}，三维点位会整体错。"
)
return warnings
def bridge_args(specs: Sequence[CameraSpec]) -> List[str]:
"""生成 ros_gz_bridge parameter_bridge 的话题规则。
只做**传输**：gz 消息类型 → ROS 消息类型，话题名保持 gz 侧的原样。
改名与格式换算交给适配器 —— 分开的原因是桥接器改不了 frame_id，
而 frame_id 恰恰是最容易错又最不报错的那一项。让适配器统一处理
"名字 + 坐标系 + 单位"，逻辑集中在一处。
注：/d405/image 这类"gz 原名"话题在 ROS 图里也会可见。
这是有意保留的：排查"到底是桥接断了还是适配器断了"时，
直接 echo 原名话题能一刀切开。
"""
args: List[str] = []
for s in specs:
args.append(f"{s.gz_image}@sensor_msgs/msg/Image[gz.msgs.Image")
args.append(f"{s.gz_info}@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo")
if s.depth:
# gz 的深度是 float32（本机实测 4 字节/像素），桥接器支持为 32FC1。
# 若哪天桥接器不再支持，这里会静默少一个话题 —— 验收时用
# `ros2 topic list | grep aligned_depth` 确认它在。
args.append(f"{s.gz_depth}@sensor_msgs/msg/Image[gz.msgs.Image")
return args
def depth_m_to_mm16(depth_m: np.ndarray, max_mm: int = MAX_MM) -> np.ndarray:
"""深度：32FC1 米 → 16UC1 毫米。0 表示"无读数"（与真机 realsense 一致）。
规则（每一条都对应一种真机会出现的情况）：
非有限值（nan / inf） → 0。gz 在没命中时可能给 inf，
真机 realsense 给 0；统一成 0。
<= 0 → 0。真机对无效像素就是给 0，
负深度没有物理意义。
0 < d <= max_mm/1000 → round(d*1000)
> max_mm/1000（65.535m） → 0（见 MAX_MM 的说明：不饱和）
舍入后为 0 的极小正数 → 0（例如 0.0001m，落不进 uint16）
返回与输入同形状的 uint16 数组。
"""
a = np.asarray(depth_m)
if a.dtype != np.float32:
a = a.astype(np.float32, copy=False)
out = np.zeros(a.shape, dtype=np.uint16)
finite = np.isfinite(a)
if not finite.any():
return out
# 只对有限且为正的像素做换算；先把它们挑出来，避免对 nan 做比较
# （nan 参与的比较全是 False，但显式挑出来意图更清楚，也少一遍遍历）
cand = finite & (a > 0.0)
if not cand.any():
return out
mm = np.rint(a[cand].astype(np.float64) * 1000.0)
ok = (mm > 0.0) & (mm <= float(max_mm))
if ok.any():
out[cand] = np.where(ok, mm, 0.0).astype(np.uint16)
return out
def optical_rpy() -> Tuple[float, float, float]:
"""本体 → 光学系的固定旋转，供静态 TF 使用。"""
return OPTICAL_RPY
def rpy_to_quat(rpy: Sequence[float]) -> Tuple[float, float, float, float]:
"""(roll, pitch, yaw) → (x, y, z, w)。只用于发布静态 TF。"""
r, p, y = (float(v) for v in rpy)
cr, sr = math.cos(r / 2.0), math.sin(r / 2.0)
cp, sp = math.cos(p / 2.0), math.sin(p / 2.0)
cy, sy = math.cos(y / 2.0), math.sin(y / 2.0)
return (
sr * cp * cy - cr * sp * sy,
cr * sp * cy + sr * cp * sy,
cr * cp * sy - sr * sp * cy,
cr * cp * cy + sr * sp * sy,
)
src/arm_sim_gazebo/arm_sim_gazebo/sim_site.py
+214
View File
@@ -0,0 +1,214 @@
"""读 `config/site.yaml` 的 `sim.*` 段。
=============================================================================
为什么需要这个模块
=============================================================================
`config/site.yaml` 的 `sim` 段（world / physics_enable / joint_prefix）与
`devices/sim/gazebo/manifest.yaml` 的 `config_schema` 一直宣称"改这里就生效"，
但**没有任何代码读它**：world 路径硬编码在 robot_description._WORLD_REL 里。
于是产生最难受的一类 bug：用户按文档改了 `site.yaml`，启动后行为毫无变化，
也不报错。既然文档与 manifest 都把它当契约，就必须真的读它。
要读，但也不能假装所有键都接线了 —— 有些键（physics_enable / joint_prefix）
目前确实还没接。所以本模块除了读值，还提供 unwired_warnings()：
把"声明了、但当前实现并不消费"的差异显式报出来，
把静默无效变成一条看得见的告警。等哪天真的接线了，告警自然消失。
=============================================================================
"""
from __future__ import annotations
import re
from dataclasses import dataclass, field
from pathlib import Path
from typing import List, Optional
import yaml
SITE_REL = "config/site.yaml"
SITE_EXAMPLE_REL = "config/site.example.yaml"
# 与 manifest.yaml 的 config_schema 默认值保持一致。
# 三处（site.yaml 注释 / manifest / 这里）都写了默认值，改动时必须同步 ——
# 所以下面有用例盯着这里和 manifest 的一致性。
DEFAULT_WORLD = ""
DEFAULT_PHYSICS_ENABLE = True
DEFAULT_JOINT_PREFIX = "sim_"
class SimSiteError(RuntimeError):
"""site.yaml 读不了或内容不合法。"""
@dataclass
class SimSite:
"""`sim.*` 段的解析结果。"""
world: str = DEFAULT_WORLD
physics_enable: bool = DEFAULT_PHYSICS_ENABLE
joint_prefix: str = DEFAULT_JOINT_PREFIX
# 这些键在 site.yaml 里**显式**出现过吗？
# 用来区分"用户说了要关物理"和"用户没提，所以用默认值"——
# 前者值得告警，后者没啥可说。
present: set = field(default_factory=set)
source: Optional[Path] = None
def load_sim_site(root: Optional[Path] = None) -> SimSite:
"""读 `config/site.yaml` 的 sim 段。文件不存在则返回全默认值。
故意**不**在文件缺失时报错：仿真要能在没有站点的机器上跑起来
（site.yaml 里含真实设备 ID，本来就不该提交，见 README 的"要提交什么"）。
"""
base = Path(root) if root is not None else repo_root()
path = base / SITE_REL
if not path.is_file():
return SimSite()
try:
doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
except yaml.YAMLError as exc:
raise SimSiteError(
f"{path} 解析失败: {exc}\n"
f" 这一行会让仿真启动不了。修好它，或临时改名为 site.yaml.bak 忽略。"
) from exc
if not isinstance(doc, dict):
raise SimSiteError(f"{path} 的顶层不是映射（键值对）。")
raw = doc.get("sim")
if raw is None:
return SimSite(source=path)
if not isinstance(raw, dict):
raise SimSiteError(
f"{path} 的 sim 段不是映射（键值对），而是 {type(raw).__name__}。"
f"它应该像这样：\n sim:\n world: devices/sim/gazebo/worlds/assembly.world"
)
site = SimSite(source=path)
if "world" in raw:
site.world = str(raw.get("world") or "").strip()
site.present.add("world")
if "physics_enable" in raw:
site.physics_enable = bool(raw.get("physics_enable"))
site.present.add("physics_enable")
if "joint_prefix" in raw:
site.joint_prefix = str(raw.get("joint_prefix") or "").strip()
site.present.add("joint_prefix")
return site
def repo_root() -> Path:
"""仓库根。本模块在 src/arm_sim_gazebo/arm_sim_gazebo/ 下，向上走四层。"""
return Path(__file__).resolve().parents[3]
def resolve_world(
cli_world: str = "",
root: Optional[Path] = None,
fallback: Optional[Path] = None,
) -> tuple[Path, str]:
"""决定用哪个 world 文件。返回 (路径, 来源说明)。
优先级：命令行 --world > site.yaml 的 sim.world > 内置默认。
为什么要有优先级而不是"只认 site.yaml"：site.yaml 是站点级的，而调试时
经常要临时换一个世界（比如 in.sdf 里复制一份改着试）。命令行必须能压过它。
"""
base = Path(root) if root is not None else repo_root()
if cli_world and cli_world.strip():
p = Path(cli_world.strip())
# 相对路径按仓库根解析：用户从任意目录启动，写相对路径的直觉都是"相对仓库"。
if not p.is_absolute():
p = base / p
return p, "命令行 --world"
site = load_sim_site(base)
if site.world:
p = Path(site.world)
if not p.is_absolute():
p = base / p
return p, f"{SITE_REL} 的 sim.world"
if fallback is None:
raise SimSiteError(
"没能确定 world 文件：site.yaml 里 sim.world 为空，也没给内置默认。"
)
fb = Path(fallback)
# 内置默认也要跟前面两条一样解析成绝对路径：调用方（启动器 / launch）
# 拿到的必须是绝对路径，否则从别的目录启动时会被解析到错误的位置，
# 而那会表现成"找不到 world 文件"，看起来像配置写错了。
if not fb.is_absolute():
fb = base / fb
return fb, "内置默认（site.yaml 未指定 sim.world）"
def unwired_warnings(
site: SimSite,
actual_joint_prefix: Optional[str] = None,
physics_in_world: Optional[bool] = None,
) -> List[str]:
"""把"site.yaml 里声明了、但当前实现并不消费"的配置报出来。
这是本模块存在的一半理由：与其让 `physics_enable: false` 静默无效，
不如明说"这个键还没接线"。下面两个键目前确实没接：
physics_enable 世界文件里的重力/步长是写死的，没有按它开关。
joint_prefix 仿真关节前缀由 arm.sim.urdf.xacro 的 arm_prefix 决定，
且 controllers.yaml 里硬编码了 sim_joint1..7；
改 site.yaml 不会传导过去。
等它们真的接线了，从这里删掉对应分支，告警就自然消失。
"""
out: List[str] = []
if "physics_enable" in site.present and physics_in_world is not None:
want_on = bool(site.physics_enable)
if want_on != bool(physics_in_world):
out.append(
f"site.yaml 写了 sim.physics_enable={site.physics_enable}，"
f"但世界文件当前是物理{'开启' if physics_in_world else '关闭'}状态，"
f"而这个键**还没有接线** —— 世界文件里的重力/步长是写死的。\n"
f" 想让物理开关生效，得改 world 文件里的 <physics>，"
f"或者先把这个键接进启动流程（见 sim_site.py 顶部说明）。"
)
if "joint_prefix" in site.present and actual_joint_prefix is not None:
want = site.joint_prefix
if want and want != actual_joint_prefix:
out.append(
f"site.yaml 写了 sim.joint_prefix={want!r}，"
f"但实际用的前缀是 {actual_joint_prefix!r}，"
f"而这个键**还没有接线**。\n"
f" 前缀来自 arm.sim.urdf.xacro 的 arm_prefix，"
f"且 config/controllers.yaml 里硬编码了 {actual_joint_prefix}joint1..7 —— "
f"三处必须一致，改一处不够。"
)
return out
def physics_enabled_in_world(world_file: Path) -> Optional[bool]:
"""世界文件里的物理到底开没开。判据是重力是不是零向量。
gz 里"关物理"的常见做法就是把 <gravity> 置零（比删掉整个 <physics>
更常用，因为 <physics> 还带着步长等其它必需项）。没有 <physics> /
<gravity> 时按"开着"算 —— gz 的默认就是开。
"""
try:
text = Path(world_file).read_text(encoding="utf-8", errors="replace")
except OSError:
return None
m = re.search(r"<gravity>\s*([^<]+)\s*</gravity>", text)
if not m:
return True
try:
vals = [float(x) for x in m.group(1).split()]
except ValueError:
return None
if len(vals) != 3:
return None
return any(abs(v) > 1e-12 for v in vals)
src/arm_sim_gazebo/launch/gazebo_sim.launch.py
+136 -5
View File
@@ -76,10 +76,12 @@ def _prepare(context, *args, **kwargs):
default_controllers,
default_world,
default_xacro,
model_link_pose,
prepare_urdf,
repo_root,
world_name,
)
from arm_sim_gazebo.sim_site import SimSiteError, resolve_world
world = LaunchConfiguration("world").perform(context).strip()
load_gripper = LaunchConfiguration("load_gripper").perform(context).strip().lower()
@@ -89,7 +91,10 @@ def _prepare(context, *args, **kwargs):
try:
root = repo_root()
world_path = world or str(default_world(root))
# 与 gazebo_sim_launcher 用同一套优先级，直接 `ros2 launch` 的人也不会
# 遇到"改了 site.yaml 没反应"：--world > site.yaml 的 sim.world > 内置默认。
world_path, _world_origin = resolve_world(world, root, fallback=default_world(root))
world_path = str(world_path)
urdf, report = prepare_urdf(
xacro_path=default_xacro(root),
@@ -99,7 +104,7 @@ def _prepare(context, *args, **kwargs):
)
wname = world_name(world_path)
missing_base = check_base_plugins(Path(world_path))
except SimDescriptionError as exc:
except (SimDescriptionError, SimSiteError) as exc:
# 描述准备失败是最常见的上手障碍（环境没 source、路径不对）。
# 直接把可读原因打出来，而不是让它变成一个看不懂的 launch traceback。
raise RuntimeError(f"\n[gazebo_sim] 仿真描述准备失败：\n{exc}\n") from exc
@@ -179,7 +184,123 @@ def _prepare(context, *args, **kwargs):
)
)
# --- 4) spawn 机器人 -----------------------------------------------------
# --- 4) 相机：桥接 → 适配器 → （世界相机）补 TF --------------------------
# 只在挂了传感器时才做。--no-sensors 时跳过是必要的：适配器会去订阅
# 永远不存在的 gz 话题，它**不会报错**，只是静默没输出 —— 而"相机没图"
# 正是最需要快速定位的问题，不该被一个假故障混淆。
if with_sensors != "false":
from arm_sim_gazebo.sim_cameras import (
DEFAULT_CAMERAS_REL,
CameraConfigError,
bridge_args,
check_fov_consistency,
load_cameras,
rpy_to_quat,
)
try:
specs = load_cameras(root=root)
except CameraConfigError as exc:
raise RuntimeError(f"\n[gazebo_sim] 仿真相机清单有问题：\n{exc}\n") from exc
# 视场一致性：内参由 cameras.yaml 的 horizontal_fov 算，而**渲染**用的是
# SDF 里的值。两者不一致时图像与内参对不上，三维点位会整体错 ——
# 而单目预览完全看不出来（gz 自己发的 camera_info 本来就是错的，
# 所以这份内参是"唯一真相"，更要保证它与渲染一致）。
# 这里只告警不拦截：真有偏差时，仍应能起仿真去现场比对。
_fov_issues = check_fov_consistency(
specs,
world_file=Path(world_path),
sensors_xacro=Path(root)
/ "devices/sim/gazebo/urdf/arm.sim.sensors.xacro",
)
# ⚠ 这里用 LogInfo 而不是 LogWarn：Humble 的 launch.actions 里没有
# LogWarn（Iron 才加），照抄新版本会直接 ImportError 让整个 launch
# 加载失败 —— 一个"只是想说句话"的动作把仿真起不来，代价太大。
# 前缀里带 ⚠ 保证它在满屏日志里仍然显眼。
for _w in _fov_issues:
actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ {_w}"))
# 4a) 世界相机的 TF 兜底
#
# Gazebo **不会**为 world 里的 <model> 产生 ROS TF —— TF 只来自
# robot_state_publisher（也就是只覆盖 URDF 里的东西）。
# D435 正属于这一类：它的物理归属是"世界"（固定支架），不在 URDF 里
# （早期挂在 URDF 的 world link 上时，被 sdformat 连整棵子树静默丢掉）。
# 少了这条 TF，应用 tf2 查询直接报 "frame does not exist" ——
# 而图像本身完全正常，很容易误判成相机坏了。
#
# 位姿从 world 文件**解析**，不在这里重复配置：唯一真相就是 world 文件。
# 另配一份等于制造"改了 world 忘了改这里"，而症状是相机看到的东西
# 与它自称的位置对不上，极难查。
world_tf = []
for s in specs:
if not s.is_world_mounted:
continue
try:
(px, py, pz), (rr, rp, ry) = model_link_pose(
Path(world_path), s.world_model, s.world_link
)
except SimDescriptionError as exc:
raise RuntimeError(
f"\n[gazebo_sim] 世界相机 {s.ros_ns} 的 TF 补不出来：\n{exc}\n"
) from exc
qx, qy, qz, qw = rpy_to_quat((rr, rp, ry))
world_tf.append(
Node(
package="tf2_ros",
executable="static_transform_publisher",
name=f"tf_{s.ros_ns}",
output="screen",
arguments=[
"--x", f"{px}", "--y", f"{py}", "--z", f"{pz}",
"--qx", f"{qx}", "--qy", f"{qy}", "--qz", f"{qz}", "--qw", f"{qw}",
"--frame-id", "world",
"--child-frame-id", s.link_frame,
],
parameters=[{"use_sim_time": True}],
)
)
actions.append(
LogInfo(
msg=f"[gazebo_sim] 世界相机 {s.ros_ns}: world → {s.link_frame} "
f"@ ({px:.3f}, {py:.3f}, {pz:.3f}) 由 world 文件解析得到"
)
)
if world_tf:
# 延后一点再发：/clock 还没起来时发布静态 TF 可能被 tf2 判成
# "未来时间戳"，而这一类问题只在很后面才以 extrapolation 报出来。
actions.append(TimerAction(period=2.0, actions=world_tf))
# 4b) 桥接：只做传输（gz 消息类型 → ROS 消息类型，话题名照抄）。
# 改名、坐标系、深度单位都交给 4c 的适配器 —— 桥接器改不了 frame_id，
# 而 frame_id 恰恰是最容易错又最不报错的那一项。
actions.append(
Node(
package="ros_gz_bridge",
executable="parameter_bridge",
name="camera_bridge",
output="screen",
arguments=bridge_args(specs),
)
)
# 4c) 适配器：盖光学系 frame_id + 深度 32FC1 米 → 16UC1 毫米 + 改成
# 应用期望的话题名。详见 sim_camera_adapter.py 顶部。
actions.append(
Node(
package="arm_sim_gazebo",
executable="sim_camera_adapter",
name="sim_camera_adapter",
output="screen",
parameters=[
{"cameras_file": str(Path(root) / DEFAULT_CAMERAS_REL)},
{"use_sim_time": True},
],
)
)
# --- 5) spawn 机器人 -----------------------------------------------------
# -world 必须给对：world 名写错时 Gazebo **不报错**，只是实体没被创建，
# 表现成"launch 一切正常但 RViz 里什么都没有"。
actions.append(
@@ -201,7 +322,7 @@ def _prepare(context, *args, **kwargs):
)
)
# --- 5) 控制器 -----------------------------------------------------------
# --- 6) 控制器 -----------------------------------------------------------
# 用 joint_trajectory_controller 而不是直接给关节发位置：应用层统一按
# rizon_arm_controller/follow_joint_trajectory 发轨迹，这个接口名与真机一致。
controllers = [
@@ -236,8 +357,18 @@ def _prepare(context, *args, **kwargs):
" ros2 control list_controllers # 两个控制器 active\n"
" ros2 topic echo /joint_states --once # 7 个 sim_joint\n"
" ros2 run tf2_ros tf2_echo world sim_tcp # 末端位姿可查\n"
" python3 scripts/check_sim_cameras.py # ★ 相机的真正验收\n"
" 机械臂应在重力下**保持站住**。若它瘫下去或消失，说明描述有问题，"
"参考 arm_sim_gazebo/urdf_normalize.py 顶部列的两个上游缺陷。"
"参考 arm_sim_gazebo/urdf_normalize.py 顶部列的两个上游缺陷。\n"
" ★ 别用 'ros2 topic hz' 或肉眼预览来验收相机 —— 那些只能证明'有图像'。\n"
" 仿真相机的错误几乎全是静默的（深度单位差 1000 倍、frame_id 盖成\n"
" 本体系、gz 发的内参是错的、位姿 rpy 写错导致相机背对目标），\n"
" 它们全都能出图、能查到 TF。必须用 check_sim_cameras.py 反投影\n"
" 到 world、检查是否命中地面/台面，才算通过。\n"
" ⚠ 深度话题是 16UC1 毫米（已从 gz 的 32FC1 米转换过）。\n"
" 内参也不是 gz 发的那份：gz 恒发 fx=277/cx=160/cy=120（它的默认\n"
" 60° 视场 + 半分辨率主点），适配器已按 cameras.yaml 的视场重算覆盖。\n"
" 所以改相机视场要同时改 cameras.yaml 和 SDF，启动时会自动比对告警。"
)
)
)
src/arm_sim_gazebo/setup.py
+11 -7
View File
@@ -34,13 +34,17 @@ setup(
license='Apache-2.0',
entry_points={
'console_scripts': [
# devices/sim/gazebo/manifest.yaml 里声明的那个可执行名，别改。
# 它只做"准备描述 + 转发给 launch 文件"，逻辑在
# arm_sim_gazebo/gazebo_sim_launcher.py。
'gazebo_sim_launcher = arm_sim_gazebo.gazebo_sim_launcher:main',
# 独立小工具：只看归一化做了什么，不启动 Gazebo。
# 排查"为什么 Gazebo 说我的 link 不存在"时非常省时间。
'urdf_normalize = arm_sim_gazebo.urdf_normalize:main',
# devices/sim/gazebo/manifest.yaml 里声明的那个可执行名，别改。
# 它只做"准备描述 + 转发给 launch 文件"，逻辑在
# arm_sim_gazebo/gazebo_sim_launcher.py。
'gazebo_sim_launcher = arm_sim_gazebo.gazebo_sim_launcher:main',
# 仿真相机适配器：把桥接过来的 gz 相机变成应用期望的 realsense
# 话题（改名 + 光学系 frame_id + 深度 米→毫米）。
# 由 gazebo_sim.launch.py 拉起，一般不手跑；单独调试时可以用。
'sim_camera_adapter = arm_sim_gazebo.sim_camera_adapter:main',
# 独立小工具：只看归一化做了什么，不启动 Gazebo。
# 排查"为什么 Gazebo 说我的 link 不存在"时非常省时间。
'urdf_normalize = arm_sim_gazebo.urdf_normalize:main',
],
},
)
src/arm_sim_gazebo/test/test_robot_description.py
+216
View File
@@ -21,6 +21,7 @@ from arm_sim_gazebo.robot_description import ( # noqa: E402
check_base_plugins,
default_world,
expand_xacro,
model_link_pose,
prepare_urdf,
repo_root,
world_name,
@@ -262,3 +263,218 @@ def test_prepare_urdf_missing_controllers_is_actionable(tmp_path):
with pytest.raises(SimDescriptionError) as ei:
prepare_urdf(controllers_file=tmp_path / "nope.yaml")
assert "控制器配置不存在" in str(ei.value)
# ---------------------------------------------------------------------------
# 从 world 文件解析模型位姿（世界相机的 TF 靠它）
# ---------------------------------------------------------------------------
# 这组测试存在的理由：Gazebo 不为 world 里的 <model> 产生 ROS TF，
# 而 D435 这类固定相机就属于这种情况。位姿靠解析而不是重复配置，
# 因此"解析对不对"直接决定了相机看到的东西和它自称的位置是否一致。
# 解析错了不会报错，只会让三维点整体偏移或转错。
def _world_file(tmp_path, body: str) -> Path:
p = tmp_path / "w.world"
p.write_text(f'<?xml version="1.0"?>\n<sdf version="1.9">\n{body}\n</sdf>\n', encoding="utf-8")
return p
def test_model_pose_identity_link(tmp_path):
"""link 没有自己的 pose 时，位姿就是模型位姿。"""
w = _world_file(
tmp_path,
"""
<world name="w">
<model name="d435">
<static>true</static>
<pose>-0.45 0.0 1.35 0.0 0.872665 3.14159</pose>
<link name="link">
<sensor name="cam" type="rgbd_camera">
<pose>0.1 0 0 0 0 0</pose>
</sensor>
</link>
</model>
</world>
""",
)
(pos, rpy) = model_link_pose(w, "d435", "link")
assert pos == pytest.approx((-0.45, 0.0, 1.35))
assert rpy == pytest.approx((0.0, 0.872665, 3.14159))
def test_link_own_pose_is_composed(tmp_path):
"""SDF 的 <pose> 是相对父元素的，模型位姿与 link 位姿必须复合。"""
w = _world_file(
tmp_path,
"""
<world name="w">
<model name="m">
<pose>1 0 0 0 0 0</pose>
<link name="l">
<pose>0 1 0 0 0 0</pose>
<visual name="v"/>
</link>
</model>
</world>
""",
)
pos, _ = model_link_pose(w, "m", "l")
assert pos == pytest.approx((1.0, 1.0, 0.0))
def test_link_translation_is_rotated_by_model_rotation(tmp_path):
"""模型绕 Z 转 90° 时，link 的 (1,0,0) 应落到世界的 (0,1,0)。"""
w = _world_file(
tmp_path,
"""
<world name="w">
<model name="m">
<pose>0 0 0 0 0 1.5707963</pose>
<link name="l">
<pose>1 0 0 0 0 0</pose>
<visual name="v"/>
</link>
</model>
</world>
""",
)
pos, rpy = model_link_pose(w, "m", "l")
assert pos == pytest.approx((0.0, 1.0, 0.0), abs=1e-6)
assert rpy[2] == pytest.approx(1.5707963, abs=1e-6)
def test_link_own_pose_ignores_child_visual_pose(tmp_path):
"""link 自己的 pose 必须在第一个 <visual>/<collision>/<sensor> 之前取，
否则会把子元素的 pose 误当成 link 的。"""
w = _world_file(
tmp_path,
"""
<world name="w">
<model name="m">
<pose>2 2 2 0 0 0</pose>
<link name="l">
<pose>1 1 1 0 0 0</pose>
<visual name="v">
<pose>9 9 9 0 0 0</pose>
</visual>
</link>
</model>
</world>
""",
)
pos, _ = model_link_pose(w, "m", "l")
assert pos == pytest.approx((3.0, 3.0, 3.0)) # 不是 2+9
def test_missing_pose_defaults_to_identity(tmp_path):
w = _world_file(
tmp_path,
"""
<world name="w">
<model name="m">
<link name="l"><visual name="v"/></link>
</model>
</world>
""",
)
pos, rpy = model_link_pose(w, "m", "l")
assert pos == pytest.approx((0.0, 0.0, 0.0))
assert rpy == pytest.approx((0.0, 0.0, 0.0))
def test_model_only_query(tmp_path):
"""link 留空 = 取模型本身位姿。"""
w = _world_file(
tmp_path,
"""
<world name="w">
<model name="m"><pose>5 6 7 0 0 0</pose><link name="l"/></model>
</world>
""",
)
pos, _ = model_link_pose(w, "m")
assert pos == pytest.approx((5.0, 6.0, 7.0))
def test_single_quotes_accepted(tmp_path):
"""手写 world 时单引号也会出现，两种都要认。"""
w = _world_file(
tmp_path,
"""
<world name='w'>
<model name='m'><pose>1 2 3 0 0 0</pose><link name='l'/></model>
</world>
""",
)
pos, _ = model_link_pose(w, "m", "l")
assert pos == pytest.approx((1.0, 2.0, 3.0))
def test_nested_model_does_not_break_extraction(tmp_path):
"""<model> 可以嵌套 <model>（组合体）。找第一个 </model> 会截断错位，
所以 _find_block 要数嵌套深度。"""
w = _world_file(
tmp_path,
"""
<world name="w">
<model name="outer">
<pose>0 0 0 0 0 0</pose>
<model name="inner">
<pose>9 9 9 0 0 0</pose>
<link name="l"><visual name="v"/></link>
</model>
<link name="outer_link"><pose>1 0 0 0 0 0</pose><visual name="v"/></link>
</model>
</world>
""",
)
pos, _ = model_link_pose(w, "outer", "outer_link")
assert pos == pytest.approx((1.0, 0.0, 0.0))
def test_missing_model_error_mentions_cameras_yaml(tmp_path):
"""报错要指向 cameras.yaml 的 world_model 字段 —— 配错时症状是
'tf 查询报 frame does not exist'，与相机本身无关，容易查错方向。"""
w = _world_file(tmp_path, '<world name="w"><model name="other"/></world>')
with pytest.raises(SimDescriptionError) as ei:
model_link_pose(w, "d435", "link")
assert "d435" in str(ei.value)
assert "cameras.yaml" in str(ei.value)
def test_missing_link_error_is_clear(tmp_path):
w = _world_file(tmp_path, '<world name="w"><model name="m"><link name="other"/></model></world>')
with pytest.raises(SimDescriptionError) as ei:
model_link_pose(w, "m", "link")
assert "link" in str(ei.value)
def test_real_assembly_world_d435_pose():
"""仓库里那份 assembly.world 的 d435 位姿必须与文件里写的一致。
这条是"防回归"的：d435 的 TF 完全依赖这个解析结果，
位姿解析悄悄出错不会报任何错，只会让相机看到的东西与自称位置不符。
"""
root = None
for cand in [Path.cwd(), Path.cwd().parent, Path(__file__).resolve().parent]:
cur = cand.resolve()
for _ in range(8):
if (cur / "devices/sim/gazebo/urdf/arm.sim.urdf.xacro").is_file():
root = cur
break
cur = cur.parent
if root:
break
if root is None:
pytest.skip("找不到仓库根")
world = root / "devices/sim/gazebo/worlds/assembly.world"
if not world.is_file():
pytest.skip("assembly.world 不存在")
pos, rpy = model_link_pose(world, "d435", "link")
assert pos == pytest.approx((-0.45, 0.0, 1.35), abs=1e-6)
# yaw 必须是 0：曾经写成 3.14159，把视线从"+X 俯视"翻成"-X 俯视"，
# 相机于是背对台面、只看得见地面。这条断言就是防止它被改回去。
assert rpy == pytest.approx((0.0, 0.872665, 0.0), abs=1e-6)
src/arm_sim_gazebo/test/test_sim_cameras.py
+407
View File
@@ -0,0 +1,407 @@
"""sim_cameras 的单测。
重点在深度换算：米 → 毫米差 1000 倍，而单目预览里**看不出任何异常**，
只能靠测试兜住。另一类是配置错误（ros_ns 重复、缺必填键），
它们会让两台相机互相覆盖话题，症状是"只有一台出图"，也很难查。
"""
import math
import numpy as np
import pytest
from arm_sim_gazebo.sim_cameras import (
MAX_MM,
CameraConfigError,
CameraSpec,
bridge_args,
depth_m_to_mm16,
load_cameras,
rpy_to_quat,
)
# --------------------------------------------------------------------- 辅助
def _write(tmp_path, text):
p = tmp_path / "cameras.yaml"
p.write_text(text, encoding="utf-8")
return p
MINIMAL = """
cameras:
- gz_ns: /d405
ros_ns: d405
link_frame: sim_d405_link
optical_frame: sim_d405_color_optical_frame
horizontal_fov: 1.5184
"""
# --------------------------------------------------------- 话题名派生
def test_topic_names_follow_realsense_convention():
"""话题名必须与真机 realsense 完全一致，否则应用要为仿真改代码。"""
s = CameraSpec(
gz_ns="/d405",
ros_ns="d405",
link_frame="sim_d405_link",
optical_frame="sim_d405_color_optical_frame",
)
assert s.ros_color == "/d405/color/image_raw"
assert s.ros_info == "/d405/color/camera_info"
assert s.ros_depth == "/d405/aligned_depth_to_color/image_raw"
assert s.gz_image == "/d405/image"
assert s.gz_depth == "/d405/depth_image"
assert s.gz_info == "/d405/camera_info"
def test_gz_ns_normalized_without_double_slash():
s = CameraSpec(gz_ns="/d405", ros_ns="d405", link_frame="l", optical_frame="o")
assert "//" not in s.gz_image
# --------------------------------------------------------- 配置加载
def test_load_minimal_config(tmp_path):
specs = load_cameras(_write(tmp_path, MINIMAL))
assert len(specs) == 1
assert specs[0].ros_ns == "d405"
assert specs[0].depth is True # 默认开深度
assert specs[0].is_world_mounted is False
def test_load_missing_file_raises_with_hint(tmp_path):
with pytest.raises(CameraConfigError) as e:
load_cameras(tmp_path / "nope.yaml")
# 报错必须给出修法，而不是一句 FileNotFoundError
assert "cameras.yaml" in str(e.value)
def test_load_empty_list_raises(tmp_path):
with pytest.raises(CameraConfigError) as e:
load_cameras(_write(tmp_path, "cameras: []\n"))
assert "cameras" in str(e.value)
def test_load_missing_required_key_raises(tmp_path):
bad = """
cameras:
- gz_ns: /d405
ros_ns: d405
link_frame: sim_d405_link
"""
with pytest.raises(CameraConfigError) as e:
load_cameras(_write(tmp_path, bad))
assert "optical_frame" in str(e.value)
def test_duplicate_ros_ns_rejected(tmp_path):
"""两台相机用同一个 ros_ns 会互相覆盖话题，表现为'只有一台出图'。"""
dup = """
cameras:
- gz_ns: /d405
ros_ns: d405
link_frame: a
optical_frame: b
horizontal_fov: 1.5184
- gz_ns: /d406
ros_ns: d405
link_frame: c
optical_frame: d
horizontal_fov: 1.5184
"""
with pytest.raises(CameraConfigError) as e:
load_cameras(_write(tmp_path, dup))
assert "重复" in str(e.value)
def test_world_mounted_flag(tmp_path):
cfg = """
cameras:
- gz_ns: /d435
ros_ns: d435
link_frame: d435_link
optical_frame: d435_color_optical_frame
horizontal_fov: 1.5184
world_model: d435
world_link: link
"""
specs = load_cameras(_write(tmp_path, cfg))
assert specs[0].is_world_mounted is True
assert specs[0].world_link == "link"
# --------------------------------------------------------- 深度换算
def test_depth_basic_conversion():
"""1.5m → 1500mm。这是最容易错的那一项（差 1000 倍）。"""
d = np.array([[1.5, 0.25]], dtype=np.float32)
mm = depth_m_to_mm16(d)
assert mm.dtype == np.uint16
assert mm.tolist() == [[1500, 250]]
def test_depth_rounding():
d = np.array([1.2344, 1.2346], dtype=np.float32)
mm = depth_m_to_mm16(d)
assert mm.tolist() == [1234, 1235]
def test_depth_non_finite_becomes_zero():
"""gz 在没命中时可能给 inf，真机 realsense 给 0 —— 统一成 0。"""
d = np.array([np.inf, -np.inf, np.nan], dtype=np.float32)
assert depth_m_to_mm16(d).tolist() == [0, 0, 0]
def test_depth_zero_and_negative_become_zero():
d = np.array([0.0, -0.5], dtype=np.float32)
assert depth_m_to_mm16(d).tolist() == [0, 0]
def test_depth_too_small_rounds_to_zero():
"""0.0001m 舍入后是 0mm —— 必须落到'无读数'，不能变成 1 或负数回绕。"""
d = np.array([0.0001, 0.0004, 0.0006], dtype=np.float32)
assert depth_m_to_mm16(d).tolist() == [0, 0, 1]
def test_depth_beyond_uint16_becomes_zero_not_saturated():
"""超出 65.535m 按'无读数'处理。
饱和到 65535 会让远处凭空出现一个 65.5m 的平面，比'无效'更有害 ——
下游会把它当成真实几何去用。
"""
d = np.array([70.0, 100.0, 0.0], dtype=np.float32)
assert depth_m_to_mm16(d).tolist() == [0, 0, 0]
def test_depth_exact_uint16_max_is_kept():
d = np.array([MAX_MM / 1000.0], dtype=np.float32)
assert depth_m_to_mm16(d).tolist() == [MAX_MM]
def test_depth_shape_preserved():
d = np.full((480, 640), 1.0, dtype=np.float32)
mm = depth_m_to_mm16(d)
assert mm.shape == (480, 640)
assert mm.dtype == np.uint16
assert (mm == 1000).all()
def test_depth_accepts_float64_input():
"""上游若给了 float64 也不能崩（gz 是 float32，但不做假设）。"""
d = np.array([2.0], dtype=np.float64)
assert depth_m_to_mm16(d).tolist() == [2000]
# --------------------------------------------------------- 桥接参数
def test_bridge_args_include_depth_when_enabled():
specs = [
CameraSpec(gz_ns="/d405", ros_ns="d405", link_frame="l", optical_frame="o", depth=True)
]
args = bridge_args(specs)
assert "/d405/image@sensor_msgs/msg/Image[gz.msgs.Image" in args
assert "/d405/camera_info@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo" in args
assert "/d405/depth_image@sensor_msgs/msg/Image[gz.msgs.Image" in args
def test_bridge_args_skip_depth_when_disabled():
specs = [
CameraSpec(gz_ns="/d405", ros_ns="d405", link_frame="l", optical_frame="o", depth=False)
]
args = bridge_args(specs)
assert not any("depth_image" in a for a in args)
assert len(args) == 2
# --------------------------------------------------------- 旋转工具
def test_rpy_to_quat_identity():
x, y, z, w = rpy_to_quat((0.0, 0.0, 0.0))
assert (x, y, z, w) == pytest.approx((0.0, 0.0, 0.0, 1.0))
def test_rpy_to_quat_is_unit_norm():
for rpy in [(-1.5708, 0.0, -1.5708), (0.0, 0.872665, 3.14159), (0.3, -0.7, 1.2)]:
x, y, z, w = rpy_to_quat(rpy)
assert np.isclose(x * x + y * y + z * z + w * w, 1.0, atol=1e-9)
def test_optical_rpy_is_the_documented_value():
"""光学系的旋转值本身也是契约：写错这一项三维点会整体转错。"""
from arm_sim_gazebo.sim_cameras import OPTICAL_RPY
assert OPTICAL_RPY == pytest.approx((-1.5708, 0.0, -1.5708))
# --------------------------------------------------------------- 内参重算
#
# 这一组是"仿真能不能用"的地基：gz 自己发的 camera_info 是错的，内参全靠
# 这里算。算错的话仿真里所有三维点位都偏，而单目预览完全正常。
def _spec(fov=1.5184, **kw):
return CameraSpec(
gz_ns="/c",
ros_ns="c",
link_frame="l",
optical_frame="o",
horizontal_fov=fov,
**kw,
)
def test_intrinsics_match_the_measured_ground_truth():
"""用逆解实测出来的值反验：87° 视场、640x480 → fx=337.2, cx=320, cy=240。
这组数是本机拿已知地面平面做最小二乘解出来的（残差中位 0.2mm），
不是从公式推的 —— 所以它能同时抓住"公式写错"和"视场理解错"。
"""
from arm_sim_gazebo.sim_cameras import intrinsics
k, p = intrinsics(_spec(), 640, 480)
assert k[0] == pytest.approx(337.2, abs=0.1) # fx
assert k[4] == pytest.approx(337.2, abs=0.1) # fy
assert k[2] == pytest.approx(320.0) # cx
assert k[5] == pytest.approx(240.0) # cy
assert p[:4] == pytest.approx([k[0], 0.0, k[2], 0.0])
assert k[1] == k[3] == k[6] == k[7] == 0.0 and k[8] == 1.0
def test_intrinsics_scales_with_fov_and_size():
"""视场变大 → 焦距变小；分辨率翻倍 → 焦距与主点同步翻倍。"""
from arm_sim_gazebo.sim_cameras import intrinsics
k_narrow, _ = intrinsics(_spec(fov=1.0), 640, 480)
k_wide, _ = intrinsics(_spec(fov=2.0), 640, 480)
assert k_narrow[0] > k_wide[0]
k_1x, _ = intrinsics(_spec(), 640, 480)
k_2x, _ = intrinsics(_spec(), 1280, 960)
assert k_2x[0] == pytest.approx(2 * k_1x[0])
assert k_2x[2] == pytest.approx(2 * k_1x[2])
assert k_2x[5] == pytest.approx(2 * k_1x[5])
def test_intrinsics_rejects_bad_fov():
from arm_sim_gazebo.sim_cameras import intrinsics
for bad in (0.0, -1.0, math.pi, 4.0):
with pytest.raises(CameraConfigError):
intrinsics(_spec(fov=bad), 640, 480)
def test_intrinsics_rejects_bad_size():
from arm_sim_gazebo.sim_cameras import intrinsics
for w, h in ((0, 480), (640, 0), (-1, 480)):
with pytest.raises(CameraConfigError):
intrinsics(_spec(), w, h)
def test_load_cameras_requires_horizontal_fov(tmp_path):
"""缺 horizontal_fov 必须报错，不能默认成某个值 —— 默认值错了看不出来。"""
p = _write(tmp_path, """
cameras:
- gz_ns: /d405
ros_ns: d405
link_frame: sim_d405_link
optical_frame: sim_d405_color_optical_frame
""")
with pytest.raises(CameraConfigError) as ei:
load_cameras(p)
assert "horizontal_fov" in str(ei.value)
def test_load_cameras_rejects_absurd_fov(tmp_path):
p = _write(tmp_path, """
cameras:
- gz_ns: /d405
ros_ns: d405
link_frame: sim_d405_link
optical_frame: sim_d405_color_optical_frame
horizontal_fov: 0
""")
with pytest.raises(CameraConfigError) as ei:
load_cameras(p)
assert "horizontal_fov" in str(ei.value)
# ----------------------------------------------------------- 视场一致性比对
def test_fov_check_flags_world_camera_mismatch(tmp_path):
"""世界相机：按 <topic> 精确匹配，能抓到 cameras.yaml 与 world 不一致。"""
from arm_sim_gazebo.sim_cameras import check_fov_consistency
world = tmp_path / "assembly.world"
world.write_text(
"""
<sdf><world name="w">
<model name="d435"><link name="link"><sensor name="s" type="rgbd_camera">
<topic>/d435</topic>
<camera><horizontal_fov>1.5184</horizontal_fov></camera>
</sensor></link></model>
</world></sdf>
""",
encoding="utf-8",
)
spec = CameraSpec(
gz_ns="/d435",
ros_ns="d435",
link_frame="d435_link",
optical_frame="d435_color_optical_frame",
horizontal_fov=1.5184,
world_model="d435",
world_link="link",
)
assert check_fov_consistency([spec], world_file=world) == []
bad = CameraSpec(**{**spec.__dict__, "horizontal_fov": 1.2})
issues = check_fov_consistency([bad], world_file=world)
assert len(issues) == 1 and "1.5184" in issues[0] and "1.2" in issues[0]
def test_fov_check_flags_urdf_camera_when_value_absent(tmp_path):
"""腕上相机：视场在 xacro 里，只能"文件里出现过才算过"，但要能抓到缺失。"""
from arm_sim_gazebo.sim_cameras import check_fov_consistency
xacro = tmp_path / "arm.sim.sensors.xacro"
xacro.write_text("<horizontal_fov>1.5184</horizontal_fov>", encoding="utf-8")
spec = _spec()
assert check_fov_consistency([spec], sensors_xacro=xacro) == []
xacro.write_text("<horizontal_fov>1.2</horizontal_fov>", encoding="utf-8")
issues = check_fov_consistency([spec], sensors_xacro=xacro)
assert len(issues) == 1 and "1.2" in issues[0]
def test_fov_check_warns_when_world_camera_sensor_missing(tmp_path):
"""world 里找不到对应 <sensor> 时要告警，而不是静默放过。"""
from arm_sim_gazebo.sim_cameras import check_fov_consistency
world = tmp_path / "assembly.world"
world.write_text("<sdf><world name='w'/></sdf>", encoding="utf-8")
spec = CameraSpec(
gz_ns="/d435",
ros_ns="d435",
link_frame="d435_link",
optical_frame="d435_color_optical_frame",
horizontal_fov=1.5184,
world_model="d435",
)
issues = check_fov_consistency([spec], world_file=world)
assert len(issues) == 1 and "找不到" in issues[0]
def test_fov_check_against_real_repo_files():
"""对仓库里真实的两份 SDF 做一次回归：现在应当一致。
这条测试的价值在于：以后谁改了 arm.sim.sensors.xacro 或 assembly.world 的
视场却忘了同步 cameras.yaml，这里就会红。内参不一致是本模块开头
警告的那类"看不出来"的错误，必须有个自动化兜底。
"""
from pathlib import Path
from arm_sim_gazebo.sim_cameras import check_fov_consistency
root = Path(__file__).resolve().parents[3]
specs = load_cameras(root=root)
issues = check_fov_consistency(
specs,
world_file=root / "devices/sim/gazebo/worlds/assembly.world",
sensors_xacro=root / "devices/sim/gazebo/urdf/arm.sim.sensors.xacro",
)
assert issues == [], "视场与 SDF 不一致：\n" + "\n".join(issues)
src/arm_sim_gazebo/test/test_sim_site.py
+172
View File
@@ -0,0 +1,172 @@
"""sim_site 的单测。
这个模块存在的理由就是"别再让 site.yaml 静默无效"，所以用例的重点是：
- 优先级真的按 --world > site.yaml > 内置默认 走；
- site.yaml 缺失/写坏时的行为是可预期的（不崩、但有清晰报错）；
- 声明了却没接线的键**必须**告警，而不是安静地忽略。
"""
from pathlib import Path
import pytest
from arm_sim_gazebo.sim_site import (
DEFAULT_JOINT_PREFIX,
SimSite,
SimSiteError,
load_sim_site,
physics_enabled_in_world,
resolve_world,
unwired_warnings,
)
def _site(tmp_path: Path, body: str) -> Path:
(tmp_path / "config").mkdir(parents=True, exist_ok=True)
(tmp_path / "config" / "site.yaml").write_text(body, encoding="utf-8")
return tmp_path
# ------------------------------------------------------------------ 读取
def test_missing_site_yaml_falls_back_to_defaults(tmp_path):
"""site.yaml 不存在不算错 —— 它含真实设备 ID，本就不该提交。"""
site = load_sim_site(tmp_path)
assert site.world == ""
assert site.physics_enable is True
assert site.joint_prefix == DEFAULT_JOINT_PREFIX
assert site.source is None
assert site.present == set()
def test_reads_sim_section(tmp_path):
root = _site(
tmp_path,
"sim:\n world: devices/sim/gazebo/worlds/assembly.world\n"
" physics_enable: false\n joint_prefix: 'sim_'\n",
)
site = load_sim_site(root)
assert site.world == "devices/sim/gazebo/worlds/assembly.world"
assert site.physics_enable is False
assert site.joint_prefix == "sim_"
assert site.present == {"world", "physics_enable", "joint_prefix"}
def test_no_sim_section_is_fine(tmp_path):
site = load_sim_site(_site(tmp_path, "robot:\n sn: X\n"))
assert site.world == ""
assert site.present == set()
def test_malformed_yaml_raises_with_hint(tmp_path):
with pytest.raises(SimSiteError) as ei:
load_sim_site(_site(tmp_path, "sim: [unclosed\n"))
assert "site.yaml" in str(ei.value)
def test_non_mapping_sim_section_raises(tmp_path):
"""sim 段写成列表属于典型手误（缩进错了），要报出来而不是当空处理。"""
with pytest.raises(SimSiteError):
load_sim_site(_site(tmp_path, "sim:\n - world\n"))
# ---------------------------------------------------------------- 优先级
def test_cli_world_beats_site_yaml(tmp_path):
root = _site(tmp_path, "sim:\n world: from_site.world\n")
p, origin = resolve_world("from_cli.world", root, fallback=Path("fallback.world"))
assert p == root / "from_cli.world"
assert "命令行" in origin
def test_site_yaml_beats_builtin_default(tmp_path):
root = _site(tmp_path, "sim:\n world: from_site.world\n")
p, origin = resolve_world("", root, fallback=Path("fallback.world"))
assert p == root / "from_site.world"
assert "sim.world" in origin
def test_builtin_default_used_when_site_silent(tmp_path):
root = _site(tmp_path, "other: 1\n")
p, origin = resolve_world("", root, fallback=Path("devices/sim/gazebo/worlds/x.world"))
assert p == root / "devices/sim/gazebo/worlds/x.world"
assert "内置默认" in origin
def test_absolute_paths_are_not_rewritten(tmp_path):
root = _site(tmp_path, "sim:\n world: from_site.world\n")
p, _ = resolve_world("/tmp/abs.world", root, fallback=Path("f.world"))
assert p == Path("/tmp/abs.world")
p2, _ = resolve_world("", tmp_path, fallback=Path("f.world"))
assert p2.is_absolute()
def test_raises_when_nothing_available(tmp_path):
with pytest.raises(SimSiteError):
resolve_world("", tmp_path, fallback=None)
# ------------------------------------------------------- 未接线键的告警
def test_unwired_joint_prefix_warns(tmp_path):
"""site.yaml 写了一个跟实际不同的前缀 → 必须告警，不能静默。"""
site = load_sim_site(_site(tmp_path, "sim:\n joint_prefix: 'robot_'\n"))
msgs = unwired_warnings(site, actual_joint_prefix="sim_")
assert len(msgs) == 1
assert "'robot_'" in msgs[0] and "'sim_'" in msgs[0]
def test_matching_joint_prefix_does_not_warn(tmp_path):
site = load_sim_site(_site(tmp_path, "sim:\n joint_prefix: 'sim_'\n"))
assert unwired_warnings(site, actual_joint_prefix="sim_") == []
def test_unset_keys_never_warn(tmp_path):
"""site.yaml 没提这些键时不该有任何告警 —— 默认值不需要解释。"""
site = load_sim_site(_site(tmp_path, "sim:\n world: w.world\n"))
assert unwired_warnings(site, actual_joint_prefix="sim_", physics_in_world=True) == []
def test_unwired_physics_warns_only_on_conflict(tmp_path):
site = load_sim_site(_site(tmp_path, "sim:\n physics_enable: false\n"))
# 世界文件里物理其实是开的 → 用户的要求没被满足，必须说
assert len(unwired_warnings(site, physics_in_world=True)) == 1
# 世界文件里物理本来就关着 → 结果与用户要求一致，没什么可说
assert unwired_warnings(site, physics_in_world=False) == []
def test_unwired_warnings_are_skipped_without_actuals():
"""拿不到"实际值"时宁可不说，也不要凭猜测告警。"""
site = SimSite(joint_prefix="robot_", physics_enable=False)
site.present = {"joint_prefix", "physics_enable"}
assert unwired_warnings(site) == []
# ------------------------------------------------- 从世界里判断物理是否开启
def test_physics_enabled_in_world(tmp_path):
on = tmp_path / "on.world"
on.write_text("<sdf><world><physics><gravity>0 0 -9.8</gravity></physics></world></sdf>")
assert physics_enabled_in_world(on) is True
off = tmp_path / "off.world"
off.write_text("<sdf><world><physics><gravity>0 0 0</gravity></physics></world></sdf>")
assert physics_enabled_in_world(off) is False
# 没写 gravity → gz 默认是开
none = tmp_path / "none.world"
none.write_text("<sdf><world/></sdf>")
assert physics_enabled_in_world(none) is True
missing = tmp_path / "nope.world"
assert physics_enabled_in_world(missing) is None
def test_real_repo_site_yaml_is_consistent():
"""对仓库真实的 site.yaml 做回归：声明的 world 必须真的存在。
这条能抓住"site.yaml 指向一个不存在的 world"——启动方会报错，
但错误发生在几十秒的 Gazebo 初始化之后，不如在这里就红。
"""
root = Path(__file__).resolve().parents[3]
site = load_sim_site(root)
if site.source is None:
pytest.skip("本机没有 config/site.yaml（属于正常情况）")
if site.world:
assert (root / site.world).is_file(), f"site.yaml 的 sim.world 不存在: {site.world}"