在这里插入图片描述

前言

本文记录学习planning模块时的一些笔记，总体流程参照https://zhuanlan.zhihu.com/p/61982682中的流程图，如上图所示。

planning_component

modules/planning/planning_component.cc
在这里插入图片描述

PlanningComponent::Init部分首先完成规划模式的选择：

  if (FLAGS_use_navigation_mode) {
    planning_base_ = std::make_unique<NaviPlanning>(injector_);
  } else {
    planning_base_ = std::make_unique<OnLanePlanning>(injector_);
  }

再完成相应的消息订阅

routing_reader_ = node_->CreateReader<RoutingResponse>(
traffic_light_reader_ = node_->CreateReader<TrafficLightDetection>(
pad_msg_reader_ = node_->CreateReader<PadMessage>(
story_telling_reader_ = node_->CreateReader<Stories>(
relative_map_reader_ = node_->CreateReader<MapMsg>(

最后消息发布

  planning_writer_ = node_->CreateWriter<ADCTrajectory>(
  rerouting_writer_ = node_->CreateWriter<RoutingRequest>(
  planning_learning_data_writer_ = node_->CreateWriter<PlanningLearningData>(

PlanningComponent::Proc的主要是检查数据，并且执行注册好的Planning，生成路线并且发布。

bool PlanningComponent::Proc(...) {
  // 1. 检查是否需要重新规划线路。
  CheckRerouting();
  // 2. 数据放入local_view_中，并且检查输入数据。
  ...
  // 3. 执行注册好的Planning，生成线路。
  planning_base_->RunOnce(local_view_, &adc_trajectory_pb);
  // 4. 发布消息
  planning_writer_->Write(std::make_shared<ADCTrajectory>(adc_trajectory_pb));
  // 5. 最后记录
  history->Add(adc_trajectory_pb);
}

RunOnce在NaviPlanning和OnLanePlanning都有，本文以常用的OnLanePlanning为例。

PS1

prediction_obstacles 从channel中获得的指针，类型为 PredictionObstacles，代码文件在 modules/prediction/proto/prediction_obstacle.proto 中，该类型的消息来自预测模块，包含了以下信息：
- Header 头结构：包含了这条消息发布的时刻（以秒为单位），目前位置的模块名，该消息的序列号（每个模块各自维护的）等，几乎每个消息都包含有这个头结构，下面就不提及了。
- PredictionObstacle 预测模块中的若干个障碍物行为：有感知模块中的障碍物（PerceptionObstacle），它包括障碍物的 id，它的三维坐标、速度加速度、边界大小（长高宽）、特殊的形状、类型（行人、非机动车、机动车）、运动轨迹等；还有时间戳，记录 GPS 给出的时刻；还有预测的时间长度；多种可能的运动轨迹；障碍物的运动趋势、优先级等。
- 开始的时间戳：记录预测开始的时刻
- 结束时间戳：预测结束的时刻
- 自动驾驶车辆的运动趋势，停止、正常行驶还是正在变道等
- Scenery 现在的场景
chassis 从channel获得的指针，类型为 Chassis，这条消息直接来自总线 CanBus，代码文件在 modules/canbus/proto/chassis.proto 中，该类包含了很多信息，主要是汽车底盘所给出的机械状态信息，比如：
- 驾驶模式，有手动驾驶、自动驾驶、仅控制方向、仅控制速度以及紧急模式
- 档位情况、引擎转速、车辆速度、里程表、燃油情况、电池电量等
- 刹车、油门踏板的力度，方向盘的旋转角度，车轮转速
- 转向灯、雾灯、大灯的工作情况
localization_estimate 从channel中获得的指针，类型为 LocalizationEstimate，代码文件在 modules/localization/proto/localization.proto 中，这条消息来自定位模块，该类主要包含了：
- Pose 位置姿势，包括车头朝向，在地图上的线速度、线加速度和相对位置，角速度、仰角度等
- 测量上述姿势的时刻
- 车辆已经经过的轨迹点列
- MSF 定位状态与质量

PS2

std::mutex是C++11标准中提供的一种互斥锁机制，用于保护共享资源的访问。当多个线程需要同时访问共享资源时，为了避免出现数据竞争等问题，需要使用互斥锁进行同步控制。

std::mutex是一种基本的互斥锁类型，可以通过lock()函数锁定互斥锁，通过unlock()函数释放互斥锁。当一个线程持有互斥锁时，其他线程想要获取该互斥锁将会被阻塞，直到持有该互斥锁的线程将锁释放。

例如，当多个线程需要同时访问同一个共享变量时，可以在访问之前使用std::mutex进行锁定，保证每个线程都可以安全地访问该变量。例如：

#include <mutex>
#include <thread>
#include <iostream>

int shared_data = 0;
std::mutex mutex;

void work(int id) {
    for (int i = 0; i < 10000; ++i) {
        mutex.lock();
        shared_data++;
        mutex.unlock();
    }
    std::cout << "Thread " << id << " finished." << std::endl;
}

int main() {
    std::thread t1(work, 1);
    std::thread t2(work, 2);
    t1.join();
    t2.join();
    std::cout << "Shared data: " << shared_data << std::endl;
    return 0;
}

在上述例子中，两个线程都会对shared_data变量进行10000次的自增操作，为了避免出现数据竞争，我们在对shared_data进行访问之前都使用了std::mutex进行了锁定，保证了线程的安全性。

on_lane_planning

modules/planning/on_lane_planning.cc
在这里插入图片描述

OnLanePlanning::Init主要实现分配具体的Planner，启动参考线提供器(reference_line_provider_)。启动参考线提供器，会另启动一个线程，执行一个定时任务，每隔50ms提供一次参考线。注意：分配Planner的部分在OnLanePlanning实例化时就已经完成。

分配Planner的实现部分

modules/planning/planner/on_lane_planner_dispatcher.cc
std::unique_ptr<Planner> OnLanePlannerDispatcher::DispatchPlanner(
    const PlanningConfig& planning_config,
    const std::shared_ptr<DependencyInjector>& injector) {
  return planner_factory_.CreateObject(
      planning_config.standard_planning_config().planner_type(0), injector);
}

可以在modules/planning/conf/planning_config.pb.txt配置文件中看到配置Planner的类型：

standard_planning_config {
  planner_type: PUBLIC_ROAD
  planner_public_road_config {
  }
}

OnLanePlanning 的主逻辑在OnLanePlanning::RunOnce中，内容比较多，有些还没仔细看，大致完成了一下的内容：

void OnLanePlanning::RunOnce(const LocalView& local_view,
                             ADCTrajectory* const ptr_trajectory_pb) {
    //  更新汽车状态和参考线的状态，如果状态无效，直接返回
    //  ...  
    // 是否为出现导航路线变换，如果是 初始化 planner
    //  加上预估的规划触发的周期 得到 stitchingTrajectory
  // planning is triggered by prediction data, but we can still use an estimated
  // cycle time for stitching
  status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state);
    //  判断是否符合交通规则
    //  开始正在的规划 planner 开始规划
  status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb);
    //  记录规划所花费的时间
    //  ...
}

status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb);中有如下代码，此部分的Plan是指分配的planner的Plan()函数

  auto status = planner_->Plan(stitching_trajectory.back(), frame_.get(),
                               ptr_trajectory_pb);

PS1

make_unique是一个C++11中的函数模板，用于创建并返回一个独占指针（unique_ptr）。它的作用是将一个裸指针封装成一个unique_ptr对象，并确保该对象是唯一持有它所指向的对象。这可以有效避免内存泄漏和多个指针指向同一个对象的问题。

public_road_planner

modules/planning/planner/public_road/public_road_planner.cc
接着来看public_road_planner的实现。

在这里插入图片描述

PublicRoadPlanner::Init实例化一个全局的scenario_manager_对象来进行场景管理。

Status PublicRoadPlanner::Init(const PlanningConfig& config) {
  config_ = config;
  scenario_manager_.Init(config);
  return Status::OK();
}

PublicRoadPlanner::Plan主要完成以下工作

Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point,
                               Frame* frame,
                               ADCTrajectory* ptr_computed_trajectory) {
  // 更新场景，决策当前应该执行什么场景
  scenario_manager_.Update(planning_start_point, *frame);
  // 获取当前场景
  scenario_ = scenario_manager_.mutable_scenario();
  // 调用Scenario的Process函数，对具体的场景进行处理
  auto result = scenario_->Process(planning_start_point, frame);

  if (FLAGS_enable_record_debug) {
    auto scenario_debug = ptr_computed_trajectory->mutable_debug()
                              ->mutable_planning_data()
                              ->mutable_scenario();
    scenario_debug->set_scenario_type(scenario_->scenario_type());
    scenario_debug->set_stage_type(scenario_->GetStage());
    scenario_debug->set_msg(scenario_->GetMsg());
  }
  // 当前场景完成
  if (result == scenario::Scenario::STATUS_DONE) {
    // only updates scenario manager when previous scenario's status is
    // STATUS_DONE
    scenario_manager_.Update(planning_start_point, *frame);
  } else if (result == scenario::Scenario::STATUS_UNKNOWN) {
    // 当前场景失败
    return Status(common::PLANNING_ERROR, "scenario returned unknown");
  }
  return Status::OK();
}