Apollo planning之PathBoundsDecider

无意2121

已于 2023-12-25 16:54:00 修改

阅读量2.8k

点赞数 5

分类专栏： Apollo planning 文章标签： c++ 自动驾驶算法

于 2022-08-14 21:21:32 首次发布

本文链接：https://blog.csdn.net/weixin_65089713/article/details/126335201

版权

Apollo planning 专栏收录该内容

8 篇文章

订阅专栏

欢迎大家关注我的B站：

偷吃薯片的Zheng同学的个人空间-偷吃薯片的Zheng同学个人主页-哔哩哔哩视频 (bilibili.com)

1 类的继承与调用关系

1.1 继承关系

PathBoundsDecider类继承了Decider类，实现了Process方法，路径边界决策主要的执行过程就在Process方法中。

// modules/planning/tasks/deciders/path_bounds_decider/path_bounds_decider.h
class PathBoundsDecider : public Decider {
... };

Decider类继承了Task类，实现Excute方法，主要是给两个变量赋值：frame和reference-line-info，并且执行Process方法。和上述的Process方法相对应

// modules/planning/tasks/deciders/decider.h
class Decider : public Task {
... };

// modules/planning/tasks/deciders/decider.cc
apollo::common::Status Decider::Execute(
    Frame* frame, ReferenceLineInfo* reference_line_info) {
  Task::Execute(frame, reference_line_info);
  return Process(frame, reference_line_info);
}

apollo::common::Status Decider::Execute(Frame* frame) {
  Task::Execute(frame);
  return Process(frame);
}

Task类，定义类保护类型的变量，是路径边界决策的输入

// modules/planning/tasks/task.h
class Task {
 public:
  // 虚方法，主要是给frame和reference_line_info赋值
  virtual common::Status Execute(Frame* frame,
                                 ReferenceLineInfo* reference_line_info);
  virtual common::Status Execute(Frame* frame);

 protected:
  // frame和reference_line_info变量
  Frame* frame_ = nullptr;
  ReferenceLineInfo* reference_line_info_ = nullptr;

  // 配置与名字
  TaskConfig config_;
  std::string name_;
... };

1.2 调用关系

主要描述task在stage中是如何创建和调用的

TaskFactory类，注册所有的task，包括decider、optimizer和other（E2E的task）。工厂模式

// modules/planning/tasks/task_factory.h
class TaskFactory {
 public:
  // 两个函数都是static属性
  static void Init(...);    // 在初始化函数中，注册所有的task
  static std::unique_ptr<Task> CreateTask(...);    // 创建具体task的实例，返回指向该实例的指针
... };

stage中task的创建与执行

创建：在stage的构造函数中根据stage配置创建task。并将指针放入到task_和task_list_中
使用：在具体的stage中，重写Process方法。调用Process方法，进而调用ExecuteTask*方法（ExecuteTaskOnReferenceLine），最后调用相应的task的Process方法

// modules/planning/scenarios/stage.h
class Stage {
 // 在构造函数中根据stage的配置创建task
 Stage(const ScenarioConfig::StageConfig& config,
        const std::shared_ptr<DependencyInjector>& injector);
 public:

 // 纯虚函数，留给具体的stage实现，不同的stage有不同的实现逻辑
 virtual StageStatus Process(
      const common::TrajectoryPoint& planning_init_point, Frame* frame) = 0;
 protected:
  // 三个执行task的函数，在每个函数中都调用类task的Excute方法，进一步调用具体task的Process方法
  bool ExecuteTaskOnReferenceLine(
      const common::TrajectoryPoint& planning_start_point, Frame* frame);

  bool ExecuteTaskOnReferenceLineForOnlineLearning(
      const common::TrajectoryPoint& planning_start_point, Frame* frame);

  bool ExecuteTaskOnOpenSpace(Frame* frame);

 protected:
  // task的map，key是TaskType，value是指向Task的指针
  std::map<TaskConfig::TaskType, std::unique_ptr<Task>> tasks_;
  // 保存Task列表
  std::vector<Task*> task_list_;
  // stage 配置
  ScenarioConfig::StageConfig config_;
...};

2 路径边界决策数据

2.1 输入和输出

输入变量就是上面提到的Task类中的保护变量，即 frame 和 reference-line-info

frame中有进行一次规划所需要的所有实时数据，reference-line-info包含所有关于参考线的信息

// modules/planning/common/frame.h
class Frame {
 private:
  static DrivingAction pad_msg_driving_action_;
  uint32_t sequence_num_ = 0;

  /* Local_view是一个结构体，包含了如下信息
  // modules/planning/common/local_view.h
  struct LocalView {
    std::shared_ptr<prediction::PredictionObstacles> prediction_obstacles;
    std::shared_ptr<canbus::Chassis> chassis;
    std::shared_ptr<localization::LocalizationEstimate> localization_estimate;
    std::shared_ptr<perception::TrafficLightDetection> traffic_light;
    std::shared_ptr<routing::RoutingResponse> routing;
    std::shared_ptr<relative_map::MapMsg> relative_map;
    std::shared_ptr<PadMessage> pad_msg;
    std::shared_ptr<storytelling::Stories> stories;
  };
  */
  LocalView local_view_;
  // 高清地图
  const hdmap::HDMap *hdmap_ = nullptr;
  common::TrajectoryPoint planning_start_point_;
  // 车辆状态
  // modules/common/vehicle_state/proto/vehicle_state.proto
  common::VehicleState vehicle_state_;
  // 参考线信息
  std::list<ReferenceLineInfo> reference_line_info_;

  bool is_near_destination_ = false;

  /**
   * the reference line info that the vehicle finally choose to drive on
   **/
  const ReferenceLineInfo *drive_reference_line_info_ = nullptr;

  ThreadSafeIndexedObstacles obstacles_;

  std::unordered_map<std::string, const perception::TrafficLight *>
      traffic_lights_;

  // current frame published trajectory
  ADCTrajectory current_frame_planned_trajectory_;

  // current frame path for future possible speed fallback
  DiscretizedPath current_frame_planned_path_;

  const ReferenceLineProvider *reference_line_provider_ = nullptr;

  OpenSpaceInfo open_space_info_;

  std::vector<routing::LaneWaypoint> future_route_waypoints_;

  common::monitor::MonitorLogBuffer monitor_logger_buffer_;
};

// modules/planning/common/reference_line_info.h

class ReferenceLineInfo {
 ...
 private:
  static std::unordered_map<std::string, bool> junction_right_of_way_map_;
  const common::VehicleState vehicle_state_;  // 车辆状态
  const common::TrajectoryPoint adc_planning_point_;  // TrajectoryPoint定义在modules/common/proto/pnc_point.proto中

  /* 参考线，以道路中心线，做过顺滑的一条轨迹，往后80米，往前130米。
  class ReferenceLine {
  ...
  private:
  struct SpeedLimit {
    double start_s = 0.0;
    double end_s = 0.0;
    double speed_limit = 0.0;  // unit m/s
    ...};
  
  // This speed limit overrides the lane speed limit
  std::vector<SpeedLimit> speed_limit_;
  std::vector<ReferencePoint> reference_points_;  // ReferencePoint包含有信息(k, dk, x, y, heading, s, l)
  hdmap::Path map_path_;
  uint32_t priority_ = 0;
  };
  */
  ReferenceLine reference_line_;

  /**
   * @brief this is the number that measures the goodness of this reference
   * line. The lower the better.
   */
  // 评价函数，值越低越好
  double cost_ = 0.0;

  bool is_drivable_ = true;
  // PathDecision包含了一条路径上的所有obstacle的决策，有两种：lateral(Nudge, Ignore)和longitudinal(Stop, Yield, Follow, Overtake, Ignore)
  PathDecision path_decision_;
  // 指针
  Obstacle* blocking_obstacle_;

  /* path的边界，结果保存在这个变量里。通过**SetCandidatePathBoundaries**方法保存到此变量
    // modules/planning/common/path_boundary.h
    class PathBoundary {
    ...
      private:
     double start_s_ = 0.0;
     double delta_s_ = 0.0;
     std::vector<std::pair<double, double>> boundary_;
     std::string label_ = "regular";
     std::string blocking_obstacle_id_ = "";
  };
  */
  std::vector<PathBoundary> candidate_path_boundaries_;
  // PathData类，包含XY坐标系和SL坐标系的相互转化
  std::vector<PathData> candidate_path_data_;

  PathData path_data_;
  PathData fallback_path_data_;
  SpeedData speed_data_;

  DiscretizedTrajectory discretized_trajectory_;

  RSSInfo rss_info_;

  /**
   * @brief SL boundary of stitching point (starting point of plan trajectory)
   * relative to the reference line
   */
  SLBoundary adc_sl_boundary_;
... };

输出信息则是都保存在 reference-line-info 中

Status PathBoundsDecider::Process(
    Frame* const frame, ReferenceLineInfo* const reference_line_info)

2.2 参数设置

例如，规划的纵向距离设置为100m，采样距离为0.5m

// modules/planning/tasks/deciders/path_bounds_decider/path_bounds_decider.cc
// s方向的距离
constexpr double kPathBoundsDeciderHorizon = 100.0;
// s方向的间隔
constexpr double kPathBoundsDeciderResolution = 0.5;

// Lane宽度
constexpr double kDefaultLaneWidth = 5.0;
// Road的道路
constexpr double kDefaultRoadWidth = 20.0;

// TODO(all): Update extra tail point base on vehicle speed.
constexpr int kNumExtraTailBoundPoint = 20;
constexpr double kPulloverLonSearchCoeff = 1.5;
constexpr double kPulloverLatSearchCoeff = 1.25;

2.3 数据结构

路径边界点用纵向距离以及上下限的横向距离来描述，相当于是一定分辨率的扫描

把障碍物分解成两个edge

// modules/planning/tasks/deciders/path_bounds_decider/path_bounds_decider.cc
namespace {
// PathBoundPoint contains: (s, l_min, l_max). 路径边界点
using PathBoundPoint = std::tuple<double, double, double>;
// PathBound contains a vector of PathBoundPoints. 路径边界
using PathBound = std::vector<PathBoundPoint>;
// ObstacleEdge contains: (is_start_s, s, l_min, l_max, obstacle_id). 障碍物的边
using ObstacleEdge = std::tuple<int, double, double, double, std::string>;
}  // namespace

  for (const auto* obstacle : indexed_obstacles.Items()) {
    // Only focus on those within-scope obstacles.
    if (!IsWithinPathDeciderScopeObstacle(*obstacle)) {
      continue;
    }
    // Only focus on obstacles that are ahead of ADC.
    if (obstacle->PerceptionSLBoundary().end_s() < adc_frenet_s_) {
      continue;
    }
    // Decompose each obstacle's rectangle into two edges: one at
    // start_s; the other at end_s.
    const auto obstacle_sl = obstacle->PerceptionSLBoundary();
    sorted_obstacles.emplace_back(
        1, obstacle_sl.start_s() - FLAGS_obstacle_lon_start_buffer,
        obstacle_sl.start_l() - FLAGS_obstacle_lat_buffer,
        obstacle_sl.end_l() + FLAGS_obstacle_lat_buffer, obstacle->Id());
    sorted_obstacles.emplace_back(
        0, obstacle_sl.end_s() + FLAGS_obstacle_lon_end_buffer,
        obstacle_sl.start_l() - FLAGS_obstacle_lat_buffer,
        obstacle_sl.end_l() + FLAGS_obstacle_lat_buffer, obstacle->Id());
  }

  std::sort(sorted_obstacles.begin(), sorted_obstacles.end(),
            [](const ObstacleEdge& lhs, const ObstacleEdge& rhs) {
              if (std::get<1>(lhs) != std::get<1>(rhs)) {
                return std::get<1>(lhs) < std::get<1>(rhs);
              } else {
                return std::get<0>(lhs) > std::get<0>(rhs);
              }
            });

通过依次遍历按纵向距离排列的ObstacleEdge后，最后得到道路边界，如下图红线

扫到ObstacleEdge进入或退出，并更新一下边界

3 代码流程及框架

3.1 fallback

fallback场景生成过程如上图所示。fallback是其他3种场景计算PathBound失败时的备选（没有办法的办法），只考虑自车信息和静态道路信息，不考虑静态障碍物。因此，这种情况下speed decider负责让自车在障碍物前停车。

Status PathBoundsDecider::GenerateFallbackPathBound(
    const ReferenceLineInfo& reference_line_info, PathBound* const path_bound) {
  // 1. Initialize the path boundaries to be an indefinitely large area.
  if (!InitPathBoundary(reference_line_info, path_bound)) { ... }
  // 2. Decide a rough boundary based on lane info and ADC's position
  std::string dummy_borrow_lane_type;
  if (!GetBoundaryFromLanesAndADC(reference_line_info,
                                  LaneBorrowInfo::NO_BORROW, 0.5, path_bound,
                                  &dummy_borrow_lane_type)) { ... }
  return Status::OK();
}

fallback主要调用以下两个函数

InitPathBoundary

生成一条默认的fall back pathbound，在正常求解轨迹无解或失败的情况下使用

bool PathBoundsDecider::InitPathBoundary(
  ...
  // Starting from ADC's current position, increment until the horizon, and
  // set lateral bounds to be infinite at every spot.
  // 从adc当前位置开始，以0.5m为间隔取点，直到终点，将 [左, 右] 边界设置为double的 [lowerst, max]
  for (double curr_s = adc_frenet_s_;
       curr_s < std::fmin(adc_frenet_s_ +
                              std::fmax(kPathBoundsDeciderHorizon,
                                        reference_line_info.GetCruiseSpeed() *
                                            FLAGS_trajectory_time_length),
                          reference_line.Length());
       curr_s += kPathBoundsDeciderResolution) {
    path_bound->emplace_back(curr_s, std::numeric_limits<double>::lowest(),
                             std::numeric_limits<double>::max());
  }
...}

GetBoundaryFromLanesAndADC

GetBoundaryFromLanesAndADC则包含了根据自车信息、车道信息计算PathBound的具体细节。首先根据当前位置获取当前车道的左右宽度，然后根据左右借道获取相邻车道的宽度（当然，fallback设定不借道），最后综合各因素，更新PathBound。

// TODO(jiacheng): this function is to be retired soon.
bool PathBoundsDecider::GetBoundaryFromLanesAndADC(
  ...
  for (size_t i = 0; i < path_bound->size(); ++i) {
    double curr_s = std::get<0>((*path_bound)[i]);
    // 1. Get the current lane width at current point.获取当前点车道的宽度
    if (!reference_line.GetLaneWidth(curr_s, &curr_lane_left_width,
                                     &curr_lane_right_width)) {
      AWARN << "Failed to get lane width at s = " << curr_s;
      curr_lane_left_width = past_lane_left_width;
      curr_lane_right_width = past_lane_right_width;
    } else {...}

    // 2. Get the neighbor lane widths at the current point.获取当前点相邻车道的宽度
    double curr_neighbor_lane_width = 0.0;
    if (CheckLaneBoundaryType(reference_line_info, curr_s, lane_borrow_info)) {
      hdmap::Id neighbor_lane_id;
      if (lane_borrow_info == LaneBorrowInfo::LEFT_BORROW) {
        // 借左车道
        ...
      } else if (lane_borrow_info == LaneBorrowInfo::RIGHT_BORROW) {
        // 借右车道
        ...
      }
    }

    // 3. 根据道路宽度，adc的位置和速度计算合适的边界。
    static constexpr double kMaxLateralAccelerations = 1.5;
    double offset_to_map = 0.0;
    reference_line.GetOffsetToMap(curr_s, &offset_to_map);

    double ADC_speed_buffer = (adc_frenet_ld_ > 0 ? 1.0 : -1.0) *
                              adc_frenet_ld_ * adc_frenet_ld_ /
                              kMaxLateralAccelerations / 2.0;
    // 向左车道借到，左边界会变成左侧车道左边界
    double curr_left_bound_lane =
        curr_lane_left_width + (lane_borrow_info == LaneBorrowInfo::LEFT_BORROW
                                    ? curr_neighbor_lane_width
                                    : 0.0);
    // 和上面类似
    double curr_right_bound_lane =
        -curr_lane_right_width -
        (lane_borrow_info == LaneBorrowInfo::RIGHT_BORROW
             ? curr_neighbor_lane_width
             : 0.0);

    double curr_left_bound = 0.0;  // 左边界
    double curr_right_bound = 0.0;  // 右边界
    // 计算左边界和右边界
    if (config_.path_bounds_decider_config()
            .is_extend_lane_bounds_to_include_adc() ||
        is_fallback_lanechange) {
      // extend path bounds to include ADC in fallback or change lane path
      // bounds.
      double curr_left_bound_adc =
          std::fmax(adc_l_to_lane_center_,
                    adc_l_to_lane_center_ + ADC_speed_buffer) +
          GetBufferBetweenADCCenterAndEdge() + ADC_buffer;
      curr_left_bound =
          std::fmax(curr_left_bound_lane, curr_left_bound_adc) - offset_to_map;

      double curr_right_bound_adc =
          std::fmin(adc_l_to_lane_center_,
                    adc_l_to_lane_center_ + ADC_speed_buffer) -
          GetBufferBetweenADCCenterAndEdge() - ADC_buffer;
      curr_right_bound =
          std::fmin(curr_right_bound_lane, curr_right_bound_adc) -
          offset_to_map;
    } else {
      curr_left_bound = curr_left_bound_lane - offset_to_map;
      curr_right_bound = curr_right_bound_lane - offset_to_map;
    }

    // 4. 更新边界.
    if (!UpdatePathBoundaryWithBuffer(i, curr_left_bound, curr_right_bound,
                                      path_bound, is_left_lane_boundary,
                                      is_right_lane_boundary)) {
      path_blocked_idx = static_cast<int>(i);
    }
... }

3.2 pull over

GetBoundaryFromRoads

与GetBoundaryFromLanesAndADC不同，GetBoundaryFromRoads函数根据道路信息计算出边界: 获取参考线信息，并对路径上的点，逐点计算新的路径边界

GetBoundaryFromStaticObstacle

根据障碍物，调整路径边界：

计算障碍物在frenet坐标系下的坐标
扫描线排序，S方向扫描
- 只关注在路径边界内的障碍物和在adc前方的障碍物（避免冗余避障，提高计算速度）
- 将障碍物分解为两个边界，开始和结束
映射障碍物ID
- Adc能从左边通过为True，否则为False
逐个点的检查path路径上的障碍物（新的和旧的）

SearchPullOverPosition

搜索pull over位置（所谓停车点）的过程：

根据pull_over_status.pull_over_type()判断是前向搜索（pull over开头第一个点），还是后向搜索（pull over末尾后一个点）
两层循环，外层控制搜索的索引 idx，内层控制进一步的索引（前向idx+1，后向idx-1）。
根据内外两层循环的索引，判断搜索到的空间是否满足车辆宽度和长度要求，判断是否可以pull over

bool PathBoundsDecider::SearchPullOverPosition( ... ) {
  // search direction
  bool search_backward = false;  // search FORWARD by default
  //01.先根据不同场景搜索一个考察可停车区域的大致端点，然后从该端点出发确定可停车区域
  double pull_over_s = 0.0;
  if (pull_over_status.pull_over_type() == PullOverStatus::EMERGENCY_PULL_OVER) {
    //紧急情况，前方停车
    if (!FindEmergencyPullOverS(reference_line_info, &pull_over_s)) { ... }
    search_backward = false;  // search FORWARD from target position
  } else if (pull_over_status.pull_over_type() == PullOverStatus::PULL_OVER) {
    if (!FindDestinationPullOverS(frame, reference_line_info, path_bound,
                                  &pull_over_s)) { ... }
    //理想的停车点是route的终点，因此要反向搜索，以找到匹配的bounds
    search_backward = true;  // search BACKWARD from target position
  } else {
    return false;
  }
 
  int idx = 0;
  if (search_backward) {
    // 1. Locate the first point before destination.
    idx = static_cast<int>(path_bound.size()) - 1;
    while (idx >= 0 && std::get<0>(path_bound[idx]) > pull_over_s) {
      --idx;
    }
    //反向搜索时，idx表示停车区域的末端
  } else {
    // 1. Locate the first point after emergency_pull_over s.
    while (idx < static_cast<int>(path_bound.size()) &&
           std::get<0>(path_bound[idx]) < pull_over_s) {
      ++idx;
    }
    //前向搜索时，idx表示停车区域的开端
  }
  if (idx < 0 || idx >= static_cast<int>(path_bound.size())) { ... }
 
  // Search for a feasible location for pull-over
  ... //根据一些条件计算pull_over_space_length 和 pull_over_space_width 
 
  // 2. Find a window that is close to road-edge.(not in any intersection)
  //02.搜索可停车的区域始末。内外2层while循环，外循环控制一个开始搜索的端点idx，因为当
  //考察的区域不符合安全性和尺寸条件时，idx也要变化。内循环控制另一个端点j。
  bool has_a_feasible_window = false;
  while ((search_backward && idx >= 0 &&
          std::get<0>(path_bound[idx]) - std::get<0>(path_bound.front()) >
              pull_over_space_length) ||
         (!search_backward && idx < static_cast<int>(path_bound.size()) &&
          std::get<0>(path_bound.back()) - std::get<0>(path_bound[idx]) >
              pull_over_space_length)) {
    int j = idx;
    bool is_feasible_window = true;
 
    ... // Check if the point of idx is within intersection.    
    //如果遇到路口，调整开始搜索的端点idx，重新搜索
    if (!junctions.empty()) {
      AWARN << "Point is in PNC-junction.";
      idx = search_backward ? idx - 1 : idx + 1;
      continue;
    }
    
    //搜索一段宽度达标、长度达标的可停车区域。即，搜索合适的端点j
    while ((search_backward && j >= 0 &&
            std::get<0>(path_bound[idx]) - std::get<0>(path_bound[j]) <
                pull_over_space_length) ||
           (!search_backward && j < static_cast<int>(path_bound.size()) &&
            std::get<0>(path_bound[j]) - std::get<0>(path_bound[idx]) <
                pull_over_space_length)) {
      double curr_s = std::get<0>(path_bound[j]);
      double curr_right_bound = std::fabs(std::get<1>(path_bound[j]));      
      reference_line_info.reference_line().GetRoadWidth(
          curr_s, &curr_road_left_width, &curr_road_right_width);      
      if (curr_road_right_width - (curr_right_bound + adc_half_width) >
          config_.path_bounds_decider_config().pull_over_road_edge_buffer()) {
        AERROR << "Not close enough to road-edge. Not feasible for pull-over.";
        is_feasible_window = false;
        break;
      }
      if(std::get<2>(path_bound[j])-std::get<1>(path_bound[j]) < pull_over_space_width) {
        AERROR << "Not wide enough to fit ADC. Not feasible for pull-over.";
        is_feasible_window = false;
        break;
      }
 
      j = search_backward ? j - 1 : j + 1;
    }
    if (j < 0) {
      return false;
    }
    //03.找到可停车区域后，获取停车目标点的位姿
    if (is_feasible_window) {
      // estimate pull over point to have the vehicle keep same safety distance
      // to front and back      
      ...
      int start_idx = j;
      int end_idx = idx;
      if (!search_backward) {
        start_idx = idx;
        end_idx = j;
      }
      //根据start_idx和end_idx计算pull_over_idx。注意index是相对于bounds的
      ...
      const auto& pull_over_point = path_bound[pull_over_idx];     
      //根据找到的停车点，设定相关信息，并根据参考线计算停车点的朝向角
      ...
      *pull_over_configuration = std::make_tuple(pull_over_x, pull_over_y,
        pull_over_theta, static_cast<int>(pull_over_idx));
      break;  //一旦找到可停车区域，退出最外层while循环，返回结果
    }
    idx = search_backward ? idx - 1 : idx + 1;
  }  
}

3.3 lane change

Status PathBoundsDecider::GenerateLaneChangePathBound(
    const ReferenceLineInfo& reference_line_info,
    std::vector<std::tuple<double, double, double>>* const path_bound) {
  // 1.初始化，和前面的步骤类似
  if (!InitPathBoundary(reference_line_info, path_bound)) {...}


  // 2. 根据道路和adc的信息获取一个大致的路径边界
  std::string dummy_borrow_lane_type;
  if (!GetBoundaryFromLanesAndADC(reference_line_info,
                                  LaneBorrowInfo::NO_BORROW, 0.1, path_bound,
                                  &dummy_borrow_lane_type, true)) {...}
 

  // 3. Remove the S-length of target lane out of the path-bound.
  GetBoundaryFromLaneChangeForbiddenZone(reference_line_info, path_bound);


  // 根据静态障碍物调整边界.
  if (!GetBoundaryFromStaticObstacles(reference_line_info.path_decision(),
                                      path_bound, &blocking_obstacle_id)) {...}
  ...
}

GetBoundaryFromLaneChangeForbiddenZone函数是lane change重要的函数。运行过程如下：

如果当前位置可以变道，则直接变道
如果有一个lane-change的起点，则直接使用它
逐个检查变道前的点的边界，改变边界的值（如果已经过了变道点，则返回）

void PathBoundsDecider::GetBoundaryFromLaneChangeForbiddenZone(
    const ReferenceLineInfo& reference_line_info, PathBound* const path_bound) {


  // 1.当前位置直接变道。
  auto* lane_change_status = injector_->planning_context()
                                 ->mutable_planning_status()
                                 ->mutable_change_lane();
  if (lane_change_status->is_clear_to_change_lane()) {
    ADEBUG << "Current position is clear to change lane. No need prep s.";
    lane_change_status->set_exist_lane_change_start_position(false);
    return;
  }


  // 2.如果已经有一个lane-change的起点，就直接使用它，否则再找一个
  double lane_change_start_s = 0.0;
  if (lane_change_status->exist_lane_change_start_position()) {
    common::SLPoint point_sl;
    reference_line.XYToSL(lane_change_status->lane_change_start_position(),
                          &point_sl);
    lane_change_start_s = point_sl.s();
  } else {
    // TODO(jiacheng): train ML model to learn this.
    // 设置为adc前方一段距离为变道起始点
    lane_change_start_s = FLAGS_lane_change_prepare_length + adc_frenet_s_;

    // Update the decided lane_change_start_s into planning-context.
    // 更新变道起始点的信息
    common::SLPoint lane_change_start_sl;
    lane_change_start_sl.set_s(lane_change_start_s);
    lane_change_start_sl.set_l(0.0);
    common::math::Vec2d lane_change_start_xy;
    reference_line.SLToXY(lane_change_start_sl, &lane_change_start_xy);
    lane_change_status->set_exist_lane_change_start_position(true);
    lane_change_status->mutable_lane_change_start_position()->set_x(
        lane_change_start_xy.x());
    lane_change_status->mutable_lane_change_start_position()->set_y(
        lane_change_start_xy.y());
  }

  // Remove the target lane out of the path-boundary, up to the decided S.
  // 逐个检查变道前的点的边界，改变边界的值
  for (size_t i = 0; i < path_bound->size(); ++i) {
    double curr_s = std::get<0>((*path_bound)[i]);
    if (curr_s > lane_change_start_s) {
      break;
    }
    double curr_lane_left_width = 0.0;
    double curr_lane_right_width = 0.0;
    double offset_to_map = 0.0;
    reference_line.GetOffsetToMap(curr_s, &offset_to_map);
    if (reference_line.GetLaneWidth(curr_s, &curr_lane_left_width,
                                    &curr_lane_right_width)) {
      double offset_to_lane_center = 0.0;
      reference_line.GetOffsetToMap(curr_s, &offset_to_lane_center);
      curr_lane_left_width += offset_to_lane_center;
      curr_lane_right_width -= offset_to_lane_center;
    }
    curr_lane_left_width -= offset_to_map;
    curr_lane_right_width += offset_to_map;

    std::get<1>((*path_bound)[i]) =
        adc_frenet_l_ > curr_lane_left_width
            ? curr_lane_left_width + GetBufferBetweenADCCenterAndEdge()
            : std::get<1>((*path_bound)[i]);
    std::get<1>((*path_bound)[i]) =
        std::fmin(std::get<1>((*path_bound)[i]), adc_frenet_l_ - 0.1);
    std::get<2>((*path_bound)[i]) =
        adc_frenet_l_ < -curr_lane_right_width
            ? -curr_lane_right_width - GetBufferBetweenADCCenterAndEdge()
            : std::get<2>((*path_bound)[i]);
    std::get<2>((*path_bound)[i]) =
        std::fmax(std::get<2>((*path_bound)[i]), adc_frenet_l_ + 0.1);
  }
}

3.4 regular

Status PathBoundsDecider::GenerateRegularPathBound(
    const ReferenceLineInfo& reference_line_info,
    const LaneBorrowInfo& lane_borrow_info, PathBound* const path_bound,
    std::string* const blocking_obstacle_id,
    std::string* const borrow_lane_type) {
  // 1.初始化边界.
  if (!InitPathBoundary(reference_line_info, path_bound)) {...}


  // 2.根据adc位置和lane信息确定大致的边界
  if (!GetBoundaryFromLanesAndADC(reference_line_info, lane_borrow_info, 0.1,
                                  path_bound, borrow_lane_type)) {...}
  // PathBoundsDebugString(*path_bound);

  // 3.根据障碍物调整道路边界
  if (!GetBoundaryFromStaticObstacles(reference_line_info.path_decision(),
                                      path_bound, blocking_obstacle_id)) {...}
  ...
}

借道有如下三种类型

  enum class LaneBorrowInfo {
    LEFT_BORROW,
    NO_BORROW,
    RIGHT_BORROW,
  };

本文参考路径边界决策 — Apollo Auto 0.0.1 文档 (daobook.github.io)

Path Bounds Decider - IcathianRain - 博客园 (cnblogs.com)

Baidu Apollo代码解析之path_bounds_decider_linxigjs的博客-CSDN博客

若侵权请联系删除