Planning模块的入口为"planning_component.h"和"planning_component.cc"两个文件，实现的功能如下：

bool PlanningComponent::Init() {injector_ = std::make_shared<DependencyInjector>();/*** modules/common/configs/config_gflags.cc** DEFINE_bool(use_navigation_mode, false,*         "Use relative position in navigation mode");* **/if (FLAGS_use_navigation_mode) {// 导航规划，主要的应用场景是开放道路的自动驾驶。planning_base_ = std::make_unique<NaviPlanning>(injector_);} else {// 车道规划planning_base_ = std::make_unique<OnLanePlanning>(injector_);}/*** 该模块对应的配置文件* modules/planning/conf/planning_config.pb.txt* **/ACHECK(ComponentBase::GetProtoConfig(&config_))<< "failed to load planning config file "<< ComponentBase::ConfigFilePath();/*** modules/planning/common/planning_gflags.cc** DEFINE_bool(planning_offline_learning, false,*         "offline learning. read record files and dump learning_data");** learning_mode: NO_LEARNING* **/if (FLAGS_planning_offline_learning ||config_.learning_mode() != PlanningConfig::NO_LEARNING) {if (!message_process_.Init(config_, injector_)) {AERROR << "failed to init MessageProcess";return false;}}planning_base_->Init(config_);//  routing_response_topic: "/apollo/routing_response"routing_reader_ = node_->CreateReader<RoutingResponse>(config_.topic_config().routing_response_topic(),[this](const std::shared_ptr<RoutingResponse>& routing) {AINFO << "Received routing data: run routing callback."<< routing->header().DebugString();std::lock_guard<std::mutex> lock(mutex_);routing_.CopyFrom(*routing);});//  traffic_light_detection_topic: "/apollo/perception/traffic_light"traffic_light_reader_ = node_->CreateReader<TrafficLightDetection>(config_.topic_config().traffic_light_detection_topic(),[this](const std::shared_ptr<TrafficLightDetection>& traffic_light) {ADEBUG << "Received traffic light data: run traffic light callback.";std::lock_guard<std::mutex> lock(mutex_);traffic_light_.CopyFrom(*traffic_light);});//  planning_pad_topic: "/apollo/planning/pad"pad_msg_reader_ = node_->CreateReader<PadMessage>(config_.topic_config().planning_pad_topic(),[this](const std::shared_ptr<PadMessage>& pad_msg) {ADEBUG << "Received pad data: run pad callback.";std::lock_guard<std::mutex> lock(mutex_);pad_msg_.CopyFrom(*pad_msg);});// story_telling_topic: "/apollo/storytelling"story_telling_reader_ = node_->CreateReader<Stories>(config_.topic_config().story_telling_topic(),[this](const std::shared_ptr<Stories>& stories) {ADEBUG << "Received story_telling data: run story_telling callback.";std::lock_guard<std::mutex> lock(mutex_);stories_.CopyFrom(*stories);});/*** modules/common/configs/config_gflags.cc** DEFINE_bool(use_navigation_mode, false,*         "Use relative position in navigation mode");* **/if (FLAGS_use_navigation_mode) {relative_map_reader_ = node_->CreateReader<MapMsg>(config_.topic_config().relative_map_topic(),[this](const std::shared_ptr<MapMsg>& map_message) {ADEBUG << "Received relative map data: run relative map callback.";std::lock_guard<std::mutex> lock(mutex_);relative_map_.CopyFrom(*map_message);});}// planning_trajectory_topic: "/apollo/planning"planning_writer_ = node_->CreateWriter<ADCTrajectory>(config_.topic_config().planning_trajectory_topic());// routing_request_topic: "/apollo/routing_request"rerouting_writer_ = node_->CreateWriter<RoutingRequest>(config_.topic_config().routing_request_topic());// planning_learning_data_topic: "/apollo/planning/learning_data"planning_learning_data_writer_ = node_->CreateWriter<PlanningLearningData>(config_.topic_config().planning_learning_data_topic());return true;
}

Proc

bool PlanningComponent::Proc(const std::shared_ptr<prediction::PredictionObstacles>&prediction_obstacles,const std::shared_ptr<canbus::Chassis>& chassis,const std::shared_ptr<localization::LocalizationEstimate>&localization_estimate) {ACHECK(prediction_obstacles != nullptr);// check and process possible rerouting request/*** 检查是否需要重新规划线路。* **/// 步骤1 CheckRerouting();// process fused input data/*** struct LocalView {* std::shared_ptr<prediction::PredictionObstacles> prediction_obstacles;* std::shared_ptr<canbus::Chassis> chassis;* std::shared_ptr<localization::LocalizationEstimate> localization_estimate;* std::shared_ptr<perception::TrafficLightDetection>traffic_light;* std::shared_ptr<routing::RoutingResponse> routing;* std::shared_ptr<relative_map::MapMsg> relative_map;* std::shared_ptr<PadMessage> pad_msg;* std::shared_ptr<storytelling::Stories> stories;* };** LocalView会保存着Planning所需要的信息* **/// 步骤2local_view_.prediction_obstacles = prediction_obstacles;local_view_.chassis = chassis;local_view_.localization_estimate = localization_estimate;{// 使用 local_view_.routing 和 routing_ 对象进行比较，判断当前的路由信息是否与本地视图中的路由信息相同。// 如果本地视图中的路由信息为空，或者当前的路由信息与本地视图中的路由信息不同，则需要更新本地视图中的路由信息。std::lock_guard<std::mutex> lock(mutex_);if (!local_view_.routing ||hdmap::PncMap::IsNewRouting(*local_view_.routing, routing_)) {local_view_.routing = std::make_shared<routing::RoutingResponse>(routing_);}}{// 用于更新本地视图中的交通灯检测信息和相对地图信息std::lock_guard<std::mutex> lock(mutex_);local_view_.traffic_light = std::make_shared<TrafficLightDetection>(traffic_light_);local_view_.relative_map = std::make_shared<MapMsg>(relative_map_);}{// 用于更新本地视图中的 PAD 消息信息。具体来说，该方法会将当前的 PAD 消息信息存储到本地视图中。std::lock_guard<std::mutex> lock(mutex_);local_view_.pad_msg = std::make_shared<PadMessage>(pad_msg_);}{// 用于更新本地视图中的故事信息。具体来说，该方法会将当前的故事信息存储到本地视图中。std::lock_guard<std::mutex> lock(mutex_);local_view_.stories = std::make_shared<Stories>(stories_);}// 用于检查输入数据是否准备就绪。具体来说，// 该方法会检查本地视图中的定位、底盘、地图、相对地图或路由信息是否准备就绪，// 如果有任何一个信息不准备就绪，则返回 false，表示输入数据不可用。if (!CheckInput()) {AERROR << "Input check failed";return false;}// learning_mode: NO_LEARNING// 如果配置文件中的 learning_mode 不为 NO_LEARNING，// 则会将本地视图中的底盘、障碍物预测、路由、stories、交通灯检测和定位信息传递给 message_process_ 对象进行处理。if (config_.learning_mode() != PlanningConfig::NO_LEARNING) {// data process for online trainingmessage_process_.OnChassis(*local_view_.chassis);message_process_.OnPrediction(*local_view_.prediction_obstacles);message_process_.OnRoutingResponse(*local_view_.routing);message_process_.OnStoryTelling(*local_view_.stories);message_process_.OnTrafficLightDetection(*local_view_.traffic_light);message_process_.OnLocalization(*local_view_.localization_estimate);}// publish learning data frame for RL test// 如果配置文件中的 learning_mode 为 RL_TEST，则会从学习数据管理器中获取最新的学习数据帧，// 并将其转换为 PlanningLearningData 消息类型，然后将该消息类型发送到消息总线中。if (config_.learning_mode() == PlanningConfig::RL_TEST) {PlanningLearningData planning_learning_data;LearningDataFrame* learning_data_frame =injector_->learning_based_data()->GetLatestLearningDataFrame();if (learning_data_frame) {planning_learning_data.mutable_learning_data_frame()->CopyFrom(*learning_data_frame);common::util::FillHeader(node_->Name(), &planning_learning_data);planning_learning_data_writer_->Write(planning_learning_data);} else {AERROR << "fail to generate learning data frame";return false;}return true;}/*** ADCTrajectory在下面文件中定义：* modules/planning/proto/planning.proto* * RunOnce是规划模块的核心代码* 根据local_view_保存的信息，生成一条adc_trajectory_pb* **/// 步骤3 // 创建一个 ADCTrajectory 对象，并使用 planning_base_->RunOnce() 方法运行规划器，生成自动驾驶车辆的轨迹信息ADCTrajectory adc_trajectory_pb;planning_base_->RunOnce(local_view_, &adc_trajectory_pb);common::util::FillHeader(node_->Name(), &adc_trajectory_pb);// modify trajectory relative time due to the timestamp change in header// 获取轨迹信息中的起始时间戳，并将其赋值给 start_time 变量auto start_time = adc_trajectory_pb.header().timestamp_sec();// 计算起始时间戳与当前时间戳之间的时间差，并将其赋值给 dt 变量。const double dt = start_time - adc_trajectory_pb.header().timestamp_sec();// 获取轨迹信息中的所有轨迹点，并遍历每个轨迹点。for (auto& p : *adc_trajectory_pb.mutable_trajectory_point()) {// 对于每个轨迹点，使用 p.relative_time() 方法获取其相对时间戳，并将其加上 dt，以调整轨迹点的时间戳。p.set_relative_time(p.relative_time() + dt);}// 步骤4// 将轨迹信息发送到消息总线中planning_writer_->Write(adc_trajectory_pb);// record in history// 步骤5// 并使用 injector_->history()->Add() 方法将轨迹信息记录到历史轨迹中。auto* history = injector_->history();history->Add(adc_trajectory_pb);return true;
}/*** rerouting对应的proto* modules/planning/proto/planning_status.proto* message ReroutingStatus {*  optional double last_rerouting_time = 1;*  optional bool need_rerouting = 2 [default = false];*  optional apollo.routing.RoutingRequest routing_request = 3;* }*  **/
void PlanningComponent::CheckRerouting() {auto* rerouting = injector_->planning_context()->mutable_planning_status()->mutable_rerouting();// modules/planning/proto/planning_status.protoif (!rerouting->need_rerouting()) {return;}// 重新规划请求设置头部信息，以便其他模块可以识别该请求的来源common::util::FillHeader(node_->Name(), rerouting->mutable_routing_request());rerouting->set_need_rerouting(false);// 使用 rerouting_writer_ 将重新规划请求发送到消息总线中。rerouting_writer_->Write(rerouting->routing_request());
}bool PlanningComponent::CheckInput() {/*** ADCTrajectory在下面文件中定义：* modules/planning/proto/planning.proto* **/ADCTrajectory trajectory_pb;// 用 trajectory_pb.mutable_decision() 方法获取 ADCTrajectory 对象中的 decision 字段// 用 mutable_main_decision() 方法获取其中的 main_decision 字段// 使用 mutable_not_ready() 方法获取 main_decision 字段中的 not_ready 字段，并将其赋值给 not_ready 变量。// ADCTrajectory 是一个 protobuf 消息类型，用于存储自动驾驶车辆的轨迹信息。// 在该代码中，使用 ADCTrajectory 对象的 decision 字段来存储决策信息，// 其中的 main_decision 字段用于存储主要决策，例如是否停车、是否变道等。not_ready 字段用于表示主要决策是否准备就绪。auto* not_ready = trajectory_pb.mutable_decision()->mutable_main_decision()->mutable_not_ready();// 检查本地视图中的定位、底盘和地图信息是否准备就绪，// 如果有任何一个信息不准备就绪，则设置 not_ready 字段的原因为相应的信息未准备就绪。if (local_view_.localization_estimate == nullptr) {not_ready->set_reason("localization not ready");} else if (local_view_.chassis == nullptr) {not_ready->set_reason("chassis not ready");} else if (HDMapUtil::BaseMapPtr() == nullptr) {not_ready->set_reason("map not ready");} else {// nothing}/*** modules/common/configs/config_gflags.cc** DEFINE_bool(use_navigation_mode, false,*         "Use relative position in navigation mode");* **/// 检查本地视图中的相对地图或路由信息是否准备就绪，如果未准备就绪，则设置 not_ready 字段的原因为相应的信息未准备就绪。if (FLAGS_use_navigation_mode) {if (!local_view_.relative_map->has_header()) {not_ready->set_reason("relative map not ready");}} else {if (!local_view_.routing->has_header()) {not_ready->set_reason("routing not ready");}}// 会检查 not_ready 字段是否包含reason，如果包含reason，则表示主要决策不准备就绪，// 将原因记录到日志中，并使用 common::util::FillHeader() 方法为轨迹消息设置头部信息，然后将轨迹消息发送到消息总线中，并返回 false。if (not_ready->has_reason()) {AERROR << not_ready->reason() << "; skip the planning cycle.";common::util::FillHeader(node_->Name(), &trajectory_pb);planning_writer_->Write(trajectory_pb);return false;}return true;
}

modules/planning/on_lane_planning.cc

RunOnce

// >RunOnce() 方法运行规划器,生成自动驾驶车辆的轨迹信息，并将其存储到 ptr_trajectory_pb 指针所指向的对象中
void OnLanePlanning::RunOnce(const LocalView& local_view, ADCTrajectory* const ptr_trajectory_pb) {// when rerouting, reference line might not be updated. In this case, planning// module maintains not-ready until be restarted.static bool failed_to_update_reference_line = false;// 更新本地视图local_view_ = local_view;// 记录当前时间戳和系统时间戳const double start_timestamp = Clock::NowInSeconds();// 获取当前系统时间戳，并将其转换为秒数const double start_system_timestamp = std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count();// 用于在调试模式下输出本地视图中的定位和底盘信息,使用 ADEBUG 宏输出本地视图中的底盘信息的调试字符串，调试字符串中包含了底盘信息的各个字段的值。ADEBUG << "Get localization:" << local_view_.localization_estimate->DebugString();ADEBUG << "Get chassis:" << local_view_.chassis->DebugString();// 步骤1：使用 injector_->vehicle_state() 方法获取车辆状态管理器，// 并使用 local_view_.localization_estimate 和 local_view_.chassis 变量作为参数，// 调用 Update() 方法进行车辆状态更新。更新后的车辆状态信息存储在车辆状态管理器中。Status status = injector_->vehicle_state()->Update(*local_view_.localization_estimate, *local_view_.chassis);/*** modules/common/vehicle_state/proto/vehicle_state.proto* message VehicleState {*   optional double x = 1 [default = 0.0];*   optional double y = 2 [default = 0.0];*   optional double z = 3 [default = 0.0];*   optional double timestamp = 4 [default = 0.0];*   optional double roll = 5 [default = 0.0];*   optional double pitch = 6 [default = 0.0];*   optional double yaw = 7 [default = 0.0];*   optional double heading = 8 [default = 0.0];*   optional double kappa = 9 [default = 0.0];*   optional double linear_velocity = 10 [default = 0.0];*   optional double angular_velocity = 11 [default = 0.0];*   optional double linear_acceleration = 12 [default = 0.0];*   optional apollo.canbus.Chassis.GearPosition gear = 13;*   optional apollo.canbus.Chassis.DrivingMode driving_mode = 14;*   optional apollo.localization.Pose pose = 15;*   optional double steering_percentage = 16;* }* **/// 获取车辆状态信息VehicleState vehicle_state = injector_->vehicle_state()->vehicle_state();// 获取车辆状态信息的时间戳const double vehicle_state_timestamp = vehicle_state.timestamp();// 使用 DCHECK_GE 宏检查当前时间戳是否晚于车辆状态信息的时间戳。// 如果当前时间戳早于车辆状态信息的时间戳，则会触发断言，并输出错误信息，指出当前时间戳比车辆状态信息的时间戳早了多少秒。DCHECK_GE(start_timestamp, vehicle_state_timestamp)<< "start_timestamp is behind vehicle_state_timestamp by " << start_timestamp - vehicle_state_timestamp << " secs";// 步骤2： 使用 status.ok() 和 util::IsVehicleStateValid() 方法判断车辆状态是否有效if (!status.ok() || !util::IsVehicleStateValid(vehicle_state)) {// 如果车辆状态无效，则生成停车轨迹，并将停车原因设置为车辆状态无效const std::string msg ="Update VehicleStateProvider failed ""or the vehicle state is out dated.";AERROR << msg;ptr_trajectory_pb->mutable_decision()->mutable_main_decision()->mutable_not_ready()->set_reason(msg);// 将错误信息保存到轨迹消息的状态中status.Save(ptr_trajectory_pb->mutable_header()->mutable_status());// 将轨迹消息的档位设置为 DRIVEptr_trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);// 使用 FillPlanningPb() 方法填充轨迹消息的头部信息。FillPlanningPb(start_timestamp, ptr_trajectory_pb);// 会清空轨迹消息中的所有轨迹点，然后根据当前车辆状态生成一系列停车轨迹点，并将其添加到轨迹消息中。GenerateStopTrajectory(ptr_trajectory_pb);return;}/*** modules/planning/common/planning_gflags.cc** DEFINE_double(message_latency_threshold, 0.02,*  "Threshold for message delay");* **/// 步骤3：// 如果当前时间与车辆状态时间戳比较接近(<0.02),则更新当前车辆状态(主要是位置)if (start_timestamp - vehicle_state_timestamp < FLAGS_message_latency_threshold) {// 调用 AlignTimeStamp() 方法将车辆状态信息的时间戳校准为当前时间戳，并将校准后的车辆状态信息赋值给 vehicle_state 变量vehicle_state = AlignTimeStamp(vehicle_state, start_timestamp);}// 步骤4： 判断当前收到的routing是否和上一次相同if (util::IsDifferentRouting(last_routing_, *local_view_.routing)) {// 如果不同，则将当前路由赋值给 last_routing_ 变量last_routing_ = *local_view_.routing;ADEBUG << "last_routing_:" << last_routing_.ShortDebugString();// 清空历史信息injector_->history()->Clear();injector_->planning_context()->mutable_planning_status()->Clear();// 更新参考线提供器，并使用 planner_->Init() 方法初始化规划器。reference_line_provider_->UpdateRoutingResponse(*local_view_.routing);planner_->Init(config_);}// 步骤5：用于检查参考线提供器是否更新失败,如果更新失败，则将 failed_to_update_reference_line 变量设置为 true。failed_to_update_reference_line = (!reference_line_provider_->UpdatedReferenceLine());// 如果参考线提供器更新失败，则生成停车轨迹，并将停车原因设置为参考线更新失败if (failed_to_update_reference_line) {const std::string msg = "Failed to update reference line after rerouting.";AERROR << msg;ptr_trajectory_pb->mutable_decision()->mutable_main_decision()->mutable_not_ready()->set_reason(msg);// 使用 status.Save() 方法将错误信息保存到轨迹消息的状态中。status.Save(ptr_trajectory_pb->mutable_header()->mutable_status());// 将轨迹消息的档位设置为 DRIVEptr_trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);// 使用 FillPlanningPb() 方法填充轨迹消息的头部信息。FillPlanningPb(start_timestamp, ptr_trajectory_pb);// 使用 GenerateStopTrajectory() 方法生成停车轨迹，并返回GenerateStopTrajectory(ptr_trajectory_pb);return;}// 步骤6：将当前车辆状态信息更新到参考线提供器中，以便参考线提供器能够根据当前车辆状态信息提供相应的参考线。reference_line_provider_->UpdateVehicleState(vehicle_state);// planning is triggered by prediction data, but we can still use an estimated// cycle time for stitching/*** DEFINE_int32(planning_loop_rate, 10, "Loop rate for planning node");** **/// 步骤7：计planning周期,默认0.1秒const double planning_cycle_time = 1.0 / static_cast<double>(FLAGS_planning_loop_rate);std::string replan_reason;/**** DEFINE_uint64(trajectory_stitching_preserved_length, 20,*           "preserved points number in trajectory stitching");** **/// 步骤8：计算拼接轨迹，并将计算结果存储在 stitching_trajectory 变量中。// FLAGS_trajectory_stitching_preserved_length：接轨迹保留的长度std::vector<TrajectoryPoint> stitching_trajectory =TrajectoryStitcher::ComputeStitchingTrajectory(vehicle_state, start_timestamp, planning_cycle_time,FLAGS_trajectory_stitching_preserved_length, true,last_publishable_trajectory_.get(), &replan_reason);/*** 计算车辆的包围框* **/// 步骤9：根据拼接后轨迹中的最后一个点，更新车辆的位置、速度、加速度、朝向等信息。// 同时，在计算车辆的包围盒信息时，需要根据车辆的位置、朝向和车辆的长度、宽度等信息，计算车辆的包围盒信息。injector_->ego_info()->Update(stitching_trajectory.back(), vehicle_state);// 首先根据当前时间戳，生成一个帧编号 frame_numconst uint32_t frame_num = static_cast<uint32_t>(seq_num_++);// 步骤10： 核心代码，初始化一个新的规划帧status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state);// 检查规划帧的初始化状态，如果初始化成功，// 则调用车辆信息的 CalculateFrontObstacleClearDistance 方法，计算车辆前方障碍物的安全距离if (status.ok()) {injector_->ego_info()->CalculateFrontObstacleClearDistance(frame_->obstacles());}/*** DEFINE_bool(enable_record_debug, true,*         "True to enable record debug info in chart format");* **/// 首先检查是否启用调试记录功能。如果启用，则调用规划帧的 RecordInputDebug 方法，将调试信息记录到轨迹信息中if (FLAGS_enable_record_debug) {frame_->RecordInputDebug(ptr_trajectory_pb->mutable_debug());}// 将当前时间与之前记录的时间戳之间的差异（以毫秒为单位）设置为latency_stats消息中的init_frame_time_ms字段的值ptr_trajectory_pb->mutable_latency_stats()->set_init_frame_time_ms(Clock::NowInSeconds() - start_timestamp);if (!status.ok()) {AERROR << status.ToString();if (FLAGS_publish_estop) {// "estop" signal check in function "Control::ProduceControlCommand()"// estop_ = estop_ || local_view_.trajectory.estop().is_estop();// we should add more information to ensure the estop being triggered.ADCTrajectory estop_trajectory;// 获取 ADCTrajectory 对象的 estop 字段EStop* estop = estop_trajectory.mutable_estop();// 表示发生了紧急停车estop->set_is_estop(true);// 将错误的详细信息设置为 estop 字段的 reason 属性，以提供关于紧急停车的原因的更多信息estop->set_reason(status.error_message());//  将 status 对象的一些信息保存到 estop_trajectory 的头部status.Save(estop_trajectory.mutable_header()->mutable_status());ptr_trajectory_pb->CopyFrom(estop_trajectory);} else {ptr_trajectory_pb->mutable_decision()->mutable_main_decision()->mutable_not_ready()->set_reason(status.ToString());status.Save(ptr_trajectory_pb->mutable_header()->mutable_status());// 调用GenerateStopTrajectory()函数生成停止轨迹GenerateStopTrajectory(ptr_trajectory_pb);}// 将车辆的档位设置为驾驶模式，然后调用FillPlanningPb()函数填充规划ptr_trajectory_pbptr_trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);FillPlanningPb(start_timestamp, ptr_trajectory_pb);frame_->set_current_frame_planned_trajectory(*ptr_trajectory_pb);// 获取当前帧的序列号n，并将帧添加到帧历史记录中const uint32_t n = frame_->SequenceNum();injector_->frame_history()->Add(n, std::move(frame_));return;}// 步骤11. 它遍历参考线信息，并对每个参考线信息执行交通决策。for (auto& ref_line_info : *frame_->mutable_reference_line_info()) {TrafficDecider traffic_decider;traffic_decider.Init(traffic_rule_configs_);// ，调用TrafficDecider对象的Execute()方法执行交通决策。auto traffic_status = traffic_decider.Execute(frame_.get(), &ref_line_info, injector_);// 如果交通决策执行成功，则继续执行下一个参考线信息// 如果交通决策执行失败或参考线信息不可行，则将参考线信息标记为不可行，并输出一条警告日志。if (!traffic_status.ok() || !ref_line_info.IsDrivable()) {ref_line_info.SetDrivable(false);AWARN << "Reference line " << ref_line_info.Lanes().Id()<< " traffic decider failed";}}// 步骤12. 调用Plan()函数来规划车辆的行驶轨迹。Plan()函数接受三个参数：起始时间戳、拼接轨迹和指向轨迹protobuf的指针。status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb);for (const auto& p : ptr_trajectory_pb->trajectory_point()) {// 使用ADEBUG宏输出每个轨迹点的调试信息。ADEBUG << p.DebugString();}// 计算代码执行时间:获取当前系统时间，并计算从start_system_timestamp到当前时间的时间差const auto end_system_timestamp = std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count();const auto time_diff_ms = (end_system_timestamp - start_system_timestamp) * 1000;ADEBUG << "total planning time spend: " << time_diff_ms << " ms.";// 将计算得到的时间差保存到轨迹protobuf的延迟统计信息中ptr_trajectory_pb->mutable_latency_stats()->set_total_time_ms(time_diff_ms);ADEBUG << "Planning latency: " << ptr_trajectory_pb->latency_stats().DebugString();// 处理规划轨迹时出现的错误情况，并在必要时设置紧急停车信号，以确保车辆安全行驶。if (!status.ok()) {status.Save(ptr_trajectory_pb->mutable_header()->mutable_status());AERROR << "Planning failed:" << status.ToString();if (FLAGS_publish_estop) { // 设置紧急停车信号AERROR << "Planning failed and set estop";// "estop" signal check in function "Control::ProduceControlCommand()"// estop_ = estop_ || local_view_.trajectory.estop().is_estop();// we should add more information to ensure the estop being triggered.EStop* estop = ptr_trajectory_pb->mutable_estop();estop->set_is_estop(true);estop->set_reason(status.error_message());}}// 如果重新规划标志为true，则使用set_replan_reason()方法设置重新规划的原因ptr_trajectory_pb->set_is_replan(stitching_trajectory.size() == 1);if (ptr_trajectory_pb->is_replan()) {ptr_trajectory_pb->set_replan_reason(replan_reason);}// 首先检查车辆是否在开放空间轨迹上行驶，// 如果是，则调用FillPlanningPb()函数填充规划protobuf，并将规划protobuf设置为当前帧的规划轨迹if (frame_->open_space_info().is_on_open_space_trajectory()) {FillPlanningPb(start_timestamp, ptr_trajectory_pb);ADEBUG << "Planning pb:" << ptr_trajectory_pb->header().DebugString();frame_->set_current_frame_planned_trajectory(*ptr_trajectory_pb);} else { // 用于处理车辆在普通道路上行驶的情况// 首先添加参考线提供者的延迟统计信息到轨迹protobuf的延迟统计信息中，并设置车辆的驾驶模式为前进模式auto* ref_line_task = ptr_trajectory_pb->mutable_latency_stats()->add_task_stats();ref_line_task->set_time_ms(reference_line_provider_->LastTimeDelay() * 1000.0);ref_line_task->set_name("ReferenceLineProvider");// TODO(all): integrate reverse gearptr_trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);FillPlanningPb(start_timestamp, ptr_trajectory_pb);ADEBUG << "Planning pb:" << ptr_trajectory_pb->header().DebugString();// 将规划protobuf设置为当前帧的规划轨迹。frame_->set_current_frame_planned_trajectory(*ptr_trajectory_pb);if (FLAGS_enable_planning_smoother) {// 调用planning_smoother_.Smooth()方法对规划轨迹进行平滑处理。planning_smoother_.Smooth(injector_->frame_history(), frame_.get(), ptr_trajectory_pb);}}// 先获取当前帧的序列号n，然后调用Add()方法将当前帧添加到帧历史记录中。const uint32_t n = frame_->SequenceNum();injector_->frame_history()->Add(n, std::move(frame_));
}

ComputeStitchingTrajectory
于根据当前车辆状态信息和上一条轨迹，计算出拼接后的轨迹，并返回拼接后的轨迹。

std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(const VehicleState& vehicle_state, const double current_timestamp,const double planning_cycle_time, const size_t preserved_points_num,const bool replan_by_offset, const PublishableTrajectory* prev_trajectory,std::string* replan_reason) {/*** DEFINE_bool(enable_trajectory_stitcher, true,*    "enable stitching trajectory");* **/// 检查拼接轨迹是否启用if (!FLAGS_enable_trajectory_stitcher) {*replan_reason = "stitch is disabled by gflag.";return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);}// 首先判断上一条轨迹是否存在。如果上一条轨迹不存在，则说明需要重新规划轨迹。if (!prev_trajectory) {*replan_reason = "replan for no previous trajectory.";return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);}// 判断车辆的驾驶模式是否为完全自动驾驶模式。如果车辆的驾驶模式不是完全自动驾驶模式，则说明车辆处于手动驾驶模式。if (vehicle_state.driving_mode() != canbus::Chassis::COMPLETE_AUTO_DRIVE) {*replan_reason = "replan for manual mode.";return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);}// 于判断上一条轨迹是否为空。如果上一条轨迹为空，则需要重新规划轨迹。size_t prev_trajectory_size = prev_trajectory->NumOfPoints();if (prev_trajectory_size == 0) {ADEBUG << "Projected trajectory at time [" << prev_trajectory->header_time()<< "] size is zero! Previous planning not exist or failed. Use ""origin car status instead.";*replan_reason = "replan for empty previous trajectory.";return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);}// 首先计算出当前车辆在上一条轨迹上的相对时间 veh_rel_timeconst double veh_rel_time = current_timestamp - prev_trajectory->header_time();// 在上一条轨迹中查找当前车辆所在的轨迹点,会返回当前车辆所在的轨迹点的下标，size_t time_matched_index = prev_trajectory->QueryLowerBoundPoint(veh_rel_time);// 如果当前车辆所在的轨迹点是上一条轨迹的第一个点，并且当前时间小于上一条轨迹的起始时间// ，则说明当前时间不在上一条轨迹的时间范围内,重新规划轨迹。if (time_matched_index == 0 && veh_rel_time < prev_trajectory->StartPoint().relative_time()) {AWARN << "current time smaller than the previous trajectory's first time";*replan_reason = "replan for current time smaller than the previous trajectory's first time.";return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);}// 如果当前车辆所在的轨迹点是上一条轨迹的最后一个点，则说明当前时间超出了上一条轨迹的时间范围。此时，将重新规划if (time_matched_index + 1 >= prev_trajectory_size) {AWARN << "current time beyond the previous trajectory's last time";*replan_reason ="replan for current time beyond the previous trajectory's last time";return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);}// 获取时间戳对应的轨迹点，包括位置、速度、加速度、曲率等信息auto time_matched_point = prev_trajectory->TrajectoryPointAt(static_cast<uint32_t>(time_matched_index));// 如果该轨迹点不包含路径点信息,重新规划轨迹。if (!time_matched_point.has_path_point()) {*replan_reason = "replan for previous trajectory missed path point";return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);}// 首先根据当前车辆状态信息，包括车辆的横向和纵向位置，调用 QueryNearestPointWithBuffer 方法，// 在上一条轨迹中查找距离当前车辆最近的轨迹点。该方法会返回当前车辆所在的轨迹点的下标size_t position_matched_index = prev_trajectory->QueryNearestPointWithBuffer({vehicle_state.x(), vehicle_state.y()}, 1.0e-6);// 计算出车辆在轨迹上的横向和纵向偏差。该方法会返回一个 FrenetFramePoint 类型的结构体，// 包含了车辆在轨迹上的横向和纵向偏差auto frenet_sd = ComputePositionProjection(vehicle_state.x(), vehicle_state.y(),prev_trajectory->TrajectoryPointAt(static_cast<uint32_t>(position_matched_index)));// 首先判断是否需要根据偏移量重新规划轨迹if (replan_by_offset) {// 则计算当前车辆在轨迹上的横向和纵向偏差auto lon_diff = time_matched_point.path_point().s() - frenet_sd.first;auto lat_diff = frenet_sd.second;ADEBUG << "Control lateral diff: " << lat_diff << ", longitudinal diff: " << lon_diff;// 判断横向偏差是否超过阈值。如果横向偏差超过阈值，则说明当前车辆已经偏离了原来的轨迹，需要重新规划轨迹。if (std::fabs(lat_diff) > FLAGS_replan_lateral_distance_threshold) { // 0.5const std::string msg = absl::StrCat("the distance between matched point and actual position is too ""large. Replan is triggered. lat_diff = ",lat_diff);AERROR << msg;*replan_reason = msg;return ComputeReinitStitchingTrajectory(planning_cycle_time,vehicle_state);}// 判断纵向偏差是否超过阈值。如果纵向偏差超过阈值，则说明当前车辆已经偏离了原来的轨迹，需要重新规划轨迹。if (std::fabs(lon_diff) > FLAGS_replan_longitudinal_distance_threshold) { // 2.5const std::string msg = absl::StrCat("the distance between matched point and actual position is too ""large. Replan is triggered. lon_diff = ",lon_diff);AERROR << msg;*replan_reason = msg;return ComputeReinitStitchingTrajectory(planning_cycle_time,vehicle_state);}} else {ADEBUG << "replan according to certain amount of lat and lon offset is disabled";}// 首先计算出当前车辆在上一条轨迹上的相对时间 veh_rel_time，即当前时间戳减去上一条轨迹的起始时间戳// 根据规划周期时间，计算出下一个时刻的相对时间 forward_rel_time，即当前时间戳加上规划周期时间double forward_rel_time = veh_rel_time + planning_cycle_time;// 在上一条轨迹中查找下一个时刻的轨迹点。该方法会返回下一个时刻的轨迹点的下标，即下一个时刻的轨迹点是上一条轨迹中的第几个点。size_t forward_time_index = prev_trajectory->QueryLowerBoundPoint(forward_rel_time);ADEBUG << "Position matched index:\t" << position_matched_index;ADEBUG << "Time matched index:\t" << time_matched_index;// 首先根据当前车辆在时间轴和空间轴上匹配到的轨迹点的下标，计算出两者的最小值,作为拼接后轨迹的起始点。auto matched_index = std::min(time_matched_index, position_matched_index);// 根据保留轨迹点数和起始点的下标，截取上一条轨迹中需要保留的轨迹点，// 并将其存储在一个 std::vector 类型的变量 stitching_trajectory 中。// 最后，根据下一个时刻的轨迹点的下标，将下一个时刻的轨迹点添加到 stitching_trajectory 中。std::vector<TrajectoryPoint> stitching_trajectory(prev_trajectory->begin() + std::max(0, static_cast<int>(matched_index - preserved_points_num)),prev_trajectory->begin() + forward_time_index + 1);ADEBUG << "stitching_trajectory size: " << stitching_trajectory.size();// 首先计算出拼接后轨迹的起始位置 zero_s，即拼接后轨迹中第一个点在上一条轨迹中的位置。const double zero_s = stitching_trajectory.back().path_point().s();for (auto& tp : stitching_trajectory) {// 遍历拼接后的轨迹中的每个点，判断该点是否包含路径点信息。如果该点不包含路径点信息，则说明上一条轨迹缺失路径点。此时，将重新规划if (!tp.has_path_point()) {*replan_reason = "replan for previous trajectory missed path point";return ComputeReinitStitchingTrajectory(planning_cycle_time,vehicle_state);}// 对于每个点，将其相对时间调整为相对于当前时间戳的时间，并将其在路径上的位置调整为相对于拼接后轨迹的起始位置的距离。tp.set_relative_time(tp.relative_time() + prev_trajectory->header_time() - current_timestamp);tp.mutable_path_point()->set_s(tp.path_point().s() - zero_s);}return stitching_trajectory;
}

ComputeReinitStitchingTrajectory

src/modules/planning/common/trajectory_stitcher.cc

// 会根据当前车辆状态信息，生成一个拼接轨迹点，并将该点存储在一个只包含该点的轨迹中。
std::vector<TrajectoryPoint>
TrajectoryStitcher::ComputeReinitStitchingTrajectory(const double planning_cycle_time, const VehicleState& vehicle_state) {TrajectoryPoint reinit_point;static constexpr double kEpsilon_v = 0.1;static constexpr double kEpsilon_a = 0.4;// 如果当前车辆状态信息中的线速度和线加速度都小于对应的阈值，则直接使用当前车辆状态信息生成拼接轨迹点if (std::abs(vehicle_state.linear_velocity()) < kEpsilon_v &&std::abs(vehicle_state.linear_acceleration()) < kEpsilon_a) {reinit_point = ComputeTrajectoryPointFromVehicleState(planning_cycle_time,vehicle_state);} // 否则，使用当前车辆状态信息预测下一个时刻的车辆状态信息，并使用预测的车辆状态信息生成拼接轨迹点else {VehicleState predicted_vehicle_state;predicted_vehicle_state = VehicleModel::Predict(planning_cycle_time, vehicle_state);reinit_point = ComputeTrajectoryPointFromVehicleState(planning_cycle_time, predicted_vehicle_state);}return std::vector<TrajectoryPoint>(1, reinit_point);
}

QueryLowerBoundPoint
modules/planning/common/trajectory/discretized_trajectory.cc

size_t DiscretizedTrajectory::QueryLowerBoundPoint(const double relative_time,const double epsilon) const
{ACHECK(!empty());// 判断relative_time是否大于等于离散轨迹中最后一个点的相对时间if (relative_time >= back().relative_time()){// 如果是则返回离散轨迹中最后一个点的下标return size() - 1;}// 否则，使用std::lower_bound()函数在离散轨迹中查找相对时间最接近的点，并返回该点的下标。auto func = [&epsilon](const TrajectoryPoint& tp,const double relative_time) {return tp.relative_time() + epsilon < relative_time;};auto it_lower = std::lower_bound(begin(), end(), relative_time, func);return std::distance(begin(), it_lower); // 用于计算两个迭代器之间的距离
}

EgoInfo::Update

bool EgoInfo::Update(const common::TrajectoryPoint& start_point,const common::VehicleState& vehicle_state)
{set_start_point(start_point);set_vehicle_state(vehicle_state);CalculateEgoBox(vehicle_state);return true;
}void set_start_point(const common::TrajectoryPoint& start_point){start_point_ = start_point;// 获取车辆的参数信息，包括车辆的最大加速度和最大减速度const auto& param = ego_vehicle_config_.vehicle_param();// 在限制车辆的加速度时，需要根据车辆的最大加速度和最大减速度，限制车辆的加速度start_point_.set_a(std::fmax(std::fmin(start_point_.a(), param.max_acceleration()),param.max_deceleration()));}void set_vehicle_state(const common::VehicleState& vehicle_state){vehicle_state_ = vehicle_state;}/*** 得到车辆的包围框* **/
void EgoInfo::CalculateEgoBox(const common::VehicleState& vehicle_state) {const auto& param = ego_vehicle_config_.vehicle_param();ADEBUG << "param: " << param.DebugString();// 计算出车辆中心到车辆前后边缘中心和车辆左右边缘中心的距Vec2d vec_to_center((param.front_edge_to_center() - param.back_edge_to_center()) / 2.0,(param.left_edge_to_center() - param.right_edge_to_center()) / 2.0);Vec2d position(vehicle_state.x(), vehicle_state.y());Vec2d center(position + vec_to_center.rotate(vehicle_state.heading()));// 根据车辆的中心点、朝向、长度和宽度等信息，计算出车辆的包围盒信息 ego_box_ego_box_ = Box2d(center, vehicle_state.heading(), param.length(), param.width());
}