一、speed_bounds_decider.cc
/******************************************************************************
* Copyright 2019 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.
*****************************************************************************/
#include "modules/planning/tasks/speed_bounds_decider/speed_bounds_decider.h"
#include <algorithm>
#include <limits>
#include <memory>
#include <string>
#include <vector>
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/planning/planning_base/common/path/path_data.h"
#include "modules/planning/planning_base/common/planning_context.h"
#include "modules/planning/planning_base/common/st_graph_data.h"
#include "modules/planning/planning_base/common/util/common.h"
#include "modules/planning/planning_base/gflags/planning_gflags.h"
#include "modules/planning/tasks/speed_bounds_decider/speed_limit_decider.h"
#include "modules/planning/tasks/speed_bounds_decider/st_boundary_mapper.h"
namespace apollo {
namespace planning {
using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::planning_internal::StGraphBoundaryDebug;
using apollo::planning_internal::STGraphDebug;
bool SpeedBoundsDecider::Init(
const std::string &config_dir, const std::string &name,
const std::shared_ptr<DependencyInjector> &injector) {
if (!Decider::Init(config_dir, name, injector)) {
return false;
}
// Load the config this task.
return Decider::LoadConfig<SpeedBoundsDeciderConfig>(&config_);
}
bool SpeedBoundsDecider::Init( {…}
Status SpeedBoundsDecider::Process(
Frame *const frame, ReferenceLineInfo *const reference_line_info) {
// retrieve data from frame and reference_line_info
const PathData &path_data = reference_line_info->path_data();
const TrajectoryPoint &init_point = frame->PlanningStartPoint();
const ReferenceLine &reference_line = reference_line_info->reference_line();
PathDecision *const path_decision = reference_line_info->path_decision();
// 1. Map obstacles into st graph
auto time1 = std::chrono::system_clock::now();
STBoundaryMapper boundary_mapper(config_, reference_line, path_data,
path_data.discretized_path().Length(),
config_.total_time(), injector_);
if (!FLAGS_use_st_drivable_boundary) {
path_decision->EraseStBoundaries();
}
if (boundary_mapper.ComputeSTBoundary(path_decision).code() ==
ErrorCode::PLANNING_ERROR) {
const std::string msg = "Mapping obstacle failed.";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
auto time2 = std::chrono::system_clock::now();
std::chrono::duration<double> diff = time2 - time1;
ADEBUG << "Time for ST Boundary Mapping = " << diff.count() * 1000
<< " msec.";
std::vector<const STBoundary *> boundaries;
for (auto *obstacle : path_decision->obstacles().Items()) {
const auto &id = obstacle->Id();
const auto &st_boundary = obstacle->path_st_boundary();
if (!st_boundary.IsEmpty()) {
if (st_boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
path_decision->Find(id)->SetBlockingObstacle(false);
} else {
path_decision->Find(id)->SetBlockingObstacle(true);
}
st_boundary.PrintDebug("_obs_st_bounds");
boundaries.push_back(&st_boundary);
}
}
const double min_s_on_st_boundaries = SetSpeedFallbackDistance(path_decision);
// 2. Create speed limit along path
SpeedLimitDecider speed_limit_decider(config_, reference_line, path_data);
SpeedLimit speed_limit;
if (!speed_limit_decider
.GetSpeedLimits(path_decision->obstacles(), &speed_limit)
.ok()) {
const std::string msg = "Getting speed limits failed!";
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
}
// 3. Get path_length as s axis search bound in st graph
const double path_data_length = path_data.discretized_path().Length();
// 4. Get time duration as t axis search bound in st graph
const double total_time_by_conf = config_.total_time();
// Load generated st graph data back to frame
StGraphData *st_graph_data = reference_line_info_->mutable_st_graph_data();
// Add a st_graph debug info and save the pointer to st_graph_data for
// optimizer logging
auto *debug = reference_line_info_->mutable_debug();
STGraphDebug *st_graph_debug = debug->mutable_planning_data()->add_st_graph();
st_graph_data->LoadData(boundaries, min_s_on_st_boundaries, init_point,
speed_limit, reference_line_info->GetCruiseSpeed(),
path_data_length, total_time_by_conf, st_graph_debug);
// Create and record st_graph debug info
RecordSTGraphDebug(*st_graph_data, st_graph_debug);
return Status::OK();
}
double SpeedBoundsDecider::SetSpeedFallbackDistance(
PathDecision *const path_decision) {
// Set min_s_on_st_boundaries to guide speed fallback.
static constexpr double kEpsilon = 1.0e-6;
double min_s_non_reverse = std::numeric_limits<double>::infinity();
double min_s_reverse = std::numeric_limits<double>::infinity();
for (auto *obstacle : path_decision->obstacles().Items()) {
const auto &st_boundary = obstacle->path_st_boundary();
if (st_boundary.IsEmpty()) {
continue;
}
const auto left_bottom_point_s = st_boundary.bottom_left_point().s();
const auto right_bottom_point_s = st_boundary.bottom_right_point().s();
const auto lowest_s = std::min(left_bottom_point_s, right_bottom_point_s);
if (left_bottom_point_s - right_bottom_point_s > kEpsilon) {
if (min_s_reverse > lowest_s) {
min_s_reverse = lowest_s;
}
} else if (min_s_non_reverse > lowest_s) {
min_s_non_reverse = lowest_s;
}
}
min_s_reverse = std::max(min_s_reverse, 0.0);
min_s_non_reverse = std::max(min_s_non_reverse, 0.0);
return min_s_non_reverse > min_s_reverse ? 0.0 : min_s_non_reverse;
}
void SpeedBoundsDecider::RecordSTGraphDebug(
const StGraphData &st_graph_data, STGraphDebug *st_graph_debug) const {
if (!FLAGS_enable_record_debug || !st_graph_debug) {
ADEBUG << "Skip record debug info";
return;
}
for (const auto &boundary : st_graph_data.st_boundaries()) {
auto boundary_debug = st_graph_debug->add_boundary();
boundary_debug->set_name(boundary->id());
switch (boundary->boundary_type()) {
case STBoundary::BoundaryType::FOLLOW:
boundary_debug->set_type(StGraphBoundaryDebug::ST_BOUNDARY_TYPE_FOLLOW);
break;
case STBoundary::BoundaryType::OVERTAKE:
boundary_debug->set_type(
StGraphBoundaryDebug::ST_BOUNDARY_TYPE_OVERTAKE);
break;
case STBoundary::BoundaryType::STOP:
boundary_debug->set_type(StGraphBoundaryDebug::ST_BOUNDARY_TYPE_STOP);
break;
case STBoundary::BoundaryType::UNKNOWN:
boundary_debug->set_type(
StGraphBoundaryDebug::ST_BOUNDARY_TYPE_UNKNOWN);
break;
case STBoundary::BoundaryType::YIELD:
boundary_debug->set_type(StGraphBoundaryDebug::ST_BOUNDARY_TYPE_YIELD);
break;
case STBoundary::BoundaryType::KEEP_CLEAR:
boundary_debug->set_type(
StGraphBoundaryDebug::ST_BOUNDARY_TYPE_KEEP_CLEAR);
break;
}
for (const auto &point : boundary->points()) {
auto point_debug = boundary_debug->add_point();
point_debug->set_t(point.x());
point_debug->set_s(point.y());
}
}
for (const auto &point : st_graph_data.speed_limit().speed_limit_points()) {
common::SpeedPoint *speed_point = st_graph_debug->add_speed_limit();
speed_point->set_s(point.first);
speed_point->set_v(point.second);
}
}
} // namespace planning
} // namespace apollo
这段代码是自动驾驶系统中的一部分,具体来说是属于Apollo自动驾驶框架的规划模块。代码的主要功能是决定车辆在特定路径上的速度限制,处理与速度相关的决策。下面是对代码主要部分的详细解释:
头文件和命名空间
- 包含了多个头文件,这些文件提供了车辆状态、路径数据、规划上下文和其他工具函数的支持。
- 使用了
apollo::planning
命名空间,这是Apollo自动驾驶软件中处理路径规划的部分。
类 SpeedBoundsDecider
这个类负责决定速度边界,主要方法包括:
Init
:初始化函数,加载配置文件。Process
:主处理函数,执行速度决策的核心逻辑。SetSpeedFallbackDistance
:设置速度回退距离,用于处理障碍物。RecordSTGraphDebug
:记录ST图的调试信息,用于调试和分析。
方法详解
Init
- 初始化并加载配置,确保依赖项正确注入。
Process
- 映射障碍物到ST图:使用
STBoundaryMapper
类,将路径上的障碍物映射到时空图(ST图)上,这是理解障碍物在时间和空间上如何影响车辆行驶的一种方式。 - 计算速度限制:使用
SpeedLimitDecider
类,根据障碍物和路径信息计算速度限制。 - 加载ST图数据:将计算得到的数据(如速度限制和障碍物信息)加载回框架的主数据结构中,供后续处理使用。
- 记录调试信息:如果启用了调试,将相关信息记录到调试结构中。
SetSpeedFallbackDistance
- 计算最小安全距离,以便在遇到障碍物时车辆可以安全减速。
RecordSTGraphDebug
- 将ST图的边界和速度限制点记录到调试信息中,帮助开发者理解和调试车辆的行为。
总结
这段代码是Apollo自动驾驶系统中处理速度决策的关键部分,它通过分析车辆的路径、障碍物信息以及其他环境因素来决定车辆的速度限制。这有助于确保车辆在保持效率的同时,也能安全地行驶。