02_apollo_modules子模块整体软件架构深入分析文档

1. 概述

  Apollo Modules是百度Apollo自动驾驶平台的核心功能模块集合，涵盖了感知、规划、控制、定位、地图等关键功能模块。该模块集合通过Cyber RT中间件实现模块间高效通信，采用模块化设计支持独立开发、测试和部署。架构遵循高内聚、低耦合原则，为自动驾驶系统提供完整功能实现，确保实时性、可靠性和安全性。

2. 软件架构图

graph TB
    subgraph "应用层"
        A1[DreamView可视化]
        A2[仿真系统]
        A3[调试工具]
        A4[监控系统]
        A5[远程诊断]
        A6[数据记录]
    end

    subgraph "功能模块层"
        subgraph "感知模块组"
            P1[perception感知]
            P2[camera视觉感知]
            P3[lidar激光雷达感知]
            P4[radar毫米波雷达感知]
            P5[fusion传感器融合]
            P6[traffic_light交通灯识别]
        end
        
        subgraph "预测模块组"
            F1[prediction预测]
            F2[行为预测]
            F3[轨迹预测]
            F4[场景分析]
        end
        
        subgraph "规划模块组"
            L1[planning规划]
            L2[路径规划]
            L3[行为决策]
            L4[运动规划]
            L5[轨迹优化]
        end
        
        subgraph "控制模块组"
            K1[control控制]
            K2[纵向控制]
            K3[横向控制]
            K4[车辆动力学]
        end
        
        subgraph "定位模块组"
            D1[localization定位]
            D2[RTK定位]
            D3[MSF多传感器融合]
            D4[NDT匹配定位]
        end
        
        subgraph "地图模块组"
            M1[map地图]
            M2[routing路径规划]
            M3[transform坐标变换]
            M4[relative_map相对地图]
        end
        
        subgraph "车辆接口模块组"
            V1[canbus车辆总线]
            V2[车辆控制]
            V3[车辆状态]
            V4[车辆校准]
        end
        
        subgraph "其他功能模块组"
            O1[audio音频]
            O2[monitor监控]
            O3[guardian守护]
            O4[drivers驱动]
            O5[third_party_perception第三方感知]
            O6[v2x车联网]
        end
    end

    subgraph "通用模块层"
        C1[common通用组件]
        C2[common_msgs消息定义]
        C3[data数据管理]
        C4[tools工具集]
        C5[contrib贡献模块]
        C6[external_command外部命令]
    end

    subgraph "Cyber RT中间件"
        CB1[通信框架]
        CB2[任务调度]
        CB3[服务发现]
        CB4[参数管理]
        CB5[记录回放]
    end

    A1 --> CB1
    A2 --> CB1
    A3 --> CB1
    A4 --> CB1
    A5 --> CB1
    A6 --> CB1
    P1 --> CB1
    P2 --> P1
    P3 --> P1
    P4 --> P1
    P5 --> P1
    P6 --> P1
    F1 --> CB1
    F2 --> F1
    F3 --> F1
    F4 --> F1
    L1 --> CB1
    L2 --> L1
    L3 --> L1
    L4 --> L1
    L5 --> L1
    K1 --> CB1
    K2 --> K1
    K3 --> K1
    K4 --> K1
    D1 --> CB1
    D2 --> D1
    D3 --> D1
    D4 --> D1
    M1 --> CB1
    M2 --> M1
    M3 --> M1
    M4 --> M1
    V1 --> CB1
    V2 --> V1
    V3 --> V1
    V4 --> V1
    O1 --> CB1
    O2 --> CB1
    O3 --> CB1
    O4 --> CB1
    O5 --> CB1
    O6 --> CB1
    C1 --> CB1
    C2 --> CB1
    C3 --> CB1
    C4 --> CB1
    C5 --> CB1
    C6 --> CB1

    style P1 fill:#f9e79f
    style F1 fill:#d5f4e6
    style L1 fill:#85c1e9
    style K1 fill:#f8c486
    style D1 fill:#e6b0aa
    style M1 fill:#bb8fce
    style V1 fill:#7fb3d3
    style CB1 fill:#a3e4d7

3. 调用流程图

sequenceDiagram
    participant Boot as 系统启动
    participant Common as common模块
    participant Transform as transform模块
    participant Map as map模块
    participant Localization as localization模块
    participant Perception as perception模块
    participant Prediction as prediction模块
    participant Routing as routing模块
    participant Planning as planning模块
    participant Control as control模块
    participant CanBus as canbus模块
    participant Drivers as drivers模块
    participant ThirdParty as third_party_perception
    participant DreamView as DreamView
    participant Monitor as monitor模块
    participant Guardian as guardian模块

    Boot->>Common: 初始化通用组件
    Common->>Boot: 通用组件初始化完成
    Boot->>Transform: 启动坐标变换服务
    Transform->>Boot: 坐标变换服务启动完成
    Boot->>Map: 启动地图服务
    Map->>Boot: 地图服务启动完成
    
    par 并行启动
        Boot->>Localization: 启动定位模块
        Boot->>Perception: 启动感知模块
        Boot->>Routing: 启动路由模块
        Boot->>Drivers: 启动传感器驱动
    end
    
    Localization->>Boot: 定位模块启动完成
    Perception->>Boot: 感知模块启动完成
    Routing->>Boot: 路由模块启动完成
    Drivers->>Boot: 传感器驱动启动完成
    
    par 第二阶段启动
        Boot->>Prediction: 启动预测模块
        Boot->>ThirdParty: 启动第三方感知
        Boot->>Planning: 启动规划模块
    end
    
    Prediction->>Boot: 预测模块启动完成
    ThirdParty->>Boot: 第三方感知启动完成
    Planning->>Boot: 规划模块启动完成
    Boot->>Control: 启动控制模块
    Control->>Boot: 控制模块启动完成
    Boot->>CanBus: 启动车辆CAN总线接口
    CanBus->>Boot: CAN总线接口启动完成
    
    Boot->>DreamView: 启动可视化界面
    DreamView->>Boot: 可视化界面启动完成
    Boot->>Monitor: 启动监控模块
    Monitor->>Boot: 监控模块启动完成
    Boot->>Guardian: 启动守护模块
    Guardian->>Boot: 守护模块启动完成

    par 运行时数据流
        Localization->>Transform: 当前位置信息
        Perception->>Prediction: 感知结果
        ThirdParty->>Prediction: 第三方感知结果
        Prediction->>Planning: 预测轨迹
        Map->>Planning: 高精地图数据
        Routing->>Planning: 导航路线
        Planning->>Control: 控制指令
        Control->>CanBus: 车辆控制命令
        CanBus->>Drivers: 车辆状态
        Drivers->>Perception: 传感器数据
        DreamView->>Monitor: 系统状态
        Guardian->>Safety: 安全监控
    end

4. 详细UML类图

4.1. 核心模块类图

classDiagram
    class ModuleBase {
        <<abstract>>
        +ModuleBase()
        +virtual ~ModuleBase()
        +Init(): bool
        +Start(): bool
        +Stop(): bool
        +GetState(): ModuleState
        #node_: std::shared_ptr~Node~
        #module_name_: std::string
        #state_: ModuleState
        #worker_thread_: std::thread
        #is_shutdown_: std::atomic_bool
    }

    class PerceptionModule {
        +PerceptionModule()
        +virtual ~PerceptionModule()
        +Init(): bool
        +Start(): bool
        +Stop(): bool
        -InitAlgorithm(): bool
        -ProcessCameraData(): void
        -ProcessLidarData(): void
        -ProcessRadarData(): void
        -FuseSensors(): void
        -camera_process_thread_: std::thread
        -lidar_process_thread_: std::thread
        -radar_process_thread_: std::thread
        -fusion_thread_: std::thread
        -camera_process_func_: std::function~void()~
        -lidar_process_func_: std::function~void()~
        -radar_process_func_: std::function~void()~
        -fusion_func_: std::function~void()~
        -camera_objs_: std::shared_ptr~PerceptionObstacles~
        -lidar_objs_: std::shared_ptr~PerceptionObstacles~
        -radar_objs_: std::shared_ptr~PerceptionObstacles~
    }

    class PredictionModule {
        +PredictionModule()
        +virtual ~PredictionModule()
        +Init(): bool
        +Start(): bool
        +Stop(): bool
        -ProcessPrediction(): void
        -GenerateScenario(): Scenario
        -GenerateTrajectory(): vector~Trajectory~
        -prediction_thread_: std::thread
        -prediction_func_: std::function~void()~
        -scenario_analyzer_: std::unique_ptr~ScenarioAnalyzer~
        -analyzer_thread_: std::thread
    }

    class PlanningModule {
        +PlanningModule()
        +virtual ~PlanningModule()
        +Init(): bool
        +Start(): bool
        +Stop(): bool
        -InitFrame(): bool
        -ProcessPlanning(): void
        -RunTaskOnThreadPool(): void
        -RunReplanTask(): void
        -RunPlanningOnMainThread(): void
        -frame_: std::unique_ptr~Frame~
        -planning_thread_pool_: std::unique_ptr~ThreadPool~
        -replan_thread_: std::thread
        -reference_line_provider_: std::unique_ptr~ReferenceLineProvider~
        -navigator_: std::unique_ptr~Navigator~
    }

    class ControlModule {
        +ControlModule()
        +virtual ~ControlModule()
        +Init(): bool
        +Start(): bool
        +Stop(): bool
        -ProduceControlCommand(): common::Status
        -ComputeControlCommand(): common::Status
        -control_core_thread_: std::thread
        -control_reference_publisher_: std::unique_ptr~ReferenceLineProvider~
        -lon_controller_: std::unique_ptr~LonController~
        -lat_controller_: std::unique_ptr~LatController~
        -controller_agent_: std::unique_ptr~ControllerAgent~
    }

    class LocalizationModule {
        +LocalizationModule()
        +virtual ~LocalizationModule()
        +Init(): bool
        +Start(): bool
        +Stop(): bool
        -InitLocalizationCore(): bool
        -ProcessLocalization(): void
        -PublishPose(): void
        -localization_core_: std::unique_ptr~LocalizationBase~
        -pose_publisher_: std::unique_ptr~Publisher~Gps~, Publisher~Imu~, Publisher~InsStat~, Publisher~Odometry~~
        -localization_filter_: std::unique_ptr~LocalizationFilter~
    }

    class RoutingModule {
        +RoutingModule()
        +virtual ~RoutingModule()
        +Init(): bool
        +Start(): bool
        +Stop(): bool
        -InitRouter(): bool
        -ProcessRoutingRequest(): void
        -router_processor_: std::unique_ptr~Router~
        -routing_response_publisher_: std::unique_ptr~Publisher~RoutingResponse~~
        -topo_graph_: std::unique_ptr~TopoGraph~
        -routing_map_: std::unique_ptr~RoutingMap~
    }

    class CanbusModule {
        +CanbusModule()
        +virtual ~CanbusModule()
        +Init(): bool
        +Start(): bool
        +Stop(): bool
        -InitCanbus(): bool
        -CheckChassis(): bool
        -PublishChassis(): bool
        -vehicle_controller_agent_: std::unique_ptr~VehicleControllerAgent~
        -chassis_publisher_: std::unique_ptr~Publisher~Chassis~~
        -chassis_created_time_: double
        -can_sender_thread_: std::thread
    }

    class TransformModule {
        +TransformModule()
        +virtual ~TransformModule()
        +Init(): bool
        +Start(): bool
        +Stop(): bool
        -InitTf2Buffer(): bool
        -PublishStaticTransform(): void
        -tf2_buffer_: std::unique_ptr~tf2_ros::Buffer~
        -tf2_static_broadcaster_: std::unique_ptr~tf2_ros::StaticTransformBroadcaster~
        -transform_handler_: std::unique_ptr~TransformHandler~
    }

    class MonitorModule {
        +MonitorModule()
        +virtual ~MonitorModule()
        +Init(): bool
        +Start(): bool
        +Stop(): bool
        -LogMessage(const MonitorMessage&): void
        -PublishMonitor(): void
        -message_buffer_: std::vector~MonitorMessage~
        -monitor_publisher_: std::unique_ptr~Publisher~MonitorMessage~~
    }

    class GuardianModule {
        +GuardianModule()
        +virtual ~GuardianModule()
        +Init(): bool
        +Start(): bool
        +Stop(): bool
        -CheckEmergency(): bool
        -PublishEmergencyStop(): void
        -emergency_stop_publisher_: std::unique_ptr~Publisher~EmergencyStop~~
        -safety_mode_: SafetyMode
    }

    ModuleBase <|-- PerceptionModule
    ModuleBase <|-- PredictionModule
    ModuleBase <|-- PlanningModule
    ModuleBase <|-- ControlModule
    ModuleBase <|-- LocalizationModule
    ModuleBase <|-- RoutingModule
    ModuleBase <|-- CanbusModule
    ModuleBase <|-- TransformModule
    ModuleBase <|-- MonitorModule
    ModuleBase <|-- GuardianModule

    class Common {
        +math: 数学计算工具
        +util: 通用工具函数
        +adapter: 数据适配器
        +config: 配置管理
        +vehicle_state: 车辆状态
        +helpers: 辅助函数
    }

    class CommonMessages {
        +Chassis: 底盘消息
        +Localization: 定位消息
        +Perception: 感知消息
        +Prediction: 预测消息
        +Planning: 规划消息
        +TrafficLight: 交通灯消息
        +Routing: 路由消息
        +PadMessage: Pad消息
    }

    class ComponentBase {
        <<abstract>>
        +ComponentBase()
        +virtual ~ComponentBase()
        +Init(): bool
        +Process(): bool
        #node_: std::shared_ptr~Node~
    }

    class Component~M0, M1, M2~ {
        +Component()
        +Init(const ComponentConfig&, std::shared_ptr~Node~): bool
        +Process(const std::shared_ptr~M0~&, const std::shared_ptr~M1~&, const std::shared_ptr~M2~&): bool
        -RegisterMessageReaders(): void
        -Proc(const std::shared_ptr~M0~&, const std::shared_ptr~M1~&, const std::shared_ptr~M2~&): bool
    }

    PerceptionModule ..> Common
    PredictionModule ..> Common
    PlanningModule ..> Common
    ControlModule ..> Common
    LocalizationModule ..> Common
    Common ..> CommonMessages
    ModuleBase <|-- ComponentBase
    ComponentBase <|-- Component

4.2. 感知模块详细类图

classDiagram
    class Perception {
        +Perception()
        +virtual ~Perception()
        +Init(): common::Status
        +Run(const SensorFramePtr&): common::Status
        +Stop(): void
        -InitAlgorithm(): common::Status
        -ProcessLidarData(): common::Status
        -ProcessCameraData(): common::Status
        -ProcessRadarData(): common::Status
        -sensor_process_timer_: std::unique_ptr~Timer~
        -lidar_process_subscriber_: std::unique_ptr~Subscriber~PointCloud~~
        -camera_process_subscriber_: std::unique_ptr~Subscriber~Image~~
        -radar_process_subscriber_: std::unique_ptr~Subscriber~ContiRadar~~
        -perception_writer_: std::unique_ptr~Writer~PerceptionObstacles~~
        -hdmap_input_: std::unique_ptr~HDMapInput~
    }

    class ObstaclePerception {
        <<abstract>>
        +ObstaclePerception()
        +virtual ~ObstaclePerception()
        +Process(const SensorFramePtr&, cv::Mat*, std::vector~cv::Rect2f~*): common::Status
        +GetType(): PerceptronType
    }

    class LidarProcessBase {
        <<abstract>>
        +LidarProcessBase()
        +virtual ~LidarProcessBase()
        +Init(): common::Status
        +Process(const pcl_util::PointCloudConstPtr&, std::vector~base::ObjectPtr~*): common::Status
        -Configure(const libconfig::Config&): common::Status
    }

    class CnnLidarDetection {
        +CnnLidarDetection()
        +virtual ~CnnLidarDetection()
        +Init(): common::Status
        +Process(const pcl_util::PointCloudConstPtr&, std::vector~base::ObjectPtr~*): common::Status
        -CropGround(): common::Status
        -Cluster(): common::Status
        -Classify(): common::Status
        -ground_detector_: std::unique_ptr~GroundDetector~
        -clusterer_: std::unique_ptr~EuclideanCluster~
        -classifier_: std::unique_ptr~Classifier~
    }

    class CameraProcessBase {
        <<abstract>>
        +CameraProcessBase()
        +virtual ~CameraProcessBase()
        +Init(): common::Status
        +Process(const cv::Mat&, std::vector~base::ObjectPtr~*): common::Status
    }

    class CameraDetector {
        +CameraDetector()
        +virtual ~CameraDetector()
        +Init(): common::Status
        +Detect(const cv::Mat&, std::vector~base::ObjectPtr~*): common::Status
        -detect_model_: std::unique_ptr~BaseImageModel~
        -tracker_: std::unique_ptr~BaseTracker~
    }

    class FuseProcessBase {
        <<abstract>>
        +FuseProcessBase()
        +virtual ~FuseProcessBase()
        +Init(): common::Status
        +Process(const SensorFramePtr&, std::vector~base::ObjectPtr~*): common::Status
    }

    class SensorDataSync {
        +SensorDataSync()
        +virtual ~SensorDataSync()
        +AddSensorFrame(const SensorFramePtr&): void
        +GetFusedFrame(SensorFramePtr*): bool
        -sensor_frame_buff_: std::deque~SensorFramePtr~
        -sync_strategy_: std::unique_ptr~BaseSyncStrategy~
    }

    class BaseSyncStrategy {
        <<abstract>>
        +BaseSyncStrategy()
        +virtual ~BaseSyncStrategy()
        +Sync(const std::vector~SensorFramePtr~&, SensorFramePtr*): bool
    }

    class ExactSyncStrategy {
        +ExactSyncStrategy()
        +virtual ~ExactSyncStrategy()
        +Sync(const std::vector~SensorFramePtr~&, SensorFramePtr*): bool
        -exact_time_diff_threshold_: double
    }

    class ApproxSyncStrategy {
        +ApproxSyncStrategy()
        +virtual ~ApproxSyncStrategy()
        +Sync(const std::vector~SensorFramePtr~&, SensorFramePtr*): bool
        -approx_time_diff_threshold_: double
    }

    class GroundDetector {
        <<abstract>>
        +GroundDetector()
        +virtual ~GroundDetector()
        +Detect(const pcl_util::PointCloudPtr&, pcl_util::PointIndicesPtr): bool
    }

    class EuclideanCluster {
        +EuclideanCluster()
        +virtual ~EuclideanCluster()
        +SetInputCloud(const pcl_util::PointCloudConstPtr&): void
        +SetClusterSize(const std::pair~int, int~&): void
        +Cluster(std::vector~pcl_util::IndicesPtr~*): bool
        -cluster_min_size_: int
        -cluster_max_size_: int
    }

    class Classifier {
        <<abstract>>
        +Classify(const base::ObjectPtr&, FeatureVector*): bool
    }

    Perception ..> ObstaclePerception
    ObstaclePerception <|-- LidarProcessBase
    ObstaclePerception <|-- CameraProcessBase
    LidarProcessBase <|-- CnnLidarDetection
    CameraProcessBase <|-- CameraDetector
    Perception ..> FuseProcessBase
    FuseProcessBase <|-- SensorDataSync
    SensorDataSync ..> BaseSyncStrategy
    BaseSyncStrategy <|-- ExactSyncStrategy
    BaseSyncStrategy <|-- ApproxSyncStrategy
    CnnLidarDetection ..> GroundDetector
    CnnLidarDetection ..> EuclideanCluster
    CnnLidarDetection ..> Classifier

5. 状态机

5.1. 模块生命周期状态机

stateDiagram-v2
    [*] --> UNINITIALIZED: 模块创建
    UNINITIALIZED --> INITIALIZING: 初始化请求
    INITIALIZING --> INITIALIZED: 初始化成功
    INITIALIZING --> ERROR: 初始化失败
    INITIALIZED --> STARTING: 启动请求
    STARTING --> RUNNING: 启动成功
    STARTING --> ERROR: 启动失败
    RUNNING --> PAUSED: 暂停请求
    PAUSED --> RUNNING: 恢复请求
    RUNNING --> STOPPING: 停止请求
    STOPPING --> STOPPED: 停止完成
    STOPPED --> SHUTDOWN: 关闭请求
    SHUTDOWN --> TERMINATED: 关闭完成
    ERROR --> SHUTDOWN: 关闭请求
    TERMINATED --> [*]: 模块销毁
    
    note right of INITIALIZED
        模块已初始化
        资源已分配
        准备进入运行状态
    end note
    
    note right of RUNNING
        模块正在运行
        接收和处理数据
        发布输出消息
    end note
    
    note right of STOPPED
        模块已停止
        不再处理数据
        保持资源状态
    end note

5.2. 感知模块状态机

stateDiagram-v2
    [*] --> IDLE: 初始化完成
    IDLE --> WAITING_SENSOR_DATA: 等待传感器数据
    WAITING_SENSOR_DATA --> PROCESSING_LIDAR: 接收到激光雷达数据
    WAITING_SENSOR_DATA --> PROCESSING_CAMERA: 接收到摄像头数据
    WAITING_SENSOR_DATA --> PROCESSING_RADAR: 接收到雷达数据
    PROCESSING_LIDAR --> FUSION: 完成激光雷达处理
    PROCESSING_CAMERA --> FUSION: 完成摄像头处理
    PROCESSING_RADAR --> FUSION: 完成雷达处理
    FUSION --> OBJECT_DETECTION: 融合完成
    OBJECT_DETECTION --> PUBLISHING: 检测到物体
    PUBLISHING --> WAITING_SENSOR_DATA: 发布完成
    PROCESSING_LIDAR --> ERROR: 处理失败
    PROCESSING_CAMERA --> ERROR: 处理失败
    PROCESSING_RADAR --> ERROR: 处理失败
    FUSION --> ERROR: 融合失败
    ERROR --> RECOVERY: 恢复尝试
    RECOVERY --> IDLE: 恢复成功
    RECOVERY --> FAULT: 恢复失败
    FAULT --> [*]: 模块故障
    
    state RECOVERY {
        [*] --> CHECK_SENSOR_HEALTH
        CHECK_SENSOR_HEALTH --> RESTART_SENSOR_PROCESS
        RESTART_SENSOR_PROCESS --> CHECK_DATA_FLOW
        CHECK_DATA_FLOW --> IDLE: 数据流恢复正常
        CHECK_DATA_FLOW --> FAULT: 数据流未恢复
    }

5.3. 规划模块状态机

stateDiagram-v2
    [*] --> IDLE: 规划模块初始化完成
    IDLE --> RECEIVING_INPUT: 接收输入数据
    RECEIVING_INPUT --> CHECKING_AVAILABILITY: 检查规划可用性
    CHECKING_AVAILABILITY --> LOCALIZATION_OK: 定位正常
    CHECKING_AVAILABILITY --> LOCALIZATION_ERROR: 定位异常
    LOCALIZATION_OK --> PERCEPTION_OK: 感知正常
    LOCALIZATION_ERROR --> WAITING_RECOVERY: 等待恢复
    PERCEPTION_OK --> ROUTING_OK: 路由正常
    PERCEPTION_OK --> PERCEPTION_ERROR: 感知异常
    ROUTING_OK --> PLANNING: 开始规划
    PERCEPTION_ERROR --> WAITING_RECOVERY: 等待恢复
    ROUTING_OK --> ROUTING_ERROR: 路由异常
    ROUTING_ERROR --> WAITING_RECOVERY: 等待恢复
    
    PLANNING --> PATH_GENERATION: 路径生成阶段
    PATH_GENERATION --> BEHAVIOR_DECISION: 行为决策阶段
    BEHAVIOR_DECISION --> MOTION_PLANNING: 运动规划阶段
    MOTION_PLANNING --> TRAJECTORY_OPTIMIZATION: 轨迹优化阶段
    TRAJECTORY_OPTIMIZATION --> PLANNING_SUCCESS: 规划成功
    TRAJECTORY_OPTIMIZATION --> PLANNING_FAILED: 规划失败
    
    PLANNING_SUCCESS --> PUBLISHING_TRAJECTORY: 发布轨迹
    PUBLISHING_TRAJECTORY --> IDLE: 发布完成
    PLANNING_FAILED --> REPLANNING_CHECK: 检查重规划
    REPLANNING_CHECK --> PLANNING: 重新规划
    REPLANNING_CHECK --> IDLE: 放弃规划
    
    WAITING_RECOVERY --> LOCALIZATION_OK: 定位恢复
    WAITING_RECOVERY --> TIMEOUT: 恢复超时
    TIMEOUT --> EMERGENCY_STOP: 紧急停车
    EMERGENCY_STOP --> [*]: 紧急停车状态
    
    state PLANNING {
        [*] --> GENERATE_PATH
        GENERATE_PATH --> MAKE_BEHAVIOR_DECISION
        MAKE_BEHAVIOR_DECISION --> GENERATE_MOTION
        GENERATE_MOTION --> OPTIMIZE_TRAJECTORY
        OPTIMIZE_TRAJECTORY --> [*]
    }

6. 源码分析

6.1. Common 模块分析

6.1.1. 通用工具类

  Common模块是Apollo系统的基础模块，提供了大量通用功能：

// modules/common/util/util.h
namespace common {
namespace util {

// 成对值创建函数
template <typename T1, typename T2>
inline std::pair<T1, T2> MakeVal(T1 &&first, T2 &&second) {
  return std::make_pair(first, second);
}

// 指针唯一性检查
template <typename T, typename... Args>
typename std::enable_if<!std::is_array<T>::value, std::unique_ptr<T>>::type
MakeUnique(Args &&... args) {
  return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
}

// 字符串分割函数
std::vector<std::string> StringSplit(const std::string &str,
                                     const std::string &delim) {
  std::vector<std::string> tokens;
  if (str.empty()) {
    return tokens;
  }

  size_t start = 0;
  size_t end = str.find(delim);
  while (end != std::string::npos) {
    tokens.push_back(str.substr(start, end - start));
    start = end + delim.length();
    end = str.find(delim, start);
  }
  tokens.push_back(str.substr(start));

  // Remove empty strings
  auto it = std::remove_if(tokens.begin(), tokens.end(),
                           [](const std::string &s) { return s.empty(); });
  tokens.erase(it, tokens.end());

  return tokens;
}

// 文件路径处理
std::string GetFileName(const std::string &path, bool remove_extension) {
  std::string result = path;
  // Remove directory part
  size_t pos = result.find_last_of("/\\");
  if (pos != std::string::npos) {
    result = result.substr(pos + 1);
  }

  // Remove extension if required
  if (remove_extension) {
    pos = result.find_last_of(".");
    if (pos != std::string::npos) {
      result = result.substr(0, pos);
    }
  }

  return result;
}

}  // namespace util
}  // namespace common

6.1.2. 数学工具

// modules/common/math/math_utils.h
namespace common {
namespace math {

// Sigmoid函数
double Sigmoid(double x) {
  return 1.0 / (1.0 + std::exp(-x));
}

// 限制值在范围内
double Clamp(double val, double min_val, double max_val) {
  return std::min(max_val, std::max(min_val, val));
}

// 角度归一化到[-π, π]
double NormalizeAngle(const double angle) {
  double a = std::fmod(angle + M_PI, 2.0 * M_PI);
  if (a < 0.0) {
    a += (2.0 * M_PI);
  }
  return a - M_PI;
}

// 笛卡尔坐标转极坐标
std::pair<double, double> Cartesian2Polar(double x, double y) {
  double r = std::sqrt(x * x + y * y);
  double theta = std::atan2(y, x);
  return std::make_pair(r, theta);
}

// 二维向量叉积
double CrossProd(const Vec2d &start_point, const Vec2d &end_point_1,
                 const Vec2d &end_point_2) {
  return (end_point_1 - start_point).CrossProd(end_point_2 - start_point);
}

// 二维向量点积
double InnerProd(const Vec2d &start_point, const Vec2d &end_point_1,
                 const Vec2d &end_point_2) {
  return (end_point_1 - start_point).InnerProd(end_point_2 - start_point);
}

}  // namespace math
}  // namespace common

6.2. Perception 模块分析

6.2.1. 感知主类

  Perception模块是Apollo的感知模块，负责处理来自各种传感器的数据并检测周围环境中的障碍物。

// modules/perception/lib/lidar/process_base.h
class ProcessBase {
 public:
  virtual ~ProcessBase() = default;
  virtual bool Init(const libconfig::Config& config) = 0;
  virtual bool Process(const pcl_util::PointCloudConstPtr& cloud,
                       std::vector<base::ObjectPtr>* objects) = 0;
  virtual std::string name() const = 0;
};

6.2.2. 点云处理

// modules/perception/lidar/lib/segmentation/cnnseg/segmentation.h
class Segmentation {
 public:
  virtual ~Segmentation() = default;
  virtual bool Init(const SegmentOption& option) = 0;
  virtual bool Segment(const SegmentData& data,
                       SegmentResult* result) = 0;
};

6.2.3. 感知主处理流程

// modules/perception/obstacle/onboard/subnode.h
class PerceptorSSD : public Subnode {
 public:
  apollo::common::Status Init() override;
  apollo::common::Status ProcEvents() override;

 private:
  bool InitFrame();
  bool Process();
  void Publish();

  std::unique_ptr<PerceptionFactory> detector_factory_ = nullptr;
  std::unique_ptr<PerceptionFactory> tracker_factory_ = nullptr;
  std::unique_ptr<PerceptionFactory> obstacle_postprocessor_factory_ = nullptr;

  std::unique_ptr<base::BaseObstacleDetector> detector_ = nullptr;
  std::unique_ptr<base::BaseObstacleTracker> tracker_ = nullptr;
  std::unique_ptr<base::BaseObstaclePostprocessor> obstacle_postprocessor_ = nullptr;

  std::unique_ptr<base::Frame> frame_ = nullptr;
  std::shared_ptr<base::SensorFrame> sensor_frame_ = nullptr;
  std::shared_ptr<base::SyncedMultiSensorFrame> synced_multi_sensor_frame_ = nullptr;
  std::shared_ptr<base::SensorManager> sensor_manager_ = nullptr;

  std::unique_ptr<base::BaseObstacleProcessor> obstacle_processor_ = nullptr;
};

6.3. Planning 模块分析

6.3.1. 规划主类

  Planning模块负责根据当前环境和导航信息生成安全舒适的行驶轨迹。

// modules/planning/planning_base.h
class Planning {
 public:
  struct ADCTrajectory {
    double total_path_length = 0.0;
    double total_path_time = 0.0;
    std::vector<TrajectoryPoint> trajectory_point;
    std::string gear;
    LatencyStats latency_stats;
  };

  virtual ~Planning() = default;
  virtual common::Status Init() = 0;
  virtual common::Status Start() = 0;
  virtual void Stop() = 0;
  virtual std::string Name() const = 0;
  virtual common::Status Plan(const planning_internal::PlanningData& planning_data,
                              ADCTrajectory* adc_trajectory) = 0;
};

6.3.2. 规划流程

// modules/planning/common/planning_gflags.h
// 规划模块的全局配置参数
DECLARE_string(localization_topic);
DECLARE_string(chaos_topic);
DECLARE_double(planning_upper_speed_limit);
DECLARE_int32(planning_thread_num);

// modules/planning/on_lane_planning.cc
class OnLanePlanning : public Planning {
 public:
  common::Status Init() override;
  common::Status Start() override;
  void Stop() override;
  std::string Name() const override;
  common::Status Plan(const common::TrajectoryPoint& planning_start_point,
                      const ADCTrajectory* reference_line_info,
                      ADCTrajectory* ptr_trajectory) override;

 private:
  std::unique_ptr<Frame> CreateFrame(
      const common::TrajectoryPoint& planning_start_point,
      const ADCTrajectory* reference_line_info);
  void RebuildLaneStitcher(const ADCTrajectory* reference_line_info);
  void ComposeTrajectory(const STGraph& speed, const Path& path,
                         ADCTrajectory* ptr_trajectory);

  std::unique_ptr<Frame> frame_ = nullptr;
  std::unique_ptr<ReferenceLineProvider> reference_line_provider_ = nullptr;
  std::unique_ptr<STGraph> st_graph_ = nullptr;
  std::unique_ptr<PathTimeGraph> path_time_graph_ = nullptr;
  std::unique_ptr<Planner> planner_ = nullptr;
  std::unique_ptr<Controller> controller_ = nullptr;
};

6.4. Control 模块分析

6.4.1. 控制主类

  Control模块负责将规划模块产生的轨迹转化为具体的车辆控制指令。

// modules/control/control_base.h
class Control {
 public:
  struct ControlCommand {
    double steering_target = 0.0;
    double steering_rate = 0.0;
    double throttle = 0.0;
    double brake = 0.0;
    bool parking_brake = false;
    Header header;
  };

  virtual ~Control() = default;
  virtual common::Status Init(const ControlDtcz& control_dtcz) = 0;
  virtual common::Status ComputeControlCommand(
      const localization::LocalizationEstimate* localization,
      const canbus::Chassis* chassis,
      const planning::ADCTrajectory* trajectory,
      ControlCommand* cmd) = 0;
  virtual void Reset() = 0;
  virtual std::string Name() const = 0;
};

6.4.2. 控制算法实现

// modules/control/controller/lon_controller.h
class LonController {
 public:
  LonController();
  virtual ~LonController() = default;

  common::Status Init(const ControllerConf& controller_conf) override;
  common::Status ComputeControlCommand(
      const localization::LocalizationEstimate& localization,
      const canbus::Chassis& chassis,
      const planning::ADCTrajectory& trajectory,
      ControlCommand* cmd) override;
  void Reset() override;

 private:
  double ComputeLongitudinalErrors(const TrajectoryAnalyzer& trajectory_analyzer,
                                   const localization::LocalizationEstimate& pose,
                                   const canbus::Chassis& chassis);

  // Control command computation
  double ComputeLongitudinalAcc(const TrajectoryAnalyzer& trajectory_analyzer,
                                const localization::LocalizationEstimate& pose,
                                const canbus::Chassis& chassis,
                                const double preview_time);

  // Input and gain scheduling
  double QueryLatencyCompensation(
      const TrajectoryAnalyzer& trajectory_analyzer,
      const localization::LocalizationEstimate& pose,
      const double preview_time);

  ControllerConf controller_conf_;
  std::unique_ptr<PidController> speed_pid_controller_;
  std::unique_ptr<PidController> station_pid_controller_;
  std::unique_ptr<LeadlagController> feedforward_controller_;

  // Vehicle parameter
  double vehicle_mass_ = 0.0;
  double wind_resistance_coefficient_ = 0.0;
  double rolling_resistance_coefficient_ = 0.0;
  double gravity_ = 0.0;

  // Control state
  double previous_latacc_ = 0.0;
  double previous_acceleration_ = 0.0;
  double previous_speed_ = 0.0;
  double previous_station_reference_ = 0.0;
  double previous_speed_reference_ = 0.0;
  double current_station_error_ = 0.0;
  double current_speed_error_ = 0.0;
  double current_acceleration_reference_ = 0.0;
  double current_speed_reference_ = 0.0;
  double current_station_reference_ = 0.0;
};

6.5. Localization 模块分析

6.5.1. 定位主类

  Localization模块负责确定车辆的精确位置。

// modules/localization/localization_base.h
class Localization {
 public:
  struct LocalizedEstimate {
    localization::LocalizationEstimate pose;
    double local_time = 0.0;
    double received_time = 0.0;
    double delay_time = 0.0;
  };

  virtual ~Localization() = default;
  virtual bool Init() = 0;
  virtual void Start() = 0;
  virtual void Stop() = 0;
  virtual void GetLocalization(LocalizedEstimate* estimate) = 0;
  virtual std::string Name() = 0;
};

6.5.2. MSF融合定位

// modules/localization/msf/localization_msf.h
class LocalizationMsf {
 public:
  LocalizationMsf();
  virtual ~LocalizationMsf();

  bool Init(const LocalizationMsfConfig& config);
  bool Process(const msf_gnss::Gnss& gnss_msg,
               const msf_gnss::InsStat& ins_stat,
               const drivers::gnss::EpochObservation& obs_msg,
               const drivers::gnss::GnssEphemeris& eph_msg,
               const localization::CorrectedImu& imu_msg,
               const localization::Gps& gps_msg,
               const std::string& time_span,
               localization::LocalizationEstimate* localization);

 private:
  bool InitLocalizationBase(const LocalizationMsfConfig& config);
  bool InitGnssLocalization(const GnssConfiguration& config);
  bool InitRtkLocalization(const RtkConfiguration& config);
  bool InitLocalVisualizer(const LocalizationMsfConfig& config);

  std::unique_ptr<GnssRtkCorrector> gnss_rtk_corrector_;
  std::unique_ptr<LocalCartesian> local_cartesian_ptr_;
  std::unique_ptr<ImuInitializer> imu_initializer_;
  std::unique_ptr<MsdekfLocVelFusor> msdekf_loc_vel_fusor_;
  std::unique_ptr<ImuFactor> imu_factor_;
  std::unique_ptr<MeasurePointFactor> measure_point_factor_;
  std::unique_ptr<MeasureVelocityFactor> measure_velocity_factor_;

  LocalizationMsfConfig config_;
  std::mutex mutex_;
};

7. 设计模式

7.1. 工厂模式

  在各个模块中广泛使用了工厂模式来创建不同类型的实例：

// 感知模块中的检测器工厂
class DetectorFactory {
 public:
  template <typename T>
  static bool Register(const std::string& type, Creator creator) {
    creators_[type] = creator;
    return true;
  }

  static std::unique_ptr<Detector> Create(const std::string& type) {
    auto it = creators_.find(type);
    if (it != creators_.end()) {
      return it->second();
    }
    return nullptr;
  }

 private:
  using Creator = std::function<std::unique_ptr<Detector>()>;
  static std::map<std::string, Creator> creators_;
};

// 使用示例
class CNNDetector : public Detector {
  // 实现细节
};

// 注册
DetectorFactory::Register<CNNDetector>("cnn_detector", 
    []() { return std::make_unique<CNNDetector>(); });

// 创建实例
auto detector = DetectorFactory::Create("cnn_detector");

7.2. 观察者模式

  Cyber RT的发布-订阅机制本质上是观察者模式的实现：

// 消息订阅
auto listener = [](const std::shared_ptr<PerceptionObstacles>& obstacles) {
    // 处理感知障碍物消息
    ProcessPerceptionObstacles(*obstacles);
};

auto perception_reader = 
    node->CreateReader<PerceptionObstacles>(FLAGS_perception_obstacle_topic, listener);

7.3. 单例模式

  许多全局管理类使用单例模式：

class Singleton {
 public:
  static Singleton* Instance() {
    static Singleton instance;
    return &instance;
  }

 private:
  Singleton() = default;
  ~Singleton() = default;
};

7.4. 策略模式

  控制模块中使用策略模式实现不同的控制算法：

class Controller {
 public:
  virtual ~Controller() = default;
  virtual common::Status Init(const ControlConf& control_conf) = 0;
  virtual common::Status ComputeControlCommand(
      const localization::LocalizationEstimate& localization,
      const canbus::Chassis& chassis,
      const planning::ADCTrajectory& trajectory,
      ControlCommand* cmd) = 0;
  virtual void Reset() = 0;
  virtual std::string Name() const = 0;
};

class LonController : public Controller {
  // 纵向控制策略实现
};

class LatController : public Controller {
  // 横向控制策略实现
};

// 控制器管理器
class ControllerAgent {
 public:
  void InitController() {
    // 根据配置创建不同的控制器
    lon_controller_ = Factory::Create(FLAGS_lon_controller);
    lat_controller_ = Factory::Create(FLAGS_lat_controller);
  }
  
 private:
  std::unique_ptr<Controller> lon_controller_;
  std::unique_ptr<Controller> lat_controller_;
};

8. 总结

  Apollo Modules子模块通过高度模块化的设计，将复杂的自动驾驶系统分解为多个功能明确、接口清晰的模块。每个模块都可以独立开发、测试和优化，同时通过Cyber RT中间件实现高效的通信和协作。这种架构不仅提高了系统的可维护性和可扩展性，也为自动驾驶技术的快速发展提供了坚实的基础。