当前位置： 首页 > ai >正文

Apollo学习——planning模块（2）之planning_component

ai 2025/8/26 8:29:09

planning_component: 包含planning组件类和planning程序的启动以及配置文件。

1.Init流程

在这里插入图片描述

1、初始化依赖注入器injector_，用来缓存数据使用

  //依赖注入器 数据缓存使用injector_ = std::make_shared<DependencyInjector>();

2、根据标志位FLAGS_use_navigation_mode，判断使用那种规划器

  //选择规划器类型if (FLAGS_use_navigation_mode) {planning_base_ = std::make_unique<NaviPlanning>(injector_); // 基于相对地图的导航规划（适用于高速公路等简单场景）。} else {planning_base_ = std::make_unique<OnLanePlanning>(injector_); // 默认的高精地图轨迹规划器}

3、更新配置及参数

  ACHECK(ComponentBase::GetProtoConfig(&config_)) // 从planning_config.pb.txt加载配置 config_就是这个文件<< "failed to load planning config file "<< ComponentBase::ConfigFilePath(); // 配置文件路径由DAG文件中的config_file_path定义。

4、planning_base初始化

  // 规划器初始化//   子步骤：//     参考线线程：启动ReferenceLineProvider线程生成平滑参考线。//     场景加载：根据配置初始化交通规则和场景（如变道、借道等）。//     规划器注册：默认使用PublicRoadPlanner planning_base_->Init(config_);

5、创建读写器
reader

reader发布模块channal数据结构功能
planning_command_reader_external_command/apollo/planning/commandPlanningCommand
traffic_light_reader_perception/apollo/perception/traffic_lightTrafficLightDetection
pad_msg_reader_external_command/apollo/planning/padPadMessage
story_telling_reader_storytelling/apollo/storytellingStories
control_interactive_reader_control/apollo/control/interactiveControlInteractiveMsg
relative_map_reader_tool/apollo/relative_mapMapMsg

writer

writerchannal数据结构功能
planning_writer_/apollo/planningADCTrajectory
command_status_writer_/apollo/planning/command_statusexternal_command::CommandStatus

client

clientservice模块channal数据结构作用
rerouting_client_external_command/apollo/external_command/lane_followapollo::external_command::LaneFollowCommand,external_command::CommandStatus

代码如下:

  // 创建读写器// 规划命令   由external_command模块的/apollo/planning/command发布planning_command_reader_ = node_->CreateReader<PlanningCommand>(config_.topic_config().planning_command_topic(),[this](const std::shared_ptr<PlanningCommand>& planning_command) {AINFO << "Received planning data: run planning callback."<< planning_command->header().DebugString();std::lock_guard<std::mutex> lock(mutex_);planning_command_.CopyFrom(*planning_command);});//交通灯  由/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);});//Pad消息 由external_command模块的/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);});// 如果故事包含“紧急避让”指令，规划器会生成相应的避障轨迹。  Stories来自场景管理模块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);});// 接收来自控制模块或其他系统的交互指令 收到“紧急停车”指令时，规划器会立即生成减速至停车的轨迹 来自控制模块control_interactive_reader_ = node_->CreateReader<ControlInteractiveMsg>(config_.topic_config().control_interative_topic(),[this](const std::shared_ptr<ControlInteractiveMsg>&control_interactive_msg) {ADEBUG << "Received story_telling data: run story_telling callback.";std::lock_guard<std::mutex> lock(mutex_);control_interactive_msg_.CopyFrom(*control_interactive_msg);});// 仅在导航模式 启用 依赖相对地图而非高精地图 来自感知模块// 导航模式：在无高精地图区域（如停车场），使用相对地图进行局部路径规划。// 动态避障：结合实时地图数据调整轨迹以避开临时障碍物。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_writer_ = node_->CreateWriter<ADCTrajectory>(config_.topic_config().planning_trajectory_topic());//创建客户端 创建重路由客户端，用于在需要重新规划路径时发送请求rerouting_client_ = node_->CreateClient<apollo::external_command::LaneFollowCommand,external_command::CommandStatus>(config_.topic_config().routing_request_topic()); // /apollo/external_command/lane_followplanning_learning_data_writer_ = node_->CreateWriter<PlanningLearningData>(config_.topic_config().planning_learning_data_topic());command_status_writer_ = node_->CreateWriter<external_command::CommandStatus>(FLAGS_planning_command_status);

2、Proc流程

1、检查路由是否正常，如果不正常调用rerouting_client_重新请求路由；
2、将接收到的数据统一存到local_view_；
3、对输入数据进行处理（向traj中添加车道线边界）并检查数据；如果异常，发布空路径返回false；
4、运行planning_base_->RunOnce(local_view_, &adc_trajectory_pb)进行路径规划。
5、对规划好的轨迹adc_trajectory_pb填充时间戳、序列号等消息。
6、设置车辆位置以及车道线信息；
7、遍历轨迹，为轨迹点增加时间信息；
8、发布规划planning_writer_->Write(adc_trajectory_pb);
9、判断状态发布状态command_status_writer_->Write(command_status);
10、记录历史数据 history->Add(adc_trajectory_pb);
代码注释：

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);AINFO <<   "Received data: run planning callback................................................";// 1、check and process possible rerouting request 重路由检查 CheckRerouting();// 2、process fused input data 输入检查与数据融合： 将预测障碍物、底盘状态、定位信息融合到local_view_结构体中，供后续规划使用local_view_.prediction_obstacles = prediction_obstacles;local_view_.chassis = chassis;local_view_.localization_estimate = localization_estimate;{std::lock_guard<std::mutex> lock(mutex_);if (!local_view_.planning_command || !common::util::IsProtoEqual(local_view_.planning_command->header(), planning_command_.header())) {local_view_.planning_command = std::make_shared<PlanningCommand>(planning_command_);}}{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_);}{std::lock_guard<std::mutex> lock(mutex_);if (!local_view_.pad_msg || !common::util::IsProtoEqual(local_view_.pad_msg->header(), pad_msg_.header())) {// Check if "CLEAR_PLANNING" PadMessage is received and process.if (pad_msg_.action() == PadMessage::CLEAR_PLANNING) {local_view_.planning_command = nullptr;planning_command_.Clear();}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_);}{std::lock_guard<std::mutex> lock(mutex_);if (!local_view_.control_interactive_msg || !common::util::IsProtoEqual(local_view_.control_interactive_msg->header(), control_interactive_msg_.header())) {local_view_.control_interactive_msg = std::make_shared<ControlInteractiveMsg>(control_interactive_msg_);}}//3、对输入数据进行检查 如果数据确实，反馈空路径if (!CheckInput()) {AINFO << "Input check failed";return false;}// 4、在非NO_LEARNING模式下，将输入数据（如底盘信息、障碍物预测）传递给学习模块if (config_.learning_mode() != PlanningConfig::NO_LEARNING) {// data process for online trainingmessage_process_.OnChassis(*local_view_.chassis);message_process_.OnPrediction(*local_view_.prediction_obstacles);if (local_view_.planning_command->has_lane_follow_command()) {message_process_.OnRoutingResponse(local_view_.planning_command->lane_follow_command());}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 强化学习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;}// 5、规划执行// 核心逻辑：//   参考线生成：基于高精地图或相对地图生成参考线。//   障碍物决策：结合预测结果进行避障决策（如停车、绕行）。//   轨迹优化：通过QP/Dynamic Programming优化路径和速度ADCTrajectory adc_trajectory_pb;// local_view_：融合了感知、定位、底盘、交通灯等实时数据的结构体。// adc_trajectory_pb：输出的轨迹容器（Protobuf格式）。planning_base_->RunOnce(local_view_, &adc_trajectory_pb);// 参考线生成  障碍物决策 轨迹优化auto start_time = adc_trajectory_pb.header().timestamp_sec();common::util::FillHeader(node_->Name(), &adc_trajectory_pb);// 填充头消息，增加序列号和时间戳// 6、设置车辆位置信息SetLocation(&adc_trajectory_pb);// 7、modify trajectory relative time due to the timestamp change in header 时间补偿处理const double dt = start_time - adc_trajectory_pb.header().timestamp_sec();for (auto& p : *adc_trajectory_pb.mutable_trajectory_point()) {//遍历轨迹点 将每个点的时间增加dtp.set_relative_time(p.relative_time() + dt);}// 8、轨迹发布与状态反馈 发布规划轨迹（ADCTrajectory）供控制模块执行planning_writer_->Write(adc_trajectory_pb);// 9、Send command execution feedback.// Error occured while executing the command. 初始化状态消息external_command::CommandStatus command_status;common::util::FillHeader(node_->Name(), &command_status);//设置状态消息的头部（模块名、时间戳）。if (nullptr != local_view_.planning_command) {command_status.set_command_id(local_view_.planning_command->command_id()); //关联当前规划命令的ID（用于多命令跟踪）。}ADCTrajectory::TrajectoryType current_trajectory_type = adc_trajectory_pb.trajectory_type();if (adc_trajectory_pb.header().status().error_code() != common::ErrorCode::OK) {command_status.set_status(external_command::CommandStatusType::ERROR); // 轨迹生成失败（如输入数据异常）。command_status.set_message(adc_trajectory_pb.header().status().msg());} else if (planning_base_->IsPlanningFinished(current_trajectory_type)) { AINFO << "Set the external_command: FINISHED";command_status.set_status(external_command::CommandStatusType::FINISHED); //规划任务完成（如到达目的地）。} else {AINFO << "Set the external_command: RUNNING";command_status.set_status(external_command::CommandStatusType::RUNNING);// 规划正常执行中。}// 反馈命令执行状态（RUNNING/FINISHED/ERROR）command_status_writer_->Write(command_status);// 10、record in history 记录历史数据 用于异常处理和数据回溯auto* history = injector_->history();history->Add(adc_trajectory_pb);return true;
}
// 重路由检查  若PlanningContext标记需要重路由，通过rerouting_client_发送请求更新路径
void PlanningComponent::CheckRerouting() {auto* rerouting = injector_->planning_context()->mutable_planning_status()->mutable_rerouting();if (!rerouting->need_rerouting()) {return;}common::util::FillHeader(node_->Name(), rerouting->mutable_lane_follow_command());auto lane_follow_command_ptr = std::make_shared<apollo::external_command::LaneFollowCommand>(rerouting->lane_follow_command());rerouting_client_->SendRequest(lane_follow_command_ptr);rerouting->set_need_rerouting(false);
}bool PlanningComponent::CheckInput() {ADCTrajectory trajectory_pb;//填充轨迹中的车辆位置和车道边界信息。SetLocation(&trajectory_pb);auto* not_ready = trajectory_pb.mutable_decision()->mutable_main_decision()->mutable_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}if (FLAGS_use_navigation_mode) {if (!local_view_.relative_map->has_header()) {not_ready->set_reason("relative map not ready");}} else {if (!local_view_.planning_command ||!local_view_.planning_command->has_header()) {not_ready->set_reason("planning_command not ready");}}if (not_ready->has_reason()) {AINFO << not_ready->reason() << "; skip the planning cycle.";common::util::FillHeader(node_->Name(), &trajectory_pb);planning_writer_->Write(trajectory_pb);return false;}return true;
}
//填充轨迹中的车辆位置和车道边界信息。
void PlanningComponent::SetLocation(ADCTrajectory* const ptr_trajectory_pb) {auto p = ptr_trajectory_pb->mutable_location_pose();// 从localization_estimate中提取车辆位置(x,y)p->mutable_vehice_location()->set_x(local_view_.localization_estimate->pose().position().x());p->mutable_vehice_location()->set_y(local_view_.localization_estimate->pose().position().y());// 计算车道左右边界点（用于可视化或控制模块参考）const Vec2d& adc_position = {local_view_.localization_estimate->pose().position().x(),local_view_.localization_estimate->pose().position().y()};Vec2d left_point, right_point;// ...设置左右边界点if (planning_base_->GenerateWidthOfLane(adc_position, left_point, right_point)) {p->mutable_left_lane_boundary_point()->set_x(left_point.x());p->mutable_left_lane_boundary_point()->set_y(left_point.y());p->mutable_right_lane_boundary_point()->set_x(right_point.x());p->mutable_right_lane_boundary_point()->set_y(right_point.y());}
}
http://www.xdnf.cn/news/6383.html

相关文章：

  • 《山东欧曼谛：美业梦想的启航港》
  • [Linux性能优化] 线程卡顿优化。Linux加入USB（HID）热插拔线程占用CPU优化。Linux中CPU使用率过高优化
  • 【steganalysis】Enhancing practicality and efficiency of deepfake detection
  • 【Linux专栏】Linux进程间关系和守护进程
  • 【Docker】Docker安装Redis
  • Claude官方63组提示词模板全解析：从工作到生活的AI应用指南
  • Mac 环境下 JDK 版本切换全指南
  • HDMI信号采集器连OBS没有声音的问题
  • 导入了lombok但是却不起作用，显示实际参数列表和形式参数列表的长度不同或者无法将类的构造器给到给定的类型
  • C# 实现雪花算法（Snowflake Algorithm）详解与应用
  • Redis（2）：Redis + Lua为什么可以实现原子性
  • Linux系统——进程结束时退出的分析与总结（关于wait与waitpid函数）
  • 红黑树解析
  • CyberDuckai入门笔记
  • 使用 GitDiagram 快速将 GitHub 仓库转换为交互式图表
  • 信奥赛CSP-J复赛集训（图和树专题）（9）：P2171 Hz吐泡泡
  • 【ALINX 实战笔记】FPGA 大神 Adam Taylor 使用 ChipScope 调试 AMD Versal 设计
  • 电力电容器故障利用沃伦森（WARENSEN）工业设备智能运维系统解决方案
  • SaaS基于云计算、大数据的Java云HIS平台信息化系统源码
  • 【Linux】Linux安装mysql
  • 2035.5.15 并查集
  • C#中BackgroundWorker的概念与用法详解
  • uniapp中vue3和pinia安装依赖npm install失败
  • calico排错思路
  • WebSocket：实时通信（如聊天应用）从零到一的深度解析
  • 养生：打造健康生活的四大支柱
  • 自用Vscode 配置c++ debug环境
  • 国产化Word处理控件Spire.Doc教程：通过C# 删除 Word 文档中的超链接
  • Window下Jmeter多机压测方法
  • linux使用普通用户，禁止root用户登录实操