机器人时间链路---工程流程示例

本工程实例采用 Ubuntu + linuxptp + ROS 2 + GigE/USB3 工业相机 ，网口用 enp3s0 举例，相机触发线用 Line1 举例。

核心原则：

header.stamp 必须表示"曝光开始 / FrameStart 对应的采集时刻"，且所有 ROS 2 消息都放在同一个时间域：Linux CLOCK_REALTIME，也就是 Unix/UTC 系统时间。 ROS 的 sensor_msgs/Image 和 CameraInfo 都明确要求 header 时间戳表示图像采集时间，而不是图像到达 ROS 回调的时间。(ROS Docs)

1. 总架构

外部 PTP GM 或本机主时钟
ptp4l
NIC PHC: /dev/ptp0
phc2sys
Linux CLOCK_REALTIME
ROS 2 camera / robot drivers
Image.header.stamp = 曝光开始或采集开始时刻
message_filters / TF2 / 控制节点

注意：在硬件时间戳模式下，LinuxPTP 文档说明 PHC 通常跟随 PTP time scale，而系统 CLOCK_REALTIME 跟随 UTC，phc2sys 负责维护两者的偏移。也就是说，不要把 /dev/ptp0 或相机原始 PTP tick 直接当成 ROS stamp 填进去 ，必须要转换到 CLOCK_REALTIME 时间域。(Linux PTP Project)

2. 硬件接线与触发策略

推荐硬触发接法：

text 复制代码

PLC / MCU / DAQ / 工业触发器
        ├── TTL/光耦输出 → Camera0 Line1
        ├── TTL/光耦输出 → Camera1 Line1
        ├── TTL/光耦输出 → Camera2 Line1
        └── 可选 → 机械臂控制器 DI / 相机曝光灯 / 示波器

不要用 ROS 2 节点在 Linux 用户态直接翻 GPIO 来做"硬触发"。那只能算软件触发，抖动取决于调度、IRQ、负载。普通工业应用可以接受毫秒级时，Linux GPIO 可能够用；想要稳定到几十微秒或更好，触发脉冲应由 PLC、DAQ、MCU、FPGA、运动控制卡或相机/光源控制器产生。

相机触发模式配置为：

text 复制代码

AcquisitionMode   = Continuous
TriggerSelector   = FrameStart 或 ExposureStart
TriggerMode       = On
TriggerSource     = Line1
TriggerActivation = RisingEdge
ExposureMode      = Timed / TriggerWidth / TriggerControlled
ExposureAuto      = Off
GainAuto          = Off

常见做法是用 FrameStart + ExposureMode=Timed：触发沿到来后，相机初始化一帧并自动开始曝光；部分相机会有固定的 exposure start delay。更严格时用 ExposureStart/ExposureEnd + TriggerControlled，触发信号直接控制曝光开始/结束。Basler 文档中也把 Frame/Line triggers 和 Exposure triggers 分成这两类，并说明 TriggerSource 可以来自电气信号或定时器等源。(Basler 文档)

3. Linux PTP 配置

3.1 检查网卡是否支持硬件时间戳

bash 复制代码

IFACE=enp3s0

sudo ethtool -T ${IFACE}
readlink -f /sys/class/net/${IFACE}/ptp/ptp*

看到类似如下的输出证明支持PHC：

text 复制代码

SOF_TIMESTAMPING_TX_HARDWARE
SOF_TIMESTAMPING_RX_HARDWARE
SOF_TIMESTAMPING_RAW_HARDWARE
PTP Hardware Clock: 0

LinuxPTP 官方文档也建议用 ethtool -T eth0 检查 MAC 是否支持硬件/软件时间戳，并展示了带 PHC 的硬件时间戳能力示例。(linuxptp.sourceforge.net)

安装：

bash 复制代码

sudo apt update
sudo apt install -y linuxptp ethtool

关掉会抢 CLOCK_REALTIME 的 NTP/chrony，避免和 phc2sys 打架：

bash 复制代码

sudo timedatectl set-ntp false
sudo systemctl disable --now chrony 2>/dev/null || true
sudo systemctl disable --now systemd-timesyncd 2>/dev/null || true

3.2 外部 Grandmaster 模式

适合：厂内已有 PTP GM、GPS GM、PTP 交换机或机器人控制系统提供 GM。

/etc/linuxptp/ptp4l-camera.conf：

ini 复制代码

[global]
domainNumber        0      
time_stamping       hardware
network_transport   UDPv4
delay_mechanism     E2E
twoStepFlag         1
summary_interval    1
logging_level       6

# 外部 GM 场景：本机只做 PTP client
clientOnly          1

domainNumber :设置 PTP 域号（Domain Number）为 0。PTP 域用于隔离不同的 PTP 网络。只有相同域号的设备才会互相通信。
time_stamping hardware: 指定时间戳由网卡硬件生成（而非软件）
network_transport UDPv4 ：使用 IPv4 UDP 作为 PTP 报文的传输方式。
delay_mechanism E2E ：使用 End-to-End（E2E）延迟测量机制。PTP slave 向 master 发送 Delay_Req，master 回 Delay_Resp，计算路径延迟。
另一种是 P2P（Peer-to-Peer），适用于有透明时钟（Transparent Clock）的交换机。
twoStepFlag 1 ：启用两步时钟模式（Two-step clock）。在 Sync 报文中只发送时间戳 ID，实际时间戳在后续的 Follow_Up 报文中发送。大多数硬件时钟和普通设备都使用两步模式。
若设为 0，则为一步模式（One-step），要求主时钟在 Sync 包中直接嵌入精确时间戳（需特殊硬件支持）。
summary_interval 1：设置状态摘要日志的输出间隔（以 log₂ 秒为单位）。1 表示每 2¹ = 2 秒输出一次同步状态摘要（如 offset、delay 等）。若设为 0，则是每 1 秒；-1 是每 0.5 秒，依此类推
logging_level 6：设置日志详细程度。数值越大，日志越详细：
0~3：基本错误/警告
6：包含同步偏移（offset）、延迟（delay）等调试信息
7：非常详细（含每个报文）

启动：

bash 复制代码

sudo ptp4l -i enp3s0 -f /etc/linuxptp/ptp4l-camera.conf -m

另一个终端启动 phc2sys：

bash 复制代码

sudo phc2sys -a -r -m -R 16

ptp4l 是 Linux 上的 IEEE 1588 PTP 实现，支持 Ordinary Clock、Boundary Clock、Transparent Clock；硬件时间戳是默认模式。phc2sys 通常用于把系统时钟同步到已经由 ptp4l 同步过的 PHC，-a -r 会根据 ptp4l 状态自动同步并把 CLOCK_REALTIME 纳入同步。(Linux PTP Project)

3.3 本机作为 Grandmaster 模式

适合：一台工控机、多相机、机械臂也同机通信，不要求 UTC 绝对溯源，只要求全系统同一时间基准。

/etc/linuxptp/ptp4l-local-gm.conf：

ini 复制代码

[global]
domainNumber        0
time_stamping       hardware
network_transport   UDPv4
delay_mechanism     E2E
twoStepFlag         1
summary_interval    1
logging_level       6

# 本机希望成为 GM：priority1 越小优先级越高
clientOnly          0
priority1           10
clockClass          248
clockAccuracy       0xFE
offsetScaledLogVariance 0xFFFF

priority1 10：PTP 主时钟选举的第一优先级参数。取值范围：0 ~ 255，数值越小，优先级越高。默认值通常是 128。
clockClass 248表示本机时钟的质量等级（Clock Class）。范围：0 ~ 255，数值越小，时钟质量越高。常见值：
6：原子钟（如 GPS 驯服的 OCXO）
52：受 GNSS 同步的本地时钟
187：普通 Holdover 模式
248：本地自由振荡（Free-running）时钟 ← 你当前的设置
255：不确定或劣质时钟
🔍 248 的含义：本机没有外部高精度时间源（如 GPS、北斗），仅靠本地晶振运行，属于最低可用等级的主时钟。其他 PTP 设备在有更好时钟源时，可能拒绝同步到本机。
clockAccuracy 0xFE：表示时钟的精度等级（Clock Accuracy），按 IEEE 1588 标准编码。0xFE 是十六进制，对应十进制 254（数越小精度越高）
offsetScaledLogVariance 0xFFFF表示时钟的稳定性（噪声/抖动），用对数方差缩放值表示。0xFFFF 是最大值，表示方差未知或极大（极不稳定）。正常值通常在 0x4000 ~ 0xC000 之间（取决于晶振质量）。

启动：

bash 复制代码

sudo ptp4l -i enp3s0 -f /etc/linuxptp/ptp4l-local-gm.conf -m

另一个终端让网卡 PHC 跟随本机 CLOCK_REALTIME：

bash 复制代码

sudo phc2sys -s CLOCK_REALTIME -c /dev/ptp0 -w -m -R 16

LinuxPTP 的 priority1 是 BMCA 选主的重要参数，数值越低优先级越高；如果相机也能当 PTP master，要么把相机设成 slave-only，要么保证工控机 priority 更高。ptp4l 文档和相机厂商文档都说明了 PTP 选主会受 priority 影响。(Linux PTP Project)

3.4 systemd 固化

/etc/systemd/system/ptp4l-camera.service：

用于相机网络的 PTP4L

ini 复制代码

[Unit]
Description=PTP4L for camera network
After=network-online.target
Wants=network-online.target

[Service]
Type=simple
ExecStart=/usr/sbin/ptp4l -i enp3s0 -f /etc/linuxptp/ptp4l-camera.conf -m
Restart=always
RestartSec=2

[Install]
WantedBy=multi-user.target

外部 GM 用这个 /etc/systemd/system/phc2sys-camera.service：

从 PTP 硬件时钟同步 Linux 系统时间。

ini 复制代码

[Unit]
Description=Synchronize system CLOCK_REALTIME from PTP PHC
After=ptp4l-camera.service
Requires=ptp4l-camera.service

[Service]
Type=simple
ExecStart=/usr/sbin/phc2sys -a -r -m -R 16
Restart=always
RestartSec=2

[Install]
WantedBy=multi-user.target

核心：ExecStart=/usr/sbin/phc2sys -a -r -m -R 16 实现slew同步系统 CLOCK_REALTIME

本机 GM 时，把 ExecStart 换成：

ini 复制代码

ExecStart=/usr/sbin/phc2sys -s CLOCK_REALTIME -c /dev/ptp0 -w -m -R 16

启用：

bash 复制代码

sudo systemctl daemon-reload
sudo systemctl enable --now ptp4l-camera.service
sudo systemctl enable --now phc2sys-camera.service

journalctl -u ptp4l-camera.service -f
journalctl -u phc2sys-camera.service -f

4. 相机配置

4.1 PTP 状态

GigE 相机如果支持 PTP，要启用相机 PTP。以常见 GenICam/Basler 风格举例：

text 复制代码

GevIEEE1588 = true
# 或某些厂商叫：
PtpEnable = true
PtpMode   = SlaveOnly / Auto

Basler 文档说明 PTP 可以同步同一网络内的多台 GigE 相机，并建议启用后等待设备充分同步；检查状态时可读取 GevIEEE1588Status、GevIEEE1588OffsetFromMaster、PtpServoStatus 等，Locked 或较小 offset 才能认为同步稳定。(Basler 文档)

字段	含义
`GevIEEE1588Status`	当前 PTP 同步状态是什么
`GevIEEE1588OffsetFromMaster`	当前与主时钟差了多少时间
`PtpServoStatus`	本地时钟控制器（servo）是否已经稳定锁定

4.2 启用 chunk timestamp（图像时间戳）

相机 SDK 里要开启 chunk timestamp / event timestamp。以 GenICam 风格：

text 复制代码

ChunkModeActive = true
ChunkSelector   = Timestamp
ChunkEnable     = true

如果支持选择 timestamp 语义，优先选：

text 复制代码

BslChunkTimestampSelector = ExposureStart
# 或 FrameStart

Basler 的 chunk timestamp selector 明确区分 ExposureStart、ExposureEnd、FrameStart 等含义。数据块需要先选择 chunk，再设置 ChunkEnable=true，应用侧还要实现 chunk 解析。(Basler 文档)

名称	本质
FrameStart	一帧采集流程开始
ExposureStart	传感器开始积累光子
ExposureEnd	传感器停止积累光子

5. ROS 2 时间戳代码

下面代码把时间域固定为 CLOCK_REALTIME。不要在驱动里用默认 this->now() 盲目打时间戳，因为一旦 use_sim_time=true，你拿到的可能不是系统时间。

5.1 `time_utils.hpp`

cpp 复制代码

#pragma once

#include <cstdint>
#include <ctime>
#include <stdexcept>

#include "builtin_interfaces/msg/time.hpp"

inline uint64_t realtime_now_ns()
{
  timespec ts {};
  if (clock_gettime(CLOCK_REALTIME, &ts) != 0) {
    throw std::runtime_error("clock_gettime(CLOCK_REALTIME) failed");
  }

  return static_cast<uint64_t>(ts.tv_sec) * 1000000000ull +
         static_cast<uint64_t>(ts.tv_nsec);
}

inline builtin_interfaces::msg::Time ros_stamp_from_unix_ns(uint64_t ns)
{
  builtin_interfaces::msg::Time t;
  t.sec = static_cast<int32_t>(ns / 1000000000ull);
  t.nanosec = static_cast<uint32_t>(ns % 1000000000ull);
  return t;
}

inline uint64_t unix_ns_from_ros_stamp(const builtin_interfaces::msg::Time & t)
{
  return static_cast<uint64_t>(t.sec) * 1000000000ull +
         static_cast<uint64_t>(t.nanosec);
}

5.2 相机 PTP timestamp 到 `CLOCK_REALTIME` 的映射

不同相机的 chunk timestamp 不一定已经是 Unix/UTC。有些是 PTP tick，有些是相机本地 tick，有些在没有 UTC GM 时会进入 arbitrary timescale。Basler 文档也说明，如果网络中没有设备同步到 UTC/TAI，相机网络可能运行在任意 epoch 的 ARB 模式。(Basler 文档)

因此驱动里建议加一个 mapper：启动时读取相机当前 timestamp latch，同时读取主机 CLOCK_REALTIME，估计 offset。PTP 已经保证相机和主机时钟频率同步，所以 offset 应稳定。

cpp 复制代码

#pragma once

#include <atomic>
#include <chrono>
#include <cstdint>
#include <limits>
#include <thread>
#include <functional>

#include "time_utils.hpp"

class CameraClockMapper
{
public:
  // read_camera_time_ns: 调用相机 SDK，返回"当前相机时间戳 ns"
  // 要用 TimestampLatch / GevTimestampControlLatch / vendor equivalent
  void calibrate(const std::function<uint64_t()> & read_camera_time_ns,
                 int samples = 30)
  {
    int64_t best_offset = 0;
    uint64_t best_rtt = std::numeric_limits<uint64_t>::max();

    for (int i = 0; i < samples; ++i) {
      const uint64_t host_t0 = realtime_now_ns();
      const uint64_t cam_t = read_camera_time_ns();
      const uint64_t host_t1 = realtime_now_ns();

      const uint64_t rtt = host_t1 - host_t0;
      const uint64_t host_mid = host_t0 + rtt / 2;

      if (rtt < best_rtt) {
        best_rtt = rtt;
        best_offset = static_cast<int64_t>(host_mid) - static_cast<int64_t>(cam_t);
      }

      std::this_thread::sleep_for(std::chrono::milliseconds(5));
    }

    offset_ns_.store(best_offset, std::memory_order_release);
    valid_.store(true, std::memory_order_release);
  }

  bool valid() const
  {
    return valid_.load(std::memory_order_acquire);
  }

  uint64_t camera_ns_to_realtime_ns(uint64_t camera_ns) const
  {
    const int64_t offset = offset_ns_.load(std::memory_order_acquire);
    return static_cast<uint64_t>(static_cast<int64_t>(camera_ns) + offset);
  }

private:
  std::atomic<bool> valid_ {false};
  std::atomic<int64_t> offset_ns_ {0};
};

相机 SDK 里如果明确返回的 chunk timestamp 已经是 Unix/UTC ns，可以不做 mapper；但工程上建议仍然打印一次 camera_stamp → realtime_stamp 差值，确认没有 37 秒、epoch 错位或单位错位。

5.3 相机驱动发布 Image 的核心逻辑

下面是厂商 SDK 无关的 ROS 2 代码骨架。把 VendorFrame、VendorCamera 换成海康、Basler、Daheng、FLIR、MV、Aravis、Spinnaker、pylon 等 SDK 即可。

cpp 复制代码

#include <memory>
#include <string>
#include <vector>
#include <cstring>

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "sensor_msgs/msg/camera_info.hpp"

#include "time_utils.hpp"
#include "camera_clock_mapper.hpp"

struct VendorFrame
{
  uint32_t width;
  uint32_t height;
  std::string encoding;     // e.g. "mono8", "bayer_rggb8", "rgb8"
  uint32_t step;
  const uint8_t * data;
  size_t data_size;

  bool has_chunk_timestamp;
  uint64_t chunk_timestamp_ns;   // 相机硬件 timestamp，单位必须确认

  bool has_trigger_timestamp;
  uint64_t trigger_realtime_ns;  // 如果由硬件触发器反馈，已是 CLOCK_REALTIME ns
};

class IndustrialCameraNode : public rclcpp::Node
{
public:
  IndustrialCameraNode() : Node("industrial_camera_node")
  {
    camera_name_ = declare_parameter<std::string>("camera_name", "cam0");
    frame_id_ = declare_parameter<std::string>(
      "frame_id", camera_name_ + "_optical_frame");

    stamp_source_ = declare_parameter<std::string>(
      "stamp_source", "camera_chunk");
    // camera_chunk / trigger_time / receive_time

    image_pub_ = create_publisher<sensor_msgs::msg::Image>(
      "image_raw", rclcpp::SensorDataQoS());

    info_pub_ = create_publisher<sensor_msgs::msg::CameraInfo>(
      "camera_info", rclcpp::SensorDataQoS());

    // 这里初始化相机 SDK、打开相机、配置 PTP、触发、chunk。
    // configure_camera();
    // mapper_.calibrate([this](){ return read_camera_timestamp_latch_ns(); });

    RCLCPP_INFO(get_logger(), "camera node started: %s", camera_name_.c_str());
  }

private:
  uint64_t choose_stamp_ns(const VendorFrame & frame)
  {
    if (stamp_source_ == "camera_chunk") {
      if (!frame.has_chunk_timestamp) {
        RCLCPP_WARN_THROTTLE(
          get_logger(), *get_clock(), 2000,
          "No camera chunk timestamp; falling back to receive time");
        return realtime_now_ns();
      }

      if (!mapper_.valid()) {
        RCLCPP_WARN_THROTTLE(
          get_logger(), *get_clock(), 2000,
          "Camera clock mapper not calibrated; falling back to receive time");
        return realtime_now_ns();
      }

      return mapper_.camera_ns_to_realtime_ns(frame.chunk_timestamp_ns);
    }

    if (stamp_source_ == "trigger_time") {
      if (frame.has_trigger_timestamp) {
        return frame.trigger_realtime_ns;
      }

      RCLCPP_WARN_THROTTLE(
        get_logger(), *get_clock(), 2000,
        "No trigger timestamp; falling back to receive time");
      return realtime_now_ns();
    }
    return realtime_now_ns();   // 最后兜底。这个不是严格采集时间，只是图像到达/回调时间。
  }

  void publish_frame(const VendorFrame & frame)
  {
    const uint64_t stamp_ns = choose_stamp_ns(frame);

    auto image = sensor_msgs::msg::Image();
    image.header.stamp = ros_stamp_from_unix_ns(stamp_ns);
    image.header.frame_id = frame_id_;

    image.height = frame.height;
    image.width = frame.width;
    image.encoding = frame.encoding;
    image.is_bigendian = false;
    image.step = frame.step;
    image.data.resize(frame.data_size);
    std::memcpy(image.data.data(), frame.data, frame.data_size);

    auto info = sensor_msgs::msg::CameraInfo();
    info.header = image.header;
    info.height = frame.height;
    info.width = frame.width;
    // TODO: 填 D, K, R, P, distortion_model

    image_pub_->publish(image);
    info_pub_->publish(info);
  }

private:
  std::string camera_name_;
  std::string frame_id_;
  std::string stamp_source_;

  CameraClockMapper mapper_;

  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
  rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info_pub_;
};

工程判断：

text 复制代码

优先级 1：相机 chunk timestamp，语义为 ExposureStart / FrameStart
优先级 2：硬件触发器反馈的 pulse time / scheduled fire time
优先级 3：SDK callback receive time，仅作兜底，不建议用于高精度同步

6. GenICam 风格相机配置伪代码

不同 SDK 写法不同，但逻辑基本如下：

cpp 复制代码

void configure_genicam_camera()
{
  // 1. 关闭自动项，保证曝光和时延可重复
  set_enum_if_exists("ExposureAuto", "Off");
  set_enum_if_exists("GainAuto", "Off");
  set_float_if_exists("ExposureTime", 5000.0);  // us，按需求改

  // 2. PTP
  set_bool_if_exists("GevIEEE1588", true);
  set_bool_if_exists("PtpEnable", true);
  set_enum_if_exists("PtpMode", "SlaveOnly");   // 如果希望本机做 GM，相机只做 slave

  // 3. 硬触发
  set_enum("AcquisitionMode", "Continuous");

  set_enum("TriggerSelector", "FrameStart");
  set_enum("TriggerMode", "On");
  set_enum("TriggerSource", "Line1");
  set_enum("TriggerActivation", "RisingEdge");

  // 如果你要用脉宽控制曝光，而不是固定曝光时间：
  // set_enum("ExposureMode", "TriggerWidth");

  // 4. chunk timestamp
  set_bool_if_exists("ChunkModeActive", true);
  set_enum_if_exists("ChunkSelector", "Timestamp");
  set_bool_if_exists("ChunkEnable", true);

  // 如果厂商支持 timestamp 语义选择：
  set_enum_if_exists("BslChunkTimestampSelector", "ExposureStart");

  // 5. 开始采集
  start_grabbing();
}

Basler 的 pylon 风格大概是：

cpp 复制代码

camera.Open();

camera.GevIEEE1588.SetValue(true);

camera.AcquisitionMode.SetValue(AcquisitionMode_Continuous);

camera.TriggerSelector.SetValue(TriggerSelector_FrameStart);
camera.TriggerMode.SetValue(TriggerMode_On);
camera.TriggerSource.SetValue(TriggerSource_Line1);
camera.TriggerActivation.SetValue(TriggerActivation_RisingEdge);

camera.ExposureAuto.SetValue(ExposureAuto_Off);
camera.ExposureTime.SetValue(5000.0);

camera.ChunkModeActive.SetValue(true);
camera.ChunkSelector.SetValue(ChunkSelector_Timestamp);
camera.ChunkEnable.SetValue(true);

// 支持时才设置
// camera.BslChunkTimestampSelector.SetValue(BslChunkTimestampSelector_ExposureStart);

camera.StartGrabbing();

7. 多相机同步节点

如果把所有相机图像都 stamped 为同一个硬触发时刻，可以用 ExactTime。如果每台相机 timestamp 是自己的 ExposureStart，存在几十微秒到几百微秒差异，用 ApproximateTime，但 slop 要设小。

ROS 2 官方 message_filters 文档要求同步双方 QoS 一致，并提供 TimeSynchronizer 和 ApproximateTime 的 C++ 用法；TimeSynchronizer 等价于 ExactTime，需要 header stamp 完全一致，ApproximateTime 允许时间戳近似匹配。(ROS Docs)

7.1 ApproximateTime 示例

cpp 复制代码

#include <memory>
#include <functional>

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"

#include "message_filters/subscriber.h"
#include "message_filters/synchronizer.h"
#include "message_filters/sync_policies/approximate_time.h"

class MultiCameraSyncNode : public rclcpp::Node
{
public:
  MultiCameraSyncNode() : Node("multi_camera_sync_node")
  {
    rclcpp::QoS qos = rclcpp::SensorDataQoS();

    cam0_sub_.subscribe(this, "/cam0/image_raw", qos.get_rmw_qos_profile());
    cam1_sub_.subscribe(this, "/cam1/image_raw", qos.get_rmw_qos_profile());

    using Policy = message_filters::sync_policies::ApproximateTime<
      sensor_msgs::msg::Image,
      sensor_msgs::msg::Image>;

    sync_ = std::make_shared<message_filters::Synchronizer<Policy>>(
      Policy(20), cam0_sub_, cam1_sub_);

    // 允许最大时间差。普通硬触发多相机建议先从 1 ms 开始，再按实测收紧。
    sync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(0.001));

    sync_->registerCallback(std::bind(
      &MultiCameraSyncNode::on_sync, this,
      std::placeholders::_1,
      std::placeholders::_2));
  }

private:
  void on_sync(
    const sensor_msgs::msg::Image::ConstSharedPtr & cam0,
    const sensor_msgs::msg::Image::ConstSharedPtr & cam1)
  {
    const int64_t t0 =
      static_cast<int64_t>(cam0->header.stamp.sec) * 1000000000ll +
      cam0->header.stamp.nanosec;

    const int64_t t1 =
      static_cast<int64_t>(cam1->header.stamp.sec) * 1000000000ll +
      cam1->header.stamp.nanosec;

    RCLCPP_DEBUG(
      get_logger(),
      "synced images, dt = %.3f ms",
      static_cast<double>(t1 - t0) / 1e6);

    // TODO: 视觉算法、检测、融合
  }

private:
  message_filters::Subscriber<sensor_msgs::msg::Image> cam0_sub_;
  message_filters::Subscriber<sensor_msgs::msg::Image> cam1_sub_;

  using Policy = message_filters::sync_policies::ApproximateTime<
    sensor_msgs::msg::Image,
    sensor_msgs::msg::Image>;

  std::shared_ptr<message_filters::Synchronizer<Policy>> sync_;
};

7.2 ExactTime 示例

同一触发器给所有图像填完全相同 stamp 时：

cpp 复制代码

#include "message_filters/time_synchronizer.h"

message_filters::Subscriber<sensor_msgs::msg::Image> cam0_sub;
message_filters::Subscriber<sensor_msgs::msg::Image> cam1_sub;

auto sync = std::make_shared<
  message_filters::TimeSynchronizer<
    sensor_msgs::msg::Image,
    sensor_msgs::msg::Image>>(cam0_sub, cam1_sub, 20);

8. TF2 与机械臂控制节点

控制节点不要用"当前 TF"，而要用 图像采集时刻的 TF：

cpp 复制代码

#include <chrono>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"

#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2/exceptions.h"

using namespace std::chrono_literals;

class VisionControlNode : public rclcpp::Node
{
public:
  VisionControlNode() : Node("vision_control_node"),
    tf_buffer_(std::make_shared<tf2_ros::Buffer>(get_clock())),
    tf_listener_(*tf_buffer_)  //TF2 对象 不允许默认构造 + 不允许赋值，只能在创建时直接初始化
  {
    sub_ = create_subscription<sensor_msgs::msg::Image>(
      "/cam0/image_raw",
      rclcpp::SensorDataQoS(),
      std::bind(&VisionControlNode::on_image, this, std::placeholders::_1));
  }

private:
  void on_image(const sensor_msgs::msg::Image::ConstSharedPtr image)
  {
    rclcpp::Time image_time(image->header.stamp, RCL_SYSTEM_TIME);

    try {
      geometry_msgs::msg::TransformStamped T_base_cam =
        tf_buffer_->lookupTransform(
          "base_link",
          image->header.frame_id,
          image_time,
          50ms);
      // 1. 用 image + T_base_cam 做识别/定位
      // 2. 查机械臂 joint state / tcp pose 在 image_time 附近的状态
      // 3. 生成控制目标
      // 4. 发送到机械臂 SDK / ros2_control

    } catch (const tf2::TransformException & ex) {
      RCLCPP_WARN(
        get_logger(),
        "TF lookup failed at image time %d.%09u: %s",
        image->header.stamp.sec,
        image->header.stamp.nanosec,
        ex.what());
    }
  }

private:
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_;

  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  tf2_ros::TransformListener tf_listener_;
};

TF2 的设计就是既能做空间变换，也能按时间查询历史 transform；官方教程里的 lookupTransform(..., when, timeout) 正是按给定时间查 TF。(ROS Docs)

机械臂同机通信时：

text 复制代码

robot_state.header.stamp = 机械臂状态采样时刻

如果机械臂 SDK 有控制器 timestamp，就做一次 controller_time → CLOCK_REALTIME 的 offset 映射；如果没有，就在读到状态包的瞬间用 CLOCK_REALTIME stamp。控制节点用 image.header.stamp 去查最近的 robot state / TF，必要时做插值。

9. launch 示例

python 复制代码

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='your_camera_pkg',
            executable='industrial_camera_node',
            namespace='cam0',
            name='camera',
            parameters=[{
                'use_sim_time': False,
                'camera_name': 'cam0',
                'frame_id': 'cam0_optical_frame',
                'stamp_source': 'camera_chunk',
            }]
        ),
        Node(
            package='your_camera_pkg',
            executable='industrial_camera_node',
            namespace='cam1',
            name='camera',
            parameters=[{
                'use_sim_time': False,
                'camera_name': 'cam1',
                'frame_id': 'cam1_optical_frame',
                'stamp_source': 'camera_chunk',
            }]
        ),
        Node(
            package='your_sync_pkg',
            executable='multi_camera_sync_node',
            name='multi_camera_sync',
            parameters=[{
                'use_sim_time': False,
            }]
        ),
        Node(
            package='your_control_pkg',
            executable='vision_control_node',
            name='vision_control',
            parameters=[{
                'use_sim_time': False,
            }]
        ),
    ])

10. CMakeLists 依赖

cmake 复制代码

cmake_minimum_required(VERSION 3.8)
project(your_sync_pkg)

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(message_filters REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)

add_executable(multi_camera_sync_node src/multi_camera_sync_node.cpp)
ament_target_dependencies(
  multi_camera_sync_node
  rclcpp
  sensor_msgs
  message_filters
)

add_executable(vision_control_node src/vision_control_node.cpp)
ament_target_dependencies(
  vision_control_node
  rclcpp
  sensor_msgs
  tf2
  tf2_ros
  geometry_msgs
)

install(TARGETS
  multi_camera_sync_node
  vision_control_node
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

11. 验收步骤

按这个顺序验收：

bash 复制代码

# 1. 网卡 PHC
sudo ethtool -T enp3s0

# 2. ptp4l 状态
journalctl -u ptp4l-camera.service -f

# 3. CLOCK_REALTIME 与 PHC offset
journalctl -u phc2sys-camera.service -f

# 4. 相机 PTP 状态
# 用相机 SDK / pylon viewer / vendor viewer 看：
# GevIEEE1588Status / PtpServoStatus / OffsetFromMaster

# 5. ROS 图像 stamp
ros2 topic echo /cam0/image_raw/header
ros2 topic echo /cam1/image_raw/header

# 6. 同步节点输出 dt
ros2 run your_sync_pkg multi_camera_sync_node

理想现象：

text 复制代码

ptp4l: master offset 稳定
phc2sys: CLOCK_REALTIME offset 稳定
camera: PTP Locked / Slave
cam0/cam1: 同一触发周期 stamp 差值稳定且小
message_filters: 稳定触发同步回调
TF2: 无大量 extrapolation into past/future

最后用示波器 或逻辑分析仪同时看：

text 复制代码

Trigger pulse
Camera0 ExposureActive
Camera1 ExposureActive
Light strobe
Robot DI / controller marker

因为 PTP 精度会受交换机、网卡、相机和网络拓扑影响；相机厂商也明确建议为了最高精度使用高质量网络硬件、PTP-aware switch 和外部 PTP clock。实际系统不要只看软件 offset，一定要测触发线和曝光输出。(Basler 文档)

12. 常见错误

用接收时间当采集时间。 这会把曝光、读出、传输、驱动排队、ROS 调度全混进 stamp。
PHC/PTP tick 直接塞进 ROS stamp。 ROS 里要统一到 CLOCK_REALTIME / Unix 时间域。
硬触发了，但相机没开 chunk timestamp。 这样你只能知道"发送触发指令的时刻"，不知道"相机感光采样的真实时刻"。

没开 Chunk 时间戳，只会发图片数据，不会带上「这张图实际是哪一刻曝光采集的」精确时间点

use_sim_time=true。 实机驱动必须关掉，或者驱动内部显式使用 RCL_SYSTEM_TIME / CLOCK_REALTIME。
message_filters QoS 不一致。 图像发布者、订阅者、filter subscriber 的 QoS 必须匹配，否则同步器收不到或对不齐。
TF 用 now 查。 应该用 image.header.stamp 查相机采集时刻的 base_link ← camera_optical_frame。
相机和机械臂状态没有同一时间域。 机械臂状态、图像、TF 都要落到同一个 CLOCK_REALTIME 时间轴上。

机器人时间链路---工程流程示例

1. 总架构

2. 硬件接线与触发策略

3. Linux PTP 配置

3.1 检查网卡是否支持硬件时间戳

3.2 外部 Grandmaster 模式

3.3 本机作为 Grandmaster 模式

3.4 systemd 固化

4. 相机配置

4.1 PTP 状态

4.2 启用 chunk timestamp（图像时间戳）

5. ROS 2 时间戳代码

5.1 time_utils.hpp

5.2 相机 PTP timestamp 到 CLOCK_REALTIME 的映射

5.3 相机驱动发布 Image 的核心逻辑

6. GenICam 风格相机配置伪代码

7. 多相机同步节点

7.1 ApproximateTime 示例

7.2 ExactTime 示例

8. TF2 与机械臂控制节点

9. launch 示例

10. CMakeLists 依赖

11. 验收步骤

12. 常见错误

5.1 `time_utils.hpp`

5.2 相机 PTP timestamp 到 `CLOCK_REALTIME` 的映射