《从0开始搭建实现apollo9.0》系列八 泊车模块解读_openspaceprestopdecider

《从0开始搭建实现apollo9.0》系列八 泊车模块解读
和行业内朋友交流发现PNC目前很多要求的是泊车相关的算法和经验，之前参与过行泊一体的项目开发调试，现结合自己的经验对开源的进行一个学习。目前很多公司采用的泊车方法和apollo类似，可能根据各自的特点和环境进行了一些更新，不过都可以从apollo这个开始入门。

Apollo9.0中泊车模块位置，

modules/planning/scenarios/valet_parking/
1

泊车阶段的开始条件：

1. planning command里存在泊车命令
2. 距离泊车点距离parking_spot_range_to_start以内

 ParkingType
  { PARALLEL_PARKING, VERTICAL_PARKING }
1
2

配置文件
pipeline.pb.txt可以看到泊车分为两个阶段，一个是ApproachingParking，一个是Parking。其中ApproachingParking可以当作是一个行车的过程，引导车辆沿主路行驶到泊车位，行车的终点为泊车车位。Parking为泊入车位过程，包括可行驶区域构建，轨迹生成，轨迹分类（档位切换），轨迹决策。

stage: {
  name: "VALET_PARKING_APPROACHING_PARKING_SPOT"
  type: "StageApproachingParkingSpot"
  enabled: true
stage: {
  name: "VALET_PARKING_PARKING"
  type: "StageParking"
1
2
3
4
5
6
7

scenario_conf.pb.txt中定义了参数

parking_spot_range_to_start: 20.0
max_valid_stop_distance: 1.0
1
2

一个为stage切换判断的距离，一个是stop距离。
valet_parking_scenario.h

class ValetParkingScenario : public Scenario {//继承基类Scenario   重写基类中的成员函数
 public:
  bool Init(std::shared_ptr<DependencyInjector> injector,
            const std::string& name) override;

  /**
   * @brief Get the scenario context.
   */
  ValetParkingContext* GetContext() override { return &context_; }

  bool IsTransferable(const Scenario* const other_scenario,
                      const Frame& frame) override;

 private:
  static bool SearchTargetParkingSpotOnPath(//不确定为何使用static类型
      const hdmap::Path& nearby_path, const std::string& target_parking_id,
      hdmap::PathOverlap* parking_space_overlap);
  static bool CheckDistanceToParkingSpot(
      const Frame& frame, const common::VehicleState& vehicle_state,
      const hdmap::Path& nearby_path, const double parking_start_range,
      const hdmap::PathOverlap& parking_space_overlap);

 private:
  bool init_ = false;
  ValetParkingContext context_;
  const hdmap::HDMap* hdmap_ = nullptr;
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

#静态成员函数知识点：

归属 : 在 C++ 类中 , 静态成员函数 是一种 特殊的函数 , 该函数属于类 , 而不是属于 类实例对象 ;
静态成员函数调用不依赖于对象 : 即使 没有创建 类 的 实例对象 , 也可以 通过 类名:: 调用 类中定义的 静态成员函数 ;
静态成员函数作用 : 静态成员函数 通常用于 执行与类本身相关的操作 , 执行该函数 不涉及到 类实例对象中的信息 , 也不能在 静态成员函数 中访问 普通的 成员变量 和 成员函数 ;
//静态函数的特点
//1.静态函数无需生成对象就可被调用
//2.静态函数不能直接调用非静态的成员变量
//3.不能使用this引用
IsTransferable函数用于判断other_scenario是否能够转移到当前的Scenario中，代码做了以下判断：

1. planning command里存在泊车命令和车位id
2. 距离泊车点距离parking_spot_range_to_start以内

if (!frame.local_view().planning_command->has_parking_command()) {
    return false;
  }
  if (other_scenario == nullptr || frame.reference_line_info().empty()) {
    return false;
  }
  std::string target_parking_spot_id;
  if (frame.local_view().planning_command->has_parking_command() &&
      frame.local_view()
          .planning_command->parking_command()
          .has_parking_spot_id()) {
    target_parking_spot_id = frame.local_view()
                                 .planning_command->parking_command()
                                 .parking_spot_id();
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

SearchTargetParkingSpotOnPath函数 确定具有当前位置到车位的路径；

if (!SearchTargetParkingSpotOnPath(nearby_path, target_parking_spot_id,
                                     &parking_space_overlap)) {
    ADEBUG << "No such parking spot found after searching all path forward "
              "possible"
           << target_parking_spot_id;
    return false;
  }
  double parking_spot_range_to_start =
      context_.scenario_config.parking_spot_range_to_start();
  if (!CheckDistanceToParkingSpot(frame, vehicle_state, nearby_path,
                                  parking_spot_range_to_start,
                                  parking_space_overlap)) {
    ADEBUG << "target parking spot found, but too far, distance larger than "
              "pre-defined distance"
           << target_parking_spot_id;
    return false;
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

CheckDistanceToParkingSpot 函数确定目标停车点距离在parking_spot_range_to_start范围内，使用delata_s来进行判断。

if (std::abs(center_point_s - vehicle_point_s) < parking_start_range) {
    return true;
  }
1
2
3

Stage1：StageApproachingParkingSpot

用于开放空间中执行泊车与靠边停车任务时，生成在公共道路上的停车点，车辆停在停车点后，会转入开放空间算法。
是引导车辆沿主路行驶到泊车位

运行了一个车道规划算法来接近指定的停车位
一旦找到所需的正确停车点，ADC就会停下来，以便倒车进入停车位
如果停车正确，接近停车位阶段结束并切换到停车阶段
否则ADC停留在该阶段，直到它接近所需的停车位
1
2
3
4

StageResult StageApproachingParkingSpot::Process(
    const common::TrajectoryPoint& planning_init_point, Frame* frame) {
  ADEBUG << "stage: StageApproachingParkingSpot";
  CHECK_NOTNULL(frame);
  StageResult result;
  auto scenario_context = GetContextAs<ValetParkingContext>();

  if (scenario_context->target_parking_spot_id.empty()) {
    return result.SetStageStatus(StageStatusType::ERROR);
  }//停车id判断，为空返回error

  *(frame->mutable_open_space_info()->mutable_target_parking_spot_id()) =
      scenario_context->target_parking_spot_id;
  frame->mutable_open_space_info()->set_pre_stop_rightaway_flag(
      scenario_context->pre_stop_rightaway_flag);
  *(frame->mutable_open_space_info()->mutable_pre_stop_rightaway_point()) =
      scenario_context->pre_stop_rightaway_point;
  auto* reference_lines = frame->mutable_reference_line_info();
  for (auto& reference_line : *reference_lines) {
    auto* path_decision = reference_line.path_decision();
    if (nullptr == path_decision) {
      continue;
    }
    auto* dest_obstacle = path_decision->Find(FLAGS_destination_obstacle_id);
    if (nullptr == dest_obstacle) {
      continue;
    }
    ObjectDecisionType decision;
    decision.mutable_ignore();
    dest_obstacle->EraseDecision();
    dest_obstacle->AddLongitudinalDecision("ignore-dest-in-valet-parking",
                                           decision);// 添加忽略障碍物信息
  }
  // 在参考线上进行规划
 // 运行了一个车道规划算法来接近指定的停车位
  result = ExecuteTaskOnReferenceLine(planning_init_point, frame);

  scenario_context->pre_stop_rightaway_flag =
      frame->open_space_info().pre_stop_rightaway_flag();
  scenario_context->pre_stop_rightaway_point =
      frame->open_space_info().pre_stop_rightaway_point();
	// 检查一旦找到所需的正确停车点，ADC就会停下来，以便倒车进入停车位
  if (CheckADCStop(*frame)) {
  	// 如果完成停车则进入泊车规划阶段
    next_stage_ = "VALET_PARKING_PARKING";
    return StageResult(StageStatusType::FINISHED);
  }
  if (result.HasError()) {
    AERROR << "StopSignUnprotectedStagePreStop planning error";
    return result.SetStageStatus(StageStatusType::ERROR);
  }
	// 未接近停车位置，停留在该阶段，直到它接近所需的停车位
  return result.SetStageStatus(StageStatusType::RUNNING);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54

在OpenSpacePreStopDecider里主要是计算不同情况下的停车点：

class OpenSpacePreStopDecider : public Decider {
 public:
  bool Init(const std::string& config_dir, const std::string& name,
            const std::shared_ptr<DependencyInjector>& injector) override;

 private:
  apollo::common::Status Process(
      Frame* frame, ReferenceLineInfo* reference_line_info) override;

  bool CheckParkingSpotPreStop(Frame* const frame,
                               ReferenceLineInfo* const reference_line_info,
                               double* target_s);

  bool CheckPullOverPreStop(Frame* const frame,
                            ReferenceLineInfo* const reference_line_info,
                            double* target_s);

  void SetParkingSpotStopFence(const double target_s, Frame* const frame,
                               ReferenceLineInfo* const reference_line_info);

  void SetPullOverStopFence(const double target_s, Frame* const frame,
                            ReferenceLineInfo* const reference_line_info);

 private:
  OpenSpacePreStopDeciderConfig config_;
  static constexpr const char* OPEN_SPACE_STOP_ID = "OPEN_SPACE_PRE_STOP";
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

Stage2：StageParking

StageResult StageParking::Process(
    const common::TrajectoryPoint& planning_init_point, Frame* frame) {
  // Open space planning doesn't use planning_init_point from upstream because
  // of different stitching strategy
  auto scenario_context = GetContextAs<ValetParkingContext>();
  frame->mutable_open_space_info()->set_is_on_open_space_trajectory(true);//确保在open_space中
  *(frame->mutable_open_space_info()->mutable_target_parking_spot_id()) =//具有车位id
      scenario_context->target_parking_spot_id;
  StageResult result = ExecuteTaskOnOpenSpace(frame);//进行task状态机
  if (result.HasError()) {
    AERROR << "StageParking planning error";
    return result.SetStageStatus(StageStatusType::ERROR);
  }
  return result.SetStageStatus(StageStatusType::RUNNING);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

StageResult Stage::ExecuteTaskOnOpenSpace(Frame* frame) {
  auto ret = common::Status::OK();
  StageResult stage_result;
  for (auto task : task_list_) {//遍历task_list_进行执行
    ret = task->Execute(frame);
    if (!ret.ok()) {
      stage_result.SetTaskStatus(ret);
      AERROR << "Failed to run tasks[" << task->Name()
             << "], Error message: " << ret.error_message();
      return stage_result;
    }
  }
1
2
3
4
5
6
7
8
9
10
11
12

task01：planning-task-open-space-roi-decider

OpenSpaceRoiDecider用于在开放空间算法中生成可行驶边界，根据规划场景的不同，如：泊车、靠边停车、驶入主路，以当前车辆位置为坐标原点，生成不同的可行驶边界与目标点。
在该decider中区分以下类型：
enum RoiType {
NOT_DEFINED = 0;
PARKING = 1;
PULL_OVER = 2;
PARK_AND_GO = 3;
NARROW_STREET_U_TURN = 4;
DEAD_END = 5;
}
在泊车模块中

  if (roi_type == OpenSpaceRoiDeciderConfig::PARKING) {
    target_parking_spot_id_ = frame->open_space_info().target_parking_spot_id();
    ParkingInfo parking_info;
    if (!GetParkingSpot(frame, &parking_info)) {//函数首先将停车位的polygon转换为顺时针，而后计算了目标车位的位姿，根据与道路的夹角，判断车位是平行车位还是垂直车位，并将车位信息存储在`parking_info`中。
      const std::string msg = "Fail to get parking boundary from map";
      AERROR << msg;
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }

    SetOrigin(parking_info, frame);//设置原点，归一化处理

    SetParkingSpotEndPose(parking_info, frame);//将目标点位姿转换并存储至parking_info
	//多态函数，区分
    if (!GetParkingBoundary(parking_info, *nearby_path_, frame,
                            &roi_boundary)) {//将目标车位转换为局部坐标系，判断车位位置在左边还是右边，算出道路边界，增加关键点信息，而后根据车位与道路的位置关系将车位边界与道路边界组合，确保凸空间，便于求解。
      const std::string msg = "Fail to get parking boundary from map";
      AERROR << msg;
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }
  }
  if (!FormulateBoundaryConstraints(roi_boundary, frame)) {//构造超平面，
    const std::string msg = "Fail to formulate boundary constraints";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

task02：planning-task-open-space-trajectory-provider

该task包括trajectory_provider和trajectory_optimizer，plan泊车轨迹。这里轨迹由两部分组成，

Status OpenSpaceTrajectoryProvider::Process() {
  ADEBUG << "trajectory provider";
  auto trajectory_data =
      frame_->mutable_open_space_info()->mutable_stitched_trajectory_result();//获取frame内存储的信息

  // generate stop trajectory at park_and_go check_stage
  if (injector_->planning_context()
          ->mutable_planning_status()
          ->mutable_park_and_go()
          ->in_check_stage()) {
    ADEBUG << "ParkAndGo Stage Check.";
    GenerateStopTrajectory(trajectory_data);
    return Status::OK();
  }
  // Start thread when getting in Process() for the first time
  if (config_.enable_open_space_planner_thread() && !thread_init_flag_) {
    task_future_ = cyber::Async(
        &OpenSpaceTrajectoryProvider::GenerateTrajectoryThread, this);//开始规划轨迹的独立线程，并加锁限制
    thread_init_flag_ = true;
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

std::vectorcommon::TrajectoryPoint stitching_trajectory_和DiscretizedTrajectory optimized_trajectory_;
在构造函数中调用并初始化混合A*算法，使用智能指针unique_ptr定义，确保独享权。

  std::unique_ptr<HybridAStar> warm_start_;
  std::unique_ptr<DistanceApproachProblem> distance_approach_;
  std::unique_ptr<DualVariableWarmStartProblem> dual_variable_warm_start_;
  std::unique_ptr<IterativeAnchoringSmoother> iterative_anchoring_smoother_;
1
2
3
4

OpenSpaceTrajectoryOptimizer::OpenSpaceTrajectoryOptimizer(
    const OpenSpaceTrajectoryOptimizerConfig& config)
    : config_(config) {
  // Load config
  AINFO << config_.DebugString();
  // Initialize hybrid astar class pointer
  warm_start_.reset(new HybridAStar(config.planner_open_space_config()));

  // Initialize dual variable warm start class pointer
  dual_variable_warm_start_.reset(
      new DualVariableWarmStartProblem(config.planner_open_space_config()));

  // Initialize distance approach trajectory smootherclass pointer
  distance_approach_.reset(
      new DistanceApproachProblem(config.planner_open_space_config()));

  // Initialize iterative anchoring smoother config class pointer
  iterative_anchoring_smoother_.reset(
      new IterativeAnchoringSmoother(config.planner_open_space_config()));
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

然后进入Plan函数：

Status OpenSpaceTrajectoryOptimizer::Plan(
    const std::vector<common::TrajectoryPoint>& stitching_trajectory,
    const std::vector<double>& end_pose, const std::vector<double>& XYbounds,
    double rotate_angle, const Vec2d& translate_origin,
    const Eigen::MatrixXi& obstacles_edges_num,
    const Eigen::MatrixXd& obstacles_A, const Eigen::MatrixXd& obstacles_b,
    const std::vector<std::vector<Vec2d>>& obstacles_vertices_vec,
    double* time_latency) {
1
2
3
4
5
6
7
8

输入包括stitching_trajectory历史轨迹last trajectory，end_pose泊车点位置，XYbounds上个task得到的roi，rotate_angle旋转角，translate_origin坐标系转换矩阵，障碍物信息等。
首先进行输入信息判断，接着进行目标点距离判断IsInitPointNearDestination。
stitching_trajectory.back().path_point()作为init_point而并非是current_point。

// Rotate and scale the state
  PathPointNormalizing(rotate_angle, translate_origin, &init_x, &init_y,
                       &init_phi);
1
2
3

进行坐标系转换。将车辆此时的位置转换到以库位左上角为原点的坐标系下。
有了起终点的位置、可行驶边界，障碍物信息后，就可以规划泊车轨迹了。

HybridAStartResult result;

  if (warm_start_->Plan(init_x, init_y, init_phi, end_pose[0], end_pose[1],
                        end_pose[2], XYbounds, obstacles_vertices_vec,
                        &result)) {
    AINFO << "State warm start problem solved successfully!";
  } else {
    AERROR << "State warm start problem failed to solve";
    return Status(ErrorCode::PLANNING_ERROR,
                  "State warm start problem failed to solve");
  }
 struct HybridAStartResult {
  std::vector<double> x;
  std::vector<double> y;
  std::vector<double> phi;
  std::vector<double> v;
  std::vector<double> a;
  std::vector<double> steer;
  std::vector<double> accumulated_s;
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

调用混合A*进行规划粗轨迹。

混合A*：

bool HybridAStar::Plan(
    double sx, double sy, double sphi, double ex, double ey, double ephi,
    const std::vector<double>& XYbounds,
    const std::vector<std::vector<common::math::Vec2d>>& obstacles_vertices_vec,
    HybridAStartResult* result,
    const std::vector<std::vector<common::math::Vec2d>>&
        soft_boundary_vertices_vec,
    bool reeds_sheep_last_straight) {
  reed_shepp_generator_->reeds_sheep_last_straight_ = reeds_sheep_last_straight;
  // clear containers
  //每周期清空缓存
  open_set_.clear();
  close_set_.clear();
  open_pq_ = decltype(open_pq_)();
  final_node_ = nullptr;
  PrintCurves print_curves;
  std::vector<std::vector<common::math::LineSegment2d>>
      obstacles_linesegments_vec;
  for (const auto& obstacle_vertices : obstacles_vertices_vec) {// 构造障碍物轮廓边界段，后面进行碰撞检测时使用
    size_t vertices_num = obstacle_vertices.size();
    std::vector<common::math::LineSegment2d> obstacle_linesegments;
    for (size_t i = 0; i < vertices_num - 1; ++i) {
      common::math::LineSegment2d line_segment =
          common::math::LineSegment2d(
              obstacle_vertices[i], obstacle_vertices[i + 1]);
      obstacle_linesegments.emplace_back(line_segment);
    }
    obstacles_linesegments_vec.emplace_back(obstacle_linesegments);
  }
  obstacles_linesegments_vec_ = std::move(obstacles_linesegments_vec);
  for (size_t i = 0; i < obstacles_linesegments_vec_.size(); i++) {//打印展示
    for (auto linesg : obstacles_linesegments_vec_[i]) {
      std::string name = std::to_string(i) + "roi_boundary";
      print_curves.AddPoint(name, linesg.start());
      print_curves.AddPoint(name, linesg.end());
    }
  }
  Vec2d sposition(sx, sy);
  Vec2d svec_to_center(
          (vehicle_param_.front_edge_to_center() -
           vehicle_param_.back_edge_to_center()) / 2.0,
          (vehicle_param_.left_edge_to_center() -
           vehicle_param_.right_edge_to_center()) / 2.0);
  Vec2d scenter(sposition + svec_to_center.rotate(sphi));
  Box2d sbox(scenter, sphi, vehicle_param_.length(), vehicle_param_.width());
  print_curves.AddPoint("vehicle_start_box", sbox.GetAllCorners());
  Vec2d eposition(ex, ey);
  print_curves.AddPoint("end_position", eposition);
  Vec2d evec_to_center(
          (vehicle_param_.front_edge_to_center() -
           vehicle_param_.back_edge_to_center()) / 2.0,
          (vehicle_param_.left_edge_to_center() -
           vehicle_param_.right_edge_to_center()) / 2.0);
  Vec2d ecenter(eposition + evec_to_center.rotate(ephi));
  Box2d ebox(ecenter, ephi, vehicle_param_.length(), vehicle_param_.width());
  print_curves.AddPoint("vehicle_end_box", ebox.GetAllCorners());
  XYbounds_ = XYbounds;
// load nodes and obstacles
//加载上面生成的边界和起始点和终点
  start_node_.reset(
      new Node3d({sx}, {sy}, {sphi}, XYbounds_, planner_open_space_config_));
  end_node_.reset(
      new Node3d({ex}, {ey}, {ephi}, XYbounds_, planner_open_space_config_));
  AINFO << "start node" << sx << "," << sy << "," << sphi;
  AINFO << "end node " << ex << "," << ey << "," << ephi;
  if (!ValidityCheck(start_node_)) {//起始点与终点进行碰撞检测，碰撞检测包括与边界的碰撞检测，与障碍物的每个边线的碰撞检测，确保所有维度都没有危险
    AERROR << "start_node in collision with obstacles";
    AERROR << start_node_->GetX()
           << "," << start_node_->GetY()
           << "," << start_node_->GetPhi();
    print_curves.PrintToLog();
    return false;
  }
  if (!ValidityCheck(end_node_)) {
    AERROR << "end_node in collision with obstacles";
    print_curves.PrintToLog();
    return false;
  }
  double map_time = Clock::NowInSeconds();
   //开始第一阶段DP，反向计算并存储目标点到某点的启发代价，生成DpMap存储，通过表内查找提高运行效率
  grid_a_star_heuristic_generator_->GenerateDpMap(//
      ex, ey, XYbounds_, obstacles_linesegments_vec_);
  ADEBUG << "map time " << Clock::NowInSeconds() - map_time;
  // load open set, pq
  open_set_.insert(start_node_->GetIndex());
  open_pq_.emplace(start_node_, start_node_->GetCost());
  // Hybrid A* begins
  size_t explored_node_num = 0;
  size_t available_result_num = 0;
  auto best_explored_num = explored_node_num;
  auto best_available_result_num = available_result_num;
  double astar_start_time = Clock::NowInSeconds();
  double heuristic_time = 0.0;
  double rs_time = 0.0;
  double node_generator_time = 0.0;
  double validity_check_time = 0.0;
  size_t max_explored_num =
      planner_open_space_config_.warm_start_config().max_explored_num();
  size_t desired_explored_num = std::min(
      planner_open_space_config_.warm_start_config().desired_explored_num(),
      planner_open_space_config_.warm_start_config().max_explored_num());
  static constexpr int kMaxNodeNum = 200000;
  std::vector<std::shared_ptr<Node3d>> candidate_final_nodes;
  while (!open_pq_.empty() &&
         open_pq_.size() < kMaxNodeNum &&
         available_result_num < desired_explored_num &&
         explored_node_num < max_explored_num) {
    std::shared_ptr<Node3d> current_node = open_pq_.top().first;
    open_pq_.pop();
    const double rs_start_time = Clock::NowInSeconds();
    std::shared_ptr<Node3d> final_node = nullptr;
    if (AnalyticExpansion(current_node, &final_node)) {//首先进行起终点距离判断，如果短rs曲线可以直接到达即结束
      if (final_node_ == nullptr ||
          final_node_->GetTrajCost() > final_node->GetTrajCost()) {
        final_node_ = final_node;
        best_explored_num = explored_node_num + 1;
        best_available_result_num = available_result_num + 1;
      }
      available_result_num++;
    }
    explored_node_num++;
    const double rs_end_time = Clock::NowInSeconds();
    rs_time += rs_end_time - rs_start_time;
    close_set_.insert(current_node->GetIndex());

    if (Clock::NowInSeconds() - astar_start_time >
            planner_open_space_config_.warm_start_config()
                .astar_max_search_time() &&
        available_result_num > 0) {
      break;
    }

    size_t begin_index = 0;
    size_t end_index = next_node_num_;
    std::unordered_set<std::string> temp_set;
    for (size_t i = begin_index; i < end_index; ++i) {
      const double gen_node_time = Clock::NowInSeconds();
      std::shared_ptr<Node3d> next_node = Next_node_generator(current_node, i);  
      // 从current_node开始遍历生成next_node，
      // steering为[-max,max]，长度为arc_length_，使用二轮阿克曼进行求解next_node的位置信息
      node_generator_time += Clock::NowInSeconds() - gen_node_time;

      // boundary check failure handle
      if (next_node == nullptr) {
        continue;
      }
      // check if the node is already in the close set
      if (close_set_.count(next_node->GetIndex()) > 0) {
        continue;
      }
      // collision check
      const double validity_check_start_time = Clock::NowInSeconds();
      if (!ValidityCheck(next_node)) {
        continue;
      }
      validity_check_time += Clock::NowInSeconds() - validity_check_start_time;
      // 判断是否是新的节点，进行cost计算，并存储相关信息，如果到达终点，
      if (open_set_.count(next_node->GetIndex()) == 0) {
        const double start_time = Clock::NowInSeconds();
        CalculateNodeCost(current_node, next_node);
        // f = h + g
        // h可以根据DPMap求出（终点为dpmap的起点）
        // g为node_cost,包括换档、转向率、转向的cost，
        const double end_time = Clock::NowInSeconds();
        heuristic_time += end_time - start_time;
        temp_set.insert(next_node->GetIndex());
        open_pq_.emplace(next_node, next_node->GetCost());
      }
    }
    open_set_.insert(temp_set.begin(), temp_set.end());
  }

  if (final_node_ == nullptr) {
    AERROR << "Hybird A* cannot find a valid path";
    print_curves.PrintToLog();
    return false;
  }

  AINFO << "open_pq_.empty()" << (open_pq_.empty() ? "true" : "false");
  AINFO << "open_pq_.size()" << open_pq_.size();
  AINFO << "desired_explored_num" << desired_explored_num;
  AINFO << "min cost is : " << final_node_->GetTrajCost();
  AINFO << "max_explored_num is " << max_explored_num;
  AINFO << "explored node num is " << explored_node_num
        << "available_result_num " << available_result_num;
  AINFO << "best_explored_num is " << best_explored_num
        << "best_available_result_num is " << best_available_result_num;
  AINFO << "cal node time is " << heuristic_time
        << "validity_check_time " << validity_check_time
        << "node_generator_time " << node_generator_time;
  AINFO << "reed shepp time is " << rs_time;
  AINFO << "hybrid astar total time is "
        << Clock::NowInSeconds() - astar_start_time;

  print_curves.AddPoint(
      "rs_point", final_node_->GetXs().front(), final_node_->GetYs().front());
  if (!GetResult(result)) {
  //回溯法求得完整路径， 
  //根据current_node->GetPreNode()，得到完整的node_list
  //对完整路径进行分段，根据档位变换进行分段，档位的切换为bool gear =
  //      std::abs(common::math::NormalizeAngle(tracking_angle - heading_angle)) <
  //      (M_PI_2);
  //每段的起终点的速度都为0，可以进行递推，得到v,a,steering等信息，
    AERROR << "GetResult failed";
    print_curves.PrintToLog();
    return false;
  }
  for (size_t i = 0; i < result->x.size(); i++) {
    print_curves.AddPoint("warm_path", result->x[i], result->y[i]);
  }
  // PrintBox print_box("warm_path_box");
  // for (size_t i = 0; i < result->x.size(); i = i + 5) {
  //   print_box.AddAdcBox(result->x[i], result->y[i], result->phi[i]);
  // }
  // print_box.PrintToLog();
  print_curves.PrintToLog();
  return true;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219

混合A星得出一个粗略路径后，先是进行分段，进行速度加速度的求取，合并成轨迹，然后进行分别平滑。

if (!warm_start_->TrajectoryPartition(result, &partition_trajectories)) {
      return Status(ErrorCode::PLANNING_ERROR, "Hybrid Astar partition failed");
    }
bool current_gear =
      std::abs(common::math::NormalizeAngle(tracking_angle - heading_angle))
      <
      (M_PI_2);
1
2
3
4
5
6
7

在计算速度加速度时有两种方法：

```cpp
if (FLAGS_use_s_curve_speed_smooth) {
      if (!GenerateSCurveSpeedAcceleration(&result)) {//根据常三阶导数特性进行求解
        AERROR << "GenerateSCurveSpeedAcceleration fail";
        return false;
      }
    } else {
      if (!GenerateSpeedAcceleration(&result)) {//普通的差值计算速度
        AERROR << "GenerateSpeedAcceleration fail";
        return false;
      }
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14

apollo使用离散点来控制轨迹的形状，并近似评估约束是否满足。这种方法的关键思想是将一维函数离散为二阶导数水平，即每段段内的jerk（这里指的是侧向jerk）是constant，所以段内的l’是随s线性变化的，并使用常数三阶导数项来“连接”连续离散点。位置变量的三阶导数通常称为jerk。因此，这个公式被命名为piecewise jerk方法。

```cpp
// In for loop
    ADEBUG << "Trajectories size in smoother is " << size;
    for (size_t i = 0; i < size; ++i) {
      LoadHybridAstarResultInEigen(&partition_trajectories[i], &xWS_vec[i],
                                   &uWS_vec[i]);//将规划结果转换为矩阵
      Eigen::MatrixXd last_time_u(2, 1);
      double init_v = 0.0;
      // Stitching point control and velocity is set for first piece of
      // trajectories. In the next ones, control and velocity are assumed to be
      // zero as the next trajectories always start from vehicle static state
      if (i == 0) {
        const double init_steer = trajectory_stitching_point.steer();
        const double init_a = trajectory_stitching_point.a();
        last_time_u << init_steer, init_a;
        init_v = trajectory_stitching_point.v();
      } else {
        last_time_u << 0.0, 0.0;
        init_v = 0.0;
      }
      // TODO(Jinyun): Further testing
      const auto smoother_start_timestamp = std::chrono::system_clock::now();
      if (FLAGS_use_iterative_anchoring_smoother) {
        AINFO << "use iterative anchoring smoother";
        if (!GenerateDecoupledTraj(//开始进行解耦平滑
                xWS_vec[i], last_time_u(1, 0), init_v, obstacles_vertices_vec,
                &state_result_ds_vec[i], &control_result_ds_vec[i],
                &time_result_ds_vec[i])) {
          AERROR << "Smoother fail at " << i << "th trajectory";
          AERROR << i << "th trajectory size is " << xWS_vec[i].cols();
          return Status(
              ErrorCode::PLANNING_ERROR,
              "iterative anchoring smoothing problem failed to solve");
        }
      } else {
        const double start_system_timestamp =
            std::chrono::duration<double>(
                std::chrono::system_clock::now().time_since_epoch())
                .count();
        AINFO << "use distance approach parallel smoother";
        if (!GenerateDistanceApproachTraj(
                xWS_vec[i], uWS_vec[i], XYbounds, obstacles_edges_num,
                obstacles_A, obstacles_b, obstacles_vertices_vec, last_time_u,
                init_v, &state_result_ds_vec[i], &control_result_ds_vec[i],
                &time_result_ds_vec[i], &l_warm_up_vec[i], &n_warm_up_vec[i],
                &dual_l_result_ds_vec[i], &dual_n_result_ds_vec[i])) {
          AERROR << "Smoother fail at " << i
                 << "th trajectory with index starts from 0";
          AERROR << i << "th trajectory size is " << xWS_vec[i].cols();
          AERROR << "State matrix: " << xWS_vec[i];
          AERROR << "Control matrix: " << uWS_vec[i];
          return Status(ErrorCode::PLANNING_ERROR,
                        "distance approach smoothing problem failed to solve");
        }
        const auto end_system_timestamp =
            std::chrono::duration<double>(
                std::chrono::system_clock::now().time_since_epoch())
                .count();
        const auto time_diff_ms =
            (end_system_timestamp - start_system_timestamp) * 1000;
        ADEBUG << "total planning time spend: " << time_diff_ms << " ms.";
        ADEBUG << i << "th trajectory size is " << xWS_vec[i].cols();
        ADEBUG << "average time spend: " << time_diff_ms / xWS_vec[i].cols()
               << " ms per point.";
        ADEBUG << "average time spend after smooth: "
               << time_diff_ms / state_result_ds_vec[i].cols()
               << " ms per point.";
        ADEBUG << i << "th smoothed trajectory size is "
               << state_result_ds_vec[i].cols();
      }
      const auto smoother_end_timestamp = std::chrono::system_clock::now();
      std::chrono::duration<double> smoother_diff =
          smoother_end_timestamp - smoother_start_timestamp;
      AINFO << "Open space trajectory smoothing total time: "
            << smoother_diff.count() * 1000.0 << " ms at the " << i
            << "th trajectory.";
      AINFO << "The " << i << "th trajectory pre-smoothing size is "
            << xWS_vec[i].cols() << "; post-smoothing size is "
            << state_result_ds_vec[i].cols();
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81

bool OpenSpaceTrajectoryOptimizer::GenerateDecoupledTraj(
    const Eigen::MatrixXd& xWS, const double init_a, const double init_v,
    const std::vector<std::vector<Vec2d>>& obstacles_vertices_vec,
    Eigen::MatrixXd* state_result_dc, Eigen::MatrixXd* control_result_dc,
    Eigen::MatrixXd* time_result_dc) {
  DiscretizedTrajectory smoothed_trajectory;
  if (!iterative_anchoring_smoother_->Smooth(//主要的平滑函数
          xWS, init_a, init_v, obstacles_vertices_vec, &smoothed_trajectory)) {
    return false;
  }

  LoadResult(smoothed_trajectory, state_result_dc, control_result_dc,
             time_result_dc);
  return true;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

路径平滑用的是fem算法，通过优化的方式来得出平滑后的路径，然后对平滑后的路径进行碰撞检测，如果发生碰撞，则收缩空间，即不让优化后的点离参考点太远，再次优化，直到不发生碰撞。检测碰撞的方式和混合A星是不一样的，这里采用的是判断两个bounding box之间是否有相交，会比混合A星中用的（判断自车bounding box和障碍物的各个边之间是否相交）运算量小的多

bool Smooth(const Eigen::MatrixXd& xWS, const double init_a,
              const double init_v,
              const std::vector<std::vector<common::math::Vec2d>>&
                  obstacles_vertices_vec,
              DiscretizedTrajectory* discretized_trajectory);
FemPosDeviationSmoother fem_pos_smoother(
      planner_open_space_config_.iterative_anchoring_smoother_config()
          .fem_pos_deviation_smoother_config());
//Cost分为三个部分：第一部分为平滑度Cost；第二部分为长度Cost；第三部分为相对原始点偏移Cost。
bool SmoothSpeed(const double init_a, const double init_v,
                   const double path_length, SpeedData* smoothed_speeds);
PiecewiseJerkSpeedProblem piecewise_jerk_problem(
      num_of_knots, delta_t, {0.0, std::abs(init_v), 0.0});

1
2
3
4
5
6
7
8
9
10
11
12
13
14

然后进行速度的平滑，由于在泊车过程中，不考虑动态障碍物，所以在速度平滑过程中没有障碍物的碰撞检测，速度规划只有qp（二次规划）过程，这个qp是基于OSQP求解器的。

task03：OPEN_SPACE_TRAJECTORY_PARTITION

OpenSpaceTrajectoryPartition首先对开放空间生成轨迹进行线性插值,而后根据路径前进或后退，将轨迹分割为不同段。最后找到当前轨迹上与自车最接近的点，作为轨迹起点，规划相应轨迹。

InterpolateTrajectory

对生成轨迹stitched_trajectory_result通过间隔时间进行线性插值，而后将插值后的轨迹存储在interpolated_trajectory中。

if (FLAGS_use_iterative_anchoring_smoother) {
    *interpolated_trajectory = stitched_trajectory_result;
    return;
  }//false时进行点间的线形插值
1
2
3
4

PartitionTrajectory

该函数遍历所有轨迹点，根据当前轨迹点与下一轨迹点的朝向，判断车辆是否换挡，如果换挡则以当前轨迹点对轨迹进行分割，从而使每段轨迹的车辆朝向相同。

*gear =
      std::abs(common::math::NormalizeAngle(tracking_angle - heading_angle)) <
              (M_PI_2)
          ? canbus::Chassis::GEAR_DRIVE
          : canbus::Chassis::GEAR_REVERSE;
 if (cur_gear != *gear) {
      is_trajectory_last_point = true;
      LoadTrajectoryPoint(trajectory_point, is_trajectory_last_point, *gear,
                          &last_pos_vec, &distance_s, trajectory);
      partitioned_trajectories->emplace_back();
      current_trajectory_gear = &(partitioned_trajectories->back());
      current_trajectory_gear->second = cur_gear;
      distance_s = 0.0;
      is_trajectory_last_point = false;
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

AdjustRelativeTimeAndS

该函数以车辆当前跟踪轨迹点为起点，对所有轨迹点进行相对时间与纵向距离调整，使当前跟踪轨迹点处的相对时间与纵向距离为0。

for (size_t i = 0; i < unpartitioned_trajectory_size; ++i) {
    TrajectoryPoint* trajectory_point =
        &(unpartitioned_trajectory_result->at(i));
    trajectory_point->set_relative_time(trajectory_point->relative_time() -
                                        time_shift);
    trajectory_point->mutable_path_point()->set_s(
        trajectory_point->path_point().s() - s_shift);
  }
1
2
3
4
5
6
7
8

InsertGearShiftTrajectory

当车辆走到轨迹末端，要走下一段轨迹时，插入停车轨迹，使车辆能够原地调整车轮转角。

const auto& curr_gear =
      partitioned_trajectories.at(current_trajectory_index).second;
  if (flag_change_to_next || !current_gear_status->gear_shift_period_finished ||
      curr_gear != ego_gear_) {
    current_gear_status->gear_shift_period_finished = false;
    if (current_gear_status->gear_shift_period_started) {
      current_gear_status->gear_shift_start_time =
          Clock::Instance()->NowInSeconds();
      current_gear_status->gear_shift_position =
          partitioned_trajectories.at(current_trajectory_index).second;
      current_gear_status->gear_shift_period_started = false;
      current_gear_status->gear_shift_period_time = 0.0;
    }
    if (current_gear_status->gear_shift_period_time >
            config_.gear_shift_period_duration() &&
        current_gear_status->gear_shift_position == ego_gear_) {
      current_gear_status->gear_shift_period_finished = true;
      current_gear_status->gear_shift_period_started = true;
      AINFO << "finished gear shift";
    } else {
      double init_kappa = partitioned_trajectories.at(current_trajectory_index)
                              .first[0]
                              .path_point()
                              .kappa();
      GenerateGearShiftTrajectory(current_gear_status->gear_shift_position,
                                  init_kappa, gear_switch_idle_time_trajectory);
      current_gear_status->gear_shift_period_time =
          Clock::Instance()->NowInSeconds() -
          current_gear_status->gear_shift_start_time;
      return true;
    }
  }
  void OpenSpaceTrajectoryPartition::GenerateGearShiftTrajectory(
    const canbus::Chassis::GearPosition& gear_position, double init_kappa,
    TrajGearPair* gear_switch_idle_time_trajectory) {
  gear_switch_idle_time_trajectory->first.clear();
  const double gear_shift_max_t = config_.gear_shift_max_t();
  const double gear_shift_unit_t = config_.gear_shift_unit_t();
  // TrajectoryPoint point;
  for (double t = 0.0; t < gear_shift_max_t; t += gear_shift_unit_t) {
    TrajectoryPoint point;
    point.mutable_path_point()->set_x(frame_->vehicle_state().x());
    point.mutable_path_point()->set_y(frame_->vehicle_state().y());
    point.mutable_path_point()->set_theta(frame_->vehicle_state().heading());
    point.mutable_path_point()->set_s(0.0);
    point.mutable_path_point()->set_kappa(init_kappa);
    point.set_relative_time(t);
    point.set_v(0.0);
    point.set_a(0.0);
    gear_switch_idle_time_trajectory->first.emplace_back(point);
  }
  ADEBUG << "gear_switch_idle_time_trajectory"
         << gear_switch_idle_time_trajectory->first.size();
  gear_switch_idle_time_trajectory->second = gear_position;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55

Status OpenSpaceTrajectoryPartition::Process() {
  const auto& open_space_info = frame_->open_space_info();
  auto open_space_info_ptr = frame_->mutable_open_space_info();
  const auto& stitched_trajectory_result =
      open_space_info.stitched_trajectory_result();

  auto* interpolated_trajectory_result_ptr =
      open_space_info_ptr->mutable_interpolated_trajectory_result();
 //对生成轨迹`stitched_trajectory_result`通过间隔时间进行线性插值，而后将插值后的轨迹存储在`interpolated_trajectory`中。
  InterpolateTrajectory(stitched_trajectory_result,
                        interpolated_trajectory_result_ptr);

  auto* partitioned_trajectories =
      open_space_info_ptr->mutable_partitioned_trajectories();
	//该函数遍历所有轨迹点，根据当前轨迹点与下一轨迹点的朝向，判断车辆是否换挡，如果换挡则以当前轨迹点对轨迹进行分割，从而使每段轨迹的车辆朝向相同。
  PartitionTrajectory(*interpolated_trajectory_result_ptr,
                      partitioned_trajectories);

  const auto& open_space_status =
      injector_->planning_context()->planning_status().open_space();
  if (!open_space_status.position_init() &&
      frame_->open_space_info().open_space_provider_success()) {
    auto* open_space_status = injector_->planning_context()
                                  ->mutable_planning_status()
                                  ->mutable_open_space();
    open_space_status->set_position_init(true);
    auto* chosen_partitioned_trajectory =
        open_space_info_ptr->mutable_chosen_partitioned_trajectory();
    auto* mutable_trajectory =
        open_space_info_ptr->mutable_stitched_trajectory_result();
    //该函数以车辆当前跟踪轨迹点为起点，对所有轨迹点进行相对时间与纵向距离调整，使当前跟踪轨迹点处的相对时间与纵向距离为0。
    AdjustRelativeTimeAndS(open_space_info.partitioned_trajectories(), 0, 0,
                           mutable_trajectory, chosen_partitioned_trajectory);
    current_trajectory_index_ = 0;
    fail_search_fallback_ = false;
    last_index_ = -1;
    return Status::OK();
  }

  // Choose the one to follow based on the closest partitioned trajectory
  size_t trajectories_size = partitioned_trajectories->size();
  // may get stop trajecotry;
  current_trajectory_index_ =
      std::min<size_t>(trajectories_size - 1, current_trajectory_index_);
  size_t current_trajectory_point_index = 0;
  bool flag_change_to_next = false;

  // Vehicle related information used to choose closest point
  UpdateVehicleInfo();

  const auto& gear =
      partitioned_trajectories->at(current_trajectory_index_).second;
  const auto& cur_trajectory =
      partitioned_trajectories->at(current_trajectory_index_).first;
  size_t trajectory_size = cur_trajectory.size();
  CHECK_GT(trajectory_size, 0U);
  flag_change_to_next = CheckReachTrajectoryEnd(
      cur_trajectory, gear, trajectories_size, &current_trajectory_point_index);
  AINFO << "current_trajectory_index_" << current_trajectory_index_ << ","
        << current_trajectory_point_index << "time index" << last_index_
        << "flag_change_to_next" << flag_change_to_next;
  auto* chosen_partitioned_trajectory =
      open_space_info_ptr->mutable_chosen_partitioned_trajectory();

  auto chosen_trajectory = &(chosen_partitioned_trajectory->first);
  if (config_.use_gear_shift_trajectory()) {
    if (InsertGearShiftTrajectory(flag_change_to_next,
                                  current_trajectory_index_,
                                  open_space_info.partitioned_trajectories(),
                                  chosen_partitioned_trajectory) &&
        chosen_partitioned_trajectory->first.size() != 0) {
      chosen_trajectory = &(chosen_partitioned_trajectory->first);
      ADEBUG << "After InsertGearShiftTrajectory [" << chosen_trajectory->size()
             << "]";
      last_index_ = -1;
      return Status::OK();
    }
  }
  // Choose the closest point to track
  std::priority_queue<std::pair<size_t, double>,
                      std::vector<std::pair<size_t, double>>, comp_>
      closest_point;
  AINFO << "ego:" << std::fixed << ego_x_ << "," << ego_y_ << ","
        << vehicle_moving_direction_;
  for (size_t j = 0; j < trajectory_size; ++j) {
    const TrajectoryPoint& trajectory_point = cur_trajectory.at(j);
    const PathPoint& path_point = trajectory_point.path_point();
    const double path_point_x = path_point.x();
    const double path_point_y = path_point.y();
    const double path_point_theta = path_point.theta();
    const Vec2d tracking_vector(path_point_x - ego_x_, path_point_y - ego_y_);
    const double distance = tracking_vector.Length();
    // const double tracking_direction = tracking_vector.Angle();
    const double traj_point_moving_direction =
        gear == canbus::Chassis::GEAR_REVERSE
            ? NormalizeAngle(path_point_theta + M_PI)
            : path_point_theta;
    // const double head_track_difference = std::abs(
    //     NormalizeAngle(tracking_direction - vehicle_moving_direction_));
    const double heading_search_difference = std::abs(NormalizeAngle(
        traj_point_moving_direction - vehicle_moving_direction_));
    // AINFO << "XY" << std::fixed << path_point_x << "," << path_point_y << ","
    //       << path_point_theta << "," << distance << ","
    //       << heading_search_difference << "ego heading"
    //       << vehicle_moving_direction_;
    if (distance < distance_search_range_ &&
        heading_search_difference < heading_search_range_) {
      // get vehicle box and path point box, compute IOU
      Box2d path_point_box({path_point_x, path_point_y}, path_point_theta,
                           ego_length_, ego_width_);
      Vec2d shift_vec{shift_distance_ * std::cos(path_point_theta),
                      shift_distance_ * std::sin(path_point_theta)};
      path_point_box.Shift(shift_vec);
      double iou_ratio =
          Polygon2d(ego_box_).ComputeIoU(Polygon2d(path_point_box));
      closest_point.emplace(j, iou_ratio);
    }
  }

  if (closest_point.empty() || fail_search_fallback_) {
    fail_search_fallback_ = true;
    frame_->mutable_open_space_info()->set_fallback_flag(true);
    auto fallback_tra_pair =
        frame_->mutable_open_space_info()->mutable_fallback_trajectory();
    GenerateStopTrajectory(&fallback_tra_pair->first, -4.0);
    fallback_tra_pair->second = frame_->local_view().chassis->gear_location();
    const std::string msg =
        "Fail to find nearest trajectory point to follow stop to fallback "
        "replan";
    AERROR << msg;
    return Status::OK();
  }
  current_trajectory_point_index = closest_point.top().first;
  if (!flag_change_to_next) {
    double veh_rel_time;
    size_t time_match_index;
    double now_time = Clock::Instance()->NowInSeconds();
    auto& traj = partitioned_trajectories->at(current_trajectory_index_).first;
    if (last_index_ != -1) {
      veh_rel_time = traj[last_index_].relative_time() + now_time - last_time_;
      AINFO << std::fixed << now_time << "," << last_time_;
      time_match_index = traj.QueryLowerBoundPoint(veh_rel_time);
    } else {
      time_match_index = current_trajectory_point_index;
      last_index_ = time_match_index;
      last_time_ = now_time;
    }

    AINFO << "time_match_index" << time_match_index << "pos match index"
          << current_trajectory_point_index;
    AINFO << "TRAJ CLOSEST" << std::fixed
          << traj.at(current_trajectory_point_index).path_point().x() << ","
          << traj.at(current_trajectory_point_index).path_point().y();
    if (std::abs(traj[time_match_index].path_point().s() -
                 traj.at(current_trajectory_point_index).path_point().s()) <
        config_.speed_replan_distance()) {
      current_trajectory_point_index = time_match_index;
    } else {
      AINFO << "reset speed because matched point too far";
      last_index_ = current_trajectory_point_index;
      last_time_ = now_time;
    }
  } else {
    last_index_ = -1;
  }
  auto* mutable_trajectory =
      open_space_info_ptr->mutable_stitched_trajectory_result();
  AdjustRelativeTimeAndS(open_space_info.partitioned_trajectories(),
                         current_trajectory_index_,
                         current_trajectory_point_index, mutable_trajectory,
                         chosen_partitioned_trajectory);
  return Status::OK();
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174

task04：OPEN_SPACE_TRAJECTORY_PARTITION

OpenSpaceFallbackDecider检测开放空间下轨迹是否与障碍物发生碰撞并做出相应策略。

BuildPredictedEnvironment

根据预测输入的障碍物信息，构建出不同时间间隔的障碍物边界，将障碍物信息输入到predicted_bounding_rectangles中。

IsCollisionFreeTrajectory

判断自车轨迹与障碍物轨迹是否没有碰撞，当返回为false，根据碰撞点位置，生成匀减速停车轨迹，使车辆停车。

IsCollisionFreeEgoBox

判断车辆扩展后的box与静态障碍物是否有碰撞，当返回为false，车辆急刹并置fallback_flag为true，从而在task OpenSpaceTrajectoryProvider中重新规划轨迹。