feat: change openspace in stage_approaching_parking_spot

modules/common_msgs/planning_msgs/scenario_type.proto

@@ -78,7 +78,6 @@ enum StageType {
     EMERGENCY_STOP_STANDBY = 611;
 
     // valet parking scenario
-    VALET_PARKING_APPROACHING_PARKING_SPOT = 700;
     VALET_PARKING_PARKING = 701;
 
     // turning around scenario

modules/planning/common/util/common.cc

@@ -130,6 +130,44 @@ int BuildStopDecision(const std::string& stop_wall_id,
   return 0;
 }
 
+
+bool GetParkingSpotCenterFromRouting(
+    const Frame& frame, apollo::common::math::Vec2d* parking_spot_center) {
+  const auto& routing_request = frame.local_view().routing->routing_request();
+  if (!routing_request.has_parking_info()) {
+    return false;
+  }
+  const auto& corner_point = routing_request.parking_info().corner_point();
+  if (corner_point.point_size() <= 0) {
+    return false;
+  }
+  double center_x = 0.0;
+  double center_y = 0.0;
+  for (const auto& point : corner_point.point()) {
+    center_x += point.x();
+    center_y += point.y();
+  }
+  center_x /= static_cast<double>(corner_point.point_size());
+  center_y /= static_cast<double>(corner_point.point_size());
+  parking_spot_center->set_x(center_x);
+  parking_spot_center->set_y(center_y);
+  return true;
+}
+
+apollo::common::math::Vec2d GetParkingSpotCenterFromMap(
+    const apollo::hdmap::ParkingSpaceInfoConstPtr& target_parking_spot) {
+  const auto& points = target_parking_spot->polygon().points();
+  double center_x = 0.0;
+  double center_y = 0.0;
+  for (const auto& point : points) {
+    center_x += point.x();
+    center_y += point.y();
+  }
+  center_x /= static_cast<double>(points.size());
+  center_y /= static_cast<double>(points.size());
+  return apollo::common::math::Vec2d(center_x, center_y);
+}
+
 }  // namespace util
 }  // namespace planning
 }  // namespace apollo

modules/planning/common/util/common.h

@@ -40,6 +40,12 @@ int BuildStopDecision(const std::string& stop_wall_id,
                       const std::vector<std::string>& wait_for_obstacles,
                       const std::string& decision_tag, Frame* const frame,
                       ReferenceLineInfo* const reference_line_info);
+
+bool GetParkingSpotCenterFromRouting(
+    const Frame& frame, apollo::common::math::Vec2d* parking_spot_center);
+
+apollo::common::math::Vec2d GetParkingSpotCenterFromMap(
+    const apollo::hdmap::ParkingSpaceInfoConstPtr& target_parking_spot);
 }  // namespace util
 }  // namespace planning
 }  // namespace apollo

modules/planning/conf/scenario/valet_parking_config.pb.txt

@@ -4,72 +4,8 @@ valet_parking_config: {
   max_valid_stop_distance: 1.0
 }
 
-stage_type: VALET_PARKING_APPROACHING_PARKING_SPOT
 stage_type: VALET_PARKING_PARKING
 
-stage_config: {
-  stage_type: VALET_PARKING_APPROACHING_PARKING_SPOT
-  enabled: true
-  task_type: OPEN_SPACE_PRE_STOP_DECIDER
-  task_type: PATH_LANE_BORROW_DECIDER
-  task_type: PATH_BOUNDS_DECIDER
-  task_type: PIECEWISE_JERK_PATH_OPTIMIZER
-  task_type: PATH_ASSESSMENT_DECIDER
-  task_type: PATH_DECIDER
-  task_type: RULE_BASED_STOP_DECIDER
-  task_type: ST_BOUNDS_DECIDER
-  task_type: SPEED_BOUNDS_PRIORI_DECIDER
-  task_type: SPEED_HEURISTIC_OPTIMIZER
-  task_type: SPEED_DECIDER
-  task_type: SPEED_BOUNDS_FINAL_DECIDER
-  task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
-  task_config: {
-    task_type: OPEN_SPACE_PRE_STOP_DECIDER
-    open_space_pre_stop_decider_config {
-      stop_type: PARKING
-    }
-  }
-  task_config: {
-    task_type: PATH_LANE_BORROW_DECIDER
-    path_lane_borrow_decider_config {
-      allow_lane_borrowing: true
-    }
-  }
-  task_config: {
-    task_type: PATH_BOUNDS_DECIDER
-  }
-  task_config: {
-    task_type: PIECEWISE_JERK_PATH_OPTIMIZER
-  }
-  task_config: {
-    task_type: PATH_ASSESSMENT_DECIDER
-  }
-  task_config: {
-    task_type: PATH_DECIDER
-  }
-  task_config: {
-    task_type: SPEED_BOUNDS_PRIORI_DECIDER
-  }
-  task_config: {
-    task_type: SPEED_HEURISTIC_OPTIMIZER
-  }
-  task_config: {
-    task_type: SPEED_DECIDER
-  }
-  task_config: {
-    task_type: SPEED_BOUNDS_FINAL_DECIDER
-  }
-  task_config: {
-    task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
-  }
-  task_config: {
-    task_type: RULE_BASED_STOP_DECIDER
-  }
-  task_config: {
-    task_type: ST_BOUNDS_DECIDER
-  }
-}
-
 stage_config: {
   stage_type: VALET_PARKING_PARKING
   enabled: true

modules/planning/scenarios/park/valet_parking/BUILD

@@ -7,12 +7,10 @@ package(default_visibility = ["//visibility:public"])
 cc_library(
     name = "valet_parking",
     srcs = [
-        "stage_approaching_parking_spot.cc",
         "stage_parking.cc",
         "valet_parking_scenario.cc",
     ],
     hdrs = [
-        "stage_approaching_parking_spot.h",
         "stage_parking.h",
         "valet_parking_scenario.h",
     ],
@@ -21,6 +19,7 @@ cc_library(
         "//cyber",
         "//modules/common_msgs/planning_msgs:planning_cc_proto",
         "//modules/planning/scenarios:scenario",
+        "//modules/planning/common/util:common_lib",
     ],
 )
 
@@ -39,21 +38,6 @@ cc_test(
     ],
 )
 
-cc_test(
-    name = "stage_approaching_parking_spot_test",
-    size = "small",
-    srcs = ["stage_approaching_parking_spot_test.cc"],
-    data = [
-        "//modules/planning:planning_conf",
-    ],
-    linkopts = ["-lgomp"],
-    linkstatic = True,
-    deps = [
-        ":valet_parking",
-        "@com_google_googletest//:gtest_main",
-    ],
-)
-
 cc_test(
     name = "stage_parking_test",
     size = "small",