机器人软件栈全景

L0 驱动层      传感器/执行器原始数据，µs~ms 级，通常由内核驱动或厂商 SDK 提供
L1 硬件抽象    统一接口（ros2_control hardware_interface / 自研 HAL），1 kHz
L2 状态估计    里程计、IMU 融合、定位，50~1000 Hz，输出 TF 与位姿
L3 感知        点云分割、目标检测、抓取位姿，10~30 Hz
L4 世界模型    代价地图、语义地图、障碍物轨迹预测，5~10 Hz
L5 规划        全局路径 + 局部轨迹，0.1~10 Hz，输出带时间戳的轨迹
L6 控制        伺服循环，500 Hz~1 kHz，输出力矩/速度/位置指令
L7 行为/任务   状态机、任务编排、人机交互，事件驱动

channel: /lidar/points
type: sensor_msgs/PointCloud2
rate_hz: 10                 # 标称值
rate_tolerance: 0.9~1.1     # 允许波动范围，超范围上游告警
latency_budget_ms: 60       # 从采样到发布的最大延迟
frame_id: lidar_link
timestamp_semantics: 采样时刻（不是发布时刻）
qos: sensor_data            # best_effort, keep_last 5

class RateMonitor {
 public:
  explicit RateMonitor(double nominal_hz, double tol = 0.2)
      : nominal_(nominal_hz), tol_(tol) {}

  // 在回调入口调用，返回 false 表示频率异常
  bool tick(const rclcpp::Time & t) {
    if (last_.nanoseconds() > 0) {
      double dt = (t - last_).seconds();
      // 指数滑动平均，alpha 取 0.05 对应约 20 帧时间常数
      mean_dt_ = 0.95 * mean_dt_ + 0.05 * dt;
      double hz = 1.0 / mean_dt_;
      if (hz < nominal_ * (1 - tol_) || hz > nominal_ * (1 + tol_)) {
        ++violations_;
        return false;   // 上层据此降级或告警
      }
    }
    last_ = t;
    return true;
  }
  uint64_t violations() const { return violations_; }

 private:
  double nominal_, tol_;
  double mean_dt_ = 0.0;
  rclcpp::Time last_{0, 0, RCL_ROS_TIME};
  uint64_t violations_ = 0;
};

纯 ROS2：
  优势：生态完整（Nav2、MoveIt2、ros2_control）、工具链成熟
  代价：DDS 发现开销、默认 QoS 不实时、序列化有拷贝
  适合：原型、研究、中小规模产品、非硬实时部分

ROS2 + 实时补丁：
  做法：控制回路用 rclcpp 但配 CycloneDDS + 固定线程 + PREEMPT_RT
  优势：保留生态的同时拿到 100 µs 级抖动
  适合：多数商用移动机器人、协作机械臂

自研/混合：
  做法：控制与状态估计走自研共享内存环，感知与任务走 ROS2
  优势：确定性最好，延迟可控到 20 µs
  代价：工具链要自己造，团队规模要求高
  适合：量产型号固定、对成本与确定性极敏感的产品

方案 A：单 SoC 全包（Jetson Orin NX 16GB，100 TOPS 级）
  感知 + 规划 + 控制都在一颗芯片
  优点：成本低、无网络延迟、调试简单
  缺点：控制回路与推理争抢，需 cgroup 隔离与 CPU 亲和性绑定

方案 B：双 SoC 分工（主控 x86 + 实时 MCU/ARM）
  主控：感知、规划、任务（Ubuntu + ROS2）
  实时侧：状态估计 + 控制（FreeRTOS / PREEMPT_RT），1 kHz
  通信：EtherCAT 或 1 Gbps 以太网，周期 1 ms
  优点：实时性隔离彻底，主控重启不影响安全停机
  缺点：跨芯片时间同步与调试复杂度上升

方案 C：集中式服务器 + 边缘（多机器人）
  服务器：全局调度、地图维护、模型推理
  边缘：本地控制与避障，断网可降级运行
  优点：算力可共享，模型可统一升级
  缺点：网络分区处理必须显式设计

# 把控制线程绑到隔离核，避免与推理抢 L2 缓存
sudo cset shield --cpu 2-3 --kthread on
taskset -c 2,3 ./robot_control_node --ros-args -p use_sim_time:=false

# 实时优先级（需要 CAP_SYS_NICE 或 rtprio 限制放宽）
chrt -f 80 ./robot_control_node
# /etc/security/limits.conf: robot_user - rtprio 99

# 限制推理进程组最多使用 400% CPU（4 核）与 2 GB 内存
sudo cgcreate -g cpu,memory:/robot/inference
echo 400000 | sudo tee /sys/fs/cgroup/robot/inference/cpu.max
echo 2147483648 | sudo tee /sys/fs/cgroup/robot/inference/memory.max
sudo cgexec -g cpu,memory:/robot/inference ./yolo_infer_node

# 实时控制组：给足 CPU 权重，禁止被降级
sudo cgcreate -g cpu:/robot/realtime
echo 1000000 | sudo tee /sys/fs/cgroup/robot/realtime/cpu.weight

# 验证抖动是否达标（目标 p99 < 200 µs）
cyclictest -t1 -p80 -n -i1000 -l100000 -m -c2

// 错误：用系统时钟测周期，NTP 校时会得到负的 dt
auto dt = std::chrono::system_clock::now() - last_;

// 正确：控制回路一律用单调时钟
auto now = std::chrono::steady_clock::now();
double dt = std::chrono::duration<double>(now - last_).count();
if (dt <= 0.0 || dt > 0.1) { dt = nominal_dt_; }  // 防跳变

// ROS 时间用于与消息时间戳比较，不能用于测周期
rclcpp::Time stamp = this->now();  // 尊重 use_sim_time

message_filters::Synchronizer<ApproximateTime> sync_(
    ApproximateTime(20), image_sub_, cloud_sub_);
sync_.setMaxIntervalDuration(rclcpp::Duration::from_seconds(0.02));  // 20 ms 容差
// 容差大于传感器周期的 1/2 就会开始错配

状态：IDLE → LOCALIZING → NAVIGATING → MANIPULATING → RETURNING → CHARGING
异常转移：
  LOCALIZING 失败 → 重定位（最多 3 次）→ 仍失败 → SAFE_STOP
  NAVIGATING 卡住 > 30 s → 局部重规划 → 仍卡住 → 请求人工
  MANIPULATING 抓取失败 → 重试（最多 2 次）→ 放弃并回 NAVIGATING
  任意状态 → 急停触发 → EMERGENCY_STOP（最高优先级，可中断一切）

struct State {
  enum Id { IDLE, LOCALIZING, NAVIGATING, MANIPULATING, SAFE_STOP } id;
  double entered_at;      // 进入时刻（单调时钟）
  double timeout_s;       // 最大驻留时间
  int retry_count;
};

// 转移函数是纯函数：给定当前状态与事件，返回下一个状态
State transition(const State & s, const Event & e, const WorldModel & w) {
  switch (s.id) {
    case State::LOCALIZING:
      if (e.type == Event::LOCALIZED) {
        return {State::NAVIGATING, now(), 60.0, 0};
      }
      if (now() - s.entered_at > s.timeout_s) {
        if (s.retry_count < 3) {
          return {State::LOCALIZING, now(), 30.0, s.retry_count + 1};
        }
        return {State::SAFE_STOP, now(), 0.0, 0};   // 重试耗尽，安全停机
      }
      return s;
    case State::NAVIGATING:
      if (w.obstacle_ahead() && w.stuck_duration() > 30.0) {
        return {State::SAFE_STOP, now(), 0.0, 0};
      }
      // ...
  }
}

# rosbag2 录制：全量太大，按需选择性录制
ros2 bag record -s mcap \
  --topics /scan /odom /tf /tf_static /cmd_vel /diagnostics \
  --max-bag-size 2147483648 --max-cache-size 67108864 \
  -o /data/bags/$(date +%Y%m%d_%H%M%S)

# 回放时必须关闭实时控制输出，否则会真的驱动机器人
ros2 bag play session.mcap --clock --rate 0.5 \
  --topics /scan /odom /tf /tf_static

# 关键指标必须打点，否则线上无法归因
metrics:
  - /control/loop_jitter_us      # 目标 p99 < 200 µs
  - /localization/pose_cov       # 超过阈值触发降级
  - /perception/infer_latency_ms # p99 < 50 ms
  - /comm/ros_dds_lost_samples   # 丢帧计数，非零即告警

第一类：异常路径
  原型只处理 happy path；量产要求每个模块都有失败模式定义
  传感器掉线、数据超时、坐标系丢失、磁盘写满、网络分区

第二类：长时稳定性
  原型跑 10 分钟；量产要跑 8 小时 × 30 天
  内存泄漏 1 KB/s 在 8 小时后就是 28 MB，长跑必崩
  浮点累加误差在长时间积分后可能让协方差矩阵失去正定性

第三类：可维护性
  参数必须外置（YAML/配置中心），不能在代码里硬编码
  必须支持现场升级、灰度、回滚
  必须有日志分级与远程拉取能力

架构
  [ ] 分层数据流图已画出，每条边标注频率与延迟预算
  [ ] 每层对外接口唯一，内部实现可替换
  [ ] 实时/非实时边界明确，有 cgroup 或核隔离配置

接口
  [ ] 消息类型版本化，向后兼容策略明确
  [ ] 坐标系符合 REP-105，TF 树无环且完整
  [ ] 时间戳语义在接口文档中写明

可靠性
  [ ] 每个状态有超时与失败处理
  [ ] 关键传感器掉线有降级路径（不是直接停机）
  [ ] 看门狗覆盖所有实时线程

可观测
  [ ] 关键指标有打点，阈值有告警
  [ ] 支持选择性录制与离线回放
  [ ] 参数全部外置，支持运行时重载

交付
  [ ] 有 CI 构建、有版本号、有升级与回滚流程
  [ ] 有安全评估（见 ISO 10218 / ISO/TS 15066）