Apollo9.0 Lattice Planner算法源码学习

2024-03-20 22:44
文章标签 算法 源码 学习 planner lattice apollo9.0

本文主要是介绍Apollo9.0 Lattice Planner算法源码学习，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

一、概述

主文件：lattice_planner.cc，相关路径如下：

modules\planning\planners\lattice\lattice_planner.cc
modules\planning\planners\lattice\lattice_planner.h

 主程序函数：

Status LatticePlanner::Plan(const TrajectoryPoint& planning_start_point,Frame* frame,ADCTrajectory* ptr_computed_trajectory) {}

二、主程序学习(详细注释) 

/******************************************************************************* Copyright 2017 The Apollo Authors. All Rights Reserved.** Licensed under the Apache License, Version 2.0 (the "License");* you may not use this file except in compliance with the License.* You may obtain a copy of the License at** http://www.apache.org/licenses/LICENSE-2.0** Unless required by applicable law or agreed to in writing, software* distributed under the License is distributed on an "AS IS" BASIS,* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.* See the License for the specific language governing permissions and* limitations under the License.*****************************************************************************//*** @file**/#include "modules/planning/planners/lattice/lattice_planner.h"#include <limits>
#include <memory>
#include <utility>
#include <vector>#include "cyber/common/log.h"
#include "cyber/common/macros.h"
#include "cyber/time/clock.h"
#include "modules/common/math/cartesian_frenet_conversion.h"
#include "modules/common/math/path_matcher.h"
#include "modules/planning/planners/lattice/behavior/collision_checker.h"
#include "modules/planning/planners/lattice/behavior/path_time_graph.h"
#include "modules/planning/planners/lattice/behavior/prediction_querier.h"
#include "modules/planning/planners/lattice/trajectory_generation/backup_trajectory_generator.h"
#include "modules/planning/planners/lattice/trajectory_generation/lattice_trajectory1d.h"
#include "modules/planning/planners/lattice/trajectory_generation/trajectory1d_generator.h"
#include "modules/planning/planners/lattice/trajectory_generation/trajectory_combiner.h"
#include "modules/planning/planners/lattice/trajectory_generation/trajectory_evaluator.h"
#include "modules/planning/planning_base/gflags/planning_gflags.h"
#include "modules/planning/planning_base/math/constraint_checker/constraint_checker.h"namespace apollo {
namespace planning {using apollo::common::ErrorCode;
using apollo::common::PathPoint;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::math::CartesianFrenetConverter;
using apollo::common::math::PathMatcher;
using apollo::cyber::Clock;namespace {//---------该函数将输入的参考线进行离散化---------//
//---------输入：为ReferencePoint类型的vector----//
//---------输出：为PathPoint类型的vector---------//
std::vector<PathPoint> ToDiscretizedReferenceLine(const std::vector<ReferencePoint>& ref_points) {double s = 0.0;std::vector<PathPoint> path_points;for (const auto& ref_point : ref_points) {PathPoint path_point;    //-----Pathpoint相当于定义的一种结构体,包含了很多路径点信息path_point.set_x(ref_point.x());path_point.set_y(ref_point.y());path_point.set_theta(ref_point.heading());path_point.set_kappa(ref_point.kappa());path_point.set_dkappa(ref_point.dkappa());if (!path_points.empty()) {double dx = path_point.x() - path_points.back().x();double dy = path_point.y() - path_points.back().y();s += std::sqrt(dx * dx + dy * dy);    //-----s的计算方式：以直代曲！}path_point.set_s(s);path_points.push_back(std::move(path_point));}return path_points;
}//---------该函数将计算规划起点的Frenet坐标（状态）---------//
//---------其中调用了Cartesian 转 Frenet坐标的函数---------//
void ComputeInitFrenetState(const PathPoint& matched_point,const TrajectoryPoint& cartesian_state,std::array<double, 3>* ptr_s,std::array<double, 3>* ptr_d) {CartesianFrenetConverter::cartesian_to_frenet(matched_point.s(), matched_point.x(), matched_point.y(),matched_point.theta(), matched_point.kappa(), matched_point.dkappa(),cartesian_state.path_point().x(), cartesian_state.path_point().y(),cartesian_state.v(), cartesian_state.a(),cartesian_state.path_point().theta(),cartesian_state.path_point().kappa(), ptr_s, ptr_d);
}}  // namespace/*-----------------------------------------------
以下的planner函数就是规划主函数
输入为：planning_start_point 规划起点；frame 一次规划所需要的所有环境信息
ptr_computed_trajectory 待规划的轨迹？？
-----------------------------------------------*/
Status LatticePlanner::Plan(const TrajectoryPoint& planning_start_point,Frame* frame,ADCTrajectory* ptr_computed_trajectory) {size_t success_line_count = 0;size_t index = 0;
/*-----------------------------------------------
由于一个规划帧开始之前有定位和导航routing模块都会得到若干
条参考线，因此规划开始之前要根据给定的cost计算每条参考
线的cost，然后选择其中cost最低的一条进行离散化。SetPriorityCost
-----------------------------------------------*/for (auto& reference_line_info : *frame->mutable_reference_line_info()) {if (index != 0) {reference_line_info.SetPriorityCost(FLAGS_cost_non_priority_reference_line);} else {reference_line_info.SetPriorityCost(0.0);}/*-----------------------------------------------PlanOnReferenceLine()该函数为Lattice算法中最重要的函数，即：在选定cost最优的参考线之后，在该参考线上进行规划！-----------------------------------------------*/auto status =PlanOnReferenceLine(planning_start_point, frame, &reference_line_info);if (status != Status::OK()) {if (reference_line_info.IsChangeLanePath()) {AERROR << "Planner failed to change lane to "<< reference_line_info.Lanes().Id();} else {AERROR << "Planner failed to " << reference_line_info.Lanes().Id();}} else {success_line_count += 1;}++index;}if (success_line_count > 0) {return Status::OK();}return Status(ErrorCode::PLANNING_ERROR,"Failed to plan on any reference line.");
}/*-----------------------------------------------
对每一条参考线都会执行以下规划（？）
以下为PlanOnReferenceLine()的具体实现，分为7个步骤：
1、离散化参考线上的点，并计算s的值（目的：以直代曲，
为了在进行坐标转化以及计算障碍物距离自车的s坐标的时
候可以使用，如制作index2s表格，即根据参考线上点的索
引号映射到s值的表）
2、在参考线上计算“规划起点”的匹配点
3、根据匹配点，计算Frenet坐标系的S-L值
4、计算障碍物的S-T图（斜率表示速度）
5、生成纵横向采样路径
6、计算cost值，进行碰撞检测（依据S-T图）和动力学约束检测
7、优选出cost值最小的trajectory
-----------------------------------------------*/
Status LatticePlanner::PlanOnReferenceLine(const TrajectoryPoint& planning_init_point, Frame* frame,ReferenceLineInfo* reference_line_info) {static size_t num_planning_cycles = 0;static size_t num_planning_succeeded_cycles = 0;double start_time = Clock::NowInSeconds();double current_time = start_time;ADEBUG << "Number of planning cycles: " << num_planning_cycles << " "<< num_planning_succeeded_cycles;++num_planning_cycles;reference_line_info->set_is_on_reference_line();// 1. obtain a reference line and transform it to the PathPoint format.// 以下为参考线离散化的过程：auto ptr_reference_line =std::make_shared<std::vector<PathPoint>>(ToDiscretizedReferenceLine(reference_line_info->reference_line().reference_points()));// 2. compute the matched point of the init planning point on the reference// line.// 以下将计算规划起点的匹配点// 该函数的实现是在 path_matcher.cc 文件里面PathPoint matched_point = PathMatcher::MatchToPath(*ptr_reference_line, planning_init_point.path_point().x(),planning_init_point.path_point().y());// 3. according to the matched point, compute the init state in Frenet frame

这篇关于Apollo9.0 Lattice Planner算法源码学习的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/830994。

相关文章

Java学习手册之Filter和Listener使用方法

Java学习手册之Filter和Listener使用方法

《Java学习手册之Filter和Listener使用方法》：本文主要介绍Java学习手册之Filter和Listener使用方法的相关资料,Filter是一种拦截器,可以在请求到达Servl... 目录一、Filter（过滤器）1. Filter 的工作原理2. Filter 的配置与使用二、Listen
阅读更多...
Java 正则表达式URL 匹配与源码全解析

Java 正则表达式URL 匹配与源码全解析

《Java正则表达式URL匹配与源码全解析》在Web应用开发中,我们经常需要对URL进行格式验证,今天我们结合Java的Pattern和Matcher类,深入理解正则表达式在实际应用中... 目录1.正则表达式分解：2. 添加域名匹配 (2)3. 添加路径和查询参数匹配 (3) 4. 最终优化版本5.设计思
阅读更多...
openCV中KNN算法的实现

openCV中KNN算法的实现

《openCV中KNN算法的实现》KNN算法是一种简单且常用的分类算法,本文主要介绍了openCV中KNN算法的实现,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的... 目录KNN算法流程使用OpenCV实现KNNOpenCV 是一个开源的跨平台计算机视觉库，它提供了各
阅读更多...
Java调用C++动态库超详细步骤讲解(附源码)

Java调用C++动态库超详细步骤讲解(附源码)

《Java调用C++动态库超详细步骤讲解(附源码)》C语言因其高效和接近硬件的特性,时常会被用在性能要求较高或者需要直接操作硬件的场合,：本文主要介绍Java调用C++动态库的相关资料,文中通过代... 目录一、直接调用C++库第一步：动态库生成（vs2017+qt5.12.10）第二步：Java调用C++
阅读更多...
springboot+dubbo实现时间轮算法

springboot+dubbo实现时间轮算法

《springboot+dubbo实现时间轮算法》时间轮是一种高效利用线程资源进行批量化调度的算法,本文主要介绍了springboot+dubbo实现时间轮算法,文中通过示例代码介绍的非常详细,对大家... 目录前言一、参数说明二、具体实现1、HashedwheelTimer2、createWheel3、n
阅读更多...
Python实现无痛修改第三方库源码的方法详解

Python实现无痛修改第三方库源码的方法详解

《Python实现无痛修改第三方库源码的方法详解》很多时候,我们下载的第三方库是不会有需求不满足的情况,但也有极少的情况,第三方库没有兼顾到需求,本文将介绍几个修改源码的操作,大家可以根据需求进行选择... 目录需求不符合模拟示例 1. 修改源文件2. 继承修改3. 猴子补丁4. 追踪局部变量需求不符合很
阅读更多...
SpringBoot实现MD5加盐算法的示例代码

SpringBoot实现MD5加盐算法的示例代码

《SpringBoot实现MD5加盐算法的示例代码》加盐算法是一种用于增强密码安全性的技术,本文主要介绍了SpringBoot实现MD5加盐算法的示例代码,文中通过示例代码介绍的非常详细,对大家的学习... 目录一、什么是加盐算法二、如何实现加盐算法2.1 加盐算法代码实现2.2 注册页面中进行密码加盐2.
阅读更多...
Java时间轮调度算法的代码实现

Java时间轮调度算法的代码实现

《Java时间轮调度算法的代码实现》时间轮是一种高效的定时调度算法,主要用于管理延时任务或周期性任务,它通过一个环形数组（时间轮）和指针来实现,将大量定时任务分摊到固定的时间槽中,极大地降低了时间复杂... 目录1、简述2、时间轮的原理3. 时间轮的实现步骤3.1 定义时间槽3.2 定义时间轮3.3 使用时
阅读更多...
Spring 中 BeanFactoryPostProcessor 的作用和示例源码分析

Spring 中 BeanFactoryPostProcessor 的作用和示例源码分析

《Spring中BeanFactoryPostProcessor的作用和示例源码分析》Spring的BeanFactoryPostProcessor是容器初始化的扩展接口,允许在Bean实例化前... 目录一、概览1. 核心定位2. 核心功能详解3. 关键特性二、Spring 内置的 BeanFactory
阅读更多...
Java进阶学习之如何开启远程调式

Java进阶学习之如何开启远程调式

《Java进阶学习之如何开启远程调式》Java开发中的远程调试是一项至关重要的技能,特别是在处理生产环境的问题或者协作开发时,：本文主要介绍Java进阶学习之如何开启远程调式的相关资料,需要的朋友... 目录概述Java远程调试的开启与底层原理开启Java远程调试底层原理JVM参数总结&nbsMbKKXJx
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2