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。

相关文章

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
阅读更多...
如何通过Golang的container/list实现LRU缓存算法

如何通过Golang的container/list实现LRU缓存算法

《如何通过Golang的container/list实现LRU缓存算法》文章介绍了Go语言中container/list包实现的双向链表,并探讨了如何使用链表实现LRU缓存,LRU缓存通过维护一个双向... 目录力扣：146. LRU 缓存主要结构 List 和 Element常用方法1. 初始化链表2.
阅读更多...
golang字符串匹配算法解读

golang字符串匹配算法解读

《golang字符串匹配算法解读》文章介绍了字符串匹配算法的原理,特别是Knuth-Morris-Pratt（KMP）算法,该算法通过构建模式串的前缀表来减少匹配时的不必要的字符比较,从而提高效率,在... 目录简介KMP实现代码总结简介字符串匹配算法主要用于在一个较长的文本串中查找一个较短的字符串（称为
阅读更多...
通俗易懂的Java常见限流算法具体实现

通俗易懂的Java常见限流算法具体实现

《通俗易懂的Java常见限流算法具体实现》：本文主要介绍Java常见限流算法具体实现的相关资料,包括漏桶算法、令牌桶算法、Nginx限流和Redis+Lua限流的实现原理和具体步骤,并比较了它们的... 目录一、漏桶算法1.漏桶算法的思想和原理2.具体实现二、令牌桶算法1.令牌桶算法流程:2.具体实现2.1
阅读更多...
Java深度学习库DJL实现Python的NumPy方式

Java深度学习库DJL实现Python的NumPy方式

《Java深度学习库DJL实现Python的NumPy方式》本文介绍了DJL库的背景和基本功能,包括NDArray的创建、数学运算、数据获取和设置等,同时,还展示了如何使用NDArray进行数据预处理... 目录1 NDArray 的背景介绍1.1 架构2 JavaDJL使用2.1 安装DJL2.2 基本操
阅读更多...
Go中sync.Once源码的深度讲解

Go中sync.Once源码的深度讲解

《Go中sync.Once源码的深度讲解》sync.Once是Go语言标准库中的一个同步原语,用于确保某个操作只执行一次,本文将从源码出发为大家详细介绍一下sync.Once的具体使用,x希望对大家有... 目录概念简单示例源码解读总结概念sync.Once是Go语言标准库中的一个同步原语，用于确保某个操
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2