feat(behavior_path_planner): pull over freespace parking (autowarefou…

…ndation#2879)

* feat(behavior_path_planner): pull over freespace parking

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

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

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix from review

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* add require_increment_ explanation make the function

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* Update planning/behavior_path_planner/README.md

* fix mutex

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix typo

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix build

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* pre-commit

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

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

@@ -152,6 +152,11 @@ def launch_setup(context, *args, **kwargs):
             ("~/input/route", LaunchConfiguration("input_route_topic_name")),
             ("~/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

@@ -76,6 +76,7 @@ def launch_setup(context, *args, **kwargs):
                         "use_parkinglot": True,
                         "use_objects": True,
                         "use_points": 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

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

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
+          neighbor_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

@@ -99,6 +99,8 @@ 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::Subscription<OperationModeState>::SharedPtr operation_mode_subscriber_;
   rclcpp::Publisher<PathWithLaneId>::SharedPtr path_publisher_;
   rclcpp::Publisher<TurnIndicatorsCommand>::SharedPtr turn_signal_publisher_;
   rclcpp::Publisher<HazardLightsCommand>::SharedPtr hazard_signal_publisher_;
@@ -141,6 +143,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg);
   void onExternalApproval(const ApprovalMsg::ConstSharedPtr msg);
   void onForceApproval(const PathChangeModule::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

@@ -74,6 +74,8 @@ struct PlannerData
   AccelWithCovarianceStamped::ConstSharedPtr self_acceleration{};
   PredictedObjects::ConstSharedPtr dynamic_object{};
   OccupancyGrid::ConstSharedPtr occupancy_grid{};
+  OccupancyGrid::ConstSharedPtr costmap{};
+  OperationModeState::ConstSharedPtr operation_mode{};
   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

@@ -17,6 +17,7 @@
 
 #include <behavior_path_planner/parameters.hpp>
 #include <behavior_path_planner/scene_module/utils/path_shifter.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>
@@ -70,6 +71,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/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__UTIL__PULL_OVER__FREESPACE_PULL_OVER_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__FREESPACE_PULL_OVER_HPP_
+
+#include "behavior_path_planner/scene_module/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__UTIL__PULL_OVER__FREESPACE_PULL_OVER_HPP_

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

@@ -15,14 +15,17 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_MODULE_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_MODULE_HPP_
 
+#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
+#include "behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp"
+#include "behavior_path_planner/scene_module/utils/occupancy_grid_based_collision_detector.hpp"
+#include "behavior_path_planner/scene_module/pull_over/freespace_pull_over.hpp"
 #include "behavior_path_planner/scene_module/pull_over/geometric_pull_over.hpp"
 #include "behavior_path_planner/scene_module/pull_over/goal_searcher.hpp"
 #include "behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp"
 #include "behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp"
-#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
-#include "behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp"
-#include "behavior_path_planner/scene_module/utils/occupancy_grid_based_collision_detector.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};  // if false, keep current path idx.
   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,38 +114,44 @@ 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_;
-
   PullOverPath shift_parking_path_;
   vehicle_info_util::VehicleInfo vehicle_info_;
 
   rclcpp::Subscription<OccupancyGrid>::SharedPtr occupancy_grid_sub_;
-  rclcpp::Publisher<PoseStamped>::SharedPtr goal_pose_pub_;
 
   PUllOverStatus status_;
   std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_;
-  Pose modified_goal_pose_;
+  std::optional<GoalCandidate> modified_goal_pose_;
+  std::optional<size_t> prev_goal_id_;
   Pose refined_goal_pose_;
   GoalCandidates goal_candidates_;
   std::vector<PullOverPath> pull_over_path_candidates_;
   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;
   std::pair<double, double> calcDistanceToPathChange() const;
   PathWithLaneId generateStopPath();
   PathWithLaneId generateEmergencyStopPath();
+  void keepStoppedWithCurrentPath(PathWithLaneId & path);
 
   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/scene_module/pull_over/pull_over_parameters.hpp

@@ -68,6 +68,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/scene_module/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

@@ -344,6 +344,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

@@ -88,8 +88,14 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     createSubscriptionOptions(this));
   // todo: change to ~/input
   occupancy_grid_subscriber_ = create_subscription<OccupancyGrid>(
-    "/perception/occupancy_grid_map/map", 1,
-    std::bind(&BehaviorPathPlannerNode::onOccupancyGrid, this, _1),
+    "~/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));
+  operation_mode_subscriber_ = create_subscription<OperationModeState>(
+    "/system/operation_mode/state", 1,
+    std::bind(&BehaviorPathPlannerNode::onOperationMode, this, _1),
     createSubscriptionOptions(this));
   scenario_subscriber_ = create_subscription<Scenario>(
     "~/input/scenario", 1,
@@ -511,6 +517,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);
@@ -946,6 +954,10 @@ void BehaviorPathPlannerNode::onForceApproval(const PathChangeModule::ConstShare
   };
   planner_data_->approval.is_force_approved.module_name = getModuleName(msg->module);
   planner_data_->approval.is_force_approved.stamp = msg->header.stamp;
+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)
 {