feat(goal_planner): disable freespace feature temporarily

Signed-off-by: Mamoru Sobue <[email protected]>
autowarefoundation · Nov 25, 2024 · 1e2d52d · 1e2d52d
1 parent 740d42a
commit 1e2d52d
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 282 deletions.
diff --git a/...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
@@ -48,28 +48,15 @@ using nav_msgs::msg::OccupancyGrid;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 
-using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm;
-using autoware::freespace_planning_algorithms::AstarParam;
-using autoware::freespace_planning_algorithms::AstarSearch;
-using autoware::freespace_planning_algorithms::PlannerCommonParam;
-
 using autoware::behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams;
 using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams;
 using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
 using autoware::behavior_path_planner::utils::path_safety_checker::SafetyCheckParams;
 using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane;
 using autoware::universe_utils::Polygon2d;
 
-struct FreespacePlannerDebugData
-{
-  bool is_planning{false};
-  size_t current_goal_idx{0};
-  size_t num_goal_candidates{0};
-};
-
 struct GoalPlannerDebugData
 {
-  FreespacePlannerDebugData freespace_planner{};
   std::vector<Polygon2d> ego_polygons_expanded{};
   lanelet::ConstLanelet expanded_pull_over_lane_between_ego{};
   Polygon2d objects_extraction_polygon{};
@@ -150,15 +137,9 @@ class GoalPlannerModule : public SceneModuleInterface
     if (lane_parking_timer_) {
       lane_parking_timer_->cancel();
     }
-    if (freespace_parking_timer_) {
-      freespace_parking_timer_->cancel();
-    }
 
-    while (is_lane_parking_cb_running_.load() || is_freespace_parking_cb_running_.load()) {
+    while (is_lane_parking_cb_running_.load()) {
       const std::string running_callbacks = std::invoke([&]() {
-        if (is_lane_parking_cb_running_ && is_freespace_parking_cb_running_) {
-          return "lane parking and freespace parking";
-        }
         if (is_lane_parking_cb_running_) {
           return "lane parking";
         }
@@ -308,7 +289,6 @@ class GoalPlannerModule : public SceneModuleInterface
 
   // planner
   std::vector<std::shared_ptr<PullOverPlannerBase>> pull_over_planners_;
-  std::unique_ptr<PullOverPlannerBase> freespace_planner_;
   std::unique_ptr<FixedGoalPlannerBase> fixed_goal_planner_;
 
   // goal searcher
@@ -350,11 +330,6 @@ class GoalPlannerModule : public SceneModuleInterface
   rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_;
   std::atomic<bool> is_lane_parking_cb_running_;
 
-  // generate freespace parking paths in a separate thread
-  rclcpp::TimerBase::SharedPtr freespace_parking_timer_;
-  rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_;
-  std::atomic<bool> is_freespace_parking_cb_running_;
-
   // debug
   mutable GoalPlannerDebugData debug_data_;
   mutable PoseWithString debug_stop_pose_with_info_;
@@ -410,14 +385,6 @@ class GoalPlannerModule : public SceneModuleInterface
   bool isCrossingPossible(const PullOverPath & pull_over_path) const;
   bool hasEnoughTimePassedSincePathUpdate(const double duration) const;
 
-  // freespace parking
-  bool planFreespacePath(
-    std::shared_ptr<const PlannerData> planner_data,
-    const std::shared_ptr<GoalSearcherBase> goal_searcher, const GoalCandidates & goal_candidates,
-    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const PredictedObjects & static_target_objects);
-  bool canReturnToLaneParking(const PullOverContextData & context_data);
-
   // plan pull over path
   BehaviorModuleOutput planPullOver(const PullOverContextData & context_data);
   BehaviorModuleOutput planPullOverAsOutput(const PullOverContextData & context_data);
@@ -429,8 +396,7 @@ class GoalPlannerModule : public SceneModuleInterface
 
   // lanes and drivable area
   std::vector<DrivableLanes> generateDrivableLanes() const;
-  void setDrivableAreaInfo(
-    const PullOverContextData & context_data, BehaviorModuleOutput & output) const;
+  void setDrivableAreaInfo(BehaviorModuleOutput & output) const;
 
   // output setter
   void setOutput(const PullOverContextData & context_data, BehaviorModuleOutput & output);
@@ -455,7 +421,6 @@ class GoalPlannerModule : public SceneModuleInterface
 
   // timer for generating pull over path candidates in a separate thread
   void onTimer();
-  void onFreespaceParkingTimer();
 
   // steering factor
   void updateSteeringFactor(