Planning : OpenSpace swithable in std_planning by gflag (#1399)

modules/planning/common/planning_gflags.cc

@@ -417,6 +417,9 @@ DEFINE_bool(enable_open_space_roi_and_info, false,
             "enable the frame to load map data and calculate info for open "
             "space planner");
 
+DEFINE_bool(enable_open_space_planner_thread, false,
+            "Enable thread in open space planner for trajectory publish.");
+
 DEFINE_bool(
     parking_inwards, false,
     "true for parking the car inwards in parking spot, false for parking "
@@ -436,6 +439,3 @@ DEFINE_bool(enable_record_debug, true,
 DEFINE_double(
     default_front_clear_distance, 300.0,
     "default front clear distance value in case there is no obstacle around.");
-
-DEFINE_bool(enable_open_space_planner_thread, true,
-            "Enable thread in open space planner for trajectory publish.");

modules/planning/common/planning_gflags.h

@@ -220,6 +220,7 @@ DECLARE_double(parking_start_range);
 DECLARE_bool(enable_perception_obstacles);
 DECLARE_bool(enable_open_space_roi_and_info);
 DECLARE_bool(parking_inwards);
+DECLARE_bool(enable_open_space_planner_thread);
 
 DECLARE_bool(use_osqp_optimizer_for_qp_st);
 DECLARE_bool(use_osqp_optimizer_for_reference_line);
@@ -228,5 +229,3 @@ DECLARE_bool(export_chart);
 DECLARE_bool(enable_record_debug);
 
 DECLARE_double(default_front_clear_distance);
-
-DECLARE_bool(enable_open_space_planner_thread);

modules/planning/conf/planning_config.pb.txt

 standard_planning_config {
   planner_type : PUBLIC_ROAD
+  planner_type : OPEN_SPACE
   planner_public_road_config {
      scenario_type : LANE_FOLLOW
      scenario_type : SIDE_PASS

modules/planning/planner/BUILD

@@ -39,7 +39,14 @@ cc_library(
         "//modules/planning/planner/navi:navi_planner",
         "//modules/planning/planner/public_road:public_road_planner",
         "//modules/planning/planner/rtk:rtk_planner",
+        "//modules/planning/planner/open_space:open_space_planner",
         "//modules/planning/proto:planning_proto",
+        # for open space planner
+        "//modules/planning/common:planning_gflags",
+        "//modules/map/pnc_map",
+        "//modules/common/math",
+        "//modules/planning/proto:planning_config_proto",
+        "//modules/common/vehicle_state:vehicle_state_provider",
     ],
 )
 

modules/planning/planner/planner_dispatcher.cc

@@ -18,10 +18,10 @@
 
 #include "modules/planning/proto/planning_config.pb.h"
 
+#include "modules/planning/planner/lattice/lattice_planner.h"
+#include "modules/planning/planner/open_space/open_space_planner.h"
 #include "modules/planning/planner/public_road/public_road_planner.h"
 #include "modules/planning/planner/rtk/rtk_replay_planner.h"
-#include "modules/planning/planner/lattice/lattice_planner.h"
-// #include "modules/planning/planner/open_space/open_space_planner.h"
 
 namespace apollo {
 namespace planning {
@@ -31,10 +31,10 @@ void PlannerDispatcher::RegisterPlanners() {
       RTK, []() -> Planner* { return new RTKReplayPlanner(); });
   planner_factory_.Register(
       PUBLIC_ROAD, []() -> Planner* { return new PublicRoadPlanner(); });
+  planner_factory_.Register(LATTICE,
+                            []() -> Planner* { return new LatticePlanner(); });
   planner_factory_.Register(
-      LATTICE, []() -> Planner* { return new LatticePlanner(); });
-  // planner_factory_.Register(
-  //     OPENSPACE, []() -> Planner* { return new OpenSpacePlanner(); });
+      OPEN_SPACE, []() -> Planner* { return new OpenSpacePlanner(); });
 }
 
 }  // namespace planning

modules/planning/planner/std_planner_dispatcher.cc

@@ -15,17 +15,103 @@
  *****************************************************************************/
 
 #include "modules/planning/planner/std_planner_dispatcher.h"
+
+// for open space planner
+#include <vector>
+
 #include "modules/common/util/file.h"
-#include "modules/planning/proto/planning_config.pb.h"
 #include "modules/planning/common/planning_gflags.h"
+#include "modules/planning/proto/planning_config.pb.h"
+
+// for open space planner
+#include "modules/common/math/vec2d.h"
+#include "modules/common/vehicle_state/proto/vehicle_state.pb.h"
+#include "modules/common/vehicle_state/vehicle_state_provider.h"
+#include "modules/map/hdmap/hdmap_util.h"
+#include "modules/map/pnc_map/path.h"
+#include "modules/map/pnc_map/pnc_map.h"
 
 namespace apollo {
 namespace planning {
 
+using apollo::common::math::Polygon2d;
+using apollo::common::math::Vec2d;
+using apollo::hdmap::HDMapUtil;
+using apollo::hdmap::LaneSegment;
+using apollo::hdmap::ParkingSpaceInfoConstPtr;
+using apollo::hdmap::Path;
+
 std::unique_ptr<Planner> StdPlannerDispatcher::DispatchPlanner() {
   PlanningConfig planning_config;
   apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
-      &planning_config);
+                                         &planning_config);
+  if (FLAGS_enable_open_space_planner_thread) {
+    const hdmap::HDMap* hdmap_ = hdmap::HDMapUtil::BaseMapPtr();
+    CHECK_NOTNULL(hdmap_);
+    common::VehicleState vehicle_state_ =
+        common::VehicleStateProvider::Instance()->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;
+    HDMapUtil::BaseMap().GetNearestLaneWithHeading(
+        point, 5.0, vehicle_state_.heading(), M_PI / 3.0, &nearest_lane,
+        &vehicle_lane_s, &vehicle_lane_l);
+    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();
+    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);
+      std::unique_ptr<Path> path_ =
+          std::unique_ptr<Path>(new Path(segments_vector));
+      const auto& parking_space_overlaps = path_->parking_space_overlaps();
+      ParkingSpaceInfoConstPtr target_parking_spot = nullptr;
+      if (parking_space_overlaps.size() != 0) {
+        for (const auto& parking_overlap : parking_space_overlaps) {
+          if (parking_overlap.object_id == FLAGS_target_parking_spot_id) {
+            hdmap::Id id;
+            id.set_id(parking_overlap.object_id);
+            target_parking_spot = hdmap_->GetParkingSpaceById(id);
+          }
+        }
+      }
+      if (target_parking_spot == nullptr) {
+        std::string msg("No such parking spot found ");
+        ADEBUG << msg << FLAGS_target_parking_spot_id;
+        segments_vector.clear();
+      } else {
+        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;
+        path_->GetNearestPoint(left_bottom_point, &left_bottom_point_s,
+                               &left_bottom_point_l);
+        path_->GetNearestPoint(right_bottom_point, &right_bottom_point_s,
+                               &right_bottom_point_l);
+        Vec2d vehicle_vec(vehicle_state_.x(), vehicle_state_.y());
+        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) < FLAGS_parking_start_range) {
+          return planner_factory_.CreateObject(
+              planning_config.standard_planning_config().planner_type(1));
+        }
+      }
+    }
+  }
   return planner_factory_.CreateObject(
       planning_config.standard_planning_config().planner_type(0));
 }

modules/planning/std_planning.cc

@@ -41,12 +41,12 @@
 namespace apollo {
 namespace planning {
 
-using apollo::common::math::Vec2d;
 using apollo::common::ErrorCode;
 using apollo::common::Status;
 using apollo::common::TrajectoryPoint;
 using apollo::common::VehicleState;
 using apollo::common::VehicleStateProvider;
+using apollo::common::math::Vec2d;
 using apollo::common::time::Clock;
 using apollo::hdmap::HDMapUtil;
 using apollo::routing::RoutingResponse;
@@ -84,6 +84,9 @@ StdPlanning::~StdPlanning() {
   if (reference_line_provider_) {
     reference_line_provider_->Stop();
   }
+  if (FLAGS_enable_open_space_planner_thread) {
+    planner_->Stop();
+  }
   last_publishable_trajectory_.reset(nullptr);
   frame_.reset(nullptr);
   planner_.reset(nullptr);
@@ -141,8 +144,7 @@ Status StdPlanning::InitFrame(const uint32_t sequence_num,
                               ADCTrajectory* output_trajectory) {
   frame_.reset(new Frame(sequence_num, local_view_, planning_start_point,
                          start_time, vehicle_state,
-                         reference_line_provider_.get(),
-                         output_trajectory));
+                         reference_line_provider_.get(), output_trajectory));
   if (frame_ == nullptr) {
     return Status(ErrorCode::PLANNING_ERROR, "Fail to init frame: nullptr.");
   }