2024 百度Apollo星火自动驾驶大赛

省级选拔赛

赛题一 车辆靠边启动

赛题描述

主车在道路旁停车位启动时，如遇车辆前方存在障碍物，为保证行驶安全，主车需要与障碍车至少保持1.0米距离

评分标准

主车与障碍车距离小于1.0米，本场景扣40分

解题思路

通过扩大自车的包围盒（bounding box）尺寸，引导轨迹规划模块在计算路径时预留更多空间，从而使生成的路径更远离前方静止车辆。

前方存在障碍时，进一步扩大包围盒，可有效提升避障安全裕度。该方法通过影响 ROI 区域决策及路径采样，确保车辆在启动过程中满足最小安全距离要求。

解题过程

场景确认

仿真可视化界面显示车辆已进入 park_and_go 场景：

无需切换其他场景，可直接在当前模块进行参数调整。

修改 ADC Box 设置

修改路径：

modules/planning/tasks/open_space_roi_decider/open_space_roi_decider.cc

由于存在版本差异，建议直接替换该文件内容。以下为文件结构示意：

/******************************************************************************
 * Copyright 2019 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/**
 * @file
 **/

#include "modules/planning/tasks/open_space_roi_decider/open_space_roi_decider.h"

#include <limits>
#include <memory>
#include <utility>

#include "modules/common/math/polygon2d.h"
#include "modules/common/math/vec2d.h"
#include "modules/common/util/point_factory.h"
#include "modules/planning/planning_base/common/planning_context.h"
#include "modules/planning/planning_base/common/util/print_debug_info.h"
#include "modules/planning/planning_open_space/utils/open_space_roi_util.h"
namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::math::Box2d;
using apollo::common::math::Vec2d;
using apollo::hdmap::HDMapUtil;
using apollo::hdmap::LaneInfoConstPtr;
using apollo::hdmap::LaneSegment;
using apollo::hdmap::ParkingSpaceInfoConstPtr;
using apollo::hdmap::Path;

bool OpenSpaceRoiDecider::Init(
    const std::string &config_dir, const std::string &name,
    const std::shared_ptr<DependencyInjector> &injector) {
  if (!Decider::Init(config_dir, name, injector)) {
    return false;
  }
  hdmap_ = hdmap::HDMapUtil::BaseMapPtr();
  CHECK_NOTNULL(hdmap_);
  vehicle_params_ =
      apollo::common::VehicleConfigHelper::GetConfig().vehicle_param();
  // Load the config this task.
  bool res = Decider::LoadConfig<OpenSpaceRoiDeciderConfig>(&config_);
  AINFO << config_.DebugString();
  return res;
}

Status OpenSpaceRoiDecider::Process(Frame *frame) {
  if (frame == nullptr) {
    const std::string msg =
        "Invalid frame, fail to process the OpenSpaceRoiDecider.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  vehicle_state_ = frame->vehicle_state();
  obstacles_by_frame_ = frame->GetObstacleList();

  std::array<Vec2d, 4> spot_vertices;
  Path nearby_path;
  // @brief vector of different obstacle consisting of vertice points.The
  // obstacle and the vertices order are in counter-clockwise order
  std::vector<std::vector<common::math::Vec2d>> roi_boundary;

  const auto &roi_type = config_.roi_type();
  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)) {
      const std::string msg = "Fail to get parking boundary from map";
      AERROR << msg;
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }

    frame->mutable_open_space_info()->set_parking_type(
        parking_info.parking_type);

    SetOrigin(parking_info, frame);

    SetParkingSpotEndPose(parking_info, frame);

    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);
    }
  } else if (roi_type == OpenSpaceRoiDeciderConfig::PULL_OVER) {
    if (!GetPullOverSpot(frame, &spot_vertices, &nearby_path)) {
      const std::string msg = "Fail to get parking boundary from map";
      AERROR << msg;
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }

    SetOrigin(frame, spot_vertices);

    SetPullOverSpotEndPose(frame);

    if (!GetPullOverBoundary(frame, spot_vertices, nearby_path,
                             &roi_boundary)) {
      const std::string msg = "Fail to get parking boundary from map";
      AERROR << msg;
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }
  } else if (roi_type == OpenSpaceRoiDeciderConfig::PARK_AND_GO) {
    ADEBUG << "in Park_and_Go";
    nearby_path =
        frame->reference_line_info().front().reference_line().GetMapPath();

    ADEBUG << "nearby_path: " << nearby_path.DebugString();
    ADEBUG << "found nearby_path";
    if (!injector_->planning_context()
             ->planning_status()
             .park_and_go()
             .has_adc_init_position()) {
      const std::string msg = "ADC initial position is unavailable";
      AERROR << msg;
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }
    SetOriginFromADC(frame, nearby_path);
    ADEBUG << "SetOrigin";
    auto adc_point = common::util::PointFactory::ToPointENU(vehicle_state_);
    hdmap::LaneInfoConstPtr lane;
    double s = 0.0;
    double l = 0.0;
    if (!is_parking_out) {
      is_parking_out = HDMapUtil::BaseMap().GetNearestLaneWithHeading(
                           adc_point, 2.0, vehicle_state_.heading(), M_PI / 3.0,
                           &lane, &s, &l) == -1;
    }
    AINFO << "GetParkAndGoBoundary!!!";
    if (!GetParkingOutBoundary(nearby_path, frame, &roi_boundary)) {
      const std::string msg = "Fail to get park and go boundary from map";
      AERROR << msg;
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }

    SetParkAndGoEndPose(frame);
    ADEBUG << "SetEndPose";
  } else {
    const std::string msg =
        "chosen open space roi secenario type not implemented";
    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);
  }

  return Status::OK();
}

// get origin from ADC
void OpenSpaceRoiDecider::SetOriginFromADC(Frame *const frame,
                                           const hdmap::Path &nearby_path) {
  // get ADC box
  const auto &park_and_go_status =
      injector_->planning_context()->planning_status().park_and_go();

  const double adc_init_x = park_and_go_status.adc_init_position().x();
  const double adc_init_y = park_and_go_status.adc_init_position().y();
  const double adc_init_heading = park_and_go_status.adc_init_heading();
  common::math::Vec2d adc_init_position = {adc_init_x, adc_init_y};
  const double adc_length = vehicle_params_.length();
  const double adc_width = vehicle_params_.width();
  // ADC box
  Box2d adc_box(adc_init_position, adc_init_heading, adc_length + 5.0,
                adc_width + 5.0);
  // get vertices from ADC box
  std::vector<common::math::Vec2d> adc_corners;
  adc_box.GetAllCorners(&adc_corners);
  for (size_t i = 0; i < adc_corners.size(); ++i) {
    AINFO << "ADC [" << i << "]x: " << std::setprecision(9)
          << adc_corners[i].x();
    AINFO << "ADC [" << i << "]y: " << std::setprecision(9)
          << adc_corners[i].y();
  }
  auto left_top = adc_corners[1];

  ADEBUG << "left_top x: " << std::setprecision(9) << left_top.x();
  ADEBUG << "left_top y: " << std::setprecision(9) << left_top.y();

  // rotate the points to have the lane to be horizontal to x axis positive
  // direction and scale them base on the origin point
  // heading angle
  double heading;
  if (!nearby_path.GetHeadingAlongPath(left_top, &heading)) {
    AERROR << "fail to get heading on reference line";
    return;
  }

  frame->mutable_open_space_info()->set_origin_heading(
      common::math::NormalizeAngle(heading));
  ADEBUG << "heading: " << heading;
  frame->mutable_open_space_info()->mutable_origin_point()->set_x(left_top.x());
  frame->mutable_open_space_info()->mutable_origin_point()->set_y(left_top.y());
}

void OpenSpaceRoiDecider::SetOrigin(
    Frame *const frame, const std::array<common::math::Vec2d, 4> &vertices) {
  auto left_top = vertices[0];
  auto right_top = vertices[3];
  // rotate the points to have the lane to be horizontal to x axis positive
  // direction and scale them base on the origin point
  Vec2d heading_vec = right_top - left_top;
  frame->mutable_open_space_info()->set_origin_heading(heading_vec.Angle());
  frame->mutable_open_space_info()->mutable_origin_point()->set_x(left_top.x());
  frame->mutable_open_space_info()->mutable_origin_point()->set_y(left_top.y());
}

void OpenSpaceRoiDecider::SetOrigin(const ParkingInfo &parking_info,
                                    Frame *const frame) {
  auto left_top = parking_info.corner_points[0];
  auto right_top = parking_info.corner_points[1];
  // rotate the points to have the lane to be horizontal to x axis positive
  // direction and scale them base on the origin point
  Vec2d heading_vec = right_top - left_top;
  frame->mutable_open_space_info()->set_origin_heading(heading_vec.Angle());
  frame->mutable_open_space_info()->mutable_origin_point()->set_x(left_top.x());
  frame->mutable_open_space_info()->mutable_origin_point()->set_y(left_top.y());
}

void OpenSpaceRoiDecider::SetParkingSpotEndPose(const ParkingInfo &parking_info,
                                                Frame *const frame) {
  auto left_top = parking_info.corner_points[0];
  auto left_down = parking_info.corner_points[3];
  auto right_down = parking_info.corner_points[2];
  auto right_top = parking_info.corner_points[1];

  const auto &origin_point = frame->open_space_info().origin_point();
  const auto &origin_heading = frame->open_space_info().origin_heading();

  // End pose is set in normalized boundary
  left_top -= origin_point;
  left_top.SelfRotate(-origin_heading);
  left_down -= origin_point;
  left_down.SelfRotate(-origin_heading);
  right_top -= origin_point;
  right_top.SelfRotate(-origin_heading);
  right_down -= origin_point;
  right_down.SelfRotate(-origin_heading);
  Vec2d end_pt;
  double parking_heading = 0;
  // now only support parking space at right road side
  const double parking_depth_buffer = config_.parking_depth_buffer();
  if (parking_info.parking_type == ParkingType::VERTICAL_PARKING) {
    const bool parking_inwards = config_.parking_inwards();
    if (parking_inwards) {
      parking_heading = (left_down - left_top).Angle();
      Vec2d middle_top = (left_top + right_top) / 2.0;
      end_pt = middle_top + Vec2d::CreateUnitVec2d(parking_heading) *
                                (vehicle_params_.front_edge_to_center() +
                                 parking_depth_buffer);
    } else {
      parking_heading = (left_top - left_down).Angle();
      Vec2d middle_down = (left_down + right_down) / 2.0;
      end_pt = middle_down + Vec2d::CreateUnitVec2d(parking_heading) *
                                 (vehicle_params_.back_edge_to_center() +
                                  parking_depth_buffer);
    }
  } else {
    parking_heading = (right_top - left_top).Angle();
    Vec2d middle_left = (left_top + left_down) / 2.0;
    end_pt = middle_left +
             Vec2d::CreateUnitVec2d(parking_heading) *
                 (vehicle_params_.back_edge_to_center() + parking_depth_buffer);
  }
  auto *end_pose =
      frame->mutable_open_space_info()->mutable_open_space_end_pose();
  end_pose->push_back(end_pt.x());
  end_pose->push_back(end_pt.y());
  end_pose->push_back(parking_heading);
  end_pose->push_back(0.0);
}

void OpenSpaceRoiDecider::SetPullOverSpotEndPose(Frame *const frame) {
  const auto &pull_over_status =
      injector_->planning_context()->planning_status().pull_over();
  const double pull_over_x = pull_over_status.position().x();
  const double pull_over_y = pull_over_status.position().y();
  double pull_over_theta = pull_over_status.theta();

  // Normalize according to origin_point and origin_heading
  const auto &origin_point = frame->open_space_info().origin_point();
  const auto &origin_heading = frame->open_space_info().origin_heading();
  Vec2d center(pull_over_x, pull_over_y);
  center -= origin_point;
  center.SelfRotate(-origin_heading);
  pull_over_theta =
      common::math::NormalizeAngle(pull_over_theta - origin_heading);

  auto *end_pose =
      frame->mutable_open_space_info()->mutable_open_space_end_pose();
  end_pose->push_back(center.x());
  end_pose->push_back(center.y());
  end_pose->push_back(pull_over_theta);
  // end pose velocity set to be zero
  end_pose->push_back(0.0);
}

void OpenSpaceRoiDecider::SetParkAndGoEndPose(Frame *const frame) {
  const double kSTargetBuffer = config_.end_pose_s_distance();
  const double kSpeedRatio = 0.1;  // after adjust speed is 10% of speed limit
  // get vehicle current location
  // get vehicle s,l info
  auto park_and_go_status = injector_->planning_context()
                                ->mutable_planning_status()
                                ->mutable_park_and_go();

  const double adc_init_x = park_and_go_status->adc_init_position().x();
  const double adc_init_y = park_and_go_status->adc_init_position().y();

  ADEBUG << "ADC position (x): " << std::setprecision(9) << adc_init_x;
  ADEBUG << "ADC position (y): " << std::setprecision(9) << adc_init_y;

  const common::math::Vec2d adc_position = {adc_init_x, adc_init_y};
  common::SLPoint adc_position_sl;

  // get nearest reference line
  const auto &reference_line_list = frame->reference_line_info();
  ADEBUG << reference_line_list.size();
  const auto reference_line_info = std::min_element(
      reference_line_list.begin(), reference_line_list.end(),
      [&](const ReferenceLineInfo &ref_a, const ReferenceLineInfo &ref_b) {
        common::SLPoint adc_position_sl_a;
        common::SLPoint adc_position_sl_b;
        ref_a.reference_line().XYToSL(adc_position, &adc_position_sl_a);
        ref_b.reference_line().XYToSL(adc_position, &adc_position_sl_b);
        return std::fabs(adc_position_sl_a.l()) <
               std::fabs(adc_position_sl_b.l());
      });

  const auto &reference_line = reference_line_info->reference_line();
  reference_line.XYToSL(adc_position, &adc_position_sl);

  // target is at reference line
  const double target_s = adc_position_sl.s() + kSTargetBuffer;
  const auto reference_point = reference_line.GetReferencePoint(target_s);
  const double target_x = reference_point.x();
  const double target_y = reference_point.y();
  double target_theta = reference_point.heading();

  park_and_go_status->mutable_adc_adjust_end_pose()->set_x(target_x);
  park_and_go_status->mutable_adc_adjust_end_pose()->set_y(target_y);

  ADEBUG << "center.x(): " << std::setprecision(9) << target_x;
  ADEBUG << "center.y(): " << std::setprecision(9) << target_y;
  ADEBUG << "target_theta: " << std::setprecision(9) << target_theta;

  // Normalize according to origin_point and origin_heading
  const auto &origin_point = frame->open_space_info().origin_point();
  const auto &origin_heading = frame->open_space_info().origin_heading();
  Vec2d center(target_x, target_y);
  center -= origin_point;
  center.SelfRotate(-origin_heading);
  target_theta = common::math::NormalizeAngle(target_theta - origin_heading);

  auto *end_pose =
      frame->mutable_open_space_info()->mutable_open_space_end_pose();

  end_pose->push_back(center.x());
  end_pose->push_back(center.y());
  end_pose->push_back(target_theta);

  ADEBUG << "ADC position (x): " << std::setprecision(9) << (*end_pose)[0];
  ADEBUG << "ADC position (y): " << std::setprecision(9) << (*end_pose)[1];
  ADEBUG << "reference_line ID: " << reference_line_info->Lanes().Id();

  // end pose velocity set to be speed limit
  double target_speed = reference_line.GetSpeedLimitFromS(target_s);
  end_pose->push_back(kSpeedRatio * target_speed);
}

void OpenSpaceRoiDecider::GetRoadBoundary(
    const hdmap::Path &nearby_path, const double center_line_s,
    const common::math::Vec2d &origin_point, const double origin_heading,
    std::vector<Vec2d> *left_lane_boundary,
    std::vector<Vec2d> *right_lane_boundary,
    std::vector<Vec2d> *center_lane_boundary_left,
    std::vector<Vec2d> *center_lane_boundary_right,
    std::vector<double> *center_lane_s_left,
    std::vector<double> *center_lane_s_right,
    std::vector<double> *left_lane_road_width,
    std::vector<double> *right_lane_road_width) {
  double start_s = center_line_s - config_.roi_longitudinal_range_start();
  double end_s = center_line_s + config_.roi_longitudinal_range_end();

  hdmap::MapPathPoint start_point = nearby_path.GetSmoothPoint(start_s);
  double last_check_point_heading = start_point.heading();
  double index = 0.0;
  double check_point_s = start_s;

  // For the road boundary, add key points to left/right side boundary
  // separately. Iterate s_value to check key points at a step of
  // roi_line_segment_length. Key points include: start_point, end_point, points
  // where path curvature is large, points near left/right road-curb corners
  while (check_point_s <= end_s) {
    hdmap::MapPathPoint check_point = nearby_path.GetSmoothPoint(check_point_s);
    double check_point_heading = check_point.heading();
    bool is_center_lane_heading_change =
        std::abs(common::math::NormalizeAngle(check_point_heading -
                                              last_check_point_heading)) >
        config_.roi_line_segment_min_angle();
    last_check_point_heading = check_point_heading;

    ADEBUG << "is is_center_lane_heading_change: "
           << is_center_lane_heading_change;
    // Check if the current center-lane checking-point is start point || end
    // point || or point with larger curvature. If yes, mark it as an anchor
    // point.
    bool is_anchor_point = check_point_s == start_s || check_point_s == end_s ||
                           is_center_lane_heading_change;
    // Add key points to the left-half boundary
    AddBoundaryKeyPoint(nearby_path, check_point_s, start_s, end_s,
                        is_anchor_point, true, center_lane_boundary_left,
                        left_lane_boundary, center_lane_s_left,
                        left_lane_road_width);
    // Add key points to the right-half boundary
    AddBoundaryKeyPoint(nearby_path, check_point_s, start_s, end_s,
                        is_anchor_point, false, center_lane_boundary_right,
                        right_lane_boundary, center_lane_s_right,
                        right_lane_road_width);
    if (check_point_s == end_s) {
      break;
    }
    index += 1.0;
    check_point_s = start_s + index * config_.roi_line_segment_length();
    check_point_s = check_point_s >= end_s ? end_s : check_point_s;
  }

  size_t left_point_size = left_lane_boundary->size();
  size_t right_point_size = right_lane_boundary->size();
  for (size_t i = 0; i < left_point_size; i++) {
    left_lane_boundary->at(i) -= origin_point;
    left_lane_boundary->at(i).SelfRotate(-origin_heading);
  }
  for (size_t i = 0; i < right_point_size; i++) {
    right_lane_boundary->at(i) -= origin_point;
    right_lane_boundary->at(i).SelfRotate(-origin_heading);
  }
}

void OpenSpaceRoiDecider::GetRoadBoundaryFromMap(
    const hdmap::Path &nearby_path, const double center_line_s,
    const Vec2d &origin_point, const double origin_heading,
    std::vector<Vec2d> *left_lane_boundary,
    std::vector<Vec2d> *right_lane_boundary,
    std::vector<Vec2d> *center_lane_boundary_left,
    std::vector<Vec2d> *center_lane_boundary_right,
    std::vector<double> *center_lane_s_left,
    std::vector<double> *center_lane_s_right,
    std::vector<double> *left_lane_road_width,
    std::vector<double> *right_lane_road_width) {
  // Longitudinal range can be asymmetric.
  double start_s = center_line_s - config_.roi_longitudinal_range_start();
  double end_s = center_line_s + config_.roi_longitudinal_range_end();
  hdmap::MapPathPoint start_point = nearby_path.GetSmoothPoint(start_s);

  double check_point_s = start_s;

  while (check_point_s <= end_s) {
    hdmap::MapPathPoint check_point = nearby_path.GetSmoothPoint(check_point_s);

    // get road boundaries
    double left_road_width = nearby_path.GetRoadLeftWidth(check_point_s);
    double right_road_width = nearby_path.GetRoadRightWidth(check_point_s);

    double current_road_width = std::max(left_road_width, right_road_width);

    // get road boundaries at current location
    common::PointENU check_point_xy;
    std::vector<hdmap::RoadRoiPtr> road_boundaries;
    std::vector<hdmap::JunctionInfoConstPtr> junctions;
    check_point_xy.set_x(check_point.x());
    check_point_xy.set_y(check_point.y());
    hdmap_->GetRoadBoundaries(check_point_xy, current_road_width,
                              &road_boundaries, &junctions);

    if (check_point_s < center_line_s) {
      for (size_t i = 0;
           i < (*road_boundaries.at(0)).left_boundary.line_points.size(); i++) {
        right_lane_boundary->emplace_back(
            Vec2d((*road_boundaries.at(0)).left_boundary.line_points[i].x(),
                  (*road_boundaries.at(0)).left_boundary.line_points[i].y()));
      }
      for (size_t i = 0;
           i < (*road_boundaries.at(0)).right_boundary.line_points.size();
           i++) {
        left_lane_boundary->emplace_back(
            Vec2d((*road_boundaries.at(0)).right_boundary.line_points[i].x(),
                  (*road_boundaries.at(0)).right_boundary.line_points[i].y()));
      }
    } else {
      for (size_t i = 0;
           i < (*road_boundaries.at(0)).left_boundary.line_points.size(); i++) {
        left_lane_boundary->emplace_back(
            Vec2d((*road_boundaries.at(0)).left_boundary.line_points[i].x(),
                  (*road_boundaries.at(0)).left_boundary.line_points[i].y()));
      }
      for (size_t i = 0;
           i < (*road_boundaries.at(0)).right_boundary.line_points.size();
           i++) {
        right_lane_boundary->emplace_back(
            Vec2d((*road_boundaries.at(0)).right_boundary.line_points[i].x(),
                  (*road_boundaries.at(0)).right_boundary.line_points[i].y()));
      }
    }

    center_lane_boundary_right->emplace_back(check_point);
    center_lane_boundary_left->emplace_back(check_point);
    center_lane_s_left->emplace_back(check_point_s);
    center_lane_s_right->emplace_back(check_point_s);
    left_lane_road_width->emplace_back(left_road_width);
    right_lane_road_width->emplace_back(right_road_width);

    check_point_s = check_point_s + config_.roi_line_segment_length_from_map();
  }

  size_t left_point_size = left_lane_boundary->size();
  size_t right_point_size = right_lane_boundary->size();
  ADEBUG << "right_road_boundary size: " << right_lane_boundary->size();
  ADEBUG << "left_road_boundary size: " << left_lane_boundary->size();
  for (size_t i = 0; i < left_point_size; i++) {
    left_lane_boundary->at(i) -= origin_point;
    left_lane_boundary->at(i).SelfRotate(-origin_heading);
    ADEBUG << "left_road_boundary: [" << std::setprecision(9)
           << left_lane_boundary->at(i).x() << ", "
           << left_lane_boundary->at(i).y() << "]";
  }
  for (size_t i = 0; i < right_point_size; i++) {
    right_lane_boundary->at(i) -= origin_point;
    right_lane_boundary->at(i).SelfRotate(-origin_heading);
    ADEBUG << "right_road_boundary: [" << std::setprecision(9)
           << right_lane_boundary->at(i).x() << ", "
           << right_lane_boundary->at(i).y() << "]";
  }
  if (!left_lane_boundary->empty()) {
    sort(left_lane_boundary->begin(), left_lane_boundary->end(),
         [](const Vec2d &first_pt, const Vec2d &second_pt) {
           return first_pt.x() < second_pt.x() ||
                  (first_pt.x() == second_pt.x() &&
                   first_pt.y() < second_pt.y());
         });
    auto unique_end =
        std::unique(left_lane_boundary->begin(), left_lane_boundary->end());
    left_lane_boundary->erase(unique_end, left_lane_boundary->end());
  }
  if (!right_lane_boundary->empty()) {
    sort(right_lane_boundary->begin(), right_lane_boundary->end(),
         [](const Vec2d &first_pt, const Vec2d &second_pt) {
           return first_pt.x() < second_pt.x() ||
                  (first_pt.x() == second_pt.x() &&
                   first_pt.y() < second_pt.y());
         });
    auto unique_end =
        std::unique(right_lane_boundary->begin(), right_lane_boundary->end());
    right_lane_boundary->erase(unique_end, right_lane_boundary->end());
  }
}

void OpenSpaceRoiDecider::AddBoundaryKeyPoint(
    const hdmap::Path &nearby_path, const double check_point_s,
    const double start_s, const double end_s, const bool is_anchor_point,
    const bool is_left_curb, std::vector<Vec2d> *center_lane_boundary,
    std::vector<Vec2d> *curb_lane_boundary, std::vector<double> *center_lane_s,
    std::vector<double> *road_width) {
  // Check if current central-lane checking point's mapping on the left/right
  // road boundary is a key point. The road boundary point is a key point if
  // one of the following two confitions is satisfied:
  // 1. the current central-lane point is an anchor point: (a start/end point
  // or the point on path with large curvatures)
  // 2. the point on the left/right lane boundary is close to a curb corner
  // As indicated below:
  // (#) Key Point Type 1: Lane anchor points
  // (*) Key Point Type 2: Curb-corner points
  //                                                         #
  // Path Direction -->                                     /    /   #
  // Left Lane Boundary   #--------------------------------#    /   /
  //                                                           /   /
  // Center Lane          - - - - - - - - - - - - - - - - - - /   /
  //                                                             /
  // Right Lane Boundary  #--------*                 *----------#
  //                                \               /
  //                                 *-------------*

  // road width changes slightly at the turning point of a path
  // TODO(SHU): 1. consider distortion introduced by curvy road; 2. use both
  // round boundaries for single-track road; 3. longitudinal range may not be
  // symmetric
  const double previous_distance_s =
      std::min(config_.roi_line_segment_length(), check_point_s - start_s);
  const double next_distance_s =
      std::min(config_.roi_line_segment_length(), end_s - check_point_s);

  hdmap::MapPathPoint current_check_point =
      nearby_path.GetSmoothPoint(check_point_s);

  double current_check_point_heading = current_check_point.heading();
  double current_road_width =
      is_left_curb ? nearby_path.GetRoadLeftWidth(check_point_s)
                   : nearby_path.GetRoadRightWidth(check_point_s);
  // If the current center-lane checking point is an anchor point, then add
  // current left/right curb boundary point as a key point
  if (is_anchor_point) {
    double point_vec_cos =
        is_left_curb ? std::cos(current_check_point_heading + M_PI / 2.0)
                     : std::cos(current_check_point_heading - M_PI / 2.0);
    double point_vec_sin =
        is_left_curb ? std::sin(current_check_point_heading + M_PI / 2.0)
                     : std::sin(current_check_point_heading - M_PI / 2.0);
    Vec2d curb_lane_point = Vec2d(current_road_width * point_vec_cos,
                                  current_road_width * point_vec_sin);
    curb_lane_point = curb_lane_point + current_check_point;
    center_lane_boundary->push_back(current_check_point);
    curb_lane_boundary->push_back(curb_lane_point);
    center_lane_s->push_back(check_point_s);
    road_width->push_back(current_road_width);
    return;
  }
  double previous_road_width =
      is_left_curb
          ? nearby_path.GetRoadLeftWidth(check_point_s - previous_distance_s)
          : nearby_path.GetRoadRightWidth(check_point_s - previous_distance_s);
  double next_road_width =
      is_left_curb
          ? nearby_path.GetRoadLeftWidth(check_point_s + next_distance_s)
          : nearby_path.GetRoadRightWidth(check_point_s + next_distance_s);
  double previous_curb_segment_angle =
      (current_road_width - previous_road_width) / previous_distance_s;
  double next_segment_angle =
      (next_road_width - current_road_width) / next_distance_s;
  double current_curb_point_delta_theta =
      next_segment_angle - previous_curb_segment_angle;
  // If the delta angle between the previous curb segment and the next curb
  // segment is large (near a curb corner), then add current curb_lane_point
  // as a key point.
  if (std::abs(current_curb_point_delta_theta) >
      config_.curb_heading_tangent_change_upper_limit()) {
    double point_vec_cos =
        is_left_curb ? std::cos(current_check_point_heading + M_PI / 2.0)
                     : std::cos(current_check_point_heading - M_PI / 2.0);
    double point_vec_sin =
        is_left_curb ? std::sin(current_check_point_heading + M_PI / 2.0)
                     : std::sin(current_check_point_heading - M_PI / 2.0);
    Vec2d curb_lane_point = Vec2d(current_road_width * point_vec_cos,
                                  current_road_width * point_vec_sin);
    curb_lane_point = curb_lane_point + current_check_point;
    center_lane_boundary->push_back(current_check_point);
    curb_lane_boundary->push_back(curb_lane_point);
    center_lane_s->push_back(check_point_s);
    road_width->push_back(current_road_width);
  }
}

bool OpenSpaceRoiDecider::GetParkingBoundary(
    const ParkingInfo &parking_info, const hdmap::Path &nearby_path,
    Frame *const frame,
    std::vector<std::vector<common::math::Vec2d>> *const roi_parking_boundary) {
  auto left_top = parking_info.corner_points[0];
  ADEBUG << std::fixed << "left_top: " << left_top.x() << ", " << left_top.y();
  auto left_down = parking_info.corner_points[3];
  ADEBUG << std::fixed << "left_down: " << left_down.x() << ", "
         << left_down.y();
  auto right_down = parking_info.corner_points[2];
  ADEBUG << std::fixed << "right_down: " << right_down.x() << ", "
         << right_down.y();
  auto right_top = parking_info.corner_points[1];
  ADEBUG << std::fixed << "right_top: " << right_top.x() << ", "
         << right_top.y();

  const auto &origin_point = frame->open_space_info().origin_point();
  ADEBUG << std::fixed << "origin_point: " << origin_point.x() << ", "
         << origin_point.y();
  const auto &origin_heading = frame->open_space_info().origin_heading();

  double left_top_s = 0.0;
  double left_top_l = 0.0;
  double right_top_s = 0.0;
  double right_top_l = 0.0;
  if (!(nearby_path.GetProjection(left_top, &left_top_s, &left_top_l) &&
        nearby_path.GetProjection(right_top, &right_top_s, &right_top_l))) {
    AERROR << "fail to get parking spot points' projections on reference line";
    return false;
  }

  left_top -= origin_point;
  left_top.SelfRotate(-origin_heading);
  left_down -= origin_point;
  left_down.SelfRotate(-origin_heading);
  right_top -= origin_point;
  right_top.SelfRotate(-origin_heading);
  right_down -= origin_point;
  right_down.SelfRotate(-origin_heading);

  const double center_line_s = (left_top_s + right_top_s) / 2.0;
  std::vector<Vec2d> left_lane_boundary;
  std::vector<Vec2d> right_lane_boundary;
  // The pivot points on the central lane, mapping with the key points on
  // the left lane boundary.
  std::vector<Vec2d> center_lane_boundary_left;
  // The pivot points on the central lane, mapping with the key points on
  // the right lane boundary.
  std::vector<Vec2d> center_lane_boundary_right;
  // The s-value for the anchor points on the center_lane_boundary_left.
  std::vector<double> center_lane_s_left;
  // The s-value for the anchor points on the center_lane_boundary_right.
  std::vector<double> center_lane_s_right;
  // The left-half road width between the pivot points on the
  // center_lane_boundary_left and key points on the
  // left_lane_boundary.
  std::vector<double> left_lane_road_width;
  // The right-half road width between the pivot points on the
  // center_lane_boundary_right and key points on the
  // right_lane_boundary.
  std::vector<double> right_lane_road_width;

  GetRoadBoundary(nearby_path, center_line_s, origin_point, origin_heading,
                  &left_lane_boundary, &right_lane_boundary,
                  &center_lane_boundary_left, &center_lane_boundary_right,
                  &center_lane_s_left, &center_lane_s_right,
                  &left_lane_road_width, &right_lane_road_width);

  // If smaller than zero, the parking spot is on the right of the lane
  // Left, right, down or opposite of the boundary is decided when viewing the
  // parking spot upward
  const double average_l = (left_top_l + right_top_l) / 2.0;
  std::vector<Vec2d> boundary_points;

  // TODO(jiaxuan): Write a half-boundary formation function and call it twice
  // to avoid duplicated manipulations on the left and right sides
  if (average_l < 0) {
    // if average_l is lower than zero, the parking spot is on the right
    // lane boundary and assume that the lane half width is average_l
    ADEBUG << "average_l is less than 0 in OpenSpaceROI";
    size_t point_size = right_lane_boundary.size();
    for (size_t i = 0; i < point_size; i++) {
      right_lane_boundary[i].SelfRotate(origin_heading);
      right_lane_boundary[i] += origin_point;
      right_lane_boundary[i] -= center_lane_boundary_right[i];
      right_lane_boundary[i] /= right_lane_road_width[i];
      right_lane_boundary[i] *= (-average_l);
      right_lane_boundary[i] += center_lane_boundary_right[i];
      right_lane_boundary[i] -= origin_point;
      right_lane_boundary[i].SelfRotate(-origin_heading);
    }

    auto point_left_to_left_top_connor_s = std::lower_bound(
        center_lane_s_right.begin(), center_lane_s_right.end(), left_top_s);
    size_t point_left_to_left_top_connor_index = std::distance(
        center_lane_s_right.begin(), point_left_to_left_top_connor_s);
    point_left_to_left_top_connor_index =
        point_left_to_left_top_connor_index == 0
            ? point_left_to_left_top_connor_index
            : point_left_to_left_top_connor_index - 1;
    auto point_left_to_left_top_connor_itr =
        right_lane_boundary.begin() + point_left_to_left_top_connor_index;
    auto point_right_to_right_top_connor_s = std::upper_bound(
        center_lane_s_right.begin(), center_lane_s_right.end(), right_top_s);
    size_t point_right_to_right_top_connor_index = std::distance(
        center_lane_s_right.begin(), point_right_to_right_top_connor_s);
    auto point_right_to_right_top_connor_itr =
        right_lane_boundary.begin() + point_right_to_right_top_connor_index;

    std::copy(right_lane_boundary.begin(), point_left_to_left_top_connor_itr,
              std::back_inserter(boundary_points));

    std::vector<Vec2d> parking_spot_boundary{left_top, left_down, right_down,
                                             right_top};

    std::copy(parking_spot_boundary.begin(), parking_spot_boundary.end(),
              std::back_inserter(boundary_points));

    std::copy(point_right_to_right_top_connor_itr, right_lane_boundary.end(),
              std::back_inserter(boundary_points));

    std::reverse_copy(left_lane_boundary.begin(), left_lane_boundary.end(),
                      std::back_inserter(boundary_points));

    // reinsert the initial point to the back to from closed loop
    boundary_points.push_back(right_lane_boundary.front());

    // disassemble line into line2d segments
    for (size_t i = 0; i < point_left_to_left_top_connor_index; i++) {
      std::vector<Vec2d> segment{right_lane_boundary[i],
                                 right_lane_boundary[i + 1]};
      roi_parking_boundary->push_back(segment);
    }

    std::vector<Vec2d> left_stitching_segment{
        right_lane_boundary[point_left_to_left_top_connor_index], left_top};
    roi_parking_boundary->push_back(left_stitching_segment);

    std::vector<Vec2d> left_parking_spot_segment{left_top, left_down};
    std::vector<Vec2d> down_parking_spot_segment{left_down, right_down};
    std::vector<Vec2d> right_parking_spot_segment{right_down, right_top};
    roi_parking_boundary->push_back(left_parking_spot_segment);
    roi_parking_boundary->push_back(down_parking_spot_segment);
    roi_parking_boundary->push_back(right_parking_spot_segment);

    std::vector<Vec2d> right_stitching_segment{
        right_top, right_lane_boundary[point_right_to_right_top_connor_index]};
    roi_parking_boundary->push_back(right_stitching_segment);

    size_t right_lane_boundary_last_index = right_lane_boundary.size() - 1;
    for (size_t i = point_right_to_right_top_connor_index;
         i < right_lane_boundary_last_index; i++) {
      std::vector<Vec2d> segment{right_lane_boundary[i],
                                 right_lane_boundary[i + 1]};
      roi_parking_boundary->push_back(segment);
    }

    size_t left_lane_boundary_last_index = left_lane_boundary.size() - 1;
    for (size_t i = left_lane_boundary_last_index; i > 0; i--) {
      std::vector<Vec2d> segment{left_lane_boundary[i],
                                 left_lane_boundary[i - 1]};
      roi_parking_boundary->push_back(segment);
    }

  } else {
    // if average_l is higher than zero, the parking spot is on the left
    // lane boundary and assume that the lane half width is average_l
    ADEBUG << "average_l is greater than 0 in OpenSpaceROI";
    size_t point_size = left_lane_boundary.size();
    for (size_t i = 0; i < point_size; i++) {
      left_lane_boundary[i].SelfRotate(origin_heading);
      left_lane_boundary[i] += origin_point;
      left_lane_boundary[i] -= center_lane_boundary_left[i];
      left_lane_boundary[i] /= left_lane_road_width[i];
      left_lane_boundary[i] *= average_l;
      left_lane_boundary[i] += center_lane_boundary_left[i];
      left_lane_boundary[i] -= origin_point;
      left_lane_boundary[i].SelfRotate(-origin_heading);
      ADEBUG << "left_lane_boundary[" << i << "]: " << left_lane_boundary[i].x()
             << ", " << left_lane_boundary[i].y();
    }

    auto point_right_to_right_top_connor_s = std::lower_bound(
        center_lane_s_left.begin(), center_lane_s_left.end(), right_top_s);
    size_t point_right_to_right_top_connor_index = std::distance(
        center_lane_s_left.begin(), point_right_to_right_top_connor_s);

    auto point_right_to_right_top_connor_itr =
        left_lane_boundary.begin() + point_right_to_right_top_connor_index;

    auto point_left_to_left_top_connor_s = std::upper_bound(
        center_lane_s_left.begin(), center_lane_s_left.end(), left_top_s);
    size_t point_left_to_left_top_connor_index = std::distance(
        center_lane_s_left.begin(), point_left_to_left_top_connor_s);
    point_left_to_left_top_connor_index =
        point_left_to_left_top_connor_index == 0
            ? point_left_to_left_top_connor_index
            : point_left_to_left_top_connor_index - 1;
    auto point_left_to_left_top_connor_itr =
        left_lane_boundary.begin() + point_left_to_left_top_connor_index;

    std::copy(right_lane_boundary.begin(), right_lane_boundary.end(),
              std::back_inserter(boundary_points));

    std::reverse_copy(point_left_to_left_top_connor_itr,
                      left_lane_boundary.end(),
                      std::back_inserter(boundary_points));

    std::vector<Vec2d> parking_spot_boundary{left_top, left_down, right_down,
                                             right_top};
    std::copy(parking_spot_boundary.begin(), parking_spot_boundary.end(),
              std::back_inserter(boundary_points));

    std::reverse_copy(left_lane_boundary.begin(),
                      point_right_to_right_top_connor_itr,
                      std::back_inserter(boundary_points));

    // reinsert the initial point to the back to from closed loop
    boundary_points.push_back(right_lane_boundary.front());

    // disassemble line into line2d segments
    size_t right_lane_boundary_last_index = right_lane_boundary.size() - 1;
    for (size_t i = 0; i < right_lane_boundary_last_index; i++) {
      std::vector<Vec2d> segment{right_lane_boundary[i],
                                 right_lane_boundary[i + 1]};
      roi_parking_boundary->push_back(segment);
    }

    size_t left_lane_boundary_last_index = left_lane_boundary.size() - 1;
    for (size_t i = left_lane_boundary_last_index;
         i > point_right_to_right_top_connor_index; i--) {
      std::vector<Vec2d> segment{left_lane_boundary[i],
                                 left_lane_boundary[i - 1]};
      roi_parking_boundary->push_back(segment);
    }

    std::vector<Vec2d> left_stitching_segment{
        left_lane_boundary[point_right_to_right_top_connor_index], right_top};
    roi_parking_boundary->push_back(left_stitching_segment);

    std::vector<Vec2d> right_parking_spot_segment{right_top, right_down};
    std::vector<Vec2d> down_parking_spot_segment{right_down, left_down};
    std::vector<Vec2d> left_parking_spot_segment{left_down, left_top};
    roi_parking_boundary->push_back(right_parking_spot_segment);
    roi_parking_boundary->push_back(down_parking_spot_segment);
    roi_parking_boundary->push_back(left_parking_spot_segment);

    std::vector<Vec2d> right_stitching_segment{
        left_top, left_lane_boundary[point_left_to_left_top_connor_index]};
    roi_parking_boundary->push_back(right_stitching_segment);

    for (size_t i = point_left_to_left_top_connor_index; i > 0; --i) {
      std::vector<Vec2d> segment{left_lane_boundary[i],
                                 left_lane_boundary[i - 1]};
      roi_parking_boundary->push_back(segment);
    }
  }

  // Fuse line segments into convex contraints
  if (!FuseLineSegments(roi_parking_boundary)) {
    AERROR << "FuseLineSegments failed in parking ROI";
    return false;
  }
  // Get xy boundary
  auto xminmax = std::minmax_element(
      boundary_points.begin(), boundary_points.end(),
      [](const Vec2d &a, const Vec2d &b) { return a.x() < b.x(); });
  auto yminmax = std::minmax_element(
      boundary_points.begin(), boundary_points.end(),
      [](const Vec2d &a, const Vec2d &b) { return a.y() < b.y(); });
  std::vector<double> ROI_xy_boundary{xminmax.first->x(), xminmax.second->x(),
                                      yminmax.first->y(), yminmax.second->y()};
  auto *xy_boundary =
      frame->mutable_open_space_info()->mutable_ROI_xy_boundary();
  xy_boundary->assign(ROI_xy_boundary.begin(), ROI_xy_boundary.end());

  Vec2d vehicle_xy = Vec2d(vehicle_state_.x(), vehicle_state_.y());
  vehicle_xy -= origin_point;
  vehicle_xy.SelfRotate(-origin_heading);
  if (vehicle_xy.x() < ROI_xy_boundary[0] ||
      vehicle_xy.x() > ROI_xy_boundary[1] ||
      vehicle_xy.y() < ROI_xy_boundary[2] ||
      vehicle_xy.y() > ROI_xy_boundary[3]) {
    AERROR << "vehicle outside of xy boundary of parking ROI";
    return false;
  }
  return true;
}

bool OpenSpaceRoiDecider::GetPullOverBoundary(
    Frame *const frame, const std::array<common::math::Vec2d, 4> &vertices,
    const hdmap::Path &nearby_path,
    std::vector<std::vector<common::math::Vec2d>> *const roi_parking_boundary) {
  auto left_top = vertices[0];
  auto left_down = vertices[1];
  auto right_down = vertices[2];
  auto right_top = vertices[3];

  const auto &origin_point = frame->open_space_info().origin_point();
  const auto &origin_heading = frame->open_space_info().origin_heading();

  double left_top_s = 0.0;
  double left_top_l = 0.0;
  double right_top_s = 0.0;
  double right_top_l = 0.0;
  if (!(nearby_path.GetProjection(left_top, &left_top_s, &left_top_l) &&
        nearby_path.GetProjection(right_top, &right_top_s, &right_top_l))) {
    AERROR << "fail to get parking spot points' projections on reference line";
    return false;
  }

  left_top -= origin_point;
  left_top.SelfRotate(-origin_heading);
  left_down -= origin_point;
  left_down.SelfRotate(-origin_heading);
  right_top -= origin_point;
  right_top.SelfRotate(-origin_heading);
  right_down -= origin_point;
  right_down.SelfRotate(-origin_heading);

  const double center_line_s = (left_top_s + right_top_s) / 2.0;
  std::vector<Vec2d> left_lane_boundary;
  std::vector<Vec2d> right_lane_boundary;
  std::vector<Vec2d> center_lane_boundary_left;
  std::vector<Vec2d> center_lane_boundary_right;
  std::vector<double> center_lane_s_left;
  std::vector<double> center_lane_s_right;
  std::vector<double> left_lane_road_width;
  std::vector<double> right_lane_road_width;

  GetRoadBoundary(nearby_path, center_line_s, origin_point, origin_heading,
                  &left_lane_boundary, &right_lane_boundary,
                  &center_lane_boundary_left, &center_lane_boundary_right,
                  &center_lane_s_left, &center_lane_s_right,
                  &left_lane_road_width, &right_lane_road_width);

  // Load boundary as line segments in counter-clockwise order
  std::reverse(left_lane_boundary.begin(), left_lane_boundary.end());

  std::vector<Vec2d> boundary_points;
  std::copy(right_lane_boundary.begin(), right_lane_boundary.end(),
            std::back_inserter(boundary_points));
  std::copy(left_lane_boundary.begin(), left_lane_boundary.end(),
            std::back_inserter(boundary_points));

  size_t right_lane_boundary_last_index = right_lane_boundary.size() - 1;
  for (size_t i = 0; i < right_lane_boundary_last_index; i++) {
    std::vector<Vec2d> segment{right_lane_boundary[i],
                               right_lane_boundary[i + 1]};
    roi_parking_boundary->push_back(segment);
  }

  size_t left_lane_boundary_last_index = left_lane_boundary.size() - 1;
  for (size_t i = left_lane_boundary_last_index; i > 0; i--) {
    std::vector<Vec2d> segment{left_lane_boundary[i],
                               left_lane_boundary[i - 1]};
    roi_parking_boundary->push_back(segment);
  }

  // Fuse line segments into convex contraints
  if (!FuseLineSegments(roi_parking_boundary)) {
    return false;
  }
  // Get xy boundary
  auto xminmax = std::minmax_element(
      boundary_points.begin(), boundary_points.end(),
      [](const Vec2d &a, const Vec2d &b) { return a.x() < b.x(); });
  auto yminmax = std::minmax_element(
      boundary_points.begin(), boundary_points.end(),
      [](const Vec2d &a, const Vec2d &b) { return a.y() < b.y(); });
  std::vector<double> ROI_xy_boundary{xminmax.first->x(), xminmax.second->x(),
                                      yminmax.first->y(), yminmax.second->y()};
  auto *xy_boundary =
      frame->mutable_open_space_info()->mutable_ROI_xy_boundary();
  xy_boundary->assign(ROI_xy_boundary.begin(), ROI_xy_boundary.end());

  Vec2d vehicle_xy = Vec2d(vehicle_state_.x(), vehicle_state_.y());
  vehicle_xy -= origin_point;
  vehicle_xy.SelfRotate(-origin_heading);
  if (vehicle_xy.x() < ROI_xy_boundary[0] ||
      vehicle_xy.x() > ROI_xy_boundary[1] ||
      vehicle_xy.y() < ROI_xy_boundary[2] ||
      vehicle_xy.y() > ROI_xy_boundary[3]) {
    AERROR << "vehicle outside of xy boundary of parking ROI";
    return false;
  }
  return true;
}

bool OpenSpaceRoiDecider::GetParkAndGoBoundary(
    Frame *const frame, const hdmap::Path &nearby_path,
    std::vector<std::vector<common::math::Vec2d>> *const roi_parking_boundary) {
  const auto &park_and_go_status =
      injector_->planning_context()->planning_status().park_and_go();
  const double adc_init_x = park_and_go_status.adc_init_position().x();
  const double adc_init_y = park_and_go_status.adc_init_position().y();
  const double adc_init_heading = park_and_go_status.adc_init_heading();
  common::math::Vec2d adc_init_position = {adc_init_x, adc_init_y};
  const double adc_length = vehicle_params_.length();
  const double adc_width = vehicle_params_.width();
  // ADC box
  Box2d adc_box(adc_init_position, adc_init_heading, adc_length, adc_width);
  // get vertices from ADC box
  std::vector<common::math::Vec2d> adc_corners;
  adc_box.GetAllCorners(&adc_corners);
  auto left_top = adc_corners[1];
  auto right_top = adc_corners[0];

  const auto &origin_point = frame->open_space_info().origin_point();
  const auto &origin_heading = frame->open_space_info().origin_heading();

  double left_top_s = 0.0;
  double left_top_l = 0.0;
  double right_top_s = 0.0;
  double right_top_l = 0.0;
  if (!(nearby_path.GetProjection(left_top, &left_top_s, &left_top_l) &&
        nearby_path.GetProjection(right_top, &right_top_s, &right_top_l))) {
    AERROR << "fail to get parking spot points' projections on reference line";
    return false;
  }
  left_top -= origin_point;
  left_top.SelfRotate(-origin_heading);
  right_top -= origin_point;
  right_top.SelfRotate(-origin_heading);

  const double center_line_s = (left_top_s + right_top_s) / 2.0;
  std::vector<Vec2d> left_lane_boundary;
  std::vector<Vec2d> right_lane_boundary;
  std::vector<Vec2d> center_lane_boundary_left;
  std::vector<Vec2d> center_lane_boundary_right;
  std::vector<double> center_lane_s_left;
  std::vector<double> center_lane_s_right;
  std::vector<double> left_lane_road_width;
  std::vector<double> right_lane_road_width;

  if (config_.use_road_boundary_from_map()) {
    GetRoadBoundaryFromMap(
        nearby_path, center_line_s, origin_point, origin_heading,
        &left_lane_boundary, &right_lane_boundary, &center_lane_boundary_left,
        &center_lane_boundary_right, &center_lane_s_left, &center_lane_s_right,
        &left_lane_road_width, &right_lane_road_width);
  } else {
    GetRoadBoundary(nearby_path, center_line_s, origin_point, origin_heading,
                    &left_lane_boundary, &right_lane_boundary,
                    &center_lane_boundary_left, &center_lane_boundary_right,
                    &center_lane_s_left, &center_lane_s_right,
                    &left_lane_road_width, &right_lane_road_width);
  }

  // Load boundary as line segments in counter-clockwise order
  std::reverse(left_lane_boundary.begin(), left_lane_boundary.end());

  std::vector<Vec2d> boundary_points;
  std::copy(right_lane_boundary.begin(), right_lane_boundary.end(),
            std::back_inserter(boundary_points));
  std::copy(left_lane_boundary.begin(), left_lane_boundary.end(),
            std::back_inserter(boundary_points));

  size_t right_lane_boundary_last_index = right_lane_boundary.size() - 1;
  for (size_t i = 0; i < right_lane_boundary_last_index; i++) {
    std::vector<Vec2d> segment{right_lane_boundary[i],
                               right_lane_boundary[i + 1]};
    AINFO << "right segment";
    AINFO << "right_road_boundary: [" << std::setprecision(9)
           << right_lane_boundary[i].x() << ", " << right_lane_boundary[i].y()
           << "]";
    AINFO << "right_road_boundary: [" << std::setprecision(9)
           << right_lane_boundary[i + 1].x() << ", "
           << right_lane_boundary[i + 1].y() << "]";
    roi_parking_boundary->push_back(segment);
  }

  size_t left_lane_boundary_last_index = left_lane_boundary.size() - 1;
  for (size_t i = left_lane_boundary_last_index; i > 0; i--) {
    std::vector<Vec2d> segment{left_lane_boundary[i],
                               left_lane_boundary[i - 1]};
    roi_parking_boundary->push_back(segment);
  }

  PrintCurves print_curves;
  for (auto it : *roi_parking_boundary) {
    for (auto pt : it) {
      // pt.SelfRotate(origin_heading);
      // pt += origin_point;
      print_curves.AddPoint("roi_parking_boundary", pt);
    }
  }
  print_curves.PrintToLog();

  ADEBUG << "roi_parking_boundary size: [" << roi_parking_boundary->size()
         << "]";

  // Fuse line segments into convex contraints
  if (!FuseLineSegments(roi_parking_boundary)) {
    return false;
  }

  ADEBUG << "roi_parking_boundary size: [" << roi_parking_boundary->size()
         << "]";
  // Get xy boundary
  auto xminmax = std::minmax_element(
      boundary_points.begin(), boundary_points.end(),
      [](const Vec2d &a, const Vec2d &b) { return a.x() < b.x(); });
  auto yminmax = std::minmax_element(
      boundary_points.begin(), boundary_points.end(),
      [](const Vec2d &a, const Vec2d &b) { return a.y() < b.y(); });
  std::vector<double> ROI_xy_boundary{xminmax.first->x(), xminmax.second->x(),
                                      yminmax.first->y(), yminmax.second->y()};
  auto *xy_boundary =
      frame->mutable_open_space_info()->mutable_ROI_xy_boundary();
  xy_boundary->assign(ROI_xy_boundary.begin(), ROI_xy_boundary.end());

  Vec2d vehicle_xy = Vec2d(vehicle_state_.x(), vehicle_state_.y());
  vehicle_xy -= origin_point;
  vehicle_xy.SelfRotate(-origin_heading);
  if (vehicle_xy.x() < ROI_xy_boundary[0] ||
      vehicle_xy.x() > ROI_xy_boundary[1] ||
      vehicle_xy.y() < ROI_xy_boundary[2] ||
      vehicle_xy.y() > ROI_xy_boundary[3]) {
    AERROR << "vehicle outside of xy boundary of parking ROI";
    return false;
  }
  return true;
}

bool OpenSpaceRoiDecider::GetParkingSpot(Frame *const frame,
                                         ParkingInfo *parking_info) {
  if (frame == nullptr) {
    AERROR << "Invalid frame, fail to GetParkingSpotFromMap from frame. ";
    return false;
  }
  const auto &parking_spot_id_string =
      frame->open_space_info().target_parking_spot_id();
  hdmap::Id parking_spot_id = hdmap::MakeMapId(parking_spot_id_string);
  auto parking_spot = hdmap_->GetParkingSpaceById(parking_spot_id);
  if (!nearby_path_) {
    GetNearbyPath(frame->local_view().planning_command->lane_follow_command(),
                  parking_spot, &nearby_path_);
  }
  // points in polygon is always clockwise
  auto points = parking_spot->polygon().points();
  OpenSpaceRoiUtil::UpdateParkingPointsOrder(*nearby_path_, &points);
  Vec2d center_point(0, 0);
  for (size_t i = 0; i < points.size(); i++) {
    center_point += points[i];
  }
  center_point /= 4.0;
  double lane_heading = 0;
  parking_info->center_point = center_point;
  nearby_path_->GetHeadingAlongPath(center_point, &lane_heading);
  double s, l;
  nearby_path_->GetProjection(center_point, &s, &l);
  if (l > 0) {
    parking_info->is_on_left = true;
    std::swap(points[1], points[3]);
  } else {
    parking_info->is_on_left = false;
  }
  double diff_angle = common::math::AngleDiff(
      lane_heading, parking_spot->parking_space().heading());
  if (std::fabs(diff_angle) < M_PI / 3.0) {
    parking_info->parking_type = ParkingType::PARALLEL_PARKING;
  } else {
    parking_info->parking_type = ParkingType::VERTICAL_PARKING;
  }
  parking_info->corner_points = points;
  double parallel_dist =
      parking_info->corner_points[0].DistanceTo(parking_info->corner_points[1]);
  double verticle_dist =
      parking_info->corner_points[0].DistanceTo(parking_info->corner_points[3]);
  if (parallel_dist > verticle_dist) {
    parking_info->parking_type = ParkingType::PARALLEL_PARKING;
  } else {
    parking_info->parking_type = ParkingType::VERTICAL_PARKING;
  }
  return true;
}

bool OpenSpaceRoiDecider::GetPullOverSpot(
    Frame *const frame, std::array<common::math::Vec2d, 4> *vertices,
    hdmap::Path *nearby_path) {
  const auto &pull_over_status =
      injector_->planning_context()->planning_status().pull_over();
  if (!pull_over_status.has_position() ||
      !pull_over_status.position().has_x() ||
      !pull_over_status.position().has_y() || !pull_over_status.has_theta()) {
    AERROR << "Pull over position not set in planning context";
    return false;
  }

  if (frame->reference_line_info().size() > 1) {
    AERROR << "Should not be in pull over when changing lane in open space "
              "planning";
    return false;
  }

  *nearby_path =
      frame->reference_line_info().front().reference_line().GetMapPath();

  // Construct left_top, left_down, right_down, right_top points
  double pull_over_x = pull_over_status.position().x();
  double pull_over_y = pull_over_status.position().y();
  const double pull_over_theta = pull_over_status.theta();
  const double pull_over_length_front = pull_over_status.length_front();
  const double pull_over_length_back = pull_over_status.length_back();
  const double pull_over_width_left = pull_over_status.width_left();
  const double pull_over_width_right = pull_over_status.width_right();

  Vec2d center_shift_vec((pull_over_length_front - pull_over_length_back) * 0.5,
                         (pull_over_width_left - pull_over_width_right) * 0.5);
  center_shift_vec.SelfRotate(pull_over_theta);
  pull_over_x += center_shift_vec.x();
  pull_over_y += center_shift_vec.y();

  const double half_length =
      (pull_over_length_front + pull_over_length_back) / 2.0;
  const double half_width =
      (pull_over_width_left + pull_over_width_right) / 2.0;

  const double cos_heading = std::cos(pull_over_theta);
  const double sin_heading = std::sin(pull_over_theta);

  const double dx1 = cos_heading * half_length;
  const double dy1 = sin_heading * half_length;
  const double dx2 = sin_heading * half_width;
  const double dy2 = -cos_heading * half_width;

  Vec2d left_top(pull_over_x - dx1 + dx2, pull_over_y - dy1 + dy2);
  Vec2d left_down(pull_over_x - dx1 - dx2, pull_over_y - dy1 - dy2);
  Vec2d right_down(pull_over_x + dx1 - dx2, pull_over_y + dy1 - dy2);
  Vec2d right_top(pull_over_x + dx1 + dx2, pull_over_y + dy1 + dy2);

  std::array<Vec2d, 4> pull_over_vertices{left_top, left_down, right_down,
                                          right_top};
  *vertices = std::move(pull_over_vertices);

  return true;
}

void OpenSpaceRoiDecider::SearchTargetParkingSpotOnPath(
    const hdmap::Path &nearby_path,
    ParkingSpaceInfoConstPtr *target_parking_spot) {
  const auto &parking_space_overlaps = nearby_path.parking_space_overlaps();
  for (const auto &parking_overlap : parking_space_overlaps) {
    if (parking_overlap.object_id == target_parking_spot_id_) {
      hdmap::Id id;
      id.set_id(parking_overlap.object_id);
      *target_parking_spot = hdmap_->GetParkingSpaceById(id);
    }
  }
}

bool OpenSpaceRoiDecider::FuseLineSegments(
    std::vector<std::vector<common::math::Vec2d>> *line_segments_vec) {
  static constexpr double kEpsilon = 1.0e-8;
  auto cur_segment = line_segments_vec->begin();
  while (cur_segment != line_segments_vec->end() - 1) {
    auto next_segment = cur_segment + 1;
    auto cur_last_point = cur_segment->back();
    auto next_first_point = next_segment->front();
    // Check if they are the same points
    if (cur_last_point.DistanceTo(next_first_point) > kEpsilon) {
      ++cur_segment;
      continue;
    }
    if (cur_segment->size() < 2 || next_segment->size() < 2) {
      AERROR << "Single point line_segments vec not expected";
      return false;
    }
    size_t cur_segments_size = cur_segment->size();
    auto cur_second_to_last_point = cur_segment->at(cur_segments_size - 2);
    auto next_second_point = next_segment->at(1);
    if (CrossProd(cur_second_to_last_point, cur_last_point, next_second_point) <
        0.0) {
      cur_segment->push_back(next_second_point);
      next_segment->erase(next_segment->begin(), next_segment->begin() + 2);
      if (next_segment->empty()) {
        line_segments_vec->erase(next_segment);
      }
    } else {
      ++cur_segment;
    }
  }
  return true;
}

bool OpenSpaceRoiDecider::FormulateBoundaryConstraints(
    const std::vector<std::vector<common::math::Vec2d>> &roi_parking_boundary,
    Frame *const frame) {
  // Gather vertice needed by warm start and distance approach
  if (!LoadObstacleInVertices(roi_parking_boundary, frame)) {
    AERROR << "fail at LoadObstacleInVertices()";
    return false;
  }
  // Transform vertices into the form of Ax>b
  if (!LoadObstacleInHyperPlanes(frame)) {
    AERROR << "fail at LoadObstacleInHyperPlanes()";
    return false;
  }
  return true;
}

bool OpenSpaceRoiDecider::LoadObstacleInVertices(
    const std::vector<std::vector<common::math::Vec2d>> &roi_parking_boundary,
    Frame *const frame) {
  auto *mutable_open_space_info = frame->mutable_open_space_info();
  const auto &open_space_info = frame->open_space_info();
  auto *obstacles_vertices_vec =
      mutable_open_space_info->mutable_obstacles_vertices_vec();
  auto *obstacles_edges_num_vec =
      mutable_open_space_info->mutable_obstacles_edges_num();

  // load vertices for parking boundary (not need to repeat the first
  // vertice to get close hull)
  size_t parking_boundaries_num = roi_parking_boundary.size();
  size_t perception_obstacles_num = 0;

  for (size_t i = 0; i < parking_boundaries_num; ++i) {
    obstacles_vertices_vec->push_back(roi_parking_boundary[i]);
  }

  Eigen::MatrixXi parking_boundaries_obstacles_edges_num(parking_boundaries_num,
                                                         1);
  for (size_t i = 0; i < parking_boundaries_num; i++) {
    if (roi_parking_boundary[i].size() <= 1U) {
      AERROR << "Roi parking boundary is invalid: "
             << roi_parking_boundary[i].size();
      return false;
    }
    parking_boundaries_obstacles_edges_num(i, 0) =
        static_cast<int>(roi_parking_boundary[i].size()) - 1;
  }

  if (config_.enable_perception_obstacles()) {
    if (perception_obstacles_num == 0) {
      ADEBUG << "no obstacle given by perception";
    }

    // load vertices for perception obstacles(repeat the first vertice at the
    // last to form closed convex hull)
    const auto &origin_point = open_space_info.origin_point();
    const auto &origin_heading = open_space_info.origin_heading();
    for (const auto &obstacle : obstacles_by_frame_->Items()) {
      if (FilterOutObstacle(*frame, *obstacle)) {
        continue;
      }
      ++perception_obstacles_num;

      std::vector<Vec2d> vertices_ccw;
      if (config_.expand_polygon_of_obstacle_by_distance()) {
        common::math::Polygon2d original_polygon =
            obstacle->PerceptionPolygon();
        original_polygon.ExpandByDistance(config_.perception_obstacle_buffer());
        original_polygon.CalculateVertices(-1.0 * origin_point);
        vertices_ccw = original_polygon.GetAllVertices();
      } else {
        Box2d original_box = obstacle->PerceptionBoundingBox();
        original_box.Shift(-1.0 * origin_point);
        original_box.LongitudinalExtend(config_.perception_obstacle_buffer());
        original_box.LateralExtend(config_.perception_obstacle_buffer());
        vertices_ccw = original_box.GetAllCorners();
      }

      // TODO(Jinyun): Check correctness of ExpandByDistance() in polygon
      // Polygon2d buffered_box(original_box);
      // buffered_box = buffered_box.ExpandByDistance(
      //    config_.perception_obstacle_buffer());
      // TODO(Runxin): Rotate from origin instead
      // original_box.RotateFromCenter(-1.0 * origin_heading);

      std::vector<Vec2d> vertices_cw;
      while (!vertices_ccw.empty()) {
        auto current_corner_pt = vertices_ccw.back();
        current_corner_pt.SelfRotate(-1.0 * origin_heading);
        vertices_cw.push_back(current_corner_pt);
        vertices_ccw.pop_back();
      }
      // As the perception obstacle is a closed convex set, the first vertice
      // is repeated at the end of the vector to help transform all four edges
      // to inequality constraint
      vertices_cw.push_back(vertices_cw.front());
      obstacles_vertices_vec->push_back(vertices_cw);
    }

    // obstacle boundary box is used, thus the edges are set to be 4
    Eigen::MatrixXi perception_obstacles_edges_num =
        4 * Eigen::MatrixXi::Ones(perception_obstacles_num, 1);

    obstacles_edges_num_vec->resize(
        parking_boundaries_obstacles_edges_num.rows() +
            perception_obstacles_edges_num.rows(),
        1);
    *(obstacles_edges_num_vec) << parking_boundaries_obstacles_edges_num,
        perception_obstacles_edges_num;

  } else {
    obstacles_edges_num_vec->resize(
        parking_boundaries_obstacles_edges_num.rows(), 1);
    *(obstacles_edges_num_vec) << parking_boundaries_obstacles_edges_num;
  }

  mutable_open_space_info->set_obstacles_num(parking_boundaries_num +
                                             perception_obstacles_num);
  return true;
}

bool OpenSpaceRoiDecider::FilterOutObstacle(const Frame &frame,
                                            const Obstacle &obstacle) {
  if (obstacle.IsVirtual() || !obstacle.IsStatic()) {
    return true;
  }

  const auto &open_space_info = frame.open_space_info();
  const auto &origin_point = open_space_info.origin_point();
  const auto &origin_heading = open_space_info.origin_heading();
  const auto &obstacle_box = obstacle.PerceptionBoundingBox();
  auto obstacle_center_xy = obstacle_box.center();

  // xy_boundary in xmin, xmax, ymin, ymax.
  const auto &roi_xy_boundary = open_space_info.ROI_xy_boundary();
  obstacle_center_xy -= origin_point;
  obstacle_center_xy.SelfRotate(-origin_heading);
  if (obstacle_center_xy.x() < roi_xy_boundary[0] ||
      obstacle_center_xy.x() > roi_xy_boundary[1] ||
      obstacle_center_xy.y() < roi_xy_boundary[2] ||
      obstacle_center_xy.y() > roi_xy_boundary[3]) {
    return true;
  }

  // Translate the end pose back to world frame with endpose in x, y, phi, v
  const auto &end_pose = open_space_info.open_space_end_pose();
  Vec2d end_pose_x_y(end_pose[0], end_pose[1]);
  end_pose_x_y.SelfRotate(origin_heading);
  end_pose_x_y += origin_point;

  // Get vehicle state
  Vec2d vehicle_x_y(vehicle_state_.x(), vehicle_state_.y());

  // Use vehicle position and end position to filter out obstacle
  const double vehicle_center_to_obstacle =
      obstacle_box.DistanceTo(vehicle_x_y);
  const double end_pose_center_to_obstacle =
      obstacle_box.DistanceTo(end_pose_x_y);
  const double filtering_distance =
      config_.perception_obstacle_filtering_distance();
  if (vehicle_center_to_obstacle > filtering_distance &&
      end_pose_center_to_obstacle > filtering_distance) {
    return true;
  }
  return false;
}

bool OpenSpaceRoiDecider::LoadObstacleInHyperPlanes(Frame *const frame) {
  *(frame->mutable_open_space_info()->mutable_obstacles_A()) =
      Eigen::MatrixXd::Zero(
          frame->open_space_info().obstacles_edges_num().sum(), 2);
  *(frame->mutable_open_space_info()->mutable_obstacles_b()) =
      Eigen::MatrixXd::Zero(
          frame->open_space_info().obstacles_edges_num().sum(), 1);
  // vertices using H-representation
  if (!GetHyperPlanes(
          frame->open_space_info().obstacles_num(),
          frame->open_space_info().obstacles_edges_num(),
          frame->open_space_info().obstacles_vertices_vec(),
          frame->mutable_open_space_info()->mutable_obstacles_A(),
          frame->mutable_open_space_info()->mutable_obstacles_b())) {
    AERROR << "Fail to present obstacle in hyperplane";
    return false;
  }
  return true;
}

bool OpenSpaceRoiDecider::GetHyperPlanes(
    const size_t &obstacles_num, const Eigen::MatrixXi &obstacles_edges_num,
    const std::vector<std::vector<Vec2d>> &obstacles_vertices_vec,
    Eigen::MatrixXd *A_all, Eigen::MatrixXd *b_all) {
  if (obstacles_num != obstacles_vertices_vec.size()) {
    AERROR << "obstacles_num != obstacles_vertices_vec.size()";
    return false;
  }

  A_all->resize(obstacles_edges_num.sum(), 2);
  b_all->resize(obstacles_edges_num.sum(), 1);

  int counter = 0;
  double kEpsilon = 1.0e-5;
  // start building H representation
  for (size_t i = 0; i < obstacles_num; ++i) {
    size_t current_vertice_num = obstacles_edges_num(i, 0);
    Eigen::MatrixXd A_i(current_vertice_num, 2);
    Eigen::MatrixXd b_i(current_vertice_num, 1);

    // take two subsequent vertices, and computer hyperplane
    for (size_t j = 0; j < current_vertice_num; ++j) {
      Vec2d v1 = obstacles_vertices_vec[i][j];
      Vec2d v2 = obstacles_vertices_vec[i][j + 1];

      Eigen::MatrixXd A_tmp(2, 1), b_tmp(1, 1), ab(2, 1);
      // find hyperplane passing through v1 and v2
      if (std::abs(v1.x() - v2.x()) < kEpsilon) {
        if (v2.y() < v1.y()) {
          A_tmp << 1, 0;
          b_tmp << v1.x();
        } else {
          A_tmp << -1, 0;
          b_tmp << -v1.x();
        }
      } else if (std::abs(v1.y() - v2.y()) < kEpsilon) {
        if (v1.x() < v2.x()) {
          A_tmp << 0, 1;
          b_tmp << v1.y();
        } else {
          A_tmp << 0, -1;
          b_tmp << -v1.y();
        }
      } else {
        Eigen::MatrixXd tmp1(2, 2);
        tmp1 << v1.x(), 1, v2.x(), 1;
        Eigen::MatrixXd tmp2(2, 1);
        tmp2 << v1.y(), v2.y();
        ab = tmp1.inverse() * tmp2;
        double a = ab(0, 0);
        double b = ab(1, 0);

        if (v1.x() < v2.x()) {
          A_tmp << -a, 1;
          b_tmp << b;
        } else {
          A_tmp << a, -1;
          b_tmp << -b;
        }
      }

      // store vertices
      A_i.block(j, 0, 1, 2) = A_tmp.transpose();
      b_i.block(j, 0, 1, 1) = b_tmp;
    }

    A_all->block(counter, 0, A_i.rows(), 2) = A_i;
    b_all->block(counter, 0, b_i.rows(), 1) = b_i;
    counter += static_cast<int>(current_vertice_num);
  }
  return true;
}

bool OpenSpaceRoiDecider::IsInParkingLot(
    const double adc_init_x, const double adc_init_y,
    const double adc_init_heading, std::array<Vec2d, 4> *parking_lot_vertices) {
  std::vector<ParkingSpaceInfoConstPtr> parking_lots;
  // make sure there is only one parking lot in search range
  const double kDistance = 1.0;
  auto adc_parking_spot =
      common::util::PointFactory::ToPointENU(adc_init_x, adc_init_y, 0);
  ADEBUG << "IsInParkingLot";
  ADEBUG << hdmap_;
  ADEBUG << hdmap_->GetParkingSpaces(adc_parking_spot, kDistance,
                                     &parking_lots);
  if (hdmap_->GetParkingSpaces(adc_parking_spot, kDistance, &parking_lots) ==
      0) {
    GetParkSpotFromMap(parking_lots.front(), parking_lot_vertices);
    AINFO << "Get park lot from map!!";
    return true;
  }
  return false;
}

void OpenSpaceRoiDecider::GetParkSpotFromMap(
    ParkingSpaceInfoConstPtr parking_lot, std::array<Vec2d, 4> *vertices) {
  // left or right of the parking lot is decided when viewing the parking spot
  // open upward
  Vec2d left_top = parking_lot->polygon().points().at(3);
  Vec2d left_down = parking_lot->polygon().points().at(0);
  Vec2d right_down = parking_lot->polygon().points().at(1);
  Vec2d right_top = parking_lot->polygon().points().at(2);

  std::array<Vec2d, 4> parking_vertices{left_top, left_down, right_down,
                                        right_top};

  *vertices = std::move(parking_vertices);
  Vec2d tmp = (*vertices)[0];
  ADEBUG << "Parking Lot";
  ADEBUG << "parking_lot_vertices: (" << tmp.x() << ", " << tmp.y() << ")";
}

void OpenSpaceRoiDecider::GetAllLaneSegments(
    const routing::RoutingResponse &routing_response,
    std::vector<routing::LaneSegment> *routing_segments) {
  routing_segments->clear();
  for (const auto &road : routing_response.road()) {
    for (const auto &passage : road.passage()) {
      for (const auto &segment : passage.segment()) {
        routing_segments->emplace_back(segment);
      }
    }
  }
}

bool OpenSpaceRoiDecider::GetNearbyPath(
    const apollo::routing::RoutingResponse &routing_response,
    const ParkingSpaceInfoConstPtr &parking_spot,
    std::shared_ptr<hdmap::Path> *nearby_path) {
  LaneInfoConstPtr nearest_lane;
  if (nullptr == parking_spot) {
    AERROR << "The parking spot id is invalid!" << parking_spot->id().id();
    return false;
  }
  auto parking_space = parking_spot->parking_space();
  auto overlap_ids = parking_space.overlap_id();
  if (overlap_ids.empty()) {
    AERROR << "There is no lane overlaps with the parking spot: "
           << parking_spot->id().id();
    return false;
  }
  std::vector<routing::LaneSegment> lane_segments;
  GetAllLaneSegments(routing_response, &lane_segments);
  bool has_found_nearest_lane = false;
  size_t nearest_lane_index = 0;
  for (auto id : overlap_ids) {
    auto overlaps = hdmap_->GetOverlapById(id)->overlap();
    for (auto object : overlaps.object()) {
      if (!object.has_lane_overlap_info()) {
        continue;
      }
      nearest_lane = hdmap_->GetLaneById(object.id());
      if (nearest_lane == nullptr) {
        continue;
      }
      // Check if the lane is contained in the routing response.
      for (auto &segment : lane_segments) {
        if (segment.id() == nearest_lane->id().id()) {
          has_found_nearest_lane = true;
          break;
        }
        ++nearest_lane_index;
      }
      if (has_found_nearest_lane) {
        break;
      }
    }
  }
  if (!has_found_nearest_lane) {
    AERROR << "Cannot find the lane nearest to the parking spot when "
              "GetParkingSpot!";
  }

  // Get the lane nearest to the current position of the vehicle. If the
  // vehicle has not reached the nearest lane to the parking spot, set the
  // lane nearest to the vehicle as "nearest_lane".
  LaneInfoConstPtr nearest_lane_to_vehicle;
  auto point = common::util::PointFactory::ToPointENU(vehicle_state_);
  double vehicle_lane_s = 0.0;
  double vehicle_lane_l = 0.0;
  int status = hdmap_->GetNearestLaneWithHeading(
      point, 10.0, vehicle_state_.heading(), M_PI / 2.0,
      &nearest_lane_to_vehicle, &vehicle_lane_s, &vehicle_lane_l);
  if (status == 0) {
    size_t nearest_lane_to_vehicle_index = 0;
    bool has_found_nearest_lane_to_vehicle = false;
    for (auto &segment : lane_segments) {
      if (segment.id() == nearest_lane_to_vehicle->id().id()) {
        has_found_nearest_lane_to_vehicle = true;
        break;
      }
      ++nearest_lane_to_vehicle_index;
    }
    // The vehicle has not reached the nearest lane to the parking spot。
    if (has_found_nearest_lane_to_vehicle &&
        nearest_lane_to_vehicle_index < nearest_lane_index) {
      nearest_lane = nearest_lane_to_vehicle;
    }
  }

  // Find parking spot by getting nearestlane
  ParkingSpaceInfoConstPtr target_parking_spot = nullptr;
  LaneSegment nearest_lanesegment =
      LaneSegment(nearest_lane, nearest_lane->accumulate_s().front(),
                  nearest_lane->accumulate_s().back());
  std::vector<LaneSegment> segments_vector;
  int next_lanes_num = nearest_lane->lane().successor_id_size();
  if (next_lanes_num != 0) {
    auto next_lane_id = nearest_lane->lane().successor_id(0);
    segments_vector.push_back(nearest_lanesegment);
    auto next_lane = hdmap_->GetLaneById(next_lane_id);
    LaneSegment next_lanesegment =
        LaneSegment(next_lane, next_lane->accumulate_s().front(),
                    next_lane->accumulate_s().back());
    segments_vector.push_back(next_lanesegment);
    size_t succeed_lanes_num = next_lane->lane().successor_id_size();
    if (succeed_lanes_num != 0) {
      auto succeed_lane_id = next_lane->lane().successor_id(0);
      auto succeed_lane = hdmap_->GetLaneById(succeed_lane_id);
      LaneSegment succeed_lanesegment =
          LaneSegment(succeed_lane, succeed_lane->accumulate_s().front(),
                      succeed_lane->accumulate_s().back());
      segments_vector.push_back(succeed_lanesegment);
    }
    *nearby_path = std::make_shared<Path>(segments_vector);
  } else {
    segments_vector.push_back(nearest_lanesegment);
    *nearby_path = std::make_shared<Path>(segments_vector);
  }
  return true;
}
bool OpenSpaceRoiDecider::AdjustPointsOrderToClockwise(
    std::vector<Vec2d> *polygon) {
  if (!OpenSpaceRoiUtil::IsPolygonClockwise(*polygon)) {
    // counter clockwise reverse it
    ADEBUG << "point is anticlockwise,reverse";
    std::reverse(polygon->begin(), polygon->end());
    return true;
  } else {
    return false;
  }
}

bool OpenSpaceRoiDecider::GetParkingOutBoundary(
    const hdmap::Path &nearby_path, Frame *const frame,
    std::vector<std::vector<common::math::Vec2d>> *const roi_parking_boundary) {
  const auto &park_and_go_status =
      injector_->planning_context()->planning_status().park_and_go();
  const double adc_init_x = park_and_go_status.adc_init_position().x();
  const double adc_init_y = park_and_go_status.adc_init_position().y();
  const double adc_init_heading = park_and_go_status.adc_init_heading();
  common::math::Vec2d adc_init_position = {adc_init_x, adc_init_y};
  const double adc_length = vehicle_params_.length();
  const double adc_width = vehicle_params_.width();
  AINFO << std::fixed << "adc_init_x is " << adc_init_x << "adc_init_y is "
        << adc_init_y << "adc_init_heading is " << adc_init_heading;
  // ADC box
  Box2d adc_box(adc_init_position, adc_init_heading, adc_length + 2.0,
                adc_width + 2.0);
  // get vertices from ADC box
  std::vector<common::math::Vec2d> adc_corners;
  adc_box.GetAllCorners(&adc_corners);
  auto right_top = adc_corners[0];
  auto left_top = adc_corners[1];
  auto left_down = adc_corners[2];
  auto right_down = adc_corners[3];

  double left_top_s = 0.0;
  double left_top_l = 0.0;
  double right_top_s = 0.0;
  double right_top_l = 0.0;
  double left_down_s = 0.0;
  double left_down_l = 0.0;
  double right_down_s = 0.0;
  double right_down_l = 0.0;
  if (!(nearby_path.GetProjection(left_top, &left_top_s, &left_top_l) &&
        nearby_path.GetProjection(right_top, &right_top_s, &right_top_l))) {
    AERROR << "fail to get parking spot points' projections on reference line";
    return false;
  }

  if (!(nearby_path.GetProjection(left_down, &left_down_s, &left_down_l) &&
        nearby_path.GetProjection(right_down, &right_down_s, &right_down_l))) {
    AERROR << "fail to get parking spot points' projections on reference line";
    return false;
  }
  if (fabs(left_top_l + right_top_l) > fabs(left_down_l + right_down_l)) {
    frame->mutable_open_space_info()->mutable_origin_point()->set_x(
        right_down.x());
    frame->mutable_open_space_info()->mutable_origin_point()->set_y(
        right_down.y());
    double heading;
    if (!nearby_path.GetHeadingAlongPath(right_down, &heading)) {
      AERROR << "fail to get heading on reference line";
      return false;
    }
    frame->mutable_open_space_info()->set_origin_heading(
        common::math::NormalizeAngle(heading));
    std::swap(right_down, left_top);
    std::swap(right_top, left_down);
    std::swap(right_down_s, left_top_s);
    std::swap(right_down_l, left_top_l);
    std::swap(right_top_s, left_down_s);
    std::swap(right_top_l, left_down_l);
  }
  const auto &origin_point = frame->open_space_info().origin_point();
  ADEBUG << std::fixed << "origin_point: " << origin_point.x() << ", "
         << origin_point.y();
  const auto &origin_heading = frame->open_space_info().origin_heading();
  left_top -= origin_point;
  left_top.SelfRotate(-origin_heading);
  left_down -= origin_point;
  left_down.SelfRotate(-origin_heading);
  right_top -= origin_point;
  right_top.SelfRotate(-origin_heading);
  right_down -= origin_point;
  right_down.SelfRotate(-origin_heading);

  const double center_line_s = (left_top_s + right_top_s) / 2.0;
  std::vector<Vec2d> left_lane_boundary;
  std::vector<Vec2d> right_lane_boundary;
  // The pivot points on the central lane, mapping with the key points on
  // the left lane boundary.
  std::vector<Vec2d> center_lane_boundary_left;
  // The pivot points on the central lane, mapping with the key points on
  // the right lane boundary.
  std::vector<Vec2d> center_lane_boundary_right;
  // The s-value for the anchor points on the center_lane_boundary_left.
  std::vector<double> center_lane_s_left;
  // The s-value for the anchor points on the center_lane_boundary_right.
  std::vector<double> center_lane_s_right;
  // The left-half road width between the pivot points on the
  // center_lane_boundary_left and key points on the
  // left_lane_boundary.
  std::vector<double> left_lane_road_width;
  // The right-half road width between the pivot points on the
  // center_lane_boundary_right and key points on the
  // right_lane_boundary.
  std::vector<double> right_lane_road_width;

  GetRoadBoundary(nearby_path, center_line_s, origin_point, origin_heading,
                  &left_lane_boundary, &right_lane_boundary,
                  &center_lane_boundary_left, &center_lane_boundary_right,
                  &center_lane_s_left, &center_lane_s_right,
                  &left_lane_road_width, &right_lane_road_width);

  // If smaller than zero, the parking spot is on the right of the lane
  // Left, right, down or opposite of the boundary is decided when viewing the
  // parking spot upward
  const double average_l = (left_top_l + right_top_l) / 2.0;
  const double average_s = (left_top_s + right_top_s) / 2.0;

  std::vector<Vec2d> boundary_points;

  // TODO(jiaxuan): Write a half-boundary formation function and call it twice
  // to avoid duplicated manipulations on the left and right sides
  if (average_l < 0) {
    // if average_l is lower than zero, the parking spot is on the right
    // lane boundary and assume that the lane half width is average_l
    ADEBUG << "average_l is less than 0 in OpenSpaceROI";
    size_t point_size = right_lane_boundary.size();
    for (size_t i = 0; i < point_size; i++) {
      right_lane_boundary[i].SelfRotate(origin_heading);
      right_lane_boundary[i] += origin_point;
      right_lane_boundary[i] -= center_lane_boundary_right[i];
      right_lane_boundary[i] /= right_lane_road_width[i];
      if (center_lane_s_right[i] < (average_s)) {
        right_lane_boundary[i] *= (std::fabs(left_top_l));
      } else {
        right_lane_boundary[i] *= (std::fabs(right_top_l));
      }
      right_lane_boundary[i] += center_lane_boundary_right[i];
      right_lane_boundary[i] -= origin_point;
      right_lane_boundary[i].SelfRotate(-origin_heading);
    }

    auto point_left_to_left_top_connor_s = std::lower_bound(
        center_lane_s_right.begin(), center_lane_s_right.end(), left_top_s);
    size_t point_left_to_left_top_connor_index = std::distance(
        center_lane_s_right.begin(), point_left_to_left_top_connor_s);
    point_left_to_left_top_connor_index =
        point_left_to_left_top_connor_index == 0
            ? point_left_to_left_top_connor_index
            : point_left_to_left_top_connor_index - 1;
    auto point_left_to_left_top_connor_itr =
        right_lane_boundary.begin() + point_left_to_left_top_connor_index;
    auto point_right_to_right_top_connor_s = std::upper_bound(
        center_lane_s_right.begin(), center_lane_s_right.end(), right_top_s);
    size_t point_right_to_right_top_connor_index = std::distance(
        center_lane_s_right.begin(), point_right_to_right_top_connor_s);
    auto point_right_to_right_top_connor_itr =
        right_lane_boundary.begin() + point_right_to_right_top_connor_index;

    std::copy(right_lane_boundary.begin(), point_left_to_left_top_connor_itr,
              std::back_inserter(boundary_points));

    std::vector<Vec2d> parking_spot_boundary{left_top, left_down, right_down,
                                             right_top};

    std::copy(parking_spot_boundary.begin(), parking_spot_boundary.end(),
              std::back_inserter(boundary_points));

    std::copy(point_right_to_right_top_connor_itr, right_lane_boundary.end(),
              std::back_inserter(boundary_points));

    std::reverse_copy(left_lane_boundary.begin(), left_lane_boundary.end(),
                      std::back_inserter(boundary_points));

    // reinsert the initial point to the back to from closed loop
    boundary_points.push_back(right_lane_boundary.front());

    // disassemble line into line2d segments
    for (size_t i = 0; i < point_left_to_left_top_connor_index; i++) {
      std::vector<Vec2d> segment{right_lane_boundary[i],
                                 right_lane_boundary[i + 1]};
      roi_parking_boundary->push_back(segment);
    }

    std::vector<Vec2d> left_stitching_segment{
        right_lane_boundary[point_left_to_left_top_connor_index], left_top};
    roi_parking_boundary->push_back(left_stitching_segment);

    std::vector<Vec2d> left_parking_spot_segment{left_top, left_down};
    std::vector<Vec2d> down_parking_spot_segment{left_down, right_down};
    std::vector<Vec2d> right_parking_spot_segment{right_down, right_top};
    roi_parking_boundary->push_back(left_parking_spot_segment);
    roi_parking_boundary->push_back(down_parking_spot_segment);
    roi_parking_boundary->push_back(right_parking_spot_segment);

    std::vector<Vec2d> right_stitching_segment{
        right_top, right_lane_boundary[point_right_to_right_top_connor_index]};
    roi_parking_boundary->push_back(right_stitching_segment);

    size_t right_lane_boundary_last_index = right_lane_boundary.size() - 1;
    for (size_t i = point_right_to_right_top_connor_index;
         i < right_lane_boundary_last_index; i++) {
      std::vector<Vec2d> segment{right_lane_boundary[i],
                                 right_lane_boundary[i + 1]};
      roi_parking_boundary->push_back(segment);
    }

    size_t left_lane_boundary_last_index = left_lane_boundary.size() - 1;
    for (size_t i = left_lane_boundary_last_index; i > 0; i--) {
      std::vector<Vec2d> segment{left_lane_boundary[i],
                                 left_lane_boundary[i - 1]};
      roi_parking_boundary->push_back(segment);
    }

  } else {
    // if average_l is higher than zero, the parking spot is on the left
    // lane boundary and assume that the lane half width is average_l
    ADEBUG << "average_l is greater than 0 in OpenSpaceROI";
    size_t point_size = left_lane_boundary.size();
    for (size_t i = 0; i < point_size; i++) {
      left_lane_boundary[i].SelfRotate(origin_heading);
      left_lane_boundary[i] += origin_point;
      left_lane_boundary[i] -= center_lane_boundary_left[i];
      left_lane_boundary[i] /= left_lane_road_width[i];
      if (center_lane_s_right[i] < (average_s)) {
        left_lane_boundary[i] *= (std::fabs(right_top_l));
      } else {
        left_lane_boundary[i] *= (std::fabs(left_top_l));
      }
      left_lane_boundary[i] += center_lane_boundary_left[i];
      left_lane_boundary[i] -= origin_point;
      left_lane_boundary[i].SelfRotate(-origin_heading);
      ADEBUG << "left_lane_boundary[" << i << "]: " << left_lane_boundary[i].x()
             << ", " << left_lane_boundary[i].y();
    }

    auto point_right_to_right_top_connor_s = std::lower_bound(
        center_lane_s_left.begin(), center_lane_s_left.end(), right_top_s);
    size_t point_right_to_right_top_connor_index = std::distance(
        center_lane_s_left.begin(), point_right_to_right_top_connor_s);
    if (point_right_to_right_top_connor_index > 0) {
      --point_right_to_right_top_connor_index;
    }
    auto point_right_to_right_top_connor_itr =
        left_lane_boundary.begin() + point_right_to_right_top_connor_index;

    auto point_left_to_left_top_connor_s = std::upper_bound(
        center_lane_s_left.begin(), center_lane_s_left.end(), left_top_s);
    size_t point_left_to_left_top_connor_index = std::distance(
        center_lane_s_left.begin(), point_left_to_left_top_connor_s);
    auto point_left_to_left_top_connor_itr =
        left_lane_boundary.begin() + point_left_to_left_top_connor_index;

    std::copy(right_lane_boundary.begin(), right_lane_boundary.end(),
              std::back_inserter(boundary_points));

    std::reverse_copy(point_left_to_left_top_connor_itr,
                      left_lane_boundary.end(),
                      std::back_inserter(boundary_points));

    std::vector<Vec2d> parking_spot_boundary{left_top, left_down, right_down,
                                             right_top};
    std::copy(parking_spot_boundary.begin(), parking_spot_boundary.end(),
              std::back_inserter(boundary_points));

    std::reverse_copy(left_lane_boundary.begin(),
                      point_right_to_right_top_connor_itr,
                      std::back_inserter(boundary_points));

    // reinsert the initial point to the back to from closed loop
    boundary_points.push_back(right_lane_boundary.front());

    // disassemble line into line2d segments
    size_t right_lane_boundary_last_index = right_lane_boundary.size() - 1;
    for (size_t i = 0; i < right_lane_boundary_last_index; i++) {
      std::vector<Vec2d> segment{right_lane_boundary[i],
                                 right_lane_boundary[i + 1]};
      roi_parking_boundary->push_back(segment);
    }

    size_t left_lane_boundary_last_index = left_lane_boundary.size() - 1;
    for (size_t i = left_lane_boundary_last_index;
         i > point_left_to_left_top_connor_index; i--) {
      std::vector<Vec2d> segment{left_lane_boundary[i],
                                 left_lane_boundary[i - 1]};
      roi_parking_boundary->push_back(segment);
    }

    std::vector<Vec2d> left_stitching_segment{
        left_lane_boundary[point_left_to_left_top_connor_index], left_top};
    roi_parking_boundary->push_back(left_stitching_segment);

    std::vector<Vec2d> left_parking_spot_segment{left_top, left_down};
    std::vector<Vec2d> down_parking_spot_segment{left_down, right_down};
    std::vector<Vec2d> right_parking_spot_segment{right_down, right_top};
    roi_parking_boundary->push_back(left_parking_spot_segment);
    roi_parking_boundary->push_back(down_parking_spot_segment);
    roi_parking_boundary->push_back(right_parking_spot_segment);

    std::vector<Vec2d> right_stitching_segment{
        right_top, left_lane_boundary[point_right_to_right_top_connor_index]};
    roi_parking_boundary->push_back(right_stitching_segment);

    for (size_t i = point_right_to_right_top_connor_index; i > 0; --i) {
      std::vector<Vec2d> segment{left_lane_boundary[i],
                                 left_lane_boundary[i - 1]};
      roi_parking_boundary->push_back(segment);
    }
  }
  PrintCurves print_curves;
  // Fuse line segments into convex contraints
  if (!FuseLineSegments(roi_parking_boundary)) {
    AERROR << "FuseLineSegments failed in parking ROI";
    return false;
  }
  for (auto it : *roi_parking_boundary)
    print_curves.AddPoint("free_space_roi", it);
  print_curves.PrintToLog();
  // Get xy boundary
  auto xminmax = std::minmax_element(
      boundary_points.begin(), boundary_points.end(),
      [](const Vec2d &a, const Vec2d &b) { return a.x() < b.x(); });
  auto yminmax = std::minmax_element(
      boundary_points.begin(), boundary_points.end(),
      [](const Vec2d &a, const Vec2d &b) { return a.y() < b.y(); });
  std::vector<double> ROI_xy_boundary{xminmax.first->x(), xminmax.second->x(),
                                      yminmax.first->y(), yminmax.second->y()};
  auto *xy_boundary =
      frame->mutable_open_space_info()->mutable_ROI_xy_boundary();
  xy_boundary->assign(ROI_xy_boundary.begin(), ROI_xy_boundary.end());

  Vec2d vehicle_xy = Vec2d(vehicle_state_.x(), vehicle_state_.y());
  vehicle_xy -= origin_point;
  vehicle_xy.SelfRotate(-origin_heading);
  if (vehicle_xy.x() < ROI_xy_boundary[0] ||
      vehicle_xy.x() > ROI_xy_boundary[1] ||
      vehicle_xy.y() < ROI_xy_boundary[2] ||
      vehicle_xy.y() > ROI_xy_boundary[3]) {
    AERROR << "vehicle outside of xy boundary of parking ROI";
    return false;
  }
  AINFO << "success get ROI";
  return true;
}

bool OpenSpaceRoiDecider::AddParkingSpaceBoundary(
    Frame *const frame, const hdmap::Path &nearby_path,
    std::vector<std::vector<common::math::Vec2d>> *const roi_parking_boundary) {
  const auto &park_and_go_status =
      injector_->planning_context()->planning_status().park_and_go();
  const double adc_init_x = park_and_go_status.adc_init_position().x();
  const double adc_init_y = park_and_go_status.adc_init_position().y();
  const double adc_init_heading = park_and_go_status.adc_init_heading();
  common::math::Vec2d adc_init_position = {adc_init_x, adc_init_y};
  const double adc_length = vehicle_params_.length();
  const double adc_width = vehicle_params_.width();
  // ADC box
  double shift_distance =
      vehicle_params_.front_edge_to_center() - 0.5 * adc_length;
  adc_init_position = adc_init_position +
                      Vec2d::CreateUnitVec2d(adc_init_heading) * shift_distance;
  Box2d adc_box(adc_init_position, adc_init_heading, adc_length, adc_width);
  // get vertices from ADC box
  std::vector<common::math::Vec2d> adc_corners;
  adc_box.GetAllCorners(&adc_corners);
  PrintCurves print_curves;
  for (auto pt : adc_corners) {
    print_curves.AddPoint("adc_corners", pt);
  }
  for (const auto &parking_overlap : nearby_path.parking_space_overlaps()) {
    hdmap::Id id;
    id.set_id(parking_overlap.object_id);
    const auto target_parking_spot = hdmap_->GetParkingSpaceById(id);
    if (!target_parking_spot) {
      continue;
    }
    const auto parking_polygon = target_parking_spot->polygon();
    for (const auto &pt : parking_polygon.points()) {
      print_curves.AddPoint(id.id() + "parking_polygon", pt);
    }
    bool is_in_polygon = true;
    for (const auto &pt : adc_corners) {
      if (!parking_polygon.IsPointIn(pt)) {
        is_in_polygon = false;
        break;
      }
    }
    if (is_in_polygon) {
      auto points = parking_polygon.points();
      OpenSpaceRoiUtil::UpdateParkingPointsOrder(nearby_path, &points);
      std::vector<Vec2d> parking_boundary;
      for (size_t i = 0; i < points.size(); i++) {
        int t = static_cast<int>(i + 1) % static_cast<int>(points.size());
        parking_boundary.emplace_back(points.at(t).x(), points.at(t).y());
      }
      roi_parking_boundary->push_back(parking_boundary);
    }
  }
  print_curves.PrintToLog();
  return true;
}

}  // namespace planning
}  // namespace apollo

将自车包围盒宽度参数调整为 5.0，以扩大计算范围：

轨迹验证

仿真界面显示轨迹成功生成，路径远离前车，避障计算符合预期：

保证车辆离对车1米距离

配置文件路径：

modules/planning/scenarios/park_and_go/conf/park_and_go_adjust/open_space_roi_decider.pb.txt

关键参数设置为 1.0，确保车辆与障碍车的最小横向安全距离为 1 米。

roi_type: PARK_AND_GO
roi_line_segment_min_angle: 0.15
roi_line_segment_length: 0.2
perception_obstacle_buffer: 1.0
end_pose_s_distance: 10.0
roi_longitudinal_range_start : 10.0
roi_longitudinal_range_end : 20.0

从仿真截图可见，轨迹生成后的主车与障碍车辆的距离符合 1 米要求

避障逻辑生效

赛题二 红绿灯路口-行人避让

赛题描述

当主车在监测到前方的红灯时, 主车停车在停止线前1.5-2.0米处，不得超过停止线。当红灯变为绿灯后，主车可以继续行驶，但需要注意人行横道上可能仍有行人通行。在这种情况下，主车必须等待行人安全通过后才能继续前行

评分标准

主车未避让行人，本场景记0分，主车停止距离未达到1.5-2.0m距离内，本场景扣20分，速度每超速1m/s, 本场景每帧扣2分。

解题思路

修改红绿灯停车规则的停车距离参数；

调整人行横道模块中的停车距离、安全间隔等参数；

设置合适的人车横向距离阈值；

屏蔽影响判断的轨迹预测逻辑，避免系统提前放行。

解题过程

默认行为分析

初始仿真结果如下所示：

存在以下问题：

• 停止线检测存在偏差，停车位置未满足 1.5–2.0 米范围；
• 主车在行人未完全通过人行横道时便提前启动，未做有效避让。

修改红绿灯停车规则配置

配置路径如下：

modules/planning/traffic_rules/traffic_light/conf/default_conf.pb.txt

enabled: true
stop_distance: 1.25
max_stop_deceleration: 4.0

调整人行横道模块参数

配置路径如下：

modules/planning/traffic_rules/crosswalk/conf/default_conf.pb.txt

stop_distance: 1.25
max_stop_deceleration: 4.0
min_pass_s_distance: 1.0
expand_s_distance: 2.0
stop_strict_l_distance: 6.5
stop_loose_l_distance: 6.5
stop_timeout: 1e4

屏蔽不必要的避障条件逻辑

源文件路径：

modules/planning/traffic_rules/crosswalk/crosswalk.cc

屏蔽(4) when l_distance <= strict_l_distance条件

修改结果验证

赛题三 路口--减速通行

赛题描述

主车行驶至路口时，需降低车辆速度至5米/秒通过路口，并在通过路口后恢复正常速度

评分标准

若主车未在指定区域内限速，每超出1m/s的速度，场景每帧扣2分

解题思路

在指定路口区域内通过限速接口设定速度上限，从而控制主车以限定速度（≤ 5 m/s）通过路口区域。通过调用 ReferenceLine 中的 AddSpeedLimit() 接口，并根据地图提供的路口重叠段（overlap）位置进行限速区间约束。

解题过程

路口 ID 获取与限速区域设定

获取目标路口的 object_id，并结合该路口的 start_s 与 end_s 坐标值作为限速区间依据。

在以下路径中插入限速逻辑（可选择任意交通规则模块，此处以人行横道为例）：

modules/planning/traffic_rules/crosswalk/crosswalk.cc（任意规则）

插入限速 API 调用逻辑

Status Crosswalk::ApplyRule(Frame* const frame,
                            ReferenceLineInfo* const reference_line_info) {
  CHECK_NOTNULL(frame);
  CHECK_NOTNULL(reference_line_info);
  ReferenceLine* reference_line = reference_line_info->mutable_reference_line();
  const std::vector<PathOverlap>& crosswalk_overlaps
          = reference_line_info->reference_line().map_path().crosswalk_overlaps();
  for (const auto& crosswalk_overlap : crosswalk_overlaps) {
      if (crosswalk_overlap.object_id == "Crosswalk_62")
          reference_line->AddSpeedLimit(
                  crosswalk_overlap.start_s - 3,
                  crosswalk_overlap.end_s + 2,
                  4.8);
  }

  if (!FindCrosswalks(reference_line_info)) {
    injector_->planning_context()->mutable_planning_status()->clear_crosswalk();
    return Status::OK();
  }

  MakeDecisions(frame, reference_line_info);
  return Status::OK();
}

限速效果验证

赛题四 人行道--跟车行驶

赛题描述

在本场景中，主车跟车行驶，当前方道路存在人行道时且有行人在人行道上通行，车辆需要停车避让，为了保证安全，主车需要与前车保持2-2.5米的停车距离，等待行人完全通过人行道时再通过该路口；当有行人在人行道上通行时主车应在障碍车后方停车，不得借道绕行

评分标准

当遇到前方障碍物车辆遇到行人通过人行道时，会减速停车停止在停止线前，主车遇到这种情况时，应在障碍车后方停车，不得借道绕行，如绕行该场景得0分；主车距离前车大于2.5m，或小于2m，本场景扣20分

解题思路

调整障碍物停车距离的参数配置，确保主车在检测到前车停车时能够在 2~2.5 米范围内停止；

修改借道行驶模块的决策条件，禁止主车在人行道前发生前车绕行操作。

解题过程

增加绕行限制条件

修改路径：

modules/planning/tasks/lane_borrow_path/lane_borrow_path.cc

停止距离

修改主规划模块的配置文件：

modules/planning/planning_component/conf/planning.conf

--max_stop_distance_obstacle=2.0
--min_stop_distance_obstacle=0.0

验证运行效果

赛题五 道路施工，换道行驶

赛题描述

主车前方道路施工，导致该段道路无法通行。主车应重新规划路线选取旁边匝道通过该区域

评分标准

主车前方道路施工，导致该段道路无法通行。主车应重新规划路线选取旁边匝道通过该区域

解题思路

本场景主要涉及路径重规划逻辑。通过修改路径规划器逻辑，在检测到施工阻断区域后主动指定一条旁路（匝道）替代默认主路作为通行路线。具体实现方式为：在路由请求处理逻辑中插入条件判断，根据起点 lane_id 定向添加额外中间路段，从而绕开受阻区域。

解题过程

路由模块修改

路径：

modules/routing/routing.cc

/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/routing/routing.h"

#include <limits>
#include <unordered_map>

#include "modules/common/util/point_factory.h"
#include "modules/map/hdmap/hdmap_common.h"
#include "modules/routing/common/routing_gflags.h"

namespace apollo {
namespace routing {

using apollo::common::ErrorCode;
using apollo::common::PointENU;

std::string Routing::Name() const { return FLAGS_routing_node_name; }

Routing::Routing()
    : monitor_logger_buffer_(common::monitor::MonitorMessageItem::ROUTING) {}

apollo::common::Status Routing::Init() {
  const auto routing_map_file = apollo::hdmap::RoutingMapFile();
  AINFO << "Use routing topology graph path: " << routing_map_file;
  navigator_ptr_.reset(new Navigator(routing_map_file));

  hdmap_ = apollo::hdmap::HDMapUtil::BaseMapPtr();
  ACHECK(hdmap_) << "Failed to load map file:" << apollo::hdmap::BaseMapFile();

  return apollo::common::Status::OK();
}

apollo::common::Status Routing::Start() {
  if (!navigator_ptr_->IsReady()) {
    AERROR << "Navigator is not ready!";
    return apollo::common::Status(ErrorCode::ROUTING_ERROR,
                                  "Navigator not ready");
  }
  AINFO << "Routing service is ready.";
  monitor_logger_buffer_.INFO("Routing started");
  return apollo::common::Status::OK();
}

std::vector<routing::RoutingRequest> Routing::FillLaneInfoIfMissing(
    const routing::RoutingRequest& routing_request) {
  std::vector<routing::RoutingRequest> fixed_requests;
  std::unordered_map<int, std::vector<LaneWaypoint>>
      additional_lane_waypoint_map;
  routing::RoutingRequest fixed_request(routing_request);
  for (int i = 0; i < routing_request.waypoint_size(); ++i) {
    LaneWaypoint lane_waypoint(routing_request.waypoint(i));
    if (lane_waypoint.has_id()) {
      continue;
    }

    // fill lane info when missing
    const auto point =
        common::util::PointFactory::ToPointENU(lane_waypoint.pose());
    std::vector<std::shared_ptr<const hdmap::LaneInfo>> lanes;
    // look for lanes with bigger radius if not found
    constexpr double kRadius = 0.3;
    for (int i = 0; i < 20; ++i) {
      hdmap_->GetLanes(point, kRadius + i * kRadius, &lanes);
      if (lanes.size() > 0) {
        break;
      }
    }
    if (lanes.empty()) {
      AERROR << "Failed to find nearest lane from map at position: "
             << point.DebugString();
      return fixed_requests;  // return empty vector
    }
    for (size_t j = 0; j < lanes.size(); ++j) {
      double s = 0.0;
      double l = 0.0;
      lanes[j]->GetProjection({point.x(), point.y()}, &s, &l);
      if (j == 0) {
        auto waypoint_info = fixed_request.mutable_waypoint(i);
        waypoint_info->set_id(lanes[j]->id().id());
        waypoint_info->set_s(s);
      } else {
        // additional candidate lanes
        LaneWaypoint new_lane_waypoint(lane_waypoint);
        new_lane_waypoint.set_id(lanes[j]->id().id());
        new_lane_waypoint.set_s(s);
        additional_lane_waypoint_map[i].push_back(new_lane_waypoint);
      }
    }
  }
  // first routing_request
  // fixed_requests.push_back(fixed_request);
  routing::RoutingRequest new_request;

  // start point
  LaneWaypoint start_waypoint(fixed_request.waypoint(0));
  *new_request.add_waypoint() = start_waypoint;

  if (start_waypoint.id() == "Lane_1427") {
      AINFO << "We are in the problem 5.";
      LaneWaypoint p;
      p.set_id("Lane_1423");
      p.set_s(10);
      p.mutable_pose()->set_x(423856);
      p.mutable_pose()->set_y(4438069);

      *new_request.add_waypoint() = p;
  }

  // copy the rest of the waypoints except the last one
  for (int i = 1; i < fixed_request.waypoint_size() - 1; ++i) {
    LaneWaypoint lane_waypoint(fixed_request.waypoint(i));
    *new_request.add_waypoint() = lane_waypoint;
  }

  // end point
  LaneWaypoint end_waypoint(fixed_request.waypoint(fixed_request.waypoint_size() - 1));
  // if (end_waypoint.id() == "Lane_491") {
  //     AINFO << "We are in the problem 6.";
  //     end_waypoint.set_s(end_waypoint.s());
  // } else
  //     end_waypoint.set_s(end_waypoint.s() + 2.75);

  *new_request.add_waypoint() = end_waypoint;

  fixed_requests.push_back(new_request);

  // additional routing_requests because of lane overlaps
  for (const auto& m : additional_lane_waypoint_map) {
    size_t cur_size = fixed_requests.size();
    for (size_t i = 0; i < cur_size; ++i) {
      // use index to iterate while keeping push_back
      for (const auto& lane_waypoint : m.second) {
        routing::RoutingRequest new_request(fixed_requests[i]);
        auto waypoint_info = new_request.mutable_waypoint(m.first);
        waypoint_info->set_id(lane_waypoint.id());
        waypoint_info->set_s(lane_waypoint.s());
        fixed_requests.push_back(new_request);
      }
    }
  }

  for (const auto& fixed_request : fixed_requests) {
    ADEBUG << "Fixed routing request:" << fixed_request.DebugString();
  }
  return fixed_requests;
}

bool Routing::Process(
    const std::shared_ptr<routing::RoutingRequest>& routing_request,
    routing::RoutingResponse* const routing_response) {
  if (nullptr == routing_request) {
    AWARN << "Routing request is empty!";
    return true;
  }
  CHECK_NOTNULL(routing_response);
  AINFO << "Get new routing request:" << routing_request->DebugString();

  const auto& fixed_requests = FillLaneInfoIfMissing(*routing_request);
  double min_routing_length = std::numeric_limits<double>::max();
  for (const auto& fixed_request : fixed_requests) {
    routing::RoutingResponse routing_response_temp;
    if (navigator_ptr_->SearchRoute(fixed_request, &routing_response_temp)) {
      const double routing_length =
          routing_response_temp.measurement().distance();
      if (routing_length < min_routing_length) {
        routing_response->CopyFrom(routing_response_temp);
        min_routing_length = routing_length;
      }
    }
  }
  if (min_routing_length < std::numeric_limits<double>::max()) {
    monitor_logger_buffer_.INFO("Routing success!");
    return true;
  }

  AERROR << "Failed to search route with navigator.";
  monitor_logger_buffer_.WARN("Routing failed! " +
                              routing_response->status().msg());
  return false;
}

}  // namespace routing
}  // namespace apollo

效果验证

赛题六 自主泊车--障碍物堵塞

赛题描述

在此情景中，主车需驶回主车的固定停车位。当主车驶入停车场，若发现前方有障碍物阻挡其前进路径，并且附近泊车位空无一车时，主车可以选择绕过这些空置的泊车位，直接前往主车的固定停车位中

评分标准

主车在规定时间内未成功回到车位，本场景分计0分

解题思路

增加车位选择与切换能力；

扩大 ROI（Region of Interest）区域边界，提供绕行轨迹规划空间；

修改道路边界计算逻辑以放宽路径限制；

允许路径轻微偏离标准车道以避障；

更改进入泊车场景的触发条件，提高场景适应性。

解题过程

默认行为分析

识别目标车位

识别目标车位确保在当前路径下的车位识别逻辑可正确识别目标车位 ID

模块路径：

modules/planning/scenarios/valet_parking/valet_parking_scenario.cc

/******************************************************************************
 * Copyright 2019 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/**
 * @file
 **/

#include "modules/planning/scenarios/valet_parking/valet_parking_scenario.h"

#include "modules/planning/planning_base/common/frame.h"
#include "modules/planning/scenarios/valet_parking/stage_approaching_parking_spot.h"
#include "modules/planning/scenarios/valet_parking/stage_parking.h"

namespace apollo {
namespace planning {

using apollo::common::VehicleState;
using apollo::common::math::Vec2d;
using apollo::hdmap::ParkingSpaceInfoConstPtr;
using apollo::hdmap::Path;
using apollo::hdmap::PathOverlap;

bool ValetParkingScenario::Init(std::shared_ptr<DependencyInjector> injector,
                                const std::string& name) {
  if (init_) {
    return true;
  }

  if (!Scenario::Init(injector, name)) {
    AERROR << "failed to init scenario" << Name();
    return false;
  }

  if (!Scenario::LoadConfig<ScenarioValetParkingConfig>(
          &context_.scenario_config)) {
    AERROR << "fail to get config of scenario" << Name();
    return false;
  }
  hdmap_ = hdmap::HDMapUtil::BaseMapPtr();
  CHECK_NOTNULL(hdmap_);
  init_ = true;
  return true;
}

bool ValetParkingScenario::IsTransferable(const Scenario* const other_scenario,
                                          const Frame& frame) {
  // TODO(all) Implement available parking spot detection by preception results
  // 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 = "ParkingSpace_81";
  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();
  // } else {
  //   AINFO << "No parking space id from routing";
  //   return false;
  }

  if (target_parking_spot_id.empty()) {
    return false;
  }

  const auto& nearby_path =
      frame.reference_line_info().front().reference_line().map_path();
  PathOverlap parking_space_overlap;
  const auto& vehicle_state = frame.vehicle_state();

  if (!SearchTargetParkingSpotOnPath(nearby_path, target_parking_spot_id,
                                     &parking_space_overlap)) {
    AINFO << "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)) {
    AINFO << "target parking spot found, but too far, distance larger than "
              "pre-defined distance"
           << target_parking_spot_id;
    return false;
  }
  context_.target_parking_spot_id = target_parking_spot_id;
  return true;
}

bool ValetParkingScenario::SearchTargetParkingSpotOnPath(
    const Path& nearby_path, const std::string& target_parking_id,
    PathOverlap* parking_space_overlap) {
  const auto& parking_space_overlaps = nearby_path.parking_space_overlaps();
  for (const auto& parking_overlap : parking_space_overlaps) {
    if (parking_overlap.object_id == target_parking_id) {
      *parking_space_overlap = parking_overlap;
      return true;
    }
  }
  return false;
}

bool ValetParkingScenario::CheckDistanceToParkingSpot(
    const Frame& frame, const VehicleState& vehicle_state,
    const Path& nearby_path, const double parking_start_range,
    const PathOverlap& parking_space_overlap) {
  // TODO(Jinyun) parking overlap s are wrong on map, not usable
  const hdmap::HDMap* hdmap = hdmap::HDMapUtil::BaseMapPtr();
  hdmap::Id id;
  double center_point_s, center_point_l;
  id.set_id(parking_space_overlap.object_id);
  ParkingSpaceInfoConstPtr target_parking_spot_ptr =
      hdmap->GetParkingSpaceById(id);
  Vec2d left_bottom_point = target_parking_spot_ptr->polygon().points().at(0);
  Vec2d right_bottom_point = target_parking_spot_ptr->polygon().points().at(1);
  Vec2d right_top_point = target_parking_spot_ptr->polygon().points().at(2);
  Vec2d left_top_point = target_parking_spot_ptr->polygon().points().at(3);
  Vec2d center_point = (left_bottom_point + right_bottom_point +
                        right_top_point + left_top_point) /
                       4.0;
  nearby_path.GetNearestPoint(center_point, &center_point_s, &center_point_l);
  double vehicle_point_s = 0.0;
  double vehicle_point_l = 0.0;
  Vec2d vehicle_vec(vehicle_state.x(), vehicle_state.y());
  nearby_path.GetNearestPoint(vehicle_vec, &vehicle_point_s, &vehicle_point_l);
  if (std::abs(center_point_s - vehicle_point_s) < parking_start_range) {
    return true;
  }
  return false;
}

}  // namespace planning
}  // namespace apollo

扩大roi边界让车可以停入

模块路径：

modules/planning/tasks/open_space_roi_decider/open_space_roi_decider.cc

搜索原left_top = parking_info.corner_points[0];位置

  auto left_top = parking_info.corner_points[0];
  ADEBUG << std::fixed << "left_top: " << left_top.x() << ", " << left_top.y();
  auto left_down = parking_info.corner_points[3];
  ADEBUG << std::fixed << "left_down: " << left_down.x() << ", "
         << left_down.y();
  auto right_down = parking_info.corner_points[2];
  ADEBUG << std::fixed << "right_down: " << right_down.x() << ", "
         << right_down.y();
  auto right_top = parking_info.corner_points[1];
  ADEBUG << std::fixed << "right_top: " << right_top.x() << ", "
         << right_top.y();

替换为

  auto& corner_points = parking_info.corner_points;
  // extend the parking spot to the road boundary
  double kExtentionRatio = 3;
  double width = corner_points[1].x() - corner_points[0].x();
  double length = corner_points[1].y() - corner_points[2].y();
  double extention_length = kExtentionRatio * length;
  double extention_width = kExtentionRatio * width;

  auto left_top = common::math::Vec2d(
      corner_points[0].x() - extention_width, corner_points[0].y() - extention_length);
  auto right_top = common::math::Vec2d(
      corner_points[1].x() + extention_width, corner_points[1].y() - extention_length);
  auto left_down = common::math::Vec2d(
      corner_points[3].x() - extention_width, corner_points[3].y() + extention_length);
  auto right_down = common::math::Vec2d(
      corner_points[2].x() + extention_width, corner_points[2].y() + extention_length);

放宽道路边界限制，支持绕行

模块路径：

modules/planning/planning_interface_base/task_base/common/path_util/path_bounds_decider_util.cc

扩大道路边界绕过前车

更改PathBoundsDeciderUtil::GetBoundaryFromSelfLane

bool PathBoundsDeciderUtil::GetBoundaryFromSelfLane(
    const ReferenceLineInfo& reference_line_info, const SLState& init_sl_state,
    PathBoundary* const path_bound) {
  // Sanity checks.
  CHECK_NOTNULL(path_bound);
  ACHECK(!path_bound->empty());
  const ReferenceLine& reference_line = reference_line_info.reference_line();
  double adc_lane_width =
      GetADCLaneWidth(reference_line, init_sl_state.first[0]);
  // Go through every point, update the boundary based on lane info and
  // ADC's position.
  double past_lane_left_width = adc_lane_width / 2.0;
  double past_lane_right_width = adc_lane_width / 2.0;
  int path_blocked_idx = -1;
  for (size_t i = 0; i < path_bound->size(); ++i) {
    double curr_s = (*path_bound)[i].s;
    // 1. Get the current lane width at current point.
    double curr_lane_left_width = 0.0;
    double curr_lane_right_width = 0.0;
    double offset_to_lane_center = 0.0;
    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 {
      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;
      past_lane_left_width = curr_lane_left_width;
      past_lane_right_width = curr_lane_right_width;
    }

    // 3. Calculate the proper boundary based on lane-width, ADC's position,
    //    and ADC's velocity.
    double offset_to_map = 0.0;
    reference_line.GetOffsetToMap(curr_s, &offset_to_map);

    double curr_left_bound = 0.0;
    double curr_right_bound = 0.0;
    curr_left_bound = curr_lane_left_width - offset_to_map;
    curr_right_bound = -curr_lane_right_width - offset_to_map;
    // 4. Update the boundary.
    if (!UpdatePathBoundaryWithBuffer(curr_left_bound, curr_right_bound,
                                      BoundType::LANE, BoundType::LANE, "", "",
                                      &path_bound->at(i))) {
      path_blocked_idx = static_cast<int>(i);
    }
    if (path_blocked_idx != -1) {
      break;
    }
  }

  // Add Parking Space to Path Boundaries
  const auto& junction_overlaps = reference_line.map_path().junction_overlaps();
  for (const auto& junction_overlap : junction_overlaps) {
      const auto &start_s = junction_overlap.start_s, &end_s = junction_overlap.end_s;
      for (size_t i = 0; i < path_bound->size(); ++i) {
          auto& path_bound_point = path_bound->at(i);
          if (path_bound_point.s >= start_s && path_bound_point.s <= end_s) {
              path_bound_point.l_lower.l -= 2.0; // right bound extend
              // path_bound_point.l_upper.l += 2.0; // left bound extend
              // AINFO << path_bound_point.s << " "
              //       << path_bound_point.l_lower.l << " "
              //       << path_bound_point.l_upper.l;
          }
      }
      // debug print all attr
      AINFO << "junction overlap object_id: " << junction_overlap.object_id
            << " junction overlap start_s: " << junction_overlap.start_s
            << " junction overlap end_s: " << junction_overlap.end_s;
  }

  PathBoundsDeciderUtil::TrimPathBounds(path_blocked_idx, path_bound);

  return true;
}

更改进入场景时机

配置路径:

modules/planning/scenarios/valet_parking/conf/scenario_conf.pb.txt

parking_spot_range_to_start: 9.0
max_valid_stop_distance: 1.0

允许路径偏离以绕过前车

配置路径:

modules/planning/tasks/lane_follow_path/conf/default_conf.pb.txt

is_extend_lane_bounds_to_include_adc: false
extend_buffer: 0.2
path_optimizer_config {
  l_weight: 3.0
  dl_weight: 15.0
  ddl_weight: 100.0
  dddl_weight: 1500.0
  lateral_derivative_bound_default: 2.0
  path_reference_l_weight:100
}

仿真效果展示

国预选赛

赛题一 障碍物绕行场景

赛题描述

主车行驶过程中，前方监测到静态障碍物后借道绕行。距离目标障碍物横向距离至少保持 1 米，借道避障限速不得超过3m/s

评分标准

主车与目标障碍物横向距离少于1米，本场景分扣20分。速度每超出1m/s的速度，场景每帧扣2分。

解题思路

增加横向避障缓冲区，确保主车与静态障碍物之间最小横向距离不小于 1 米；

对绕行区段添加速度限制规则，控制通过该区域的最高速度不超过 3 m/s。

解题过程

观察默认情况

默认情况下绕过距离不足，同时无限速

配置横向避让缓冲距离

配置路径：

modules/planning/planning_component/conf/planning.conf

在配置文件内

添加--obstacle_lat_buffer=1.1给横向边距增加距离

添加障碍物区域限速逻辑

模块路径：

modules/planning/traffic_rules/crosswalk/crosswalk.cc

插入限速api调用

Status Crosswalk::ApplyRule(Frame* const frame,
                            ReferenceLineInfo* const reference_line_info) {
  CHECK_NOTNULL(frame);
  CHECK_NOTNULL(reference_line_info);
   ReferenceLine* reference_line = reference_line_info->mutable_reference_line();

    // for every obstacle
    for (const auto* obstacle : frame->obstacles()) {
        if (obstacle->IsVirtual() || !obstacle->IsStatic()) {
            continue;
        }

        auto bbx = obstacle->PerceptionBoundingBox();
        auto corners = bbx.GetAllCorners();
        std::vector<apollo::common::SLPoint> sl_points;
        for (const auto& corner : corners) {
            apollo::common::SLPoint sl_point;
            reference_line->XYToSL(corner, &sl_point);
            sl_points.push_back(sl_point);
        }
        // min and max
        double start_s = std::min_element(sl_points.begin(), sl_points.end(),
                                     [](const apollo::common::SLPoint& a, const apollo::common::SLPoint& b) {
                                         return a.s() < b.s();
                                     })->s();
        double end_s = std::max_element(sl_points.begin(), sl_points.end(),
                                      [](const apollo::common::SLPoint& a, const apollo::common::SLPoint& b) {
                                        return a.s() < b.s();
                                      })->s();
        reference_line->AddSpeedLimit(start_s - 10,
                                        end_s + 10,
                                        2.5);
    }

  if (!FindCrosswalks(reference_line_info)) {
    injector_->planning_context()->mutable_planning_status()->clear_crosswalk();
    return Status::OK();
  }

  MakeDecisions(frame, reference_line_info);
  return Status::OK();
}

效果图示

绕行路径未生效

但是另一个场景仍然无法通过，排查绕行路径未生效问题

排查后发现存在如下逻辑跳出：

modules/planning/tasks/lane_borrow_path/lane_borrow_path.cc

  if (!IsSidePassableObstacle(*reference_line_info_)) {

   return false;

  }

条件不满足

注释处理后效果

赛题二 减速通行场景

赛题描述

主车在通过交汇路口时，需降低车辆速度至5米/秒通过

评分标准

若主车未在指定区域内限速，每超出1m/s的速度，场景每帧扣2分

解题思路

通过调用路径规划模块中的速度限制接口 AddSpeedLimit()，针对特定区域（交叉路口）施加速度限制，确保主车在通过交汇区域时速度不超过 5 m/s。

解题过程

插入限速逻辑

模块路径：

modules/planning/traffic_rules/crosswalk/crosswalk.cc

  const std::vector<PathOverlap>& crosswalk_overlaps
          = reference_line_info->reference_line().map_path().crosswalk_overlaps();
  for (const auto& crosswalk_overlap : crosswalk_overlaps) {
          reference_line->AddSpeedLimit(
                  crosswalk_overlap.start_s - 3,
                  crosswalk_overlap.end_s + 2,
                  4.3);
  }

效果验证

赛题三 靠边启动场景

赛题描述

主车在道路旁启动时，若前方道路因障碍物堵塞，无法正常通行，主车应绕行障碍物以安全到达目的地。

评分标准

主车在规定时间内未完成相关任务，本场景分计0分

解题思路

启用借道逻辑；

优化路径光滑性和偏移限制参数；

调整变道路径的代价权重；

辅以速度规划器的响应参数调整，提升跟车与绕行效率。

解题过程

启用借道与路径优化配置

修改路径：

modules/planning/tasks/lane_borrow_path/conf/default_conf.pb.txt

is_allow_lane_borrowing: true
lane_borrow_max_speed: 20.0
long_term_blocking_obstacle_cycle_threshold: 3
path_optimizer_config {
  l_weight: 1.0
  dl_weight: 15.0
  ddl_weight: 1000.0
  dddl_weight: 50000.0
  path_reference_l_weight: 10.0
  lateral_derivative_bound_default: 5.0
}

主车道跟车路径参数优化

配置路径：

modules/planning/tasks/lane_follow_path/conf/default_conf.pb.txt

is_extend_lane_bounds_to_include_adc: false
extend_buffer: 0.2
path_optimizer_config {
  l_weight: 3.0
  dl_weight: 15.0
  ddl_weight: 100.0
  dddl_weight: 1700.0
  lateral_derivative_bound_default: 2.0
  path_reference_l_weight:100
}

调整速度曲线的响应策略

模块路径：

modules/planning/tasks/piecewise_jerk_speed/conf/default_conf.pb.txt

acc_weight: 200.0
jerk_weight: 0.01
kappa_penalty_weight: 0.0
ref_s_weight: 10.0
ref_v_weight: 20.0
follow_distance_buffer: 0.1

在部分场景中，若规划轨迹过于保守或存在明显缓慢响应，可以直接对其进行偏移操作

验证效果

线下国赛

赛题一 障碍物绕行场景

赛题描述

主车行驶过程中，前方监测到静态障碍物后借道绕行。距离目标障碍物横向距离至少保持 1 米，借道避障限速不得超过3m/s

评分标准

主车与目标障碍物横向距离少于1米，本场景分扣20分。速度每超出1m/s的速度，场景每帧扣2分。

解题思路

和预选一样

解题过程

效果验证

赛题二 动态障碍物场景

赛题描述

当主车前方出现机动车和非机动车时，若前车速度大于3 m/s 则跟随前车行驶，前车速度小于3m/s，通过左侧车道对前车绕行。主车跟车行驶，当前方道路存在人行道时且有行人在人行道上通行，车辆需要停车避让，为了保证安全，主车需要与前车保持2-2.5米的停车距离，等待行人完全通过人行道时再通过该路口；当有行人在人行道上通行时主车应在障碍车后方停车，不得借道绕行

评分标准

当前车速度大于3 m/s 跟随前车行驶，当前车速度小于3m/s，对前车绕行。当遇到前方障碍物车辆遇到行人通过人行道时，会减速停车停止在停止线前，主车遇到这种情况时，应在障碍车后方停车，不得借道绕行，如绕行该场景得0分；主车距离前车大于2.5m，或小于2m，本场景扣20分

解题思路

将低速车辆视为“可绕行的静态障碍物”处理；

对障碍物速度阈值与状态分类进行调整；

精细控制借道条件与绕行逻辑，确保满足交通规则；

加入对行人、人行道等特殊场景的保护判断，防止误绕行。

解题过程

修改障碍物阻挡判定逻辑

修改文件：

modules/planning/planning_base/common/obstacle_blocking_analyzer.cc

注释掉以下段落中关于“车辆类型跳过”的判断逻辑，使车辆也能参与静态障碍物的阻挡判断：

  for (const auto* other_obstacle :
       reference_line_info.path_decision().obstacles().Items()) {
    if (other_obstacle->Id() == obstacle.Id()) {
      continue;
    }
    if (other_obstacle->IsVirtual()) {
      continue;
    }
    if (other_obstacle->Perception().type() !=
        apollo::perception::PerceptionObstacle::VEHICLE) {
      continue;
    }
    const auto& other_boundary = other_obstacle->PerceptionSLBoundary();
    const auto& this_boundary = obstacle.PerceptionSLBoundary();
    if (other_boundary.start_l() > this_boundary.end_l() ||
        other_boundary.end_l() < this_boundary.start_l()) {
      // not blocking the backside vehicle
      continue;
    }
    double delta_s = other_boundary.start_s() - this_boundary.end_s();
    if (delta_s < 0.0 || delta_s > kObstaclesDistanceThreshold) {
      continue;
    }
    return false;
  }

重定义静态障碍物判定

修改配置：

modules/planning/planning_component/conf/planning.conf

添加如下参数，将障碍物“静止阈值”设为接近 3m/s：

--max_stop_distance_obstacle=2.0
--min_stop_distance_obstacle=0.0

--static_obstacle_speed_threshold=2.95

对应的速度也要拉高

--planning_upper_speed_limit=16.5

--default_cruise_speed=16.5

放宽路径边界限制

在模块：

modules/planning/planning_interface_base/task_base/common/path_util/path_bounds_decider_util.cc

移除路径边界构造时对“完全静止障碍物”的判断限制，允许更早识别低速车辆为阻挡体。

优化绕行借道逻辑

在借道模块：

modules/planning/tasks/lane_borrow_path/lane_borrow_path.cc

和前面一样bool LaneBorrowPath::IsBlockingObstacleFarFromIntersection(添加ReferenceLineInfo::CROSSWALK跳过条件

同时

替换LaneBorrowPath::IsNecessaryToBorrowLane()为

bool LaneBorrowPath::IsNecessaryToBorrowLane() {
  auto* mutable_path_decider_status = injector_->planning_context()
                                          ->mutable_planning_status()
                                          ->mutable_path_decider();
  if (mutable_path_decider_status->is_in_path_lane_borrow_scenario()) {
    UpdateSelfPathInfo();
    // If originally borrowing neighbor lane:
    if (use_self_lane_ >= 6) {
      // If have been able to use self-lane for some time, then switch to
      // non-lane-borrowing.
      mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(false);
      decided_side_pass_direction_.clear();
      AINFO << "Switch from LANE-BORROW path to SELF-LANE path.";
    }
  } else {
    // If originally not borrowing neighbor lane:
    AINFO << "Blocking obstacle ID["
          << mutable_path_decider_status->front_static_obstacle_id() << "]";
    // ADC requirements check for lane-borrowing:
    if (!HasSingleReferenceLine(*frame_)) {
        AINFO << "Has multiple reference lines.";
      return false;
    }
    if (!IsWithinSidePassingSpeedADC(*frame_)) {
        AINFO << "Not within side-passing speed.";
      return false;
    }
    // find the distance to crosswalk
    for (const auto& crosswalk_overlap :
         reference_line_info_->reference_line().map_path().crosswalk_overlaps()) {
        AINFO << "Crosswalk overlap: " << crosswalk_overlap.DebugString();
        double distance = crosswalk_overlap.start_s - reference_line_info_->AdcSlBoundary().end_s();
        AINFO << "Distance to crosswalk: " << distance;
        if (distance < 50.0) {
            AINFO << "Too close to crosswalk.";
            return false;
        }
    }

    // find there is a car speed at 2.9-3.0
    for (auto obstacle: frame_->obstacles()) {
        // vec3d
        auto velocity = obstacle->Perception().velocity();
        // norm
        double speed = std::sqrt(velocity.x() * velocity.x() + velocity.y() * velocity.y());
        if (speed >= 2.85 && speed <= 3.0) {
            AINFO << "Find a car speed at 2.9-3.0";
            goto can_borrow;
        }
    }

    // Obstacle condition check for lane-borrowing:
    if (!IsBlockingObstacleFarFromIntersection(*reference_line_info_)) {
        AINFO << "Blocking obstacle is too close to intersection.";
      return false;
    }
    if (!IsLongTermBlockingObstacle()) {
        AINFO << "Blocking obstacle is not long-term.";
      return false;
    }
    if (!IsBlockingObstacleWithinDestination(*reference_line_info_)) {
        AINFO << "Blocking obstacle is within destination.";
      return false;
    }
    // if (!IsSidePassableObstacle(*reference_line_info_)) {
    //  AINFO << "Blocking obstacle is not side-passable.";
    //   return false;
    // }
can_borrow:
    // switch to lane-borrowing
    if (decided_side_pass_direction_.empty()) {
      // first time init decided_side_pass_direction
      bool left_borrowable;
      bool right_borrowable;
      CheckLaneBorrow(*reference_line_info_, &left_borrowable,
                      &right_borrowable);
      if (!left_borrowable && !right_borrowable) {
        mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(false);
        AINFO << "LEFT AND RIGHT LANE CAN NOT BORROW";
        return false;
      } else {
        mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(true);
        if (left_borrowable) {
          decided_side_pass_direction_.push_back(
              SidePassDirection::LEFT_BORROW);
        }
        if (right_borrowable) {
          decided_side_pass_direction_.push_back(
              SidePassDirection::RIGHT_BORROW);
        }
      }
    }
    use_self_lane_ = 0;
    AINFO << "Switch from SELF-LANE path to LANE-BORROW path.";
  }
  return mutable_path_decider_status->is_in_path_lane_borrow_scenario();
}

效果验证

赛题三 车辆靠边启动场景

赛题描述

当主车准备启动，若检测到泊车位旁有其他车辆通行，为避免碰撞，主车应保持停车状态，让行其他车辆，直至通行无阻后方可继续行驶。同时，主车在道路旁启动时，若前方或前后方道路因障碍物堵塞，无法正常通行，主车应绕行障碍物以安全到达目的地。

评分标准

主车在规定时间内未完成相关任务，本场景分计0分

解题思路

本题涉及到openspace算法 与常规线性分析流程相比不同 需要对openspace流程有较深的理解 和 有较强的调试能力

解题过程

具体解题方法不再给出

赛题四 减速通行场景

赛题描述

主车在通过减速带时需降低车辆速度至4米/秒通过；在通过交汇路口、施工区域时，需降低车辆速度至5米/秒通过

评分标准

若主车未在指定区域内限速，每超出1m/s的速度，场景每帧扣2分

解题思路

本场景核心是对不同区域（如减速带、路口、施工区域）动态设定速度限制。通过在参考线 (ReferenceLine) 上调用限速 API，根据地图元素（Overlap）的位置进行速度管理，从而满足赛题中的限制要求。

解题过程

修改模块

模块路径：

modules/planning/traffic_rules/crosswalk/crosswalk.cc

Status Crosswalk::ApplyRule(Frame* const frame,
                            ReferenceLineInfo* const reference_line_info) {
  CHECK_NOTNULL(frame);
  CHECK_NOTNULL(reference_line_info);
   ReferenceLine* reference_line = reference_line_info->mutable_reference_line();
    const auto& map_path = reference_line_info->reference_line().map_path();
    for (const auto& junction_overlap : map_path.junction_overlaps()) {
        reference_line->AddSpeedLimit(
                junction_overlap.start_s - 5,
                junction_overlap.end_s + 2,
                3.8);
    }
      for (const auto& speed_bump : map_path.speed_bump_overlaps()) {
        reference_line->AddSpeedLimit(
                speed_bump.start_s - 8,
                speed_bump.end_s + 2,
                3.8);
    }
    for (const auto& pnc_junction_overlap : map_path.pnc_junction_overlaps()) {
        reference_line->AddSpeedLimit(
                pnc_junction_overlap.start_s - 5,
                pnc_junction_overlap.end_s + 2,
                3.8);
    }
    // for every obstacle
    for (const auto* obstacle : frame->obstacles()) {
        if (obstacle->IsVirtual() || !obstacle->IsStatic()) {
            continue;
        }

        auto bbx = obstacle->PerceptionBoundingBox();
        auto corners = bbx.GetAllCorners();
        std::vector<apollo::common::SLPoint> sl_points;
        for (const auto& corner : corners) {
            apollo::common::SLPoint sl_point;
            reference_line->XYToSL(corner, &sl_point);
            sl_points.push_back(sl_point);
        }
        // min and max
        double start_s = std::min_element(sl_points.begin(), sl_points.end(),
                                     [](const apollo::common::SLPoint& a, const apollo::common::SLPoint& b) {
                                         return a.s() < b.s();
                                     })->s();
        double end_s = std::max_element(sl_points.begin(), sl_points.end(),
                                      [](const apollo::common::SLPoint& a, const apollo::common::SLPoint& b) {
                                        return a.s() < b.s();
                                      })->s();
        reference_line->AddSpeedLimit(start_s - 10,
                                        end_s + 10,
                                        2.5);
    }

  if (!FindCrosswalks(reference_line_info)) {
    injector_->planning_context()->mutable_planning_status()->clear_crosswalk();
    return Status::OK();
  }

  MakeDecisions(frame, reference_line_info);
  return Status::OK();
}

效果验证