feat(behavior_path_planner): pull over freespace parking

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py

@@ -67,6 +67,10 @@ def launch_setup(context, *args, **kwargs):
             ("~/input/vector_map", LaunchConfiguration("map_topic_name")),
             ("~/input/perception", "/perception/object_recognition/objects"),
             ("~/input/occupancy_grid_map", "/perception/occupancy_grid_map/map"),
+            (
+                "~/input/costmap",
+                "/planning/scenario_planning/parking/costmap_generator/occupancy_grid",
+            ),
             ("~/input/odometry", "/localization/kinematic_state"),
             ("~/input/accel", "/localization/acceleration"),
             ("~/input/scenario", "/planning/scenario_planning/scenario"),

launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py

@@ -62,7 +62,7 @@ def launch_setup(context, *args, **kwargs):
                         "vehicle_frame": "base_link",
                         "map_frame": "map",
                         "update_rate": 10.0,
-                        "activate_by_scenario": True,
+                        "activate_by_scenario": False,
                         "grid_min_value": 0.0,
                         "grid_max_value": 1.0,
                         "grid_resolution": 0.2,

planning/behavior_path_planner/CMakeLists.txt

@@ -30,6 +30,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/util/pull_over/util.cpp
   src/util/pull_over/shift_pull_over.cpp
   src/util/pull_over/geometric_pull_over.cpp
+  src/util/pull_over/freespace_pull_over.cpp
   src/util/pull_over/goal_searcher.cpp
   src/util/pull_out/util.cpp
   src/util/pull_out/shift_pull_out.cpp

planning/behavior_path_planner/README.md

@@ -341,6 +341,14 @@ Generate two backward arc paths.
 | backward_parking_velocity                | [m/s] | double | velocity when backward parking                                            | -1.38         |
 | backward_parking_lane_departure_margin   | [m/s] | double | lane departure margin for front right corner of ego-vehicle when backward | 0.0           |
 
+###### Parameters freespace parking
+
+| Name                     | Unit | Type | Description                              | Default value |
+| :----------------------- | :--- | :--- | :--------------------------------------- | :------------ |
+| enable_freespace_parking | [-]  | bool | flag whether to enable freespace parking | true          |
+
+See [reespace_planner](../freespace_planner/README.md) for other parameters.
+
 #### Unimplemented parts / limitations for pull over
 
 - parking on the right shoulder is not allowed

planning/behavior_path_planner/config/pull_over/pull_over.param.yaml

@@ -47,6 +47,37 @@
       backward_parking_lane_departure_margin: 0.0
       arc_path_interval: 1.0
       pull_over_max_steer_angle: 0.35  # 20deg
+      # freespace parking
+      enable_freespace_parking: true
+      freespace_parking:
+        planning_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
+        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
+        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
+          neighbour_radius: 8.0
+          margin: 1.0
       # hazard on when parked
       hazard_on_threshold_distance: 1.0
       hazard_on_threshold_velocity: 0.5

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -97,6 +97,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::Subscription<Scenario>::SharedPtr scenario_subscriber_;
   rclcpp::Subscription<PredictedObjects>::SharedPtr perception_subscriber_;
   rclcpp::Subscription<OccupancyGrid>::SharedPtr occupancy_grid_subscriber_;
+  rclcpp::Subscription<OccupancyGrid>::SharedPtr costmap_subscriber_;
   rclcpp::Publisher<PathWithLaneId>::SharedPtr path_publisher_;
   rclcpp::Publisher<TurnIndicatorsCommand>::SharedPtr turn_signal_publisher_;
   rclcpp::Publisher<HazardLightsCommand>::SharedPtr hazard_signal_publisher_;
@@ -145,6 +146,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg);
   void onPerception(const PredictedObjects::ConstSharedPtr msg);
   void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg);
+  void onCostMap(const OccupancyGrid::ConstSharedPtr msg);
   void onMap(const HADMapBin::ConstSharedPtr map_msg);
   void onRoute(const LaneletRoute::ConstSharedPtr route_msg);
   SetParametersResult onSetParam(const std::vector<rclcpp::Parameter> & parameters);

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -118,6 +118,7 @@ struct PlannerData
   AccelWithCovarianceStamped::ConstSharedPtr self_acceleration{};
   PredictedObjects::ConstSharedPtr dynamic_object{};
   OccupancyGrid::ConstSharedPtr occupancy_grid{};
+  OccupancyGrid::ConstSharedPtr costmap{};
   PathWithLaneId::SharedPtr reference_path{std::make_shared<PathWithLaneId>()};
   PathWithLaneId::SharedPtr prev_output_path{std::make_shared<PathWithLaneId>()};
   lanelet::ConstLanelets current_lanes{};

planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp

@@ -18,6 +18,7 @@
 #include "behavior_path_planner/util/path_shifter/path_shifter.hpp"
 
 #include <behavior_path_planner/parameters.hpp>
+#include <freespace_planning_algorithms/abstract_algorithm.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
@@ -71,6 +72,26 @@ std::pair<TurnIndicatorsCommand, double> getPathTurnSignal(
   const ShiftLine & shift_line, const Pose & pose, const double & velocity,
   const BehaviorPathPlannerParameters & common_parameter);
 
+PathWithLaneId convertWayPointsToPathWithLaneId(
+  const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity);
+
+std::vector<size_t> getReversingIndices(const PathWithLaneId & path);
+
+std::vector<PathWithLaneId> dividePath(
+  const PathWithLaneId & path, const std::vector<size_t> indices);
+
+void correctDividedPathVelocity(std::vector<PathWithLaneId> & divided_paths);
+
+bool isCloseToPath(const PathWithLaneId & path, const Pose & pose, const double distance_threshold);
+
+// only two points is supported
+std::vector<double> splineTwoPoints(
+  std::vector<double> base_s, std::vector<double> base_x, const double begin_diff,
+  const double end_diff, std::vector<double> new_s);
+
+std::vector<Pose> interpolatePose(
+  const Pose & start_pose, const Pose & end_pose, const double resample_interval);
+
 }  // namespace behavior_path_planner::util
 
 #endif  // BEHAVIOR_PATH_PLANNER__PATH_UTILITIES_HPP_

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp

@@ -18,11 +18,14 @@
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/util/geometric_parallel_parking/geometric_parallel_parking.hpp"
 #include "behavior_path_planner/util/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
+#include "behavior_path_planner/util/pull_over/freespace_pull_over.hpp"
 #include "behavior_path_planner/util/pull_over/geometric_pull_over.hpp"
 #include "behavior_path_planner/util/pull_over/goal_searcher.hpp"
 #include "behavior_path_planner/util/pull_over/pull_over_parameters.hpp"
 #include "behavior_path_planner/util/pull_over/shift_pull_over.hpp"
 
+#include <freespace_planning_algorithms/astar_search.hpp>
+#include <freespace_planning_algorithms/rrtstar.hpp>
 #include <lane_departure_checker/lane_departure_checker.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
@@ -44,17 +47,27 @@ using nav_msgs::msg::OccupancyGrid;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 
+using freespace_planning_algorithms::AbstractPlanningAlgorithm;
+using freespace_planning_algorithms::AstarParam;
+using freespace_planning_algorithms::AstarSearch;
+using freespace_planning_algorithms::PlannerCommonParam;
+using freespace_planning_algorithms::RRTStar;
+using freespace_planning_algorithms::RRTStarParam;
+
 enum PathType {
   NONE = 0,
   SHIFT,
   ARC_FORWARD,
   ARC_BACKWARD,
+  FREESPACE,
 };
 
 struct PUllOverStatus
 {
-  PullOverPath pull_over_path{};
+  std::shared_ptr<PullOverPath> pull_over_path{};
+  std::shared_ptr<PullOverPath> lane_parking_pull_over_path{};
   size_t current_path_idx{0};
+  bool require_increment_{true};
   std::shared_ptr<PathWithLaneId> prev_stop_path{nullptr};
   lanelet::ConstLanelets current_lanes{};
   lanelet::ConstLanelets pull_over_lanes{};
@@ -65,6 +78,8 @@ struct PUllOverStatus
   bool has_decided_velocity{false};
   bool has_requested_approval{false};
   std::optional<Pose> stop_pose{};
+  bool is_ready{false};
+  bool during_freespace_parking{false};
 };
 
 class PullOverModule : public SceneModuleInterface
@@ -99,8 +114,9 @@ class PullOverModule : public SceneModuleInterface
   PullOverParameters parameters_;
 
   std::vector<std::shared_ptr<PullOverPlannerBase>> pull_over_planners_;
+  std::unique_ptr<PullOverPlannerBase> freespace_planner_;
   std::shared_ptr<GoalSearcherBase> goal_searcher_;
-
+  // std::unique_ptr<AbstractPlanningAlgorithm> freespace_planner_;
   PullOverPath shift_parking_path_;
   vehicle_info_util::VehicleInfo vehicle_info_;
 
@@ -116,13 +132,16 @@ class PullOverModule : public SceneModuleInterface
   std::optional<Pose> closest_start_pose_;
   GeometricParallelParking parallel_parking_planner_;
   ParallelParkingParameters parallel_parking_parameters_;
-  std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_;
+  std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_stopped_;
+  std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_stuck_;
+
   tier4_autoware_utils::LinearRing2d vehicle_footprint_;
   std::unique_ptr<rclcpp::Time> last_received_time_;
   std::unique_ptr<rclcpp::Time> last_approved_time_;
+  std::unique_ptr<rclcpp::Time> last_increment_time_;
   std::unique_ptr<Pose> last_approved_pose_;
 
-  void incrementPathIndex();
+  bool incrementPathIndex();
   PathWithLaneId getCurrentPath() const;
   Pose calcRefinedGoal(const Pose & goal_pose) const;
   ParallelParkingParameters getGeometricPullOverParameters() const;
@@ -131,6 +150,8 @@ class PullOverModule : public SceneModuleInterface
   PathWithLaneId generateEmergencyStopPath();
 
   bool isStopped();
+  bool isStopped(
+    std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> & odometry_buffer, const double time);
   bool hasFinishedCurrentPath();
   bool hasFinishedPullOver();
   void updateOccupancyGrid();
@@ -141,15 +162,23 @@ class PullOverModule : public SceneModuleInterface
 
   TurnSignalInfo calcTurnSignalInfo() const;
 
+  bool planFreespacePath();
+  bool returnToLaneParking();
+  bool isStuck();
+
   // timer for generating pull over path candidates
   void onTimer();
-  rclcpp::TimerBase::SharedPtr timer_;
-  rclcpp::CallbackGroup::SharedPtr timer_cb_group_;
+  rclcpp::TimerBase::SharedPtr lane_parking_timer_;
+  rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_;
   std::mutex mutex_;
 
   // debug
   void setDebugData();
   void printParkingPositionError() const;
+
+  void onFreespaceParkingTimer();
+  rclcpp::TimerBase::SharedPtr freespace_parking_timer_;
+  rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_;
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/freespace_pull_over.hpp

@@ -0,0 +1,60 @@
+// 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__SCENE_MODULE__PULL_OVER__FREESPACE_PULL_OVER_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__FREESPACE_PULL_OVER_HPP_
+
+#include "behavior_path_planner/util/pull_over/pull_over_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::AstarParam;
+using freespace_planning_algorithms::AstarSearch;
+using freespace_planning_algorithms::PlannerCommonParam;
+using freespace_planning_algorithms::RRTStar;
+using freespace_planning_algorithms::RRTStarParam;
+
+class FreespacePullOver : public PullOverPlannerBase
+{
+public:
+  FreespacePullOver(
+    rclcpp::Node & node, const PullOverParameters & parameters,
+    const vehicle_info_util::VehicleInfo & vehicle_info);
+
+  PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; }
+
+  boost::optional<PullOverPath> plan(const Pose & goal_pose) override;
+
+protected:
+  PlannerCommonParam getCommonParam(rclcpp::Node & node) const;
+  AstarParam getAstarParam(rclcpp::Node & node) const;
+  RRTStarParam getRRTStarParam(rclcpp::Node & node) const;
+
+  std::unique_ptr<AbstractPlanningAlgorithm> planner_;
+  double velocity_;
+  bool use_back_;
+};
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__FREESPACE_PULL_OVER_HPP_

planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_parameters.hpp

@@ -69,6 +69,8 @@ struct PullOverParameters
   double backward_parking_lane_departure_margin;
   double arc_path_interval;
   double pull_over_max_steer_angle;
+  // freespace parking
+  bool enable_freespace_parking;
   // hazard
   double hazard_on_threshold_distance;
   double hazard_on_threshold_velocity;

planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_planner_base.hpp

@@ -39,6 +39,7 @@ enum class PullOverPlannerType {
   SHIFT,
   ARC_FORWARD,
   ARC_BACKWARD,
+  FREESPACE,
 };
 
 struct PullOverPath

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -329,6 +329,10 @@ void generateDrivableArea(
   PathWithLaneId & path, const std::vector<DrivableLanes> & lanes, const double vehicle_length,
   const std::shared_ptr<const PlannerData> planner_data, const bool is_driving_forward = true);
 
+void generateDrivableArea(
+  PathWithLaneId & path, const double vehicle_length, const double vehicle_width,
+  const double margin, const bool is_driving_forward = true);
+
 lanelet::ConstLineStrings3d getMaximumDrivableArea(
   const std::shared_ptr<const PlannerData> & planner_data);
 

planning/behavior_path_planner/package.xml

@@ -47,6 +47,7 @@
   <depend>autoware_auto_tf2</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
   <depend>behaviortree_cpp_v3</depend>
+  <depend>freespace_planning_algorithms</depend>
   <depend>geometry_msgs</depend>
   <depend>interpolation</depend>
   <depend>lane_departure_checker</depend>

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -92,6 +92,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   occupancy_grid_subscriber_ = create_subscription<OccupancyGrid>(
     "~/input/occupancy_grid_map", 1, std::bind(&BehaviorPathPlannerNode::onOccupancyGrid, this, _1),
     createSubscriptionOptions(this));
+  costmap_subscriber_ = create_subscription<OccupancyGrid>(
+    "~/input/costmap", 1, std::bind(&BehaviorPathPlannerNode::onCostMap, this, _1),
+    createSubscriptionOptions(this));
   scenario_subscriber_ = create_subscription<Scenario>(
     "~/input/scenario", 1,
     [this](const Scenario::ConstSharedPtr msg) {
@@ -598,6 +601,8 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam()
   p.backward_parking_lane_departure_margin = dp("backward_parking_lane_departure_margin", 0.0);
   p.arc_path_interval = dp("arc_path_interval", 1.0);
   p.pull_over_max_steer_angle = dp("pull_over_max_steer_angle", 0.35);  // 20deg
+  // freespace parking
+  p.enable_freespace_parking = dp("enable_freespace_parking", true);
   // hazard
   p.hazard_on_threshold_distance = dp("hazard_on_threshold_distance", 1.0);
   p.hazard_on_threshold_velocity = dp("hazard_on_threshold_velocity", 0.5);
@@ -1040,6 +1045,11 @@ void BehaviorPathPlannerNode::onOccupancyGrid(const OccupancyGrid::ConstSharedPt
   const std::lock_guard<std::mutex> lock(mutex_pd_);
   planner_data_->occupancy_grid = msg;
 }
+void BehaviorPathPlannerNode::onCostMap(const OccupancyGrid::ConstSharedPtr msg)
+{
+  const std::lock_guard<std::mutex> lock(mutex_pd_);
+  planner_data_->costmap = msg;
+}
 void BehaviorPathPlannerNode::onMap(const HADMapBin::ConstSharedPtr msg)
 {
   const std::lock_guard<std::mutex> lock(mutex_pd_);