ROS2 架构：节点、DDS 与生命周期

// 多线程执行器 + 回调组：让控制回调与推理回调并发
auto control_group =
    create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
auto infer_group =
    create_callback_group(rclcpp::CallbackGroupType::Reentrant);

rclcpp::SubscriptionOptions ctrl_opt;
ctrl_opt.callback_group = control_group;
auto sub_ctrl = create_subscription<sensor_msgs::msg::JointState>(
    "/joint_states", rclcpp::SensorDataQoS(),
    std::bind(&Node::onJointState, this, _1), ctrl_opt);

auto sub_img = create_subscription<sensor_msgs::msg::Image>(
    "/camera/image_raw", rclcpp::SensorDataQoS(),
    std::bind(&Node::onImage, this, _1), infer_opt);

rclcpp::executors::MultiThreadedExecutor exec(
    rclcpp::ExecutorOptions(), 4 /* 线程数 */);
exec.add_node(shared_from_this());
exec.spin();

Reliability
  RELIABLE      ：保证送达，失败会重传（TCP 语义）
  BEST_EFFORT   ：尽力送达，可能丢（UDP 语义）

Durability
  VOLATILE      ：只给当前及之后的订阅者（默认）
  TRANSIENT_LOCAL：为晚加入的订阅者保留历史（地图、标定结果用）

History
  KEEP_LAST(n)  ：只保留最近 n 条（实时数据用）
  KEEP_ALL      ：全保留（受 resource_limits 限制）

rclcpp::SensorDataQoS()   // BEST_EFFORT, KEEP_LAST 5, VOLATILE
rclcpp::SystemDefaultsQoS() // RELIABLE, KEEP_LAST 10, VOLATILE
rclcpp::ServicesQoS()     // RELIABLE, KEEP_LAST 10, 用于 service
rclcpp::ParametersQoS()   // RELIABLE + TRANSIENT_LOCAL，参数事件

# 查看实际生效的 QoS
ros2 topic info /scan --verbose
# Type: sensor_msgs/msg/LaserScan
# Publisher count: 1
# Node name: lidar_driver
#   Reliability: BEST_EFFORT
#   Durability: VOLATILE
#   History (Depth): KEEP_LAST (5)
# Subscription count: 0        ← 收不到数据时先看这里

# 检查两个端点是否匹配
ros2 topic echo /scan --qos-reliability reliable   # 强制用 RELIABLE 订阅试试

节点数与启动时间（实测经验值）：
  5 个节点   ：< 1 s
  20 个节点  ：2~5 s
  50 个节点  ：10~30 s，组播包开始丢
  100 个节点 ：分钟级，甚至发现失败

<!-- 方案一：改用 CycloneDDS 并开启 discovery server（ROS2 内置支持） -->
<!-- 环境变量方式，无需改代码 -->
<!-- export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp -->
<!-- export ROS_DISCOVERY_SERVER=192.168.1.10:11811 -->

# 方案二：限制组播到指定网卡，避免跨网段泄漏
export CYCLONEDDS_URI='<CycloneDDS><Domain><General>
  <NetworkInterfaceAddress>eth0</NetworkInterfaceAddress>
  <AllowMulticast>spdp</AllowMulticast>
  </General></Domain></CycloneDDS>'

# 方案三：Fast DDS 的 XML 配置，显式设定参与者与传输
export FASTRTPS_DEFAULT_PROFILES_FILE=/etc/ros/fastdds_profile.xml

Fast DDS (rmw_fastrtps_cpp，ROS2 默认)
  优势：与 ROS2 同步发布、文档多、工具（如 Fast DDS Discovery Server）完整
  注意：默认会为每个端点开线程，节点多时线程数爆炸
  调优：在 XML 里把线程池设为固定大小，关闭不必要的内置端点

CycloneDDS (rmw_cyclonedds_cpp)
  优势：内存占用低、线程模型更省、组播行为更可控
  注意：TRANSIENT_LOCAL 的样本数上限需显式配置
  调优：CYCLONEDDS_URI 里设 MaxMessageSize 与 FragmentSize

实测经验（20 节点、100 话题、千兆网）：
  Fast DDS 默认配置：CPU 占用约 15%，启动 3 s
  CycloneDDS 默认配置：CPU 占用约 6%，启动 1.5 s
  结论：资源受限平台优先考虑 CycloneDDS

             create
  (none) ─────────────→ unconfigured
                            │ configure()
                            ▼
                        inactive  ←──────────┐
                            │ activate()      │ deactivate()
                            ▼                 │
                         active ──────────────┘
                            │ shutdown()
                            ▼
                       finalized

关键语义：
  inactive：已加载配置与资源，但不发布/不执行，可安全地做标定
  active  ：正常对外服务
  error processing：任一过渡失败进入，可清理后回到 unconfigured

class ArmController : public rclcpp_lifecycle::LifecycleNode {
  CallbackReturn on_configure(const State &) override {
    loadParameters();            // 读参数、校验范围
    pub_ = create_lifecycle_publisher<JointTrajectory>("/arm/cmd", 10);
    return CallbackReturn::SUCCESS;
  }
  CallbackReturn on_activate(const State &) override {
    pub_->on_activate();         // 生命周期发布者必须显式激活
    timer_ = create_wall_timer(1ms, [this]{ tick(); });
    return CallbackReturn::SUCCESS;
  }
  CallbackReturn on_deactivate(const State &) override {
    timer_.reset(); pub_->on_deactivate();
    return CallbackReturn::SUCCESS;
  }
};

# 把多个节点组合进一个进程
ros2 component load /ComponentManager composition_demo \
  my_pkg::PerceptionNode
ros2 component load /ComponentManager composition_demo \
  my_pkg::PlanningNode

# 查看已加载组件与它们之间的话题端点类型
ros2 component list
ros2 topic info /points --verbose   # 关注 "Node name" 与是否 intra-process

rclcpp::NodeOptions opts;
opts.use_intra_process_comms(true);
auto node = std::make_shared<MyNode>(opts);
// 发布时必须用 unique_ptr 版本，否则退化为拷贝
pub_->publish(std::make_unique<Msg>(...));
// 注意：intra-process 下 QoS 的 durability 必须为 VOLATILE

# 方式一：手工加载，适合调试
ros2 run rclcpp_components component_container_mt --ros-args \
  -r __node:=robot_container
ros2 component load /robot_container perception perception::Detector

# 方式二：从 YAML 一次性启动，适合量产部署
ros2 run rclcpp_components component_container_mt \
  --ros-args --params-file /etc/robot/container.yaml

# container.yaml：声明式加载，无需人工干预
/robot_container:
  ros__parameters:
    use_intra_process_comms: true
    composable_node_descriptions:
      - package: perception
        plugin: perception::Detector
        name: detector
        parameters:
          - { name: conf_threshold, value: 0.45 }
      - package: planning
        plugin: planning::GlobalPlanner
        name: global_planner

this->declare_parameter<double>("max_velocity", 1.5);
this->declare_parameter<int>("control_rate_hz", 500);

auto cb = [this](const std::vector<rclcpp::Parameter> & ps) {
  for (const auto & p : ps) {
    if (p.get_name() == "max_velocity") {
      double v = p.as_double();
      if (v <= 0.0 || v > 5.0) {
        return rcl_interfaces::msg::SetParametersResult()  // 拒绝非法值
            .set__successful(false).set__reason("out of range");
      }
      max_vel_ = v;
    }
  }
  return rcl_interfaces::msg::SetParametersResult().set__successful(true);
};
param_cb_ = add_on_set_parameters_callback(cb);

ros2 param get /arm_controller max_velocity
ros2 param set /arm_controller max_velocity 2.0
ros2 param dump /arm_controller > arm_params.yaml   # 落盘便于版本管理
ros2 param load /arm_controller tuned.yaml

// 实时回调内的正确做法：只记录到无锁环形缓冲，由非实时线程落盘
if (unlikely(jitter_us > threshold)) {
  ring_.push({cycle_id_, jitter_us});   // 无锁，无分配
}
// 后台线程消费并限流输出

ros2 run demo_nodes_cpp talker --ros-args -p use_sim_time:=true
ros2 topic hz /clock        # 仿真时钟频率，通常 100~1000 Hz

# 网络隔离的最小方案：独立网段 + 组播域隔离 + 防火墙
# 只允许特定端口：DDS 默认 7400-7500 (UDP) 与 11811 (Discovery Server)
sudo ufw allow from 192.168.10.0/24 to any port 7400:7500 proto udp
sudo ufw allow from 192.168.10.0/24 to any port 11811 proto udp

# 生成 keystore 与各节点的密钥材料
ros2 security create_keystore /etc/robot/keys
ros2 security create_enclave /etc/robot/keys /robot/control_node
ros2 security create_enclave /etc/robot/keys /robot/perception_node

# 权限文件：默认允许全部，需手工收紧到最小权限
# /etc/robot/keys/enclaves/robot/control_node/permissions.xml

# 启动时启用
export ROS_SECURITY_ENABLE=true
export ROS_SECURITY_STRATEGY=Enforce   # 可选 Permissive 用于灰度
export ROS_SECURITY_KEYSTORE=/etc/robot/keys