/******************************************************************************
* 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/tasks/optimizers/road_graph/trajectory_cost.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/vec2d.h"
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/common/util/point_factory.h"
#include "modules/planning/common/planning_gflags.h"
namespace apollo {
namespace planning {
TrajectoryCost::TrajectoryCost(const DpPolyPathConfig &config,
const ReferenceLine &reference_line,
const bool is_change_lane_path,
const std::vector<const Obstacle *> &obstacles,
const common::VehicleParam &vehicle_param,
const SpeedData &heuristic_speed_data,
const common::SLPoint &init_sl_point,
const SLBoundary &adc_sl_boundary)
: config_(config),
reference_line_(&reference_line),
is_change_lane_path_(is_change_lane_path),
vehicle_param_(vehicle_param),
heuristic_speed_data_(heuristic_speed_data),
init_sl_point_(init_sl_point),
adc_sl_boundary_(adc_sl_boundary) {
const double total_time =
std::min(heuristic_speed_data_.TotalTime(), FLAGS_prediction_total_time);
num_of_time_stamps_ = static_cast<uint32_t>( //总的采样点
std::floor(total_time / config.eval_time_interval())); //std::floor(5.88) = 5 ... std::ceil(5.88) =6
//遍历障碍物列表
for (const auto *ptr_obstacle : obstacles) {
if (ptr_obstacle->IsIgnore()) {
continue; //break跳出整个循环体,执行其他语句 continue跳出本次循环执行下一次循环
} else if (ptr_obstacle->LongitudinalDecision().has_stop()) {
continue;
}
const auto &sl_boundary = ptr_obstacle->PerceptionSLBoundary();
const double adc_left_l = //自车左边界
init_sl_point_.l() + vehicle_param_.left_edge_to_center();
const double adc_right_l = //自车右边界
init_sl_point_.l() - vehicle_param_.right_edge_to_center();
//如果有一点不满足说明发生碰撞??? maybe
if (adc_left_l + FLAGS_lateral_ignore_buffer < sl_boundary.start_l() ||
adc_right_l - FLAGS_lateral_ignore_buffer > sl_boundary.end_l()) {
continue;
}
bool is_bycycle_or_pedestrian = //是否是自行车或者行人
(ptr_obstacle->Perception().type() ==
perception::PerceptionObstacle::BICYCLE ||
ptr_obstacle->Perception().type() ==
perception::PerceptionObstacle::PEDESTRIAN);
if (ptr_obstacle->IsVirtual()) {
// Virtual obstacle
continue;
} else if (ptr_obstacle->IsStatic() || is_bycycle_or_pedestrian) {
//如果是自行车或者行人或者是静态障碍物那么将sl_boundary push进静态障碍物列表中
static_obstacle_sl_boundaries_.push_back(std::move(sl_boundary));
} else {
std::vector<Box2d> box_by_time;
//每一动态障碍物依据时间存入 box_by_time 得到该动态障碍物在整个规划时刻的B
apollo DP cost计算 trajectory_cost 代码注释
于 2023-03-22 16:15:33 首次发布
![](https://img-home.csdnimg.cn/images/20240711042549.png)