1. 架构全景

┌─────────────────────────────────────────────────────────────────┐
│                    大脑 (Large Brain) 1-5Hz                      │
│  ┌──────────┐ ┌──────────┐ ┌───────────┐ ┌──────────────────┐  │
│  │ 自然语言  │ │ 场景理解  │ │ 任务推理   │ │ 长期记忆/世界模型 │  │
│  │ 交互系统  │ │ VLM系统  │ │ LLM系统   │ │ Memory System    │  │
│  └────┬─────┘ └────┬─────┘ └─────┬─────┘ └───────┬──────────┘  │
│       │            │             │                │              │
│       └────────────┴──────┬──────┴────────────────┘              │
│                           │ Strategy Pattern                     │
│                     TaskCommand                                 │
├───────────────────────────┼──────────────────────────────────────┤
│                    桥接层 (Bridge) 30Hz                           │
│  ┌──────────────┐ ┌───────────────┐ ┌─────────────────────────┐ │
│  │ 感知融合系统  │ │ 行为规划系统   │ │ 状态机协调器 (Mediator) │ │
│  │ PerceptionFusion│ BehaviorPlanner│ │ StateMachineCoordinator │ │
│  └──────┬───────┘ └───────┬───────┘ └───────────┬─────────────┘ │
│         │                 │                     │                 │
│         └─────────┬───────┘                     │                 │
│                   │ Observer Pattern             │                 │
│              MotionPlan                          │                 │
├───────────────────┼─────────────────────────────┼─────────────────┤
│                   │    小脑 (Small Brain) 100Hz-1KHz              │
│  ┌────────────┐ ┌─┴──────────┐ ┌────────────┐ ┌──────────────┐ │
│  │ 速度控制器  │ │ 轨迹跟踪器  │ │ 安全监控器  │ │ 反射行为系统  │ │
│  │ VelCtrl   │ │ TrajTracker│ │ SafetyMon  │ │ ReflexSystem │ │
│  └─────┬──────┘ └─────┬──────┘ └─────┬──────┘ └──────┬───────┘ │
│        │              │              │                │           │
│        └──────────────┴──────┬───────┴────────────────┘           │
│                              │ Command Pattern                    │
│                         MotorCommand                             │
├──────────────────────────────┼────────────────────────────────────┤
│                    硬件抽象层 (HAL)                               │
│  ┌────────┐ ┌────────┐ ┌────────┐ ┌────────┐ ┌──────────────┐  │
│  │  IMU   │ │ Lidar  │ │ Camera │ │ Motor  │ │ ForceSensor  │  │
│  │ Sensor │ │ Sensor │ │ Sensor │ │ Driver │ │ Driver       │  │
│  └────────┘ └────────┘ └────────┘ └────────┘ └──────────────┘  │
│                    Bridge Pattern + Factory Pattern               │
└──────────────────────────────────────────────────────────────────┘

---

2. ECS 核心设计

2.1 为什么用 ECS

传统 ROS2 节点是"对象继承"模式——功能膨胀后节点越来越重，耦合越来越紧。ECS 的核心优势：

维度	OOP/继承	ECS/组合
新增功能	改继承树/加接口	加一个 Component + System
数据局部性	分散在堆上	连续内存，Cache友好
并行	需要锁	System 间读写分离，天然并行
测试	Mock整个节点	只测 System 函数

2.2 Entity 定义

// entity.hpp
using EntityId = uint64_t;

class Entity {
public:
    explicit Entity(EntityId id) : id_(id) {}
    EntityId id() const { return id_; }

    template<typename T>
    void add_component(T&& component);

    template<typename T>
    T& get_component();

    template<typename T>
    bool has_component() const;

    template<typename T>
    void remove_component();

private:
    EntityId id_;
    // 组件存储委托给 Registry（见下文）
    class Registry* registry_{nullptr};
};

机器人场景中的实体类型：

Entity	描述	典型组件组合
RobotEntity	机器人本体	Transform, Velocity, JointState, Battery
SensorEntity	传感器	SensorType, Transform, RawData, Calibration
TaskEntity	任务实例	TaskType, Goal, Progress, Priority
ObstacleEntity	障碍物	Transform, BoundingBox, Velocity, RiskLevel
RegionEntity	语义区域	Polygon, SemanticLabel, AffordanceList

2.3 Component 设计原则

// components.hpp — 全部为 POD 或轻量结构，无虚函数，无业务逻辑

// ===== 基础空间组件 =====
struct Transform {
    Eigen::Vector3d position{0, 0, 0};
    Eigen::Quaterniond orientation{1, 0, 0, 0};
    std::string frame_id{"base_link"};
    rclcpp::Time stamp;
};

struct Velocity {
    Eigen::Vector3d linear{0, 0, 0};
    Eigen::Vector3d angular{0, 0, 0};
};

// ===== 传感器组件 =====
struct LidarData {
    std::vector<Eigen::Vector3f> points;
    float min_range{0.1f};
    float max_range{40.0f};
    rclcpp::Time stamp;
};

struct ImuData {
    Eigen::Vector3d accel;
    Eigen::Vector3d gyro;
    Eigen::Quaterniond orientation;
    rclcpp::Time stamp;
};

struct CameraData {
    std::shared_ptr<sensor_msgs::msg::Image> image;
    std::shared_ptr<sensor_msgs::msg::CameraInfo> info;
    rclcpp::Time stamp;
};

// ===== 状态组件 =====
struct RobotState {
    enum State { IDLE, NAVIGATING, MANIPULATING, CHARGING, EMERGENCY };
    State current{IDLE};
    State previous{IDLE};
    rclcpp::Time state_changed_at;
};

struct BatteryStatus {
    float percentage{1.0f};
    float voltage{0.0f};
    bool charging{false};
};

struct JointState {
    std::vector<std::string> joint_names;
    std::vector<double> positions;
    std::vector<double> velocities;
    std::vector<double> efforts;
};

// ===== 任务组件 =====
struct NavGoal {
    Eigen::Vector3d target_position;
    Eigen::Quaterniond target_orientation;
    std::string frame_id{"map"};
    float tolerance_xy{0.1f};
    float tolerance_yaw{0.1f};
};

struct TaskContext {
    std::string task_type;  // "navigate", "pick", "place", "patrol"
    std::string description;
    int priority{0};
    float timeout{30.0f};
    std::map<std::string, std::string> params;
};

struct TaskProgress {
    enum Status { PENDING, RUNNING, PAUSED, SUCCEEDED, FAILED, CANCELLED };
    Status status{PENDING};
    float progress{0.0f};  // 0.0 ~ 1.0
    std::string feedback;
    rclcpp::Time started_at;
};

// ===== 感知结果组件 =====
struct DetectedObjects {
    struct Object {
        Eigen::Vector3d center;
        Eigen::Vector3d size;
        std::string label;
        float confidence;
        int track_id{-1};
    };
    std::vector<Object> objects;
    rclcpp::Time stamp;
};

struct OccupancyGrid {
    std::vector<int8_t> data;
    uint32_t width{0};
    uint32_t height{0};
    float resolution{0.05f};
    Eigen::Vector3d origin;
};

struct SceneUnderstanding {
    std::string scene_type;  // "office", "corridor", "elevator"
    std::vector<std::string> affordances;
    float confidence{0.0f};
    std::string natural_language_desc;
};

// ===== 安全组件 =====
struct SafetyZone {
    enum Level { CLEAR, CAUTION, WARNING, CRITICAL };
    Level level{CLEAR};
    float min_obstacle_distance{std::numeric_limits<float>::max()};
    Eigen::Vector3d closest_obstacle_pos;
};

struct EmergencyState {
    bool triggered{false};
    std::string reason;
    rclcpp::Time triggered_at;
    bool requires_manual_reset{false};
};

2.4 System 设计

System 是纯函数式逻辑处理器，从 Registry 读取 Component，写入 Component，不持有状态：

// system.hpp
class System {
public:
    virtual ~System() = default;
    virtual void update(Registry& registry, float dt, const rclcpp::Time& now) = 0;
    virtual int priority() const { return 0; }  // 执行优先级
    virtual std::string name() const = 0;
};

// ===== 大脑系统 (1-5Hz) =====

class ReasoningSystem : public System {
    // 读取: TaskContext, DetectedObjects, SceneUnderstanding
    // 写入: NavGoal, TaskProgress
    void update(Registry& reg, float dt, const rclcpp::Time& now) override;
    std::string name() const override { return "ReasoningSystem"; }
};

class SceneUnderstandingSystem : public System {
    // 读取: CameraData, LidarData, DetectedObjects
    // 写入: SceneUnderstanding
    void update(Registry& reg, float dt, const rclcpp::Time& now) override;
    std::string name() const override { return "SceneUnderstandingSystem"; }
};

// ===== 桥接层系统 (30Hz) =====

class PerceptionFusionSystem : public System {
    // 读取: LidarData, ImuData, CameraData
    // 写入: OccupancyGrid, DetectedObjects, Transform
    void update(Registry& reg, float dt, const rclcpp::Time& now) override;
    std::string name() const override { return "PerceptionFusionSystem"; }
};

class BehaviorPlannerSystem : public System {
    // 读取: NavGoal, SceneUnderstanding, SafetyZone
    // 写入: Trajectory (见下方)
    void update(Registry& reg, float dt, const rclcpp::Time& now) override;
    std::string name() const override { return "BehaviorPlannerSystem"; }
};

// ===== 小脑系统 (100Hz-1KHz) =====

class VelocityControllerSystem : public System {
    // 读取: Trajectory, Velocity, Transform
    // 写入: MotorCommand
    void update(Registry& reg, float dt, const rclcpp::Time& now) override;
    std::string name() const override { return "VelocityControllerSystem"; }
};

class SafetyMonitorSystem : public System {
    // 读取: DetectedObjects, Velocity, EmergencyState
    // 写入: SafetyZone, EmergencyState
    void update(Registry& reg, float dt, const rclcpp::Time& now) override;
    std::string name() const override { return "SafetyMonitorSystem"; }
};

class ReflexSystem : public System {
    // 读取: SafetyZone, EmergencyState
    // 写入: MotorCommand (可覆盖 VelocityController 的输出)
    int priority() const override { return 100; }  // 最高优先级
    void update(Registry& reg, float dt, const rclcpp::Time& now) override;
    std::string name() const override { return "ReflexSystem"; }
};

2.5 Registry（组件存储）

// registry.hpp — ECS 核心，类型安全的组件管理器
class Registry {
public:
    Entity create_entity() {
        EntityId id = next_id_++;
        auto [it, ok] = entities_.emplace(id, Entity(id));
        it->second.registry_ = this;
        return it->second;
    }

    void destroy_entity(EntityId id);

    template<typename T>
    void add_component(EntityId id, T component) {
        auto& pool = get_pool<T>();
        pool[id] = std::move(component);
    }

    template<typename T>
    T& get_component(EntityId id) {
        auto& pool = get_pool<T>();
        return pool.at(id);
    }

    template<typename T>
    bool has_component(EntityId id) const {
        auto& pool = get_pool<T>();
        return pool.find(id) != pool.end();
    }

    // 视图：高效遍历拥有指定组件子集的实体
    template<typename... Components>
    class View {
    public:
        // 迭代器，遍历同时拥有所有 Components... 的实体
        class iterator { /* ... */ };
        iterator begin();
        iterator end();
    };

    template<typename... Components>
    View<Components...> view() {
        return View<Components...>(get_pool<Components>()...);
    }

    // System 调度
    void add_system(std::unique_ptr<System> sys) {
        systems_.push_back(std::move(sys));
        // 按 priority 排序
        std::sort(systems_.begin(), systems_.end(),
            [](const auto& a, const auto& b) { return a->priority() > b->priority(); });
    }

    void update(float dt, const rclcpp::Time& now) {
        for (auto& sys : systems_) {
            sys->update(*this, dt, now);
        }
    }

private:
    EntityId next_id_{0};
    std::unordered_map<EntityId, Entity> entities_;
    std::vector<std::unique_ptr<System>> systems_;

    // 类型擦除的组件池
    struct IComponentPool { virtual ~IComponentPool() = default; };
    template<typename T>
    struct ComponentPool : IComponentPool {
        std::unordered_map<EntityId, T> data;
    };

    std::unordered_map<std::type_index, std::unique_ptr<IComponentPool>> pools_;

    template<typename T>
    ComponentPool<T>& get_pool() {
        auto key = std::type_index(typeid(T));
        if (pools_.find(key) == pools_.end()) {
            pools_[key] = std::make_unique<ComponentPool<T>>();
        }
        return static_cast<ComponentPool<T>&>(*pools_[key]);
    }
};

---

3. 大小脑分离架构

3.1 分层频率与职责

┌──────────────────────────────────────────────────────────┐
│  Layer          Freq        Latency    Technology        │
├──────────────────────────────────────────────────────────┤
│  大脑(Large)    1-5 Hz      200ms-1s   LLM/VLM/WorldModel│
│  桥接(Bridge)   30 Hz       33ms       Nav2/BehaviorTree │
│  小脑(Small)    100-1000Hz  1-10ms     MPC/PID/Reflex    │
│  硬件(HAL)      1-10KHz     0.1-1ms    Driver/FPGA       │
└──────────────────────────────────────────────────────────┘

3.2 大小脑通信接口

大小脑通过 Command/Feedback 双向通道 通信，异步解耦：

// 大脑 → 小脑 的指令通道
struct BrainCommand {
    enum Type {
        NAVIGATE_TO,        // 导航到目标
        EXECUTE_SKILL,      // 执行技能(抓取/放置/开门)
        EMERGENCY_STOP,     // 紧急停止
        RESUME,             // 恢复运行
        SWITCH_MODE         // 切换模式(自主/跟随/遥控)
    };

    Type type;
    std::string skill_name;       // EXECUTE_SKILL 时使用
    NavGoal nav_goal;             // NAVIGATE_TO 时使用
    std::map<std::string, std::string> params;
    uint64_t command_id;          // 指令唯一ID
    rclcpp::Time issued_at;
    float timeout{30.0f};
};

// 小脑 → 大脑 的反馈通道
struct CerebellumFeedback {
    enum Status {
        ACCEPTED,           // 已接收，执行中
        COMPLETED,          // 执行成功
        FAILED,             // 执行失败
        REJECTED,           // 拒绝执行(安全原因)
        PROGRESS            // 进度更新
    };

    uint64_t command_id;     // 对应 BrainCommand.id
    Status status;
    float progress{0.0f};    // 0.0 ~ 1.0
    std::string message;
    RobotState::State current_state;
    SafetyZone::Level safety_level;
    rclcpp::Time stamp;
};

3.3 ROS2 通信映射

// 大脑节点
class LargeBrainNode : public rclcpp::Node {
public:
    LargeBrainNode() : Node("large_brain") {
        // 发布指令给小脑
        cmd_pub_ = create_publisher<BrainCommand>("brain/command", 10);

        // 订阅小脑反馈
        feedback_sub_ = create_subscription<CerebellumFeedback>(
            "brain/feedback", 10,
            [this](const CerebellumFeedback::SharedPtr fb) {
                on_feedback(fb);
            });

        // 高级感知输入 (低频)
        scene_sub_ = create_subscription<SceneUnderstanding>(
            "perception/scene", rclcpp::QoS(1).best_effort(),
            [this](const SceneUnderstanding::SharedPtr msg) {
                // ECS: 更新 SceneUnderstanding 组件
                registry_.get_component<SceneUnderstanding>(robot_entity_) = *msg;
            });

        // 低频定时器 — 大脑推理循环
        brain_timer_ = create_wall_timer(
            std::chrono::milliseconds(500),  // 2Hz
            [this]() { brain_tick(); });
    }

private:
    void brain_tick() {
        // 1. 收集当前状态
        auto& state = registry_.get_component<RobotState>(robot_entity_);
        auto& scene = registry_.get_component<SceneUnderstanding>(robot_entity_);
        auto& task  = registry_.get_component<TaskContext>(robot_entity_);

        // 2. 调用推理系统
        reasoning_system_->update(registry_, 0.5, now());

        // 3. 如果产生了新指令，发送给小脑
        if (registry_.has_component<BrainCommand>(robot_entity_)) {
            auto& cmd = registry_.get_component<BrainCommand>(robot_entity_);
            cmd_pub_->publish(cmd);
        }
    }

    Registry registry_;
    Entity robot_entity_;
    std::unique_ptr<ReasoningSystem> reasoning_system_;

    rclcpp::Publisher<BrainCommand>::SharedPtr cmd_pub_;
    rclcpp::Subscription<CerebellumFeedback>::SharedPtr feedback_sub_;
    rclcpp::Subscription<SceneUnderstanding>::SharedPtr scene_sub_;
    rclcpp::TimerBase::SharedPtr brain_timer_;
};

// 小脑节点
class SmallBrainNode : public rclcpp::Node {
public:
    SmallBrainNode() : Node("small_brain") {
        // 订阅大脑指令
        cmd_sub_ = create_subscription<BrainCommand>(
            "brain/command", 10,
            [this](const BrainCommand::SharedPtr cmd) {
                on_command(cmd);
            });

        // 发布反馈给大脑
        feedback_pub_ = create_publisher<CerebellumFeedback>("brain/feedback", 10);

        // 高频定时器 — 控制循环
        control_timer_ = create_wall_timer(
            std::chrono::milliseconds(10),  // 100Hz
            [this]() { control_tick(); });

        // 超高频安全检查
        safety_timer_ = create_wall_timer(
            std::chrono::milliseconds(1),  // 1KHz
            [this]() { safety_tick(); });
    }

private:
    void control_tick() {
        // 执行控制链: 感知融合 → 行为规划 → 速度控制
        float dt = 0.01f;
        auto now_time = now();

        perception_system_->update(registry_, dt, now_time);
        planner_system_->update(registry_, dt, now_time);
        velocity_ctrl_system_->update(registry_, dt, now_time);

        // 反射系统（最高优先级，可覆盖控制输出）
        reflex_system_->update(registry_, dt, now_time);

        // 发布电机指令
        publish_motor_command();
    }

    void safety_tick() {
        float dt = 0.001f;
        safety_monitor_system_->update(registry_, dt, now());

        // 紧急状态时立即反馈大脑
        auto& emergency = registry_.get_component<EmergencyState>(robot_entity_);
        if (emergency.triggered) {
            CerebellumFeedback fb;
            fb.command_id = current_command_id_;
            fb.status = CerebellumFeedback::REJECTED;
            fb.message = emergency.reason;
            fb.safety_level = SafetyZone::CRITICAL;
            feedback_pub_->publish(fb);
        }
    }
};

---

4. 多设计模式集成

4.1 Strategy Pattern — 算法热替换

大小脑中每个功能都有多种算法实现，Strategy 模式允许运行时切换：

// strategies.hpp

// ===== 导航策略 =====
class INavigationStrategy {
public:
    virtual ~INavigationStrategy() = default;
    virtual Trajectory plan(const Transform& current, const NavGoal& goal,
                           const OccupancyGrid& grid) = 0;
    virtual std::string name() const = 0;
};

class Nav2Strategy : public INavigationStrategy {
    Trajectory plan(...) override;  // 调用 Nav2 栈
    std::string name() const override { return "nav2"; }
};

class MPPIControllerStrategy : public INavigationStrategy {
    Trajectory plan(...) override;  // Model Predictive Path Integral
    std::string name() const override { return "mppi"; }
};

class DWAStrategy : public INavigationStrategy {
    Trajectory plan(...) override;  // Dynamic Window Approach
    std::string name() const override { return "dwa"; }
};

// ===== 感知策略 =====
class IPerceptionStrategy {
public:
    virtual ~IPerceptionStrategy() = default;
    virtual DetectedObjects detect(const CameraData& camera,
                                   const LidarData& lidar) = 0;
    virtual std::string name() const = 0;
};

class YOLOPerception : public IPerceptionStrategy { /* ... */ };
class OpenVINOPerception : public IPerceptionStrategy { /* ... */ };
class BPUPerception : public IPerceptionStrategy { /* RDK BPU 推理 */ };

// ===== 推理策略（大脑）=====
class IReasoningStrategy {
public:
    virtual ~IReasoningStrategy() = default;
    virtual BrainCommand reason(const SceneUnderstanding& scene,
                                const TaskContext& task,
                                const RobotState& state) = 0;
    virtual std::string name() const = 0;
};

class LLMReasoning : public IReasoningStrategy { /* GPT/Qwen API */ };
class LocalVLMReasoning : public IReasoningStrategy { /* 本地 VLM */ };
class BehaviorTreeReasoning : public IReasoningStrategy { /* BT 推理 */ };

// ===== 策略工厂 + 运行时切换 =====
template<typename StrategyInterface>
class StrategyRegistry {
public:
    using Creator = std::function<std::unique_ptr<StrategyInterface>()>;

    void register_strategy(const std::string& name, Creator creator) {
        creators_[name] = std::move(creator);
    }

    std::unique_ptr<StrategyInterface> create(const std::string& name) {
        return creators_.at(name)();
    }

    std::vector<std::string> available() const {
        std::vector<std::string> names;
        for (const auto& [k, _] : creators_) names.push_back(k);
        return names;
    }

private:
    std::unordered_map<std::string, Creator> creators_;
};

// 系统中使用
class BehaviorPlannerSystem : public System {
public:
    BehaviorPlannerSystem() {
        nav_registry_.register_strategy("nav2",
            []() { return std::make_unique<Nav2Strategy>(); });
        nav_registry_.register_strategy("mppi",
            []() { return std::make_unique<MPPIControllerStrategy>(); });
        nav_registry_.register_strategy("dwa",
            []() { return std::make_unique<DWAStrategy>(); });

        current_nav_ = nav_registry_.create("nav2");
    }

    void set_navigation_strategy(const std::string& name) {
        current_nav_ = nav_registry_.create(name);
        RCLCPP_INFO(rclcpp::get_logger("BehaviorPlanner"),
                    "Switched to %s navigation strategy", name.c_str());
    }

    void update(Registry& reg, float dt, const rclcpp::Time& now) override {
        auto view = reg.view<Transform, NavGoal, OccupancyGrid>();
        for (auto [entity, tf, goal, grid] : view) {
            auto trajectory = current_nav_->plan(tf, goal, grid);
            reg.add_component(entity, std::move(trajectory));
        }
    }

private:
    StrategyRegistry<INavigationStrategy> nav_registry_;
    std::unique_ptr<INavigationStrategy> current_nav_;
};

4.2 Observer Pattern — 事件驱动通信

ECS 的 System 之间不直接调用，通过 EventBus 解耦：

// event_bus.hpp
struct Event {
    virtual ~Event() = default;
    virtual std::string type() const = 0;
    rclcpp::Time stamp;
};

struct EmergencyStopEvent : Event {
    std::string reason;
    std::string type() const override { return "emergency_stop"; }
};

struct TaskCompletedEvent : Event {
    uint64_t task_id;
    bool success;
    std::string type() const override { return "task_completed"; }
};

struct ObstacleProximityEvent : Event {
    float distance;
    Eigen::Vector3d position;
    std::string type() const override { return "obstacle_proximity"; }
};

class EventBus {
public:
    using Handler = std::function<void(const Event&)>;

    template<typename EventType>
    void subscribe(Handler handler) {
        auto key = std::type_index(typeid(EventType));
        handlers_[key].push_back(std::move(handler));
    }

    template<typename EventType>
    void publish(EventType event) {
        event.stamp = rclcpp::Clock().now();
        auto key = std::type_index(typeid(EventType));
        if (handlers_.count(key)) {
            for (auto& h : handlers_[key]) {
                h(event);
            }
        }
    }

private:
    std::unordered_map<std::type_index, std::vector<Handler>> handlers_;
};

// 使用示例：SafetyMonitor 发布事件，ReflexSystem 订阅
class SafetyMonitorSystem : public System {
    EventBus& bus_;
public:
    SafetyMonitorSystem(EventBus& bus) : bus_(bus) {}

    void update(Registry& reg, float dt, const rclcpp::Time& now) override {
        for (auto [entity, safety, velocity] : reg.view<SafetyZone, Velocity>()) {
            if (safety.level == SafetyZone::CRITICAL) {
                bus_.publish(EmergencyStopEvent{
                    .reason = "Obstacle too close: " + std::to_string(safety.min_obstacle_distance)
                });
            }
        }
    }
};

class ReflexSystem : public System {
    EventBus& bus_;
public:
    ReflexSystem(EventBus& bus) : bus_(bus) {
        bus_.subscribe<EmergencyStopEvent>(
            [this](const Event& e) {
                auto& estop = static_cast<const EmergencyStopEvent&>(e);
                trigger_emergency_brake(estop.reason);
            });
    }
};

4.3 State Machine Pattern — 机器人状态管理

// state_machine.hpp
template<typename State, typename Event>
class StateMachine {
public:
    using Action = std::function<void(State from, State to, const Event& evt)>;
    using Guard  = std::function<bool(const Event& evt)>;

    struct Transition {
        State from;
        State to;
        Guard guard;
        Action action;
    };

    void add_transition(State from, State to, Guard guard, Action action) {
        transitions_.push_back({from, to, guard, action});
    }

    void handle_event(const Event& evt) {
        for (auto& t : transitions_) {
            if (current_ == t.from && (!t.guard || t.guard(evt))) {
                State previous = current_;
                current_ = t.to;
                if (t.action) t.action(previous, current_, evt);
                return;
            }
        }
    }

    State current_state() const { return current_; }

private:
    State current_;
    std::vector<Transition> transitions_;
};

// 机器人主状态机
using RobotSM = StateMachine<RobotState::State, BrainCommand>;

RobotSM create_robot_state_machine(Registry& reg, EventBus& bus) {
    RobotSM sm;
    sm.start(RobotState::IDLE);

    // IDLE → NAVIGATING
    sm.add_transition(RobotState::IDLE, RobotState::NAVIGATING,
        [](const BrainCommand& cmd) { return cmd.type == BrainCommand::NAVIGATE_TO; },
        [&](RobotState::State from, RobotState::State to, const BrainCommand& cmd) {
            RCLCPP_INFO(rclcpp::get_logger("SM"), "Start navigating");
        });

    // NAVIGATING → MANIPULATING
    sm.add_transition(RobotState::NAVIGATING, RobotState::MANIPULATING,
        [](const BrainCommand& cmd) { return cmd.type == BrainCommand::EXECUTE_SKILL; },
        [&](auto from, auto to, const BrainCommand& cmd) {
            RCLCPP_INFO(rclcpp::get_logger("SM"), "Switch to manipulation: %s",
                        cmd.skill_name.c_str());
        });

    // ANY → EMERGENCY (from any state)
    for (auto s : {RobotState::IDLE, RobotState::NAVIGATING,
                   RobotState::MANIPULATING, RobotState::CHARGING}) {
        sm.add_transition(s, RobotState::EMERGENCY,
            [](const BrainCommand& cmd) { return cmd.type == BrainCommand::EMERGENCY_STOP; },
            [&](auto from, auto to, const BrainCommand& cmd) {
                bus.publish(EmergencyStopEvent{.reason = "Manual emergency stop"});
            });
    }

    // EMERGENCY → IDLE (需要手动恢复)
    sm.add_transition(RobotState::EMERGENCY, RobotState::IDLE,
        [](const BrainCommand& cmd) { return cmd.type == BrainCommand::RESUME; },
        [&](auto from, auto to, const BrainCommand&) {
            RCLCPP_INFO(rclcpp::get_logger("SM"), "Emergency cleared, resuming");
        });

    return sm;
}

4.4 Command Pattern — 动作执行与撤销

// command.hpp
class ICommand {
public:
    virtual ~ICommand() = default;
    virtual bool execute() = 0;
    virtual bool undo() = 0;
    virtual std::string name() const = 0;
};

class NavigateCommand : public ICommand {
    NavGoal goal_;
    Registry& reg_;
    Entity robot_;
    NavGoal previous_goal_;  // 用于 undo
public:
    NavigateCommand(Registry& reg, Entity robot, NavGoal goal)
        : reg_(reg), robot_(robot), goal_(std::move(goal)) {}

    bool execute() override {
        previous_goal_ = reg_.get_component<NavGoal>(robot_);
        reg_.add_component(robot_, goal_);
        return true;
    }

    bool undo() override {
        reg_.add_component(robot_, previous_goal_);
        return true;
    }

    std::string name() const override { return "NavigateCommand"; }
};

class SkillCommand : public ICommand {
    std::string skill_name_;
    std::map<std::string, std::string> params_;
    Registry& reg_;
    Entity robot_;
public:
    bool execute() override;
    bool undo() override;
    std::string name() const override { return "SkillCommand:" + skill_name_; }
};

// 命令调度器（支持队列、撤销、超时）
class CommandDispatcher {
public:
    void dispatch(std::unique_ptr<ICommand> cmd) {
        if (cmd->execute()) {
            history_.push_back(std::move(cmd));
        }
    }

    bool undo_last() {
        if (history_.empty()) return false;
        auto& cmd = history_.back();
        cmd->undo();
        history_.pop_back();
        return true;
    }

private:
    std::vector<std::unique_ptr<ICommand>> history_;
};

4.5 Mediator Pattern — 大小脑协调器

// brain_mediator.hpp — 大小脑之间的唯一协调通道
class BrainMediator {
public:
    // 大脑调用：下达指令
    void issue_command(BrainCommand cmd) {
        auto it = pending_commands_.find(cmd.command_id);
        if (it != pending_commands_.end()) {
            RCLCPP_WARN(logger_, "Duplicate command %lu", cmd.command_id);
            return;
        }
        pending_commands_[cmd.command_id] = cmd;

        // 根据优先级和安全状态决定是否立即下发
        if (can_execute(cmd)) {
            send_to_cerebellum(cmd);
        } else {
            deferred_queue_.push(cmd);
        }
    }

    // 小脑调用：汇报状态
    void report_feedback(CerebellumFeedback fb) {
        pending_commands_.erase(fb.command_id);

        // 通知大脑
        if (feedback_callback_) {
            feedback_callback_(fb);
        }

        // 如果有排队的指令，尝试执行下一个
        if (!deferred_queue_.empty() && fb.status != CerebellumFeedback::REJECTED) {
            auto next = deferred_queue_.top();
            deferred_queue_.pop();
            send_to_cerebellum(next);
        }
    }

    // 查询当前安全状态（大脑决策前可调用）
    SafetyZone::Level current_safety_level() const {
        return latest_safety_level_;
    }

    void set_feedback_callback(std::function<void(const CerebellumFeedback&)> cb) {
        feedback_callback_ = std::move(cb);
    }

private:
    bool can_execute(const BrainCommand& cmd) const {
        // 紧急停止总是立即执行
        if (cmd.type == BrainCommand::EMERGENCY_STOP) return true;
        // 安全等级为 CRITICAL 时拒绝新任务
        if (latest_safety_level_ == SafetyZone::CRITICAL) return false;
        return true;
    }

    void send_to_cerebellum(const BrainCommand& cmd) {
        // 通过 ROS2 topic 或 service 发送
        // 实际实现中调用 rclcpp publisher
    }

    std::unordered_map<uint64_t, BrainCommand> pending_commands_;
    std::priority_queue<BrainCommand> deferred_queue_;
    SafetyZone::Level latest_safety_level_{SafetyZone::CLEAR};
    std::function<void(const CerebellumFeedback&)> feedback_callback_;
    rclcpp::Logger logger_{rclcpp::get_logger("BrainMediator")};
};

4.6 Bridge Pattern — 硬件抽象

// hal.hpp — 硬件抽象层，隔离具体硬件实现
class IMotorDriver {
public:
    virtual ~IMotorDriver() = default;
    virtual void set_velocity(double left, double right) = 0;
    virtual std::pair<double, double> get_velocity() = 0;
    virtual std::pair<double, double> get_odometry() = 0;
};

class ISensorDriver {
public:
    virtual ~ISensorDriver() = default;
    virtual std::string sensor_type() const = 0;
    virtual bool initialize() = 0;
    virtual sensor_msgs::msg::PointCloud2 get_pointcloud() = 0;
};

// VMR 底盘实现
class VMRMotorDriver : public IMotorDriver {
    std::shared_ptr<slamtec::LidarRobotPlatform> platform_;
public:
    void set_velocity(double left, double right) override {
        // 调用 VMR SDK
        platform_->setVelocity(left, right);
    }
    // ...
};

// RDK 底盘实现
class RDKMotorDriver : public IMotorDriver {
    // BPU + hobot 串口通信
    // ...
};

// HAL Factory
class HALFactory {
public:
    static std::unique_ptr<IMotorDriver> create_motor_driver(const std::string& platform) {
        if (platform == "vmr") return std::make_unique<VMRMotorDriver>();
        if (platform == "rdk") return std::make_unique<RDKMotorDriver>();
        throw std::runtime_error("Unknown platform: " + platform);
    }
};

4.7 Lifecycle Pattern — ROS2 节点生命周期

// 每个核心节点都使用 LifecycleNode，支持受控启停
class CerebellumLifecycleNode : public rclcpp_lifecycle::LifecycleNode {
public:
    CerebellumLifecycleNode()
        : rclcpp_lifecycle::LifecycleNode("cerebellum") {}

    CallbackReturn on_configure(const rclcpp_lifecycle::State&) override {
        RCLCPP_INFO(get_logger(), "Configuring cerebellum...");
        // 初始化 Registry, 注册 System, 分配 Entity
        registry_ = std::make_unique<Registry>();
        robot_entity_ = registry_->create_entity();

        // 添加组件
        registry_->add_component<Transform>(robot_entity_, Transform{});
        registry_->add_component<Velocity>(robot_entity_, Velocity{});
        registry_->add_component<RobotState>(robot_entity_, RobotState{});

        // 注册策略
        auto planner = std::make_unique<BehaviorPlannerSystem>();
        planner->set_navigation_strategy("nav2");  // 默认策略

        // 注册系统
        registry_->add_system(std::make_unique<PerceptionFusionSystem>());
        registry_->add_system(std::move(planner));
        registry_->add_system(std::make_unique<VelocityControllerSystem>());
        registry_->add_system(std::make_unique<SafetyMonitorSystem>(event_bus_));
        registry_->add_system(std::make_unique<ReflexSystem>(event_bus_));

        return CallbackReturn::SUCCESS;
    }

    CallbackReturn on_activate(const rclcpp_lifecycle::State&) override {
        RCLCPP_INFO(get_logger(), "Activating cerebellum...");
        // 启动控制定时器
        control_timer_ = create_wall_timer(
            std::chrono::milliseconds(10),
            [this]() { control_tick(); });
        safety_timer_ = create_wall_timer(
            std::chrono::milliseconds(1),
            [this]() { safety_tick(); });
        return CallbackReturn::SUCCESS;
    }

    CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override {
        control_timer_.reset();
        safety_timer_.reset();
        return CallbackReturn::SUCCESS;
    }

    CallbackReturn on_cleanup(const rclcpp_lifecycle::State&) override {
        registry_.reset();
        return CallbackReturn::SUCCESS;
    }

private:
    std::unique_ptr<Registry> registry_;
    Entity robot_entity_;
    EventBus event_bus_;
    rclcpp::TimerBase::SharedPtr control_timer_;
    rclcpp::TimerBase::SharedPtr safety_timer_;
};

---

5. 项目目录结构

embodied_brain_ros2/
├── CMakeLists.txt
├── package.xml
├── AGENTS.md                          # 项目级编码规范
├── docs/
│   └── architecture.md                # 本文件
├── msg/
│   ├── BrainCommand.msg
│   ├── CerebellumFeedback.msg
│   ├── SafetyZone.msg
│   └── SceneUnderstanding.msg
├── include/embodied_brain/
│   ├── ecs/
│   │   ├── entity.hpp
│   │   ├── registry.hpp
│   │   ├── system.hpp
│   │   └── event_bus.hpp
│   ├── components/
│   │   ├── transform.hpp
│   │   ├── sensors.hpp
│   │   ├── state.hpp
│   │   ├── task.hpp
│   │   ├── perception.hpp
│   │   └── safety.hpp
│   ├── systems/
│   │   ├── large_brain/
│   │   │   ├── reasoning_system.hpp
│   │   │   └── scene_understanding_system.hpp
│   │   ├── bridge/
│   │   │   ├── perception_fusion_system.hpp
│   │   │   └── behavior_planner_system.hpp
│   │   └── small_brain/
│   │       ├── velocity_controller_system.hpp
│   │       ├── safety_monitor_system.hpp
│   │       └── reflex_system.hpp
│   ├── patterns/
│   │   ├── strategy.hpp
│   │   ├── state_machine.hpp
│   │   ├── command.hpp
│   │   ├── mediator.hpp
│   │   └── hal_bridge.hpp
│   └── nodes/
│       ├── large_brain_node.hpp
│       ├── small_brain_lifecycle_node.hpp
│       └── hal_node.hpp
├── src/
│   ├── ecs/
│   │   └── registry.cpp
│   ├── systems/
│   │   ├── large_brain/
│   │   │   ├── reasoning_system.cpp
│   │   │   └── scene_understanding_system.cpp
│   │   ├── bridge/
│   │   │   ├── perception_fusion_system.cpp
│   │   │   └── behavior_planner_system.cpp
│   │   └── small_brain/
│   │       ├── velocity_controller_system.cpp
│   │       ├── safety_monitor_system.cpp
│   │       └── reflex_system.cpp
│   ├── patterns/
│   │   ├── strategy.cpp
│   │   ├── state_machine.cpp
│   │   ├── command.cpp
│   │   └── mediator.cpp
│   ├── hal/
│   │   ├── vmr_driver.cpp
│   │   └── rdk_driver.cpp
│   ├── nodes/
│   │   ├── large_brain_node.cpp
│   │   ├── small_brain_lifecycle_node.cpp
│   │   └── hal_node.cpp
│   └── main.cpp
├── strategies/
│   ├── navigation/
│   │   ├── nav2_strategy.hpp
│   │   ├── mppi_strategy.hpp
│   │   └── dwa_strategy.hpp
│   ├── perception/
│   │   ├── yolo_strategy.hpp
│   │   ├── openvino_strategy.hpp
│   │   └── bpu_strategy.hpp
│   └── reasoning/
│       ├── llm_strategy.hpp
│       ├── vlm_strategy.hpp
│       └── behavior_tree_strategy.hpp
├── config/
│   ├── default_params.yaml
│   ├── navigation_strategies.yaml
│   └── safety_thresholds.yaml
├── launch/
│   ├── full_system.launch.py
│   ├── large_brain_only.launch.py
│   └── small_brain_only.launch.py
└── test/
    ├── test_registry.cpp
    ├── test_state_machine.cpp
    ├── test_strategy_switch.cpp
    ├── test_brain_mediator.cpp
    └── test_safety_reflex.cpp

---

6. 设计模式速查表

设计模式	应用位置	解决的问题
ECS	全局架构	继承膨胀→组合灵活；数据局部性→Cache友好
Strategy	导航/感知/推理算法	算法热替换，不同场景切换策略
Observer/EventBus	System 间通信	解耦，避免 System 直接依赖
State Machine	机器人主状态	状态转换安全、可审计
Command	任务执行	支持撤销、队列、超时管理
Mediator	大小脑协调	集中仲裁，防止大脑和小脑直接耦合
Bridge	硬件抽象层	隔离硬件差异，支持多平台
Factory	策略/驱动创建	统一创建接口，运行时按名创建
Lifecycle	ROS2 节点	受控启停，状态可查
Builder	Entity 构建	复杂实体逐步装配

---

7. 与 NVIDIA Isaac 架构的对应

Isaac 层级	本架构对应	频率	关键 System
高级推理	LargeBrainNode	1-5Hz	ReasoningSystem, SceneUnderstandingSystem
感知和规划	Bridge 层	30Hz	PerceptionFusionSystem, BehaviorPlannerSystem
实时控制框架	SmallBrainNode	100-1KHz	VelocityControllerSystem, SafetyMonitorSystem, ReflexSystem
硬件抽象层	HALNode	1-10KHz	IMotorDriver, ISensorDriver (Bridge Pattern)

---

8. 关键设计决策与理由

8.1 为什么大小脑分离而不是单节点

• 频率隔离：大脑 2Hz 和小脑 1KHz 共存会导致低频逻辑拖慢高频控制
• 故障隔离：小脑崩溃不影响大脑决策（可切换到安全模式），大脑超时不影响小脑保持安全
• 部署灵活：大脑可以跑在云端/GPU服务器，小脑必须跑在本地嵌入式

8.2 为什么用 ECS 而不是纯 OOP

• 机器人场景中 Entity 类型不断变化（新传感器、新障碍物、新任务），继承树无法预测
• System 天然并行：只读组件的系统可以并行执行（如感知融合和安全监控可同时跑）
• 测试简单：System 是纯函数，输入 Registry 输出 Registry，无需 Mock ROS 节点

8.3 为什么 Mediator 而不是直接 Topic 通信

• 大脑需要知道小脑是否"能执行"当前指令，直接 Topic 通信缺少仲裁
• 安全等级为 CRITICAL 时，Mediator 可以自动暂缓大脑的非紧急指令
• 便于实现指令优先级、排队、超时、撤销等复杂调度逻辑

8.4 为什么用 EventBus 而不是 ROS2 Topic 做内部通信

• ROS2 Topic 跨进程，有序列化和网络开销
• EventBus 进程内零拷贝，适合高频 System 间通知（1KHz 的安全事件）
• 外部通信（跨节点）仍用 ROS2 Topic/Service

---