apollo DP cost计算 trajectory_cost 代码注释

自动驾驶小学生9526

已于 2023-03-22 16:19:17 修改

阅读量192

点赞数

文章标签： c++ 开发语言

于 2023-03-22 16:15:33 首次发布

本文链接：https://blog.csdn.net/weixin_46198242/article/details/129712635

版权

本文详细解读 Apollo 自动驾驶框架中动态规划（DP）算法计算轨迹成本的代码实现，包括关键步骤、核心函数及变量解释，帮助理解轨迹优化过程。

摘要由CSDN通过智能技术生成

/******************************************************************************
 * 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