apollo::planning::LatticePlanner类 参考

#include <lattice_planner.h>

类 apollo::planning::LatticePlanner 继承关系图:

apollo::planning::LatticePlanner 的协作图:

Public 成员函数
virtual	~LatticePlanner ()=default
std::string	Name () override
common::Status	Init (const std::shared_ptr< DependencyInjector > &injector, const std::string &config_path="") override
void	Stop () override
common::Status	Plan (const common::TrajectoryPoint &planning_init_point, Frame *frame, ADCTrajectory *ptr_computed_trajectory) override
	Override function Plan in parent class Planner.
common::Status	PlanOnReferenceLine (const common::TrajectoryPoint &planning_init_point, Frame *frame, ReferenceLineInfo *reference_line_info) override
	Override function Plan in parent class Planner.
Public 成员函数 继承自 apollo::planning::PlannerWithReferenceLine
virtual	~PlannerWithReferenceLine ()=default
	Destructor
Public 成员函数 继承自 apollo::planning::Planner
virtual	~Planner ()=default
	Destructor
virtual void	Reset (Frame *frame)
template<typename T >
bool	LoadConfig (const std::string &custom_config_path, T *config)

额外继承的成员函数
Protected 属性 继承自 apollo::planning::Planner
std::shared_ptr< DependencyInjector >	injector_ = nullptr

详细描述

在文件 lattice_planner.h 第 37 行定义.

构造及析构函数说明

~LatticePlanner()

virtual apollo::planning::LatticePlanner::~LatticePlanner ( )

virtualdefault

成员函数说明

Init()

common::Status apollo::planning::LatticePlanner::Init ( const std::shared_ptr< DependencyInjector > &  injector, const std::string &  config_path = ""  )

inlineoverridevirtual

重载 apollo::planning::Planner .

在文件 lattice_planner.h 第 43 行定义.

                                                                  {
    return Planner::Init(injector, config_path);
  }

Name()

std::string apollo::planning::LatticePlanner::Name ( )

inlineoverridevirtual

实现了 apollo::planning::Planner.

在文件 lattice_planner.h 第 41 行定义.

{ return "LATTICE"; }

Plan()

Status apollo::planning::LatticePlanner::Plan ( const common::TrajectoryPoint &  planning_init_point, Frame *  frame, ADCTrajectory *  ptr_computed_trajectory  )

overridevirtual

Override function Plan in parent class Planner.

参数

planning_init_point	The trajectory point where planning starts.
frame	Current planning frame.

返回

OK if planning succeeds; error otherwise.

实现了 apollo::planning::Planner.

在文件 lattice_planner.cc 第 95 行定义.

                                                                    {
  size_t success_line_count = 0;
  size_t index = 0;
  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);
    }
    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()

Status apollo::planning::LatticePlanner::PlanOnReferenceLine ( const common::TrajectoryPoint &  planning_init_point, Frame *  frame, ReferenceLineInfo *  reference_line_info  )

overridevirtual

Override function Plan in parent class Planner.

参数

planning_init_point	The trajectory point where planning starts.
frame	Current planning frame.
reference_line_info	The computed reference line.

返回

OK if planning succeeds; error otherwise.

重载 apollo::planning::PlannerWithReferenceLine .

在文件 lattice_planner.cc 第 130 行定义.

                                            {
  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.
  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.
  std::array<double, 3> init_s;
  std::array<double, 3> init_d;
  ComputeInitFrenetState(matched_point, planning_init_point, &init_s, &init_d);
 
  ADEBUG << "ReferenceLine and Frenet Conversion Time = "
         << (Clock::NowInSeconds() - current_time) * 1000;
  current_time = Clock::NowInSeconds();
 
  auto ptr_prediction_querier = std::make_shared<PredictionQuerier>(
      frame->obstacles(), ptr_reference_line);
 
  // 4. parse the decision and get the planning target.
  auto ptr_path_time_graph = std::make_shared<PathTimeGraph>(
      ptr_prediction_querier->GetObstacles(), *ptr_reference_line,
      reference_line_info, init_s[0],
      init_s[0] + FLAGS_speed_lon_decision_horizon, 0.0,
      FLAGS_trajectory_time_length, init_d);
 
  double speed_limit =
      reference_line_info->reference_line().GetSpeedLimitFromS(init_s[0]);
  reference_line_info->SetLatticeCruiseSpeed(speed_limit);
 
  PlanningTarget planning_target = reference_line_info->planning_target();
  if (planning_target.has_stop_point()) {
    ADEBUG << "Planning target stop s: " << planning_target.stop_point().s()
           << "Current ego s: " << init_s[0];
  }
 
  ADEBUG << "Decision_Time = " << (Clock::NowInSeconds() - current_time) * 1000;
  current_time = Clock::NowInSeconds();
 
  // 5. generate 1d trajectory bundle for longitudinal and lateral respectively.
  Trajectory1dGenerator trajectory1d_generator(
      init_s, init_d, ptr_path_time_graph, ptr_prediction_querier);
  std::vector<std::shared_ptr<Curve1d>> lon_trajectory1d_bundle;
  std::vector<std::shared_ptr<Curve1d>> lat_trajectory1d_bundle;
  trajectory1d_generator.GenerateTrajectoryBundles(
      planning_target, &lon_trajectory1d_bundle, &lat_trajectory1d_bundle);
 
  ADEBUG << "Trajectory_Generation_Time = "
         << (Clock::NowInSeconds() - current_time) * 1000;
  current_time = Clock::NowInSeconds();
 
  // 6. first, evaluate the feasibility of the 1d trajectories according to
  // dynamic constraints.
  //   second, evaluate the feasible longitudinal and lateral trajectory pairs
  //   and sort them according to the cost.
  TrajectoryEvaluator trajectory_evaluator(
      init_s, planning_target, lon_trajectory1d_bundle, lat_trajectory1d_bundle,
      ptr_path_time_graph, ptr_reference_line);
 
  ADEBUG << "Trajectory_Evaluator_Construction_Time = "
         << (Clock::NowInSeconds() - current_time) * 1000;
  current_time = Clock::NowInSeconds();
 
  ADEBUG << "number of trajectory pairs = "
         << trajectory_evaluator.num_of_trajectory_pairs()
         << "  number_lon_traj = " << lon_trajectory1d_bundle.size()
         << "  number_lat_traj = " << lat_trajectory1d_bundle.size();
 
  // Get instance of collision checker and constraint checker
  CollisionChecker collision_checker(frame->obstacles(), init_s[0], init_d[0],
                                     *ptr_reference_line, reference_line_info,
                                     ptr_path_time_graph);
 
  // 7. always get the best pair of trajectories to combine; return the first
  // collision-free trajectory.
  size_t constraint_failure_count = 0;
  size_t collision_failure_count = 0;
  size_t combined_constraint_failure_count = 0;
 
  size_t lon_vel_failure_count = 0;
  size_t lon_acc_failure_count = 0;
  size_t lon_jerk_failure_count = 0;
  size_t curvature_failure_count = 0;
  size_t lat_acc_failure_count = 0;
  size_t lat_jerk_failure_count = 0;
 
  size_t num_lattice_traj = 0;
 
  while (trajectory_evaluator.has_more_trajectory_pairs()) {
    double trajectory_pair_cost =
        trajectory_evaluator.top_trajectory_pair_cost();
    auto trajectory_pair = trajectory_evaluator.next_top_trajectory_pair();
 
    // combine two 1d trajectories to one 2d trajectory
    auto combined_trajectory = TrajectoryCombiner::Combine(
        *ptr_reference_line, *trajectory_pair.first, *trajectory_pair.second,
        planning_init_point.relative_time());
 
    // check longitudinal and lateral acceleration
    // considering trajectory curvatures
    auto result = ConstraintChecker::ValidTrajectory(combined_trajectory);
    if (result != ConstraintChecker::Result::VALID) {
      ++combined_constraint_failure_count;
 
      switch (result) {
        case ConstraintChecker::Result::LON_VELOCITY_OUT_OF_BOUND:
          lon_vel_failure_count += 1;
          break;
        case ConstraintChecker::Result::LON_ACCELERATION_OUT_OF_BOUND:
          lon_acc_failure_count += 1;
          break;
        case ConstraintChecker::Result::LON_JERK_OUT_OF_BOUND:
          lon_jerk_failure_count += 1;
          break;
        case ConstraintChecker::Result::CURVATURE_OUT_OF_BOUND:
          curvature_failure_count += 1;
          break;
        case ConstraintChecker::Result::LAT_ACCELERATION_OUT_OF_BOUND:
          lat_acc_failure_count += 1;
          break;
        case ConstraintChecker::Result::LAT_JERK_OUT_OF_BOUND:
          lat_jerk_failure_count += 1;
          break;
        case ConstraintChecker::Result::VALID:
        default:
          // Intentional empty
          break;
      }
      continue;
    }
 
    // check collision with other obstacles
    if (collision_checker.InCollision(combined_trajectory)) {
      ++collision_failure_count;
      continue;
    }
 
    // put combine trajectory into debug data
    const auto& combined_trajectory_points = combined_trajectory;
    num_lattice_traj += 1;
    reference_line_info->SetTrajectory(combined_trajectory);
    reference_line_info->SetCost(reference_line_info->PriorityCost() +
                                 trajectory_pair_cost);
    reference_line_info->SetDrivable(true);
 
    // Print the chosen end condition and start condition
    ADEBUG << "Starting Lon. State: s = " << init_s[0] << " ds = " << init_s[1]
           << " dds = " << init_s[2];
    // cast
    auto lattice_traj_ptr =
        std::dynamic_pointer_cast<LatticeTrajectory1d>(trajectory_pair.first);
    if (!lattice_traj_ptr) {
      ADEBUG << "Dynamically casting trajectory1d ptr. failed.";
    }
 
    if (lattice_traj_ptr->has_target_position()) {
      ADEBUG << "Ending Lon. State s = " << lattice_traj_ptr->target_position()
             << " ds = " << lattice_traj_ptr->target_velocity()
             << " t = " << lattice_traj_ptr->target_time();
    }
 
    ADEBUG << "InputPose";
    ADEBUG << "XY: " << planning_init_point.ShortDebugString();
    ADEBUG << "S: (" << init_s[0] << ", " << init_s[1] << "," << init_s[2]
           << ")";
    ADEBUG << "L: (" << init_d[0] << ", " << init_d[1] << "," << init_d[2]
           << ")";
 
    ADEBUG << "Reference_line_priority_cost = "
           << reference_line_info->PriorityCost();
    ADEBUG << "Total_Trajectory_Cost = " << trajectory_pair_cost;
    ADEBUG << "OutputTrajectory";
    for (uint i = 0; i < 10; ++i) {
      ADEBUG << combined_trajectory_points[i].ShortDebugString();
    }
 
    break;
    /*
    auto combined_trajectory_path =
        ptr_debug->mutable_planning_data()->add_trajectory_path();
    for (uint i = 0; i < combined_trajectory_points.size(); ++i) {
      combined_trajectory_path->add_trajectory_point()->CopyFrom(
          combined_trajectory_points[i]);
    }
    combined_trajectory_path->set_lattice_trajectory_cost(trajectory_pair_cost);
    */
  }
 
  ADEBUG << "Trajectory_Evaluation_Time = "
         << (Clock::NowInSeconds() - current_time) * 1000;
 
  ADEBUG << "Step CombineTrajectory Succeeded";
 
  ADEBUG << "1d trajectory not valid for constraint ["
         << constraint_failure_count << "] times";
  ADEBUG << "Combined trajectory not valid for ["
         << combined_constraint_failure_count << "] times";
  ADEBUG << "Trajectory not valid for collision [" << collision_failure_count
         << "] times";
  ADEBUG << "Total_Lattice_Planning_Frame_Time = "
         << (Clock::NowInSeconds() - start_time) * 1000;
 
  if (num_lattice_traj > 0) {
    ADEBUG << "Planning succeeded";
    num_planning_succeeded_cycles += 1;
    reference_line_info->SetDrivable(true);
    return Status::OK();
  } else {
    AERROR << "Planning failed";
    if (FLAGS_enable_backup_trajectory) {
      AERROR << "Use backup trajectory";
      BackupTrajectoryGenerator backup_trajectory_generator(
          init_s, init_d, planning_init_point.relative_time(),
          std::make_shared<CollisionChecker>(collision_checker),
          &trajectory1d_generator);
      DiscretizedTrajectory trajectory =
          backup_trajectory_generator.GenerateTrajectory(*ptr_reference_line);
 
      reference_line_info->AddCost(FLAGS_backup_trajectory_cost);
      reference_line_info->SetTrajectory(trajectory);
      reference_line_info->SetDrivable(true);
      return Status::OK();
 
    } else {
      reference_line_info->SetCost(std::numeric_limits<double>::infinity());
    }
    return Status(ErrorCode::PLANNING_ERROR, "No feasible trajectories");
  }
}

Stop()

void apollo::planning::LatticePlanner::Stop ( )

inlineoverridevirtual

实现了 apollo::planning::Planner.

在文件 lattice_planner.h 第 48 行定义.

{}

---

该类的文档由以下文件生成:

• modules/planning/planners/lattice/lattice_planner.h
• modules/planning/planners/lattice/lattice_planner.cc