Planning: expand ADC width when trying to change lane.

modules/planning/tasks/dp_poly_path/dp_road_graph.cc

@@ -281,7 +281,7 @@ bool DPRoadGraph::SamplePathWaypoints(
       if (i == 0) {
         sample_l.push_back(init_sl_point_.l());
       } else {
-        sample_l.push_back(0);
+        sample_l.push_back(std::copysign(1.0, init_sl_point_.l()));
       }
     } else {
       common::util::uniform_slice(sample_right_boundary, sample_left_boundary,

modules/planning/tasks/dp_st_speed/dp_st_speed_optimizer.cc

@@ -70,10 +70,10 @@ Status DpStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
 
-  StBoundaryMapper boundary_mapper(adc_sl_boundary, st_boundary_config_,
-                                   reference_line, path_data,
-                                   dp_st_speed_config_.total_path_length(),
-                                   dp_st_speed_config_.total_time());
+  StBoundaryMapper boundary_mapper(
+      adc_sl_boundary, st_boundary_config_, reference_line, path_data,
+      dp_st_speed_config_.total_path_length(), dp_st_speed_config_.total_time(),
+      reference_line_info_->IsChangeLanePath());
 
   // step 1 get boundaries
   path_decision->EraseStBoundaries();

modules/planning/tasks/poly_st_speed/poly_st_speed_optimizer.cc

@@ -79,7 +79,8 @@ Status PolyStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
   StBoundaryMapper boundary_mapper(adc_sl_boundary, st_boundary_config_,
                                    reference_line, path_data,
                                    poly_st_speed_config_.total_path_length(),
-                                   poly_st_speed_config_.total_time());
+                                   poly_st_speed_config_.total_time(),
+                                   reference_line_info_->IsChangeLanePath());
 
   for (const auto* path_obstacle : path_decision->path_obstacles().Items()) {
     DCHECK(path_obstacle->HasLongitudinalDecision());

modules/planning/tasks/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc

@@ -75,10 +75,10 @@ Status QpSplineStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
     return Status(ErrorCode::PLANNING_ERROR, msg);
   }
 
-  StBoundaryMapper boundary_mapper(adc_sl_boundary, st_boundary_config_,
-                                   reference_line, path_data,
-                                   qp_st_speed_config_.total_path_length(),
-                                   qp_st_speed_config_.total_time());
+  StBoundaryMapper boundary_mapper(
+      adc_sl_boundary, st_boundary_config_, reference_line, path_data,
+      qp_st_speed_config_.total_path_length(), qp_st_speed_config_.total_time(),
+      reference_line_info_->IsChangeLanePath());
 
   for (const auto* path_obstacle : path_decision->path_obstacles().Items()) {
     DCHECK(path_obstacle->HasLongitudinalDecision());

modules/planning/tasks/st_graph/st_boundary_mapper.cc

@@ -61,14 +61,16 @@ StBoundaryMapper::StBoundaryMapper(const SLBoundary& adc_sl_boundary,
                                    const ReferenceLine& reference_line,
                                    const PathData& path_data,
                                    const double planning_distance,
-                                   const double planning_time)
+                                   const double planning_time,
+                                   bool is_change_lane)
     : adc_sl_boundary_(adc_sl_boundary),
       st_boundary_config_(config),
       reference_line_(reference_line),
       path_data_(path_data),
       vehicle_param_(common::VehicleConfigHelper::GetConfig().vehicle_param()),
       planning_distance_(planning_distance),
-      planning_time_(planning_time) {}
+      planning_time_(planning_time),
+      is_change_lane_(is_change_lane) {}
 
 Status StBoundaryMapper::GetGraphBoundary(PathDecision* path_decision) const {
   const auto& path_obstacles = path_decision->path_obstacles();
@@ -168,8 +170,8 @@ bool StBoundaryMapper::MapStopDecision(PathObstacle* stop_obstacle) const {
     if (!path_data_.GetPathPointWithRefS(stop_ref_s, &stop_point)) {
       AERROR << "Fail to get path point from reference s. The sl boundary of "
                 "stop obstacle "
-             << stop_obstacle->Id() << " is: "
-             << stop_obstacle->PerceptionSLBoundary().DebugString();
+             << stop_obstacle->Id()
+             << " is: " << stop_obstacle->PerceptionSLBoundary().DebugString();
       return false;
     }
 
@@ -407,13 +409,25 @@ Status StBoundaryMapper::MapWithPredictionTrajectory(
 bool StBoundaryMapper::CheckOverlap(const PathPoint& path_point,
                                     const Box2d& obs_box,
                                     const double buffer) const {
-  Vec2d vec_to_center = Vec2d((vehicle_param_.front_edge_to_center() -
-                               vehicle_param_.back_edge_to_center()) /
-                                  2.0,
-                              (vehicle_param_.left_edge_to_center() -
-                               vehicle_param_.right_edge_to_center()) /
-                                  2.0)
-                            .rotate(path_point.theta());
+  double left_delta_l = 0.0;
+  double right_delta_l = 0.0;
+  if (is_change_lane_) {
+    if ((adc_sl_boundary_.start_l() + adc_sl_boundary_.end_l()) / 2.0 > 0.0) {
+      // change to right
+      left_delta_l = 1.0;
+    } else {
+      // change to left
+      right_delta_l = 1.0;
+    }
+  }
+  Vec2d vec_to_center =
+      Vec2d((vehicle_param_.front_edge_to_center() -
+             vehicle_param_.back_edge_to_center()) /
+                2.0,
+            (vehicle_param_.left_edge_to_center() + left_delta_l -
+             vehicle_param_.right_edge_to_center() + right_delta_l) /
+                2.0)
+          .rotate(path_point.theta());
   Vec2d center = Vec2d(path_point.x(), path_point.y()) + vec_to_center;
 
   const Box2d adc_box =

modules/planning/tasks/st_graph/st_boundary_mapper.h

@@ -43,7 +43,7 @@ class StBoundaryMapper {
                    const StBoundaryConfig& config,
                    const ReferenceLine& reference_line,
                    const PathData& path_data, const double planning_distance,
-                   const double planning_time);
+                   const double planning_time, const bool is_change_lane);
 
   virtual ~StBoundaryMapper() = default;
 
@@ -90,6 +90,7 @@ class StBoundaryMapper {
   const apollo::common::VehicleParam& vehicle_param_;
   const double planning_distance_;
   const double planning_time_;
+  bool is_change_lane_ = false;
 };
 
 }  // namespace planning

modules/planning/tasks/st_graph/st_boundary_mapper_test.cc

@@ -85,7 +85,7 @@ TEST_F(StBoundaryMapperTest, check_overlap_test) {
   double planning_time = 10.0;
   SLBoundary adc_sl_boundary;
   StBoundaryMapper mapper(adc_sl_boundary, config, *reference_line_, path_data_,
-                          planning_distance, planning_time);
+                          planning_distance, planning_time, false);
   common::PathPoint path_point;
   path_point.set_x(1.0);
   path_point.set_y(1.0);
@@ -99,7 +99,7 @@ TEST_F(StBoundaryMapperTest, get_centric_acc_limit) {
   double planning_time = 8.0;
   SLBoundary adc_sl_boundary;
   StBoundaryMapper mapper(adc_sl_boundary, config, *reference_line_, path_data_,
-                          planning_distance, planning_time);
+                          planning_distance, planning_time, false);
 
   double kappa = 0.0001;
   while (kappa < 0.2) {