1.7 Path Decider

Posted on Edited on

path decider负责遍历Static Obstacle打tag，供后续使用

file : modules/planning/tasks/deciders/path_decider/path_decider.cc

Summary

遍历所有Static Obstacle，添加横纵向的决策信息（AddLongitudinalDecision/AddLateralDecision）

blocking

首先是一个常见的情况：在不处于借道场景时（之前lane borrow decider中决定），对前方blocking的障碍物做停车处理，将停止点记录下来

1
2
3
4
5
6
7
8
9
10
11
12
13
14
// - add STOP decision for blocking obstacles.
if (obstacle->Id() == blocking_obstacle_id &&
    !injector_->planning_context()
         ->planning_status()
         .path_decider()
         .is_in_path_lane_borrow_scenario()) {
  // Add stop decision
  ADEBUG << "Blocking obstacle = " << blocking_obstacle_id;
  ObjectDecisionType object_decision;
  *object_decision.mutable_stop() = GenerateObjectStopDecision(*obstacle);
  path_decision->AddLongitudinalDecision("PathDecider/blocking_obstacle",
                                         obstacle->Id(), object_decision);
  continue;
}

常规处理

接下来是普通处理，分三步：

  1. 首先排除距离参考线太远的一些Obstacle，添加横纵向信息not-in-s和not-in-l：
  • not-in-s:不在s范围内的
  • not-in-l:距离车道线（不是中心线）超过3m的
1
2
3
4
5
6
7
8
9
10
11
12
// 0. IGNORE by default and if obstacle is not in path s at all.
    ObjectDecisionType object_decision;
    object_decision.mutable_ignore();
    const auto &sl_boundary = obstacle->PerceptionSLBoundary();
    if (sl_boundary.end_s() < frenet_path.front().s() ||
        sl_boundary.start_s() > frenet_path.back().s()) {
      path_decision->AddLongitudinalDecision("PathDecider/not-in-s",
                                             obstacle->Id(), object_decision);
      path_decision->AddLateralDecision("PathDecider/not-in-s", obstacle->Id(),
                                        object_decision);
      continue;
    }
1
2
3
4
5
6
if (curr_l - lateral_radius > sl_boundary.end_l() ||
       curr_l + lateral_radius < sl_boundary.start_l()) {
     // 1. IGNORE if laterally too far away.
     path_decision->AddLateralDecision("PathDecider/not-in-l", obstacle->Id(),
                                       object_decision);
   }
  1. 接下来如果横向有重叠部分，并且无法避让，添加停止点信息
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
else if (sl_boundary.end_l() >= curr_l - min_nudge_l &&
               sl_boundary.start_l() <= curr_l + min_nudge_l) {
      // 2. STOP if laterally too overlapping.
      *object_decision.mutable_stop() = GenerateObjectStopDecision(*obstacle);

      if (path_decision->MergeWithMainStop(
              object_decision.stop(), obstacle->Id(),
              reference_line_info_->reference_line(),
              reference_line_info_->AdcSlBoundary())) {
        path_decision->AddLongitudinalDecision("PathDecider/nearest-stop",
                                               obstacle->Id(), object_decision);
      } else {
        ObjectDecisionType object_decision;
        object_decision.mutable_ignore();
        path_decision->AddLongitudinalDecision("PathDecider/not-nearest-stop",
                                               obstacle->Id(), object_decision);
      }
  1. 如果能够避让，添加避让信息
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
// 3. NUDGE if laterally very close.
      if (sl_boundary.end_l() < curr_l - min_nudge_l) {  // &&
        // sl_boundary.end_l() > curr_l - min_nudge_l - 0.3) {
        // LEFT_NUDGE
        ObjectNudge *object_nudge_ptr = object_decision.mutable_nudge();
        object_nudge_ptr->set_type(ObjectNudge::LEFT_NUDGE);
        object_nudge_ptr->set_distance_l(
            config_.path_decider_config().static_obstacle_buffer());
        path_decision->AddLateralDecision("PathDecider/left-nudge",
                                          obstacle->Id(), object_decision);
      } else if (sl_boundary.start_l() > curr_l + min_nudge_l) {  // &&
        // sl_boundary.start_l() < curr_l + min_nudge_l + 0.3) {
        // RIGHT_NUDGE
        ObjectNudge *object_nudge_ptr = object_decision.mutable_nudge();
        object_nudge_ptr->set_type(ObjectNudge::RIGHT_NUDGE);
        object_nudge_ptr->set_distance_l(
            -config_.path_decider_config().static_obstacle_buffer());
        path_decision->AddLateralDecision("PathDecider/right-nudge",
                                          obstacle->Id(), object_decision);
      }