Planning: OpenSpace: add IsTransferable in valet parking scenario

modules/planning/proto/planning_config.proto

@@ -136,7 +136,9 @@ message ScenarioTrafficLightUnprotectedRightTurnConfig {
   optional float creep_timeout = 6 [default = 10.0];  // sec
 }
 
-message ScenarioValetParkingConfig {}
+message ScenarioValetParkingConfig {
+  optional double parking_spot_range_to_start = 1 [default = 20.0];
+}
 
 message ScenarioNarrowStreetUTurnConfig {}
 

modules/planning/scenarios/valet_parking/valet_parking_scenario.cc

@@ -17,21 +17,31 @@
 /**
  * @file
  **/
-#include "modules/planning/scenarios/valet_parking/valet_parking_scenario.h"
-#include "modules/planning/scenarios/valet_parking/stage_parking.h"
-#include "modules/planning/scenarios/valet_parking/stage_approaching_parking_spot.h"
 
+#include <vector>
 
+#include "modules/planning/scenarios/valet_parking/valet_parking_scenario.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 {
 namespace scenario {
 namespace valet_parking {
 
+using apollo::common::Status;
+using apollo::common::VehicleState;
+using apollo::common::math::Box2d;
+using apollo::common::math::Vec2d;
+using apollo::hdmap::HDMapUtil;
+using apollo::hdmap::LaneSegment;
+using apollo::hdmap::ParkingSpaceInfoConstPtr;
+using apollo::hdmap::Path;
+
 apollo::common::util::Factory<
-      ScenarioConfig::StageType, Stage,
-      Stage* (*)(const ScenarioConfig::StageConfig& stage_config)>
-      ValetParkingScenario::s_stage_factory_;
+    ScenarioConfig::StageType, Stage,
+    Stage* (*)(const ScenarioConfig::StageConfig& stage_config)>
+    ValetParkingScenario::s_stage_factory_;
 
 void ValetParkingScenario::Init() {
   if (init_) {
@@ -44,6 +54,9 @@ void ValetParkingScenario::Init() {
     AERROR << "fail to get scenario specific config";
     return;
   }
+
+  hdmap_ = hdmap::HDMapUtil::BaseMapPtr();
+  CHECK_NOTNULL(hdmap_);
 }
 
 void ValetParkingScenario::RegisterStages() {
@@ -86,9 +99,124 @@ bool ValetParkingScenario::GetScenarioConfig() {
 
 bool ValetParkingScenario::IsTransferable(const Scenario& current_scenario,
                                           const Frame& frame) {
+  // TODO(all) Implement avaliable parking spot detection by preception results
+  context_.target_parking_spot_id.clear();
+  if (frame.local_view().routing->routing_request().has_parking_space() &&
+      frame.local_view().routing->routing_request().parking_space().has_id()) {
+    context_.target_parking_spot_id =
+        frame.local_view().routing->routing_request().parking_space().id().id();
+  } else {
+    const std::string msg = "No parking space id from routing";
+    AERROR << msg;
+    return false;
+  }
+
+  if (context_.target_parking_spot_id.empty()) {
+    return false;
+  }
+
+  ParkingSpaceInfoConstPtr target_parking_spot = nullptr;
+  Path nearby_path;
+  const auto& vehicle_state = frame.vehicle_state();
+  auto point = common::util::MakePointENU(vehicle_state.x(), vehicle_state.y(),
+                                          vehicle_state.z());
+  hdmap::LaneInfoConstPtr nearest_lane;
+  double vehicle_lane_s = 0.0;
+  double vehicle_lane_l = 0.0;
+  int status = HDMapUtil::BaseMap().GetNearestLaneWithHeading(
+      point, 5.0, vehicle_state.heading(), M_PI / 3.0, &nearest_lane,
+      &vehicle_lane_s, &vehicle_lane_l);
+  if (status != 0) {
+    AERROR << "GetNearestLaneWithHeading failed at IsTransferable() of valet "
+              "parking scenario";
+    return false;
+  }
+  // TODO(Jinyun) Take path from reference line
+  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) {
+    for (int i = 0; i < next_lanes_num; ++i) {
+      auto next_lane_id = nearest_lane->lane().successor_id(i);
+      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.emplace_back(next_lanesegment);
+      nearby_path = Path(segments_vector);
+      SearchTargetParkingSpotOnPath(nearby_path, &target_parking_spot);
+      if (target_parking_spot != nullptr) {
+        break;
+      }
+    }
+  } else {
+    segments_vector.push_back(nearest_lanesegment);
+    nearby_path = Path(segments_vector);
+    SearchTargetParkingSpotOnPath(nearby_path, &target_parking_spot);
+  }
+
+  if (target_parking_spot == nullptr) {
+    std::string msg(
+        "No such parking spot found after searching all path forward possible");
+    AERROR << msg << context_.target_parking_spot_id;
+    return false;
+  }
+
+  if (!CheckDistanceToParkingSpot(vehicle_state, nearby_path,
+                                  target_parking_spot)) {
+    std::string msg(
+        "target parking spot found, but too far, distance larger than "
+        "pre-defined distance");
+    AERROR << msg << context_.target_parking_spot_id;
+    return false;
+  }
+
   return true;
 }
 
+void ValetParkingScenario::SearchTargetParkingSpotOnPath(
+    const Path& nearby_path, ParkingSpaceInfoConstPtr* target_parking_spot) {
+  const auto& parking_space_overlaps = nearby_path.parking_space_overlaps();
+  if (parking_space_overlaps.size() != 0) {
+    for (const auto& parking_overlap : parking_space_overlaps) {
+      if (parking_overlap.object_id == context_.target_parking_spot_id) {
+        hdmap::Id id;
+        id.set_id(parking_overlap.object_id);
+        *target_parking_spot = hdmap_->GetParkingSpaceById(id);
+      }
+    }
+  }
+}
+
+bool ValetParkingScenario::CheckDistanceToParkingSpot(
+    const VehicleState& vehicle_state, const Path& nearby_path,
+    const ParkingSpaceInfoConstPtr& target_parking_spot) {
+  Vec2d left_bottom_point = target_parking_spot->polygon().points().at(0);
+  Vec2d right_bottom_point = target_parking_spot->polygon().points().at(1);
+  double left_bottom_point_s = 0.0;
+  double left_bottom_point_l = 0.0;
+  double right_bottom_point_s = 0.0;
+  double right_bottom_point_l = 0.0;
+  double vehicle_point_s = 0.0;
+  double vehicle_point_l = 0.0;
+  nearby_path.GetNearestPoint(left_bottom_point, &left_bottom_point_s,
+                              &left_bottom_point_l);
+  nearby_path.GetNearestPoint(right_bottom_point, &right_bottom_point_s,
+                              &right_bottom_point_l);
+  Vec2d vehicle_vec(vehicle_state.x(), vehicle_state.y());
+  nearby_path.GetNearestPoint(vehicle_vec, &vehicle_point_s, &vehicle_point_l);
+  if (std::abs((left_bottom_point_s + right_bottom_point_s) / 2 -
+               vehicle_point_s) <
+      context_.scenario_config.parking_spot_range_to_start()) {
+    return true;
+  } else {
+    return false;
+  }
+}
+
 }  // namespace valet_parking
 }  // namespace scenario
 }  // namespace planning

modules/planning/scenarios/valet_parking/valet_parking_scenario.h

@@ -23,6 +23,10 @@
 #include <memory>
 #include <string>
 
+#include "modules/map/hdmap/hdmap_util.h"
+#include "modules/map/pnc_map/path.h"
+#include "modules/map/pnc_map/pnc_map.h"
+#include "modules/map/proto/map_id.pb.h"
 #include "modules/planning/scenarios/scenario.h"
 
 namespace apollo {
@@ -54,6 +58,12 @@ class ValetParkingScenario : public Scenario {
  private:
   static void RegisterStages();
   bool GetScenarioConfig();
+  void SearchTargetParkingSpotOnPath(
+      const hdmap::Path& nearby_path,
+      hdmap::ParkingSpaceInfoConstPtr* target_parking_spot);
+  bool CheckDistanceToParkingSpot(
+      const common::VehicleState& vehicle_state, const hdmap::Path& nearby_path,
+      const hdmap::ParkingSpaceInfoConstPtr& target_parking_spot);
 
  private:
   bool init_ = false;
@@ -62,6 +72,7 @@ class ValetParkingScenario : public Scenario {
       Stage* (*)(const ScenarioConfig::StageConfig& stage_config)>
       s_stage_factory_;
   ValetParkingContext context_;
+  const hdmap::HDMap* hdmap_ = nullptr;
 };
 
 }  // namespace valet_parking

modules/planning/tasks/deciders/open_space_roi_decider.cc

@@ -101,9 +101,10 @@ bool OpenSpaceRoiDecider::VPresentationObstacle() {
     // last to form closed convex hull)
     for (const auto &obstacle : obstacles_by_frame_->Items()) {
       Box2d original_box = obstacle->PerceptionBoundingBox();
-      original_box.Shift(-1.0 *
-          *(frame_->mutable_open_space_info()->mutable_origin_point()));
-      original_box.RotateFromCenter(-1.0 *
+      original_box.Shift(
+          -1.0 * *(frame_->mutable_open_space_info()->mutable_origin_point()));
+      original_box.RotateFromCenter(
+          -1.0 *
           *(frame_->mutable_open_space_info()->mutable_origin_heading()));
       std::vector<Vec2d> vertices_ccw = original_box.GetAllCorners();
       std::vector<Vec2d> vertices_cw;
@@ -164,8 +165,7 @@ bool OpenSpaceRoiDecider::HPresentationObstacle() {
 }
 
 bool OpenSpaceRoiDecider::ObsHRep(
-    const size_t &obstacles_num,
-    const Eigen::MatrixXi &obstacles_edges_num,
+    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()) {
@@ -464,6 +464,7 @@ void OpenSpaceRoiDecider::SearchTargetParkingSpotOnPath(
   }
 }
 
+// TODO(Jinyun) Deprecate because of duplicate code in valet parking scenario
 bool OpenSpaceRoiDecider::GetMapInfo(
     ParkingSpaceInfoConstPtr *target_parking_spot,
     std::shared_ptr<Path> *nearby_path) {
@@ -473,16 +474,14 @@ bool OpenSpaceRoiDecider::GetMapInfo(
   double vehicle_lane_s = 0.0;
   double vehicle_lane_l = 0.0;
   int status = HDMapUtil::BaseMap().GetNearestLaneWithHeading(
-      point, 10.0, vehicle_state_.heading(),
-      M_PI / 2.0, &nearest_lane,
+      point, 10.0, vehicle_state_.heading(), M_PI / 2.0, &nearest_lane,
       &vehicle_lane_s, &vehicle_lane_l);
   if (status != 0) {
     AERROR << "Getlane failed at OpenSpaceRoiDecider::GetOpenSpaceROI()";
     return false;
   }
   LaneSegment nearest_lanesegment =
-      LaneSegment(nearest_lane,
-                  nearest_lane->accumulate_s().front(),
+      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();
@@ -492,8 +491,7 @@ bool OpenSpaceRoiDecider::GetMapInfo(
       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(),
+          LaneSegment(next_lane, next_lane->accumulate_s().front(),
                       next_lane->accumulate_s().back());
       segments_vector.emplace_back(next_lanesegment);
       (*nearby_path).reset(new Path(segments_vector));