百度apollo自动驾驶planning代码学习-Apollo\modules\planning\common\obstacle_blocking_analyzer代码详解

2023-10-14 07:30
文章标签 代码 学习 百度 详解 自动 驾驶 common apollo modules analyzer planning obstacle blocking

本文主要是介绍百度apollo自动驾驶planning代码学习-Apollo\modules\planning\common\obstacle_blocking_analyzer代码详解，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

概述

apollo planning模块下modules\planning\common\obstacle_blocking_analyzer.cc/.h实现了一个障碍物阻塞分析器功能？但是没有封装成类，就是几个函数。

这几个函数的作用如下，主要都是实现对障碍物的判断，可以用来判断是否可以对前方障碍物进行绕行
在这里插入图片描述

obstacle_blocking_analyzer.h

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/planning/common/frame.h"namespace apollo {
namespace planning {//判断是否为非移动的障碍物
//输入参数 道路参考线信息类对象， 被判断的障碍物对象
//判断是否足够远，是否是停止的车辆，是否是被其他障碍物阻塞了？通过这些条件来判断输入障碍物是否为非移动也就是静态障碍物。
bool IsNonmovableObstacle(const ReferenceLineInfo& reference_line_info,const Obstacle& obstacle);
/*** @brief 判断一个障碍物是否阻塞路径，需要自车绕行？* @param frame包含了一个周期内规划所有信息包括道路参考线等* @param 感兴趣的障碍物obstacle* @param 判断一个障碍物是否停止的速度阈值block_obstacle_min_speed* @param 到前方阻塞障碍物需要绕行的最小距离min_front_sidepass_distance*        如果太近了，出于安全考虑，不绕行。* @param 是否要考虑被阻塞障碍物本身被其他障碍物阻塞了enable_obstacle_blocked_check*        如果前方的障碍物也是被前面的障碍物阻塞，那么就不要尝试绕行它？*/
bool IsBlockingObstacleToSidePass(const Frame& frame, const Obstacle* obstacle,double block_obstacle_min_speed,double min_front_sidepass_distance,bool enable_obstacle_blocked_check);//获取自车和障碍物之间的距离？
//输入参数frame，包含一个周期规划的所有数据
//需要计算距离的障碍物对象
double GetDistanceBetweenADCAndObstacle(const Frame& frame,const Obstacle* obstacle);//判断障碍物是否阻塞驾驶路径
//输入参数 道路参考线类对象，障碍物类对象
bool IsBlockingDrivingPathObstacle(const ReferenceLine& reference_line,const Obstacle* obstacle);//判断是否为停止的车辆
//输入参数 道路参考线类对象，障碍物对象
//用障碍物对象的xy坐标去判断是否处在停车车道？然后又离右边道路边缘足够近？然后障碍物还属于静态障碍物，都满足则说明是停在边上的静止的车
bool IsParkedVehicle(const ReferenceLine& reference_line,const Obstacle* obstacle);}  // namespace planning
}  // namespace apollo

obstacle_blocking_analyzer.cc

#include "modules/planning/common/obstacle_blocking_analyzer.h"#include <algorithm>
#include <memory>
#include <vector>#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/util/point_factory.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/planning_gflags.h"namespace apollo {
namespace planning {using apollo::common::VehicleConfigHelper;
using apollo::hdmap::HDMapUtil;//这类定义了两个阈值，自车距离阈值35m，障碍物距离阈值15m，干什么用的？
constexpr double kAdcDistanceThreshold = 35.0;  // unit: m
constexpr double kObstaclesDistanceThreshold = 15.0;//判断是否为非移动的障碍物
//输入参数 道路参考线信息类对象， 被判断的障碍物对象
//判断是否足够远，是否是停止的车辆，是否是被其他障碍物阻塞了？通过这些条件来判断输入障碍物是否为非移动也就是静态障碍物。
bool IsNonmovableObstacle(const ReferenceLineInfo& reference_line_info,const Obstacle& obstacle) {//障碍物足够远？获取道路参考线信息类对象的AdcSlBoundary自车的SL边界//SLBoundary又是.proto文件生成的类  modules\planning\proto\sl_boundary.proto//其包含信息：S的上下边界,L的上下边界，其实就是自车SL边界矩形？const SLBoundary& adc_sl_boundary = reference_line_info.AdcSlBoundary();//如果障碍物的感知SL边界的起始s比自车SL边界终点s+35m还要大//就说明障碍物离自车非常远？无需考虑直接返回false?if (obstacle.PerceptionSLBoundary().start_s() >adc_sl_boundary.end_s() + kAdcDistanceThreshold) {ADEBUG << " - It is too far ahead and we are not so sure of its status.";return false;}// 静止障碍物，调用下面的函数IsParkedVehicle()判断是否为停止车辆，是的话直接返回trueif (IsParkedVehicle(reference_line_info.reference_line(), &obstacle)) {ADEBUG << "It is Parked and NON-MOVABLE.";return true;}//障碍物也被其他障碍物阻塞了？//遍历道路参考线信息对象上的路径决策对象上的障碍物对象列表for (const auto* other_obstacle :reference_line_info.path_decision().obstacles().Items()) {//如果遍历的这个障碍物刚好是输入障碍物对象，判断其id是否相同，直接跳到下一个障碍物//因为就是要判断输入的这个障碍物是否被其他障碍物阻塞，它自己当然跳过if (other_obstacle->Id() == obstacle.Id()) {continue;}//如果其他障碍物是虚拟的也直接跳到下一个障碍物if (other_obstacle->IsVirtual()) {continue;}//判断其他障碍物的SL边界(主要是横向)是否没有与输入参数障碍物SL边界干涉//如果没有也直接跳到下一个障碍物if (other_obstacle->PerceptionSLBoundary().start_l() >obstacle.PerceptionSLBoundary().end_l() ||other_obstacle->PerceptionSLBoundary().end_l() <obstacle.PerceptionSLBoundary().start_l()) {// not blocking the backside vehiclecontinue;}//判断其他障碍物的SL边界(主要是纵向)，其他障碍物的SL边界起始s小于输入障碍物的//SL边界的终点s?那这样不是SL边界有干涉吗？这个快速排斥实验条件是不是写反了？或者//其他障碍物的SL边界起始s大于输入障碍物的SL边界的终点s超过15m就认为足够远无影响//直接跳到下一个障碍物//判断条件delta_s < 0.0 根据下面函数来看是遍历的其他障碍物在输入障碍物后面无影响？double delta_s = other_obstacle->PerceptionSLBoundary().start_s() -obstacle.PerceptionSLBoundary().end_s();if (delta_s < 0.0 || delta_s > kObstaclesDistanceThreshold) {continue;}// TODO(All): Fix the segmentation bug for large vehicles, otherwise// the follow line will be problematic.//修复大型车辆的分段错误，否则下面的行可能是有问题的//如果执行到这里还没返回，就是找到第一个不会干涉输入障碍物的障碍物，就直接返回false//逻辑没有太看明白？ADEBUG << " - It is blocked by others, and will move later.";return false;}//执行到这里还没返回，说明确实输入障碍物是非移动的。ADEBUG << "IT IS NON-MOVABLE!";return true;
}/*** @brief 判断一个障碍物是否阻塞路径，需要自车绕行？* @param frame包含了一个周期内规划所有信息包括道路参考线等* @param 感兴趣的障碍物obstacle* @param 判断一个障碍物是否停止的速度阈值block_obstacle_min_speed* @param 到前方阻塞障碍物需要绕行的最小距离min_front_sidepass_distance*        如果太近了，出于安全考虑，不绕行。* @param 是否要考虑被阻塞障碍物本身被其他障碍物阻塞了enable_obstacle_blocked_check*        如果前方的障碍物也是被前面的障碍物阻塞，那么就不要尝试绕行它？*/
bool IsBlockingObstacleToSidePass(const Frame& frame, const Obstacle* obstacle,double block_obstacle_min_speed,double min_front_sidepass_distance,bool enable_obstacle_blocked_check) {// 获取必要的信息//frame里的第一条参考线信息类对象？const auto& reference_line_info = frame.reference_line_info().front();//道路参考线const auto& reference_line = reference_line_info.reference_line();//自车SL边界const SLBoundary& adc_sl_boundary = reference_line_info.AdcSlBoundary();//路径决策类对象const PathDecision& path_decision = reference_line_info.path_decision();ADEBUG << "Evaluating Obstacle: " << obstacle->Id();// 如果障碍物是虚拟的，直接返回false，无需绕行if (obstacle->IsVirtual()) {ADEBUG << " - It is virtual.";return false;}// 如果障碍物非静态，或者障碍物的速度大于判断停车的速度阈值，也认为是非静态障碍物//无需绕行if (!obstacle->IsStatic() || obstacle->speed() > block_obstacle_min_speed) {ADEBUG << " - It is non-static.";return false;}// 如果障碍物在自车后面，障碍物的SL边界起始s < 自车SL边界终点s//也直接返回false无需绕行if (obstacle->PerceptionSLBoundary().start_s() <= adc_sl_boundary.end_s()) {ADEBUG << " - It is behind ADC.";return false;}// 障碍物足够远static constexpr double kAdcDistanceSidePassThreshold = 15.0;//障碍物的SL边界的起始s 比 自车SL边界的终点s + 15m就认为障碍物足够远，无需绕行if (obstacle->PerceptionSLBoundary().start_s() >adc_sl_boundary.end_s() + kAdcDistanceSidePassThreshold) {ADEBUG << " - It is too far ahead.";return false;}// 障碍物太近了//如果自车SL边界的终点s + 最小绕行距离阈值 > 输入障碍物SL边界的起点s，出于安全考虑，放弃绕行if (adc_sl_boundary.end_s() + min_front_sidepass_distance >obstacle->PerceptionSLBoundary().start_s()) {ADEBUG << " - It is too close to side-pass.";return false;}// 障碍物没有阻塞我们的驾驶路径，直接返回false，调用IsBlockingDrivingPathObstacle函数进行判断if (!IsBlockingDrivingPathObstacle(reference_line, obstacle)) {ADEBUG << " - It is not blocking our way.";return false;}// 障碍物也被其他障碍物阻塞了，也放弃绕行//如果打开障碍物被其他障碍物阻塞的检查开关 且 输入障碍物不是个静止车辆if (enable_obstacle_blocked_check &&!IsParkedVehicle(reference_line, obstacle)) {//遍历路径决策对象上的障碍物列表for (const auto* other_obstacle : path_decision.obstacles().Items()) {//因为要判断输入障碍物是否被其他障碍物阻塞，如果是自己则直接跳到下一个障碍物if (other_obstacle->Id() == obstacle->Id()) {continue;}//如果障碍物是虚拟的也直接跳到下一个障碍物if (other_obstacle->IsVirtual()) {continue;}//快速排斥实验，判断其他障碍物的SL边界的横向坐标L区间和自车的SL边界的横向坐标L区间是否有重叠，如果没有也直接跳到下一个障碍物if (other_obstacle->PerceptionSLBoundary().start_l() >obstacle->PerceptionSLBoundary().end_l() ||other_obstacle->PerceptionSLBoundary().end_l() <obstacle->PerceptionSLBoundary().start_l()) {// not blocking the backside vehiclecontinue;}//判断其他障碍物是否在输入障碍物后方 或 其他障碍物距离输入障碍物距离足够远超过35m，则认为输入障碍物并没有被其他障碍物影响/阻塞，直接跳到下一个障碍物double delta_s = other_obstacle->PerceptionSLBoundary().start_s() -obstacle->PerceptionSLBoundary().end_s();if (delta_s < 0.0 || delta_s > kAdcDistanceThreshold) {continue;}// TODO(All): Fix the segmentation bug for large vehicles, otherwise// the follow line will be problematic.//如果前面都没有直接跳到下一个障碍物的话，说明输入障碍物确实是被其他障碍物阻塞//放弃绕行ADEBUG << " - It is blocked by others, too.";return false;}}//如果上面都没有返回的话说明可以选择绕行？ADEBUG << "IT IS BLOCKING!";return true;
}//获取自车和障碍物之间的距离？
//输入参数frame，包含一个周期规划的所有数据
//需要计算距离的障碍物对象
double GetDistanceBetweenADCAndObstacle(const Frame& frame,const Obstacle* obstacle) {//首先获取道路参考线信息类对象const auto& reference_line_info = frame.reference_line_info().front();//获取自车的SL边界const SLBoundary& adc_sl_boundary = reference_line_info.AdcSlBoundary();//获取自车和障碍物在纵向s上的距离并返回double distance_between_adc_and_obstacle =obstacle->PerceptionSLBoundary().start_s() - adc_sl_boundary.end_s();return distance_between_adc_and_obstacle;
}//判断障碍物是否阻塞驾驶路径
//输入参数 道路参考线类对象，障碍物类对象
bool IsBlockingDrivingPathObstacle(const ReferenceLine& reference_line,const Obstacle* obstacle) {//驾驶宽度，道路参考线根据障碍物SL边界获取驾驶宽度const double driving_width =reference_line.GetDrivingWidth(obstacle->PerceptionSLBoundary());//获取自车的车宽const double adc_width =VehicleConfigHelper::GetConfig().vehicle_param().width();ADEBUG << " (driving width = " << driving_width<< ", adc_width = " << adc_width << ")";//如果驾驶宽度 > 自车宽度 + 0.3m(左右一起0.3m障碍物缓冲) +  0.1m(绕行缓冲)，那么则说明障碍物没有阻塞我们的驾驶路径//FLAGS_static_obstacle_nudge_l_buffer google gflags的老用法//FLAGS_代表去modules\planning\common\planning_gflags.cc取出static_obstacle_nudge_l_buffer的值，默认为0.3mif (driving_width > adc_width + FLAGS_static_obstacle_nudge_l_buffer +FLAGS_side_pass_driving_width_l_buffer) {// TODO(jiacheng): make this a GFLAG:// side_pass_context_.scenario_config_.min_l_nudge_buffer()ADEBUG << "It is NOT blocking our path.";return false;}//如果执行到这里之前没有返回，说明障碍物阻塞了我们的路径。ADEBUG << "It is blocking our path.";return true;
}//判断是否为停止的车辆
//输入参数 道路参考线类对象，障碍物对象
//用障碍物对象的xy坐标去判断是否处在停车车道？然后又离右边道路边缘足够近？然后障碍物还属于静态障碍物，都满足则说明是停在边上的静止的车
bool IsParkedVehicle(const ReferenceLine& reference_line,const Obstacle* obstacle) {//enable_scenario_side_pass_multiple_parked_obstacles默认值为true//所以不会进入该if,使能场景绕行多辆停止车辆？if (!FLAGS_enable_scenario_side_pass_multiple_parked_obstacles) {return false;}//初始定义道路的左宽度，右宽度，最大道路右边宽度为0.0double road_left_width = 0.0;double road_right_width = 0.0;double max_road_right_width = 0.0;//参考线根据障碍物感知的SL边界 的起始点s获取道路宽度，道路中心线左边宽度放入//road_left_width，道路中心线右边宽度放入road_right_widthreference_line.GetRoadWidth(obstacle->PerceptionSLBoundary().start_s(),&road_left_width, &road_right_width);//道路中心线右边的最大宽度为道路中心线右边宽度max_road_right_width = road_right_width;//道路参考线根据障碍物感知的SL边界 的终点s获取道路宽度，中心线左边的宽度放入road_left_width，右边放入road_right_widthreference_line.GetRoadWidth(obstacle->PerceptionSLBoundary().end_s(),&road_left_width, &road_right_width);//最大道路中心线右边宽度 = 中心线右边最大宽度和右边道路宽度之间的较大值max_road_right_width = std::max(max_road_right_width, road_right_width);//判断是否在道路边缘，如果障碍物的SL边界的起始点横坐标l > 最大中心线右边宽度-0.1，说明是离边界线很近bool is_at_road_edge = std::abs(obstacle->PerceptionSLBoundary().start_l()) >max_road_right_width - 0.1;//车道vector列表std::vector<std::shared_ptr<const hdmap::LaneInfo>> lanes;//障碍物盒，障碍物对象去访问成员函数获取感知边界盒auto obstacle_box = obstacle->PerceptionBoundingBox();//这个是高精度地图模块，根据障碍物边界盒中心的xy坐标,获取其所在车道HDMapUtil::BaseMapPtr()->GetLanes(common::util::PointFactory::ToPointENU(obstacle_box.center().x(),obstacle_box.center().y()),std::min(obstacle_box.width(), obstacle_box.length()), &lanes);//初始定义在停止车道上的布尔量为falsebool is_on_parking_lane = false;//如果障碍物边界盒所处的车道里只有一个数 且 车道列表里第一个车道的车道类型为PARKING，就说明在停车车道上if (lanes.size() == 1 &&lanes.front()->lane().type() == apollo::hdmap::Lane::PARKING) {is_on_parking_lane = true;}//在停车车道上又在道路边缘则说明是停车了bool is_parked = is_on_parking_lane || is_at_road_edge;//停住了而且障碍物为静态说明是一辆停住的车return is_parked && obstacle->IsStatic();
}}  // namespace planning
}  // namespace apollo

这篇关于百度apollo自动驾驶planning代码学习-Apollo\modules\planning\common\obstacle_blocking_analyzer代码详解的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


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

相关文章

HarmonyOS学习(七)——UI（五）常用布局总结

HarmonyOS学习(七)——UI（五）常用布局总结

自适应布局 1.1、线性布局（LinearLayout） 通过线性容器Row和Column实现线性布局。Column容器内的子组件按照垂直方向排列，Row组件中的子组件按照水平方向排列。 属性说明space通过space参数设置主轴上子组件的间距，达到各子组件在排列上的等间距效果alignItems设置子组件在交叉轴上的对齐方式，且在各类尺寸屏幕上表现一致，其中交叉轴为垂直时，取值为Vert
阅读更多...
Ilya-AI分享的他在OpenAI学习到的15个提示工程技巧

Ilya-AI分享的他在OpenAI学习到的15个提示工程技巧

Ilya（不是本人，claude AI）在社交媒体上分享了他在OpenAI学习到的15个Prompt撰写技巧。 以下是详细的内容： 提示精确化:在编写提示时,力求表达清晰准确。清楚地阐述任务需求和概念定义至关重要。例:不用"分析文本",而用"判断这段话的情感倾向:积极、消极还是中性"。 快速迭代:善于快速连续调整提示。熟练的提示工程师能够灵活地进行多轮优化。例:从"总结文章"到"用
阅读更多...
Spring Security基于数据库验证流程详解

Spring Security基于数据库验证流程详解

Spring Security 校验流程图 相关解释说明（认真看哦） AbstractAuthenticationProcessingFilter 抽象类 /*** 调用 #requiresAuthentication(HttpServletRequest, HttpServletResponse) 决定是否需要进行验证操作。* 如果需要验证，则会调用 #attemptAuthentica
阅读更多...
百度/小米/滴滴/京东，中台架构比较

百度/小米/滴滴/京东，中台架构比较

小米中台建设实践 01 小米的三大中台建设：业务+数据+技术 业务中台--从业务说起 在中台建设中，需要规范化的服务接口、一致整合化的数据、容器化的技术组件以及弹性的基础设施。并结合业务情况，判定是否真的需要中台。 小米参考了业界优秀的案例包括移动中台、数据中台、业务中台、技术中台等，再结合其业务发展历程及业务现状，整理了中台架构的核心方法论，一是企业如何共享服务，二是如何为业务提供便利。
阅读更多...
【前端学习】AntV G6-08 深入图形与图形分组、自定义节点、节点动画（下）

【前端学习】AntV G6-08 深入图形与图形分组、自定义节点、节点动画（下）

【课程链接】 AntV G6：深入图形与图形分组、自定义节点、节点动画（下）_哔哩哔哩_bilibili 本章十吾老师讲解了一个复杂的自定义节点中，应该怎样去计算和绘制图形，如何给一个图形制作不间断的动画，以及在鼠标事件之后产生动画。（有点难，需要好好理解） <!DOCTYPE html><html><head><meta charset="UTF-8"><title>06
阅读更多...
学习hash总结

学习hash总结

2014/1/29/   最近刚开始学hash，名字很陌生，但是hash的思想却很熟悉，以前早就做过此类的题，但是不知道这就是hash思想而已，说白了hash就是一个映射，往往灵活利用数组的下标来实现算法，hash的作用：1、判重；2、统计次数；
阅读更多...
OpenHarmony鸿蒙开发（ Beta5.0）无感配网详解

OpenHarmony鸿蒙开发（ Beta5.0）无感配网详解

1、简介 无感配网是指在设备联网过程中无需输入热点相关账号信息，即可快速实现设备配网，是一种兼顾高效性、可靠性和安全性的配网方式。 2、配网原理 2.1 通信原理 手机和智能设备之间的信息传递，利用特有的NAN协议实现。利用手机和智能设备之间的WiFi 感知订阅、发布能力，实现了数字管家应用和设备之间的发现。在完成设备间的认证和响应后，即可发送相关配网数据。同时还支持与常规Sof
阅读更多...
活用c4d官方开发文档查询代码

活用c4d官方开发文档查询代码

当你问AI助手比如豆包，如何用python禁止掉xpresso标签时候，它会提示到 这时候要用到两个东西。https://developers.maxon.net/论坛搜索和开发文档 比如这里我就在官方找到正确的id描述 然后我就把参数标签换过来
阅读更多...
零基础学习Redis(10) -- zset类型命令使用

零基础学习Redis(10) -- zset类型命令使用

zset是有序集合，内部除了存储元素外，还会存储一个score，存储在zset中的元素会按照score的大小升序排列，不同元素的score可以重复，score相同的元素会按照元素的字典序排列。 1. zset常用命令 1.1 zadd  zadd key [NX | XX] [GT | LT]   [CH] [INCR] score member [score member ...]
阅读更多...
poj 1258 Agri-Net（最小生成树模板代码）

poj 1258 Agri-Net（最小生成树模板代码）

感觉用这题来当模板更适合。 题意就是给你邻接矩阵求最小生成树啦。~ prim代码：效率很高。172k...0ms。 #include<stdio.h>#include<algorithm>using namespace std;const int MaxN = 101;const int INF = 0x3f3f3f3f;int g[MaxN][MaxN];int n
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2