feat(start_planner): support freespace pull out (autowarefoundation#4610

)

feat(start_planner): add freespace pull out

Signed-off-by: kosuke55 <[email protected]>

planning/behavior_path_planner/CMakeLists.txt

@@ -44,6 +44,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/utils/start_planner/util.cpp
   src/utils/start_planner/shift_pull_out.cpp
   src/utils/start_planner/geometric_pull_out.cpp
+  src/utils/start_planner/freespace_pull_out.cpp
   src/utils/path_shifter/path_shifter.cpp
   src/utils/drivable_area_expansion/drivable_area_expansion.cpp
   src/utils/drivable_area_expansion/map_utils.cpp

planning/behavior_path_planner/config/start_planner/start_planner.param.yaml

@@ -36,3 +36,39 @@
       th_turn_signal_on_lateral_offset: 1.0
       intersection_search_length: 30.0
       length_ratio_for_turn_signal_deactivation_near_intersection: 0.5
+      # freespace planner
+      freespace_planner:
+        enable_freespace_planner: true
+        end_pose_search_start_distance: 20.0
+        end_pose_search_end_distance: 30.0
+        end_pose_search_interval: 2.0
+        freespace_planner_algorithm: "astar"  # options: astar, rrtstar
+        velocity: 1.0
+        vehicle_shape_margin: 1.0
+        time_limit: 3000.0
+        minimum_turning_radius: 5.0
+        maximum_turning_radius: 5.0
+        turning_radius_size: 1
+        # search configs
+        search_configs:
+          theta_size: 144
+          angle_goal_range: 6.0
+          curve_weight: 1.2
+          reverse_weight: 1.0
+          lateral_goal_range: 0.5
+          longitudinal_goal_range: 2.0
+        # costmap configs
+        costmap_configs:
+          obstacle_threshold: 30
+        # -- A* search Configurations --
+        astar:
+          only_behind_solutions: false
+          use_back: false
+          distance_heuristic_weight: 1.0
+        # -- RRT* search Configurations --
+        rrtstar:
+          enable_update: true
+          use_informed_sampling: true
+          max_planning_time: 150.0
+          neighbor_radius: 8.0
+          margin: 1.0

planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp

@@ -18,6 +18,7 @@
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp"
 #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp"
 #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp"
 #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp"
 #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp"
@@ -96,7 +97,9 @@ class StartPlannerModule : public SceneModuleInterface
   {
   }
 
+  // Condition to disable simultaneous execution
   bool isBackFinished() const { return status_.back_finished; }
+  bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; }
 
 private:
   bool canTransitSuccessState() override { return false; }
@@ -117,7 +120,15 @@ class StartPlannerModule : public SceneModuleInterface
   std::unique_ptr<rclcpp::Time> last_pull_out_start_update_time_;
   std::unique_ptr<Pose> last_approved_pose_;
 
-  std::shared_ptr<PullOutPlannerBase> getCurrentPlanner() const;
+  // generate freespace pull out paths in a separate thread
+  std::unique_ptr<PullOutPlannerBase> freespace_planner_;
+  rclcpp::TimerBase::SharedPtr freespace_planner_timer_;
+  rclcpp::CallbackGroup::SharedPtr freespace_planner_timer_cb_group_;
+  // TODO(kosuke55)
+  // Currently, we only do lock when updating a member of status_.
+  // However, we need to ensure that the value does not change when referring to it.
+  std::mutex mutex_;
+
   PathWithLaneId getFullPath() const;
   std::vector<Pose> searchPullOutStartPoses();
 
@@ -141,14 +152,20 @@ class StartPlannerModule : public SceneModuleInterface
   bool hasFinishedPullOut() const;
   void checkBackFinished();
   bool isStopped();
+  bool isStuck();
   bool hasFinishedCurrentPath();
+  void setDrivableAreaInfo(BehaviorModuleOutput & output) const;
 
   // check if the goal is located behind the ego in the same route segment.
   bool IsGoalBehindOfEgoInSameRouteSegment() const;
 
   // generate BehaviorPathOutput with stopping path and update status
   BehaviorModuleOutput generateStopOutput();
 
+  // freespace planner
+  void onFreespacePlannerTimer();
+  bool planFreespacePath();
+
   void setDebugData() const;
 };
 }  // namespace behavior_path_planner

planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp

@@ -0,0 +1,53 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_
+
+#include "behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp"
+
+#include <freespace_planning_algorithms/abstract_algorithm.hpp>
+#include <freespace_planning_algorithms/astar_search.hpp>
+#include <freespace_planning_algorithms/rrtstar.hpp>
+
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+
+#include <memory>
+#include <vector>
+
+namespace behavior_path_planner
+{
+using freespace_planning_algorithms::AbstractPlanningAlgorithm;
+using freespace_planning_algorithms::AstarSearch;
+using freespace_planning_algorithms::RRTStar;
+
+class FreespacePullOut : public PullOutPlannerBase
+{
+public:
+  FreespacePullOut(
+    rclcpp::Node & node, const StartPlannerParameters & parameters,
+    const vehicle_info_util::VehicleInfo & vehicle_info);
+
+  PlannerType getPlannerType() override { return PlannerType::FREESPACE; }
+
+  boost::optional<PullOutPath> plan(Pose start_pose, Pose end_pose) override;
+
+protected:
+  std::unique_ptr<AbstractPlanningAlgorithm> planner_;
+  double velocity_;
+  bool use_back_;
+};
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_

planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp

@@ -40,6 +40,7 @@ enum class PlannerType {
   SHIFT = 1,
   GEOMETRIC = 2,
   STOP = 3,
+  FREESPACE = 4,
 };
 
 class PullOutPlannerBase

planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp

@@ -18,11 +18,20 @@
 
 #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp"
 
+#include <freespace_planning_algorithms/abstract_algorithm.hpp>
+#include <freespace_planning_algorithms/astar_search.hpp>
+#include <freespace_planning_algorithms/rrtstar.hpp>
+
 #include <string>
 #include <vector>
 
 namespace behavior_path_planner
 {
+
+using freespace_planning_algorithms::AstarParam;
+using freespace_planning_algorithms::PlannerCommonParam;
+using freespace_planning_algorithms::RRTStarParam;
+
 struct StartPlannerParameters
 {
   double th_arrived_distance;
@@ -56,6 +65,17 @@ struct StartPlannerParameters
   double backward_search_resolution;
   double backward_path_update_duration;
   double ignore_distance_from_lane_end;
+  // freespace planner
+  bool enable_freespace_planner;
+  std::string freespace_planner_algorithm;
+  double end_pose_search_start_distance;
+  double end_pose_search_end_distance;
+  double end_pose_search_interval;
+  double freespace_planner_velocity;
+  double vehicle_shape_margin;
+  PlannerCommonParam freespace_planner_common_parameters;
+  AstarParam astar_parameters;
+  RRTStarParam rrt_star_parameters;
 };
 
 }  // namespace behavior_path_planner

planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp

@@ -78,6 +78,75 @@ StartPlannerModuleManager::StartPlannerModuleManager(
     node->declare_parameter<double>(ns + "backward_path_update_duration");
   p.ignore_distance_from_lane_end =
     node->declare_parameter<double>(ns + "ignore_distance_from_lane_end");
+  // freespace planner general params
+  {
+    std::string ns = "start_planner.freespace_planner.";
+    p.enable_freespace_planner = node->declare_parameter<bool>(ns + "enable_freespace_planner");
+    p.freespace_planner_algorithm =
+      node->declare_parameter<std::string>(ns + "freespace_planner_algorithm");
+    p.end_pose_search_start_distance =
+      node->declare_parameter<double>(ns + "end_pose_search_start_distance");
+    p.end_pose_search_end_distance =
+      node->declare_parameter<double>(ns + "end_pose_search_end_distance");
+    p.end_pose_search_interval = node->declare_parameter<double>(ns + "end_pose_search_interval");
+    p.freespace_planner_velocity = node->declare_parameter<double>(ns + "velocity");
+    p.vehicle_shape_margin = node->declare_parameter<double>(ns + "vehicle_shape_margin");
+    p.freespace_planner_common_parameters.time_limit =
+      node->declare_parameter<double>(ns + "time_limit");
+    p.freespace_planner_common_parameters.minimum_turning_radius =
+      node->declare_parameter<double>(ns + "minimum_turning_radius");
+    p.freespace_planner_common_parameters.maximum_turning_radius =
+      node->declare_parameter<double>(ns + "maximum_turning_radius");
+    p.freespace_planner_common_parameters.turning_radius_size =
+      node->declare_parameter<int>(ns + "turning_radius_size");
+    p.freespace_planner_common_parameters.maximum_turning_radius = std::max(
+      p.freespace_planner_common_parameters.maximum_turning_radius,
+      p.freespace_planner_common_parameters.minimum_turning_radius);
+    p.freespace_planner_common_parameters.turning_radius_size =
+      std::max(p.freespace_planner_common_parameters.turning_radius_size, 1);
+  }
+  //  freespace planner search config
+  {
+    std::string ns = "start_planner.freespace_planner.search_configs.";
+    p.freespace_planner_common_parameters.theta_size =
+      node->declare_parameter<int>(ns + "theta_size");
+    p.freespace_planner_common_parameters.angle_goal_range =
+      node->declare_parameter<double>(ns + "angle_goal_range");
+    p.freespace_planner_common_parameters.curve_weight =
+      node->declare_parameter<double>(ns + "curve_weight");
+    p.freespace_planner_common_parameters.reverse_weight =
+      node->declare_parameter<double>(ns + "reverse_weight");
+    p.freespace_planner_common_parameters.lateral_goal_range =
+      node->declare_parameter<double>(ns + "lateral_goal_range");
+    p.freespace_planner_common_parameters.longitudinal_goal_range =
+      node->declare_parameter<double>(ns + "longitudinal_goal_range");
+  }
+  //  freespace planner costmap configs
+  {
+    std::string ns = "start_planner.freespace_planner.costmap_configs.";
+    p.freespace_planner_common_parameters.obstacle_threshold =
+      node->declare_parameter<int>(ns + "obstacle_threshold");
+  }
+  //  freespace planner astar
+  {
+    std::string ns = "start_planner.freespace_planner.astar.";
+    p.astar_parameters.only_behind_solutions =
+      node->declare_parameter<bool>(ns + "only_behind_solutions");
+    p.astar_parameters.use_back = node->declare_parameter<bool>(ns + "use_back");
+    p.astar_parameters.distance_heuristic_weight =
+      node->declare_parameter<double>(ns + "distance_heuristic_weight");
+  }
+  //   freespace planner rrtstar
+  {
+    std::string ns = "start_planner.freespace_planner.rrtstar.";
+    p.rrt_star_parameters.enable_update = node->declare_parameter<bool>(ns + "enable_update");
+    p.rrt_star_parameters.use_informed_sampling =
+      node->declare_parameter<bool>(ns + "use_informed_sampling");
+    p.rrt_star_parameters.max_planning_time =
+      node->declare_parameter<double>(ns + "max_planning_time");
+    p.rrt_star_parameters.neighbor_radius = node->declare_parameter<double>(ns + "neighbor_radius");
+    p.rrt_star_parameters.margin = node->declare_parameter<double>(ns + "margin");
+  }
 
   // validation of parameters
   if (p.lateral_acceleration_sampling_num < 1) {
@@ -128,6 +197,11 @@ bool StartPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const
       return false;
     }
 
+    // Other modules are not needed when freespace planning
+    if (start_planner_ptr->isFreespacePlanning()) {
+      return false;
+    }
+
     return enable_simultaneous_execution_as_approved_module_;
   };
 
@@ -151,6 +225,12 @@ bool StartPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() cons
     if (!start_planner_ptr->isBackFinished()) {
       return false;
     }
+
+    // Other modules are not needed when freespace planning
+    if (start_planner_ptr->isFreespacePlanning()) {
+      return false;
+    }
+
     return enable_simultaneous_execution_as_candidate_module_;
   };