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 { <> +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 { <> +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 { <> +ObstaclePerception() +virtual ~ObstaclePerception() +Process(const SensorFramePtr&, cv::Mat*, std::vector~cv::Rect2f~*): common::Status +GetType(): PerceptronType } class LidarProcessBase { <> +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 { <> +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 { <> +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 { <> +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 { <> +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 { <> +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系统的基础模块，提供了大量通用功能：

cpp 复制代码

// 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. 数学工具

cpp 复制代码

// 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的感知模块，负责处理来自各种传感器的数据并检测周围环境中的障碍物。

cpp 复制代码

// 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. 点云处理

cpp 复制代码

// 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. 感知主处理流程

cpp 复制代码

// 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模块负责根据当前环境和导航信息生成安全舒适的行驶轨迹。

cpp 复制代码

// 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. 规划流程

cpp 复制代码

// 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模块负责将规划模块产生的轨迹转化为具体的车辆控制指令。

cpp 复制代码

// 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. 控制算法实现

cpp 复制代码

// 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模块负责确定车辆的精确位置。

cpp 复制代码

// 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融合定位

cpp 复制代码

// 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. 工厂模式

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

cpp 复制代码

// 感知模块中的检测器工厂
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的发布-订阅机制本质上是观察者模式的实现：

cpp 复制代码

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

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

7.3. 单例模式

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

cpp 复制代码

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

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

7.4. 策略模式

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

cpp 复制代码

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中间件实现高效的通信和协作。这种架构不仅提高了系统的可维护性和可扩展性，也为自动驾驶技术的快速发展提供了坚实的基础。