【Apollo学习笔记】——规划模块TASK之RULE_BASED_STOP_DECIDER

前言

在Apollo星火计划学习笔记——Apollo路径规划算法原理与实践与【Apollo学习笔记】——Planning模块讲到……Stage::Process的PlanOnReferenceLine函数会依次调用task_list中的TASK，本文将会继续以LaneFollow为例依次介绍其中的TASK部分究竟做了哪些工作。由于个人能力所限，文章可能有纰漏的地方，还请批评斧正。

在modules/planning/conf/scenario/lane_follow_config.pb.txt配置文件中，我们可以看到LaneFollow所需要执行的所有task。

stage_config: {
  stage_type: LANE_FOLLOW_DEFAULT_STAGE
  enabled: true
  task_type: LANE_CHANGE_DECIDER
  task_type: PATH_REUSE_DECIDER
  task_type: PATH_LANE_BORROW_DECIDER
  task_type: PATH_BOUNDS_DECIDER
  task_type: PIECEWISE_JERK_PATH_OPTIMIZER
  task_type: PATH_ASSESSMENT_DECIDER
  task_type: PATH_DECIDER
  task_type: RULE_BASED_STOP_DECIDER
  task_type: SPEED_BOUNDS_PRIORI_DECIDER
  task_type: SPEED_HEURISTIC_OPTIMIZER
  task_type: SPEED_DECIDER
  task_type: SPEED_BOUNDS_FINAL_DECIDER
  task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
  # task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
  task_type: RSS_DECIDER

本文将继续介绍LaneFollow的第8个TASK——RULE_BASED_STOP_DECIDER

基于规则的停止决策是规划模块的任务，属于task中的decider类别。基于规则的停止决策根据一些规则来设置停止标志。

RULE_BASED_STOP_DECIDER相关配置

modules/planning/conf/planning_config.pb.txt

default_task_config: {
  task_type: RULE_BASED_STOP_DECIDER
  rule_based_stop_decider_config {
    max_adc_stop_speed: 0.5
    max_valid_stop_distance: 1.0
    search_beam_length: 20.0
    search_beam_radius_intensity: 0.08
    search_range: 3.14
    is_block_angle_threshold: 0.5
  }
}

modules/planning/proto/task_config.proto

// RuleBasedStopDeciderConfig

message RuleBasedStopDeciderConfig {
  optional double max_adc_stop_speed = 1 [default = 0.3];
  optional double max_valid_stop_distance = 2 [default = 0.5];
  optional double search_beam_length = 3 [default = 5.0];
  optional double search_beam_radius_intensity = 4 [default = 0.08];
  optional double search_range = 5 [default = 3.14];
  optional double is_block_angle_threshold = 6 [default = 1.57];

  optional double approach_distance_for_lane_change = 10 [default = 80.0];
  optional double urgent_distance_for_lane_change = 11 [default = 50.0];
}

RULE_BASED_STOP_DECIDER总体流程

• 输入
  apollo::common::Status RuleBasedStopDecider::Process(Frame *const frame, ReferenceLineInfo *const reference_line_info)
  输入是frame和reference_line_info。

• 输出
  输出保存到reference_line_info中。

代码流程及框架
Process中的代码流程如下图所示。

apollo::common::Status RuleBasedStopDecider::Process(
    Frame *const frame, ReferenceLineInfo *const reference_line_info) {
  // 1. Rule_based stop for side pass onto reverse lane
  StopOnSidePass(frame, reference_line_info);

  // 2. Rule_based stop for urgent lane change
  if (FLAGS_enable_lane_change_urgency_checking) {
    CheckLaneChangeUrgency(frame);
  }

  // 3. Rule_based stop at path end position
  AddPathEndStop(frame, reference_line_info);

  return Status::OK();
}

主要核心的函数就是StopOnSidePass、CheckLaneChangeUrgency以及AddPathEndStop，接着分别对三者进行剖析。

StopOnSidePass

void RuleBasedStopDecider::StopOnSidePass(
    Frame *const frame, ReferenceLineInfo *const reference_line_info) {
  static bool check_clear;// 默认false
  static common::PathPoint change_lane_stop_path_point;
  // 获取path data
  const PathData &path_data = reference_line_info->path_data();
  double stop_s_on_pathdata = 0.0;
  // 找到"self"类型的路径，return
  if (path_data.path_label().find("self") != std::string::npos) {
    check_clear = false;
    change_lane_stop_path_point.Clear();
    return;
  }
  // CheckClearDone:Check if needed to check clear again for side pass
  // 如果check_clear为true，且CheckClearDone成功。设置check_clear为false
  if (check_clear &&
      CheckClearDone(*reference_line_info, change_lane_stop_path_point)) {
    check_clear = false;
  }
  // CheckSidePassStop:Check if necessary to set stop fence used for nonscenario side pass
  // 如果check_clear为false，CheckSidePassStop为true
  if (!check_clear &&
      CheckSidePassStop(path_data, *reference_line_info, &stop_s_on_pathdata)) {
    // 如果障碍物没有阻塞且可以换道，直接return
    if (!LaneChangeDecider::IsPerceptionBlocked(
            *reference_line_info,
            rule_based_stop_decider_config_.search_beam_length(),
            rule_based_stop_decider_config_.search_beam_radius_intensity(),
            rule_based_stop_decider_config_.search_range(),
            rule_based_stop_decider_config_.is_block_angle_threshold()) &&
        LaneChangeDecider::IsClearToChangeLane(reference_line_info)) {
      return;
    }
    // 检查adc是否停在了stop fence前，否返回true
    if (!CheckADCStop(path_data, *reference_line_info, stop_s_on_pathdata)) {
      // 设置stop fence，成功就执行 check_clear = true;
      if (!BuildSidePassStopFence(path_data, stop_s_on_pathdata,
                                  &change_lane_stop_path_point, frame,
                                  reference_line_info)) {
        AERROR << "Set side pass stop fail";
      }
    } else {
      if (LaneChangeDecider::IsClearToChangeLane(reference_line_info)) {
        check_clear = true;
      }
    }
  }
}

CheckClearDone

// Check if needed to check clear again for side pass
bool RuleBasedStopDecider::CheckClearDone(
    const ReferenceLineInfo &reference_line_info,
    const common::PathPoint &stop_point) {
  // 获取ADC的SL坐标
  const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
  const double adc_back_edge_s = reference_line_info.AdcSlBoundary().start_s();
  const double adc_start_l = reference_line_info.AdcSlBoundary().start_l();
  const double adc_end_l = reference_line_info.AdcSlBoundary().end_l();
  double lane_left_width = 0.0;
  double lane_right_width = 0.0;
  reference_line_info.reference_line().GetLaneWidth(
      (adc_front_edge_s + adc_back_edge_s) / 2.0, &lane_left_width,
      &lane_right_width);
  SLPoint stop_sl_point;
  // 获取停止点的SL坐标
  reference_line_info.reference_line().XYToSL(stop_point, &stop_sl_point);
  // use distance to last stop point to determine if needed to check clear
  // again
  if (adc_back_edge_s > stop_sl_point.s()) {
    if (adc_start_l > -lane_right_width || adc_end_l < lane_left_width) {
      return true;
    }
  }
  return false;
}

CheckSidePassStop

// @brief Check if necessary to set stop fence used for nonscenario side pass
bool RuleBasedStopDecider::CheckSidePassStop(
    const PathData &path_data, const ReferenceLineInfo &reference_line_info,
    double *stop_s_on_pathdata) {
  const std::vector<std::tuple<double, PathData::PathPointType, double>>
      &path_point_decision_guide = path_data.path_point_decision_guide();
  // 初始化类型
  PathData::PathPointType last_path_point_type =
      PathData::PathPointType::UNKNOWN;
  // 遍历 path_point_decision_guide
  for (const auto &point_guide : path_point_decision_guide) {
    // 若上一点在车道内，这一点在逆行车道上
    if (last_path_point_type == PathData::PathPointType::IN_LANE &&
        std::get<1>(point_guide) ==
            PathData::PathPointType::OUT_ON_REVERSE_LANE) {
      *stop_s_on_pathdata = std::get<0>(point_guide);
      // Approximate the stop fence s based on the vehicle position
      const auto &vehicle_config =
          common::VehicleConfigHelper::Instance()->GetConfig();
      const double ego_front_to_center =
          vehicle_config.vehicle_param().front_edge_to_center();
      common::PathPoint stop_pathpoint;
      // 获取stop point
      if (!path_data.GetPathPointWithRefS(*stop_s_on_pathdata,
                                          &stop_pathpoint)) {
        AERROR << "Can't get stop point on path data";
        return false;
      }
      const double ego_theta = stop_pathpoint.theta();
      Vec2d shift_vec{ego_front_to_center * std::cos(ego_theta),
                      ego_front_to_center * std::sin(ego_theta)};
      // stop_fence的位置
      const Vec2d stop_fence_pose =
          shift_vec + Vec2d(stop_pathpoint.x(), stop_pathpoint.y());
      double stop_l_on_pathdata = 0.0;
      const auto &nearby_path = reference_line_info.reference_line().map_path();
      nearby_path.GetNearestPoint(stop_fence_pose, stop_s_on_pathdata,
                                  &stop_l_on_pathdata);
      return true;
    }
    last_path_point_type = std::get<1>(point_guide);
  }
  return false;
}

IsPerceptionBlocked

参数解释：

search_beam_length 扫描长度
search_beam_radius_intensity 扫描间隔
search_range 依据ADC中心的扫描范围
is_block_angle_threshold 筛选障碍物所占角度大小的阈值

bool LaneChangeDecider::IsPerceptionBlocked(
    const ReferenceLineInfo& reference_line_info,
    const double search_beam_length, const double search_beam_radius_intensity,
    const double search_range, const double is_block_angle_threshold) {
  // search_beam_length: 20.0 //is the length of scanning beam
  // search_beam_radius_intensity: 0.08 //is the resolution of scanning
  // search_range: 3.14 	//is the scanning range centering at ADV heading
  // is_block_angle_threshold: 0.5 //is the threshold to tell how big a block angle range is perception blocking
  // 获取车辆状态、位置、航向角
  const auto& vehicle_state = reference_line_info.vehicle_state();
  const common::math::Vec2d adv_pos(vehicle_state.x(), vehicle_state.y());
  const double adv_heading = vehicle_state.heading();
  // 遍历障碍物
  for (auto* obstacle :
       reference_line_info.path_decision().obstacles().Items()) {
    // NormalizeAngle将给定的角度值规范化到一个特定的范围内（-π到π之间）
    double left_most_angle =
        common::math::NormalizeAngle(adv_heading + 0.5 * search_range);
    double right_most_angle =
        common::math::NormalizeAngle(adv_heading - 0.5 * search_range);
    bool right_most_found = false;
    // 跳过虚拟障碍物
    if (obstacle->IsVirtual()) {
      ADEBUG << "skip one virtual obstacle";
      continue;
    }
    // 获取障碍物多边形
    const auto& obstacle_polygon = obstacle->PerceptionPolygon();
    // 按角度进行搜索
    for (double search_angle = 0.0; search_angle < search_range;
         search_angle += search_beam_radius_intensity) {
      common::math::Vec2d search_beam_end(search_beam_length, 0.0);
      const double beam_heading = common::math::NormalizeAngle(
          adv_heading - 0.5 * search_range + search_angle);
      // search_beam_end绕adv_pos旋转beam_heading角度
      search_beam_end.SelfRotate(beam_heading);
      search_beam_end += adv_pos;
      // 构造线段
      common::math::LineSegment2d search_beam(adv_pos, search_beam_end);
      // 判断最右边界是否找到，并更新右边界角度
      if (!right_most_found && obstacle_polygon.HasOverlap(search_beam)) {
        right_most_found = true;
        right_most_angle = beam_heading;
      }
      // 如果最右边界已找到，且障碍物的感知多边形与搜索光束无重叠，则更新左边界角度并跳出循环。
      if (right_most_found && !obstacle_polygon.HasOverlap(search_beam)) {
        left_most_angle = beam_heading;
        break;
      }
    }
    // 如果最右边界未找到，则继续处理下一个障碍物。（说明该障碍物不在搜索范围内）
    if (!right_most_found) {
      // obstacle is not in search range
      continue;
    }
    // 判断阈值，过滤掉小的障碍物
    if (std::fabs(common::math::NormalizeAngle(
            left_most_angle - right_most_angle)) > is_block_angle_threshold) {
      return true;
    }
  }

  return false;
}

IsClearToChangeLane

这个在【Apollo学习笔记】——规划模块TASK之LANE_CHANGE_DECIDER已经有过介绍。

CheckSidePassStop

// @brief Check if necessary to set stop fence used for nonscenario side pass
bool RuleBasedStopDecider::CheckSidePassStop(
    const PathData &path_data, const ReferenceLineInfo &reference_line_info,
    double *stop_s_on_pathdata) {
  const std::vector<std::tuple<double, PathData::PathPointType, double>>
      &path_point_decision_guide = path_data.path_point_decision_guide();
  // 初始化类型
  PathData::PathPointType last_path_point_type =
      PathData::PathPointType::UNKNOWN;
  // 遍历 path_point_decision_guide
  for (const auto &point_guide : path_point_decision_guide) {
    // 若上一点在车道内，这一点在逆行车道上
    if (last_path_point_type == PathData::PathPointType::IN_LANE &&
        std::get<1>(point_guide) ==
            PathData::PathPointType::OUT_ON_REVERSE_LANE) {
      *stop_s_on_pathdata = std::get<0>(point_guide);
      // Approximate the stop fence s based on the vehicle position
      const auto &vehicle_config =
          common::VehicleConfigHelper::Instance()->GetConfig();
      const double ego_front_to_center =
          vehicle_config.vehicle_param().front_edge_to_center();
      common::PathPoint stop_pathpoint;
      // 获取stop point
      if (!path_data.GetPathPointWithRefS(*stop_s_on_pathdata,
                                          &stop_pathpoint)) {
        AERROR << "Can't get stop point on path data";
        return false;
      }
      const double ego_theta = stop_pathpoint.theta();
      Vec2d shift_vec{ego_front_to_center * std::cos(ego_theta),
                      ego_front_to_center * std::sin(ego_theta)};
      // stop_fence的位置
      const Vec2d stop_fence_pose =
          shift_vec + Vec2d(stop_pathpoint.x(), stop_pathpoint.y());
      double stop_l_on_pathdata = 0.0;
      const auto &nearby_path = reference_line_info.reference_line().map_path();
      nearby_path.GetNearestPoint(stop_fence_pose, stop_s_on_pathdata,
                                  &stop_l_on_pathdata);
      return true;
    }
    last_path_point_type = std::get<1>(point_guide);
  }
  return false;
}

BuildStopDecision

/*
 * @brief: build virtual obstacle of stop wall, and add STOP decision
 */
int BuildStopDecision(const std::string& stop_wall_id, const double stop_line_s,
                      const double stop_distance,
                      const StopReasonCode& stop_reason_code,
                      const std::vector<std::string>& wait_for_obstacles,
                      const std::string& decision_tag, Frame* const frame,
                      ReferenceLineInfo* const reference_line_info) {
  CHECK_NOTNULL(frame);
  CHECK_NOTNULL(reference_line_info);

  // 检查停止线是否在参考线上
  const auto& reference_line = reference_line_info->reference_line();
  if (!WithinBound(0.0, reference_line.Length(), stop_line_s)) {
    AERROR << "stop_line_s[" << stop_line_s << "] is not on reference line";
    return 0;
  }

  // create virtual stop wall
  const auto* obstacle =
      frame->CreateStopObstacle(reference_line_info, stop_wall_id, stop_line_s);
  if (!obstacle) {
    AERROR << "Failed to create obstacle [" << stop_wall_id << "]";
    return -1;
  }
  const Obstacle* stop_wall = reference_line_info->AddObstacle(obstacle);
  if (!stop_wall) {
    AERROR << "Failed to add obstacle[" << stop_wall_id << "]";
    return -1;
  }

  // build stop decision
  const double stop_s = stop_line_s - stop_distance;
  const auto& stop_point = reference_line.GetReferencePoint(stop_s);
  const double stop_heading =
      reference_line.GetReferencePoint(stop_s).heading();

  ObjectDecisionType stop;
  auto* stop_decision = stop.mutable_stop();
  stop_decision->set_reason_code(stop_reason_code);
  stop_decision->set_distance_s(-stop_distance);
  stop_decision->set_stop_heading(stop_heading);
  stop_decision->mutable_stop_point()->set_x(stop_point.x());
  stop_decision->mutable_stop_point()->set_y(stop_point.y());
  stop_decision->mutable_stop_point()->set_z(0.0);

  for (size_t i = 0; i < wait_for_obstacles.size(); ++i) {
    stop_decision->add_wait_for_obstacle(wait_for_obstacles[i]);
  }

  auto* path_decision = reference_line_info->path_decision();
  path_decision->AddLongitudinalDecision(decision_tag, stop_wall->Id(), stop);

  return 0;
}

ELSE:涉及到的一些其他函数

NormalizeAngle

NormalizeAngle将给定的角度值规范化到一个特定的范围内（-π到π之间）

double NormalizeAngle(const double angle) {
  double a = std::fmod(angle + M_PI, 2.0 * M_PI);
  if (a < 0.0) {
    a += (2.0 * M_PI);
  }
  return a - M_PI;
}

SelfRotate

将向量绕原点旋转$angle$角。

void Vec2d::SelfRotate(const double angle) {
  double tmp_x = x_;
  x_ = x_ * cos(angle) - y_ * sin(angle);
  y_ = tmp_x * sin(angle) + y_ * cos(angle);
}

CheckLaneChangeUrgency

检查紧急换道，当FLAGS_enable_lane_change_urgency_checking为true时，启用函数。

void RuleBasedStopDecider::CheckLaneChangeUrgency(Frame *const frame) {
  // 直接进入循环，检查每个reference_line_info
  for (auto &reference_line_info : *frame->mutable_reference_line_info()) {
    // Check if the target lane is blocked or not
    // 1. 检查目标道路是否阻塞，如果在change lane path上，就跳过
    if (reference_line_info.IsChangeLanePath()) {
      is_clear_to_change_lane_ =
          LaneChangeDecider::IsClearToChangeLane(&reference_line_info);
      is_change_lane_planning_succeed_ =
          reference_line_info.Cost() < kStraightForwardLineCost;
      continue;
    }
    // If it's not in lane-change scenario || (target lane is not blocked &&
    // change lane planning succeed), skip
    // 2.如果不是换道的场景，或者（目标lane没有阻塞）并且换道规划成功，跳过
    if (frame->reference_line_info().size() <= 1 ||
        (is_clear_to_change_lane_ && is_change_lane_planning_succeed_)) {
      continue;
    }
    // When the target lane is blocked in change-lane case, check the urgency
    // Get the end point of current routing
    const auto &route_end_waypoint =
        reference_line_info.Lanes().RouteEndWaypoint();
    // If can't get lane from the route's end waypoint, then skip
    // 3.在route的末端无法获得lane，跳过
    if (!route_end_waypoint.lane) {
      continue;
    }
    auto point = route_end_waypoint.lane->GetSmoothPoint(route_end_waypoint.s);
    auto *reference_line = reference_line_info.mutable_reference_line();
    common::SLPoint sl_point;
    // Project the end point to sl_point on current reference lane
    // 将当前参考线的点映射到frenet坐标系下
    if (reference_line->XYToSL(point, &sl_point) &&
        reference_line->IsOnLane(sl_point)) {
      // Check the distance from ADC to the end point of current routing
      double distance_to_passage_end =
          sl_point.s() - reference_line_info.AdcSlBoundary().end_s();
      // If ADC is still far from the end of routing, no need to stop, skip
      // 4. 如果adc距离routing终点较远，不需要停止，跳过
      if (distance_to_passage_end >
          rule_based_stop_decider_config_.approach_distance_for_lane_change()) {
        continue;
      }
      // In urgent case, set a temporary stop fence and wait to change lane
      // TODO(Jiaxuan Xu): replace the stop fence to more intelligent actions
      // 5.如果遇到紧急情况，设置临时的stop fence，等待换道
      const std::string stop_wall_id = "lane_change_stop";
      std::vector<std::string> wait_for_obstacles;
      util::BuildStopDecision(
          stop_wall_id, sl_point.s(),
          rule_based_stop_decider_config_.urgent_distance_for_lane_change(),
          StopReasonCode::STOP_REASON_LANE_CHANGE_URGENCY, wait_for_obstacles,
          "RuleBasedStopDecider", frame, &reference_line_info);
    }
  }
}

AddPathEndStop

void RuleBasedStopDecider::AddPathEndStop(
    Frame *const frame, ReferenceLineInfo *const reference_line_info) {
  // 路径不为空且起点到终点的距离不小于20m
  if (!reference_line_info->path_data().path_label().empty() &&
      reference_line_info->path_data().frenet_frame_path().back().s() -
              reference_line_info->path_data().frenet_frame_path().front().s() <
          FLAGS_short_path_length_threshold) { 
    // FLAGS_short_path_length_threshold: Threshold for too short path length(20m)
    
    // 创建虚拟墙的ID
    const std::string stop_wall_id =
        PATH_END_VO_ID_PREFIX + reference_line_info->path_data().path_label();
    std::vector<std::string> wait_for_obstacles;
    // 创建stop fence
    util::BuildStopDecision(
        stop_wall_id,
        reference_line_info->path_data().frenet_frame_path().back().s() - 5.0,
        0.0, StopReasonCode::STOP_REASON_REFERENCE_END, wait_for_obstacles,
        "RuleBasedStopDecider", frame, reference_line_info);
  }
}

参考

[1] 基于规则的停止决策