Planning: add pull over stop in open space pre stop decider

modules/planning/common/open_space_info.h

@@ -65,14 +65,6 @@ class OpenSpaceInfo {
   OpenSpaceInfo() = default;
   ~OpenSpaceInfo() = default;
 
-  bool open_space_pre_stop_finished() const {
-    return open_space_pre_stop_finished_;
-  }
-
-  void set_open_space_pre_stop_finished(const bool flag) {
-    open_space_pre_stop_finished_ = flag;
-  }
-
   const std::string target_parking_spot_id() const {
     return target_parking_spot_id_;
   }
@@ -308,9 +300,6 @@ class OpenSpaceInfo {
   void RecordDebug(apollo::planning_internal::Debug *ptr_debug);
 
  private:
-  // @brief vehicle needs to stop first in open space related scenarios
-  bool open_space_pre_stop_finished_ = true;
-
   std::string target_parking_spot_id_;
 
   hdmap::ParkingSpaceInfoConstPtr target_parking_spot_ = nullptr;

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

@@ -25,6 +25,9 @@ stage_config: {
     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

modules/planning/proto/BUILD

@@ -159,6 +159,7 @@ proto_library(
         ":navi_path_decider_config_proto_lib",
         ":navi_speed_decider_config_proto_lib",
         ":open_space_fallback_decider_config_proto_lib",
+        ":open_space_pre_stop_decider_config_proto_lib",
         ":open_space_roi_decider_config_proto_lib",
         ":open_space_trajectory_partition_config_proto_lib",
         ":open_space_trajectory_provider_config_proto_lib",
@@ -537,6 +538,20 @@ cc_proto_library(
     ],
 )
 
+proto_library(
+    name = "open_space_pre_stop_decider_config_proto_lib",
+    srcs = [
+        "open_space_pre_stop_decider_config.proto",
+    ],
+)
+
+cc_proto_library(
+    name = "open_space_pre_stop_decider_config_proto",
+    deps = [
+        ":open_space_pre_stop_decider_config_proto_lib",
+    ],
+)
+
 cc_proto_library(
     name = "path_decider_info_proto",
     deps = [

modules/planning/proto/decider_config.proto

@@ -17,11 +17,6 @@ message CreepDeciderConfig {
   optional double ignore_min_st_min_s = 6 [default = 15.0];  // meter
 }
 
-message OpenSpacePreStopDeciderConfig {
-  optional double rightaway_stop_distance = 1 [default = 2.0];  // meter
-  optional double stop_distance_to_target = 2 [default = 5.0];  // second
-}
-
 message SidePassSafetyConfig {
   // min lateral distance(m) for safe obstacle
   optional double min_obstacle_lateral_distance = 1 [default = 1.0];  // meter

modules/planning/proto/open_space_fallback_decider_config.proto

@@ -9,4 +9,4 @@ message OpenSpaceFallBackDeciderConfig {
   optional double open_space_fall_back_collision_distance = 2 [default = 5.0];
   // fallback stop trajectory safety gap to obstacles
   optional double open_space_fall_back_stop_distance = 3 [default = 2.0];
-}
\ No newline at end of file
+}

modules/planning/proto/open_space_pre_stop_decider_config.proto

+syntax = "proto2";
+
+package apollo.planning;
+
+message OpenSpacePreStopDeciderConfig {
+  // roi scenario definitions
+  enum StopType {
+    NOT_DEFINED = 0;
+    PARKING = 1;
+    PULL_OVER = 2;
+    NARROW_STREET_U_TURN = 3;
+  }
+  optional StopType stop_type = 1;
+  optional double rightaway_stop_distance = 2 [default = 2.0];  // meter
+  optional double stop_distance_to_target = 3 [default = 5.0];  // second
+}

modules/planning/proto/planning_config.proto

@@ -6,6 +6,7 @@ import "modules/planning/proto/decider_config.proto";
 import "modules/planning/proto/dp_st_speed_config.proto";
 import "modules/planning/proto/lane_change_decider_config.proto";
 import "modules/planning/proto/open_space_fallback_decider_config.proto";
+import "modules/planning/proto/open_space_pre_stop_decider_config.proto";
 import "modules/planning/proto/open_space_roi_decider_config.proto";
 import "modules/planning/proto/open_space_trajectory_provider_config.proto";
 import "modules/planning/proto/open_space_trajectory_partition_config.proto";

modules/planning/scenarios/park/valet_parking/stage_approaching_parking_spot.cc

@@ -50,8 +50,6 @@ Stage::StageStatus StageApproachingParkingSpot::Process(
     return StageStatus::ERROR;
   }
 
-  frame->mutable_open_space_info()->set_open_space_pre_stop_finished(
-      GetContext()->valet_parking_pre_stop_finished);
   *(frame->mutable_open_space_info()->mutable_target_parking_spot_id()) =
       GetContext()->target_parking_spot_id;
   frame->mutable_open_space_info()->set_pre_stop_rightaway_flag(
@@ -100,8 +98,6 @@ bool StageApproachingParkingSpot::CheckADCStop(const Frame& frame) {
     ADEBUG << "not a valid stop. too far from stop line.";
     return false;
   }
-
-  GetContext()->valet_parking_pre_stop_finished = true;
   return true;
 }
 

modules/planning/scenarios/park/valet_parking/valet_parking_scenario.h

@@ -37,7 +37,6 @@ namespace valet_parking {
 struct ValetParkingContext {
   ScenarioValetParkingConfig scenario_config;
   std::string target_parking_spot_id;
-  bool valet_parking_pre_stop_finished = false;
   bool pre_stop_rightaway_flag = false;
   hdmap::MapPathPoint pre_stop_rightaway_point;
 };

modules/planning/tasks/deciders/open_space_decider/open_space_pre_stop_decider.cc

@@ -26,12 +26,14 @@
 
 #include "modules/common/vehicle_state/vehicle_state_provider.h"
 #include "modules/map/pnc_map/path.h"
+#include "modules/planning/common/planning_context.h"
 #include "modules/planning/common/util/common.h"
 
 namespace apollo {
 namespace planning {
 
 using apollo::common::Status;
+using apollo::common::ErrorCode;
 using apollo::common::VehicleState;
 using apollo::common::math::Vec2d;
 using apollo::hdmap::ParkingSpaceInfoConstPtr;
@@ -46,27 +48,54 @@ Status OpenSpacePreStopDecider::Process(
     Frame* frame, ReferenceLineInfo* reference_line_info) {
   CHECK_NOTNULL(frame);
   CHECK_NOTNULL(reference_line_info);
-
-  CheckOpenSpacePreStop(frame, reference_line_info);
-
+  open_space_pre_stop_decider_config_ =
+      config_.open_space_pre_stop_decider_config();
+  double target_s = 0.0;
+  const auto& stop_type = open_space_pre_stop_decider_config_.stop_type();
+  switch (stop_type) {
+    case OpenSpacePreStopDeciderConfig::PARKING:
+      if (!CheckParkingSpotPreStop(frame, reference_line_info, &target_s)) {
+        const std::string msg = "Checking parking spot pre stop fails";
+        AERROR << msg;
+        return Status(ErrorCode::PLANNING_ERROR, msg);
+      }
+      break;
+    case OpenSpacePreStopDeciderConfig::PULL_OVER:
+      if (!CheckPullOverPreStop(frame, reference_line_info, &target_s)) {
+        const std::string msg = "Checking pull over pre stop fails";
+        AERROR << msg;
+        return Status(ErrorCode::PLANNING_ERROR, msg);
+      }
+      break;
+    default:
+      const std::string msg = "This stop type not implemented";
+      AERROR << msg;
+      return Status(ErrorCode::PLANNING_ERROR, msg);
+  }
+  SetStopFence(target_s, frame, reference_line_info);
   return Status::OK();
 }
 
-void OpenSpacePreStopDecider::CheckOpenSpacePreStop(
-    Frame* const frame, ReferenceLineInfo* const reference_line_info) {
-  const auto& open_space_config = config_.open_space_pre_stop_decider_config();
-
-  if (frame->open_space_info().open_space_pre_stop_finished()) {
-    return;
-  }
+bool OpenSpacePreStopDecider::CheckPullOverPreStop(
+    Frame* const frame, ReferenceLineInfo* const reference_line_info,
+    double* target_s) {
+  const auto& pull_over_info =
+      PlanningContext::Instance()->planning_status().pull_over();
+  const auto& pull_over_s = pull_over_info.pull_over_s();
+  *target_s = pull_over_s;
+  return true;
+}
 
-  const double adc_front_edge_s = reference_line_info->AdcSlBoundary().end_s();
-  const VehicleState& vehicle_state = frame->vehicle_state();
+bool OpenSpacePreStopDecider::CheckParkingSpotPreStop(
+    Frame* const frame, ReferenceLineInfo* const reference_line_info,
+    double* target_s) {
   const auto& target_parking_spot_id =
       frame->open_space_info().target_parking_spot_id();
   const auto& nearby_path = reference_line_info->reference_line().map_path();
-  AERROR_IF(target_parking_spot_id.empty())
-      << "no target parking spot id found when setting pre stop fence";
+  if (target_parking_spot_id.empty()) {
+    AERROR << "no target parking spot id found when setting pre stop fence";
+    return false;
+  }
 
   double target_area_center_s = 0.0;
   bool target_area_found = false;
@@ -97,24 +126,38 @@ void OpenSpacePreStopDecider::CheckOpenSpacePreStop(
       target_area_found = true;
     }
   }
-  AERROR_IF(!target_area_found)
-      << "no target parking spot found on reference line";
 
+  if (!target_area_found) {
+    AERROR << "no target parking spot found on reference line";
+    return false;
+  }
+  *target_s = target_area_center_s;
+  return true;
+}
+
+void OpenSpacePreStopDecider::SetStopFence(
+    const double target_s, Frame* const frame,
+    ReferenceLineInfo* const reference_line_info) {
+  const auto& nearby_path = reference_line_info->reference_line().map_path();
+  const double adc_front_edge_s = reference_line_info->AdcSlBoundary().end_s();
+  const VehicleState& vehicle_state = frame->vehicle_state();
   double stop_line_s = 0.0;
-  double stop_distance_to_target = open_space_config.stop_distance_to_target();
+  double stop_distance_to_target =
+      open_space_pre_stop_decider_config_.stop_distance_to_target();
   double static_linear_velocity_epsilon = 1.0e-2;
   CHECK_GE(stop_distance_to_target, 1.0e-8);
-  double target_vehicle_offset = target_area_center_s - adc_front_edge_s;
+  double target_vehicle_offset = target_s - adc_front_edge_s;
   if (target_vehicle_offset > stop_distance_to_target) {
-    stop_line_s = target_area_center_s - stop_distance_to_target;
+    stop_line_s = target_s - stop_distance_to_target;
   } else if (std::abs(target_vehicle_offset) < stop_distance_to_target) {
-    stop_line_s = target_area_center_s + stop_distance_to_target;
+    stop_line_s = target_s + stop_distance_to_target;
   } else if (target_vehicle_offset < -stop_distance_to_target) {
     if (!frame->open_space_info().pre_stop_rightaway_flag()) {
       // TODO(Jinyun) Use constant comfortable deacceleration rather than
       // distance by config to set stop fence
       stop_line_s =
-          adc_front_edge_s + open_space_config.rightaway_stop_distance();
+          adc_front_edge_s +
+          open_space_pre_stop_decider_config_.rightaway_stop_distance();
       if (std::abs(vehicle_state.linear_velocity()) <
           static_linear_velocity_epsilon) {
         stop_line_s = adc_front_edge_s;
@@ -132,8 +175,7 @@ void OpenSpacePreStopDecider::CheckOpenSpacePreStop(
     }
   }
 
-  const std::string stop_wall_id =
-      OPEN_SPACE_VO_ID_PREFIX + target_parking_spot_id;
+  const std::string stop_wall_id = OPEN_SPACE_STOP_ID;
   std::vector<std::string> wait_for_obstacles;
   frame->mutable_open_space_info()->set_open_space_pre_stop_fence_s(
       stop_line_s);

modules/planning/tasks/deciders/open_space_decider/open_space_pre_stop_decider.h

@@ -20,7 +20,7 @@
 
 #pragma once
 
-#include "modules/planning/proto/decider_config.pb.h"
+#include "modules/planning/proto/open_space_pre_stop_decider_config.pb.h"
 
 #include "cyber/common/macros.h"
 
@@ -39,11 +39,20 @@ class OpenSpacePreStopDecider : public Decider {
   apollo::common::Status Process(
       Frame* frame, ReferenceLineInfo* reference_line_info) override;
 
-  void CheckOpenSpacePreStop(Frame* const frame,
-                             ReferenceLineInfo* const reference_line_info);
+  bool CheckParkingSpotPreStop(Frame* const frame,
+                               ReferenceLineInfo* const reference_line_info,
+                               double* target_s);
+
+  bool CheckPullOverPreStop(Frame* const frame,
+                            ReferenceLineInfo* const reference_line_info,
+                            double* target_s);
+
+  void SetStopFence(const double target_s, Frame* const frame,
+                    ReferenceLineInfo* const reference_line_info);
 
  private:
-  static constexpr const char* OPEN_SPACE_VO_ID_PREFIX = "OP_";
+  static constexpr const char* OPEN_SPACE_STOP_ID = "OPEN_SPACE_PRE_STOP";
+  OpenSpacePreStopDeciderConfig open_space_pre_stop_decider_config_;
 };
 
 }  // namespace planning

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

@@ -25,6 +25,9 @@ stage_config: {
     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
@@ -73,6 +76,9 @@ stage_config: {
     task_type: OPEN_SPACE_FALLBACK_DECIDER
     task_config: {
         task_type: OPEN_SPACE_ROI_DECIDER
+        open_space_roi_decider_config {
+            roi_type: PARKING
+        }
     }
     task_config: {
         task_type: OPEN_SPACE_TRAJECTORY_PROVIDER