feat(behavior_path_planner): pull over freespace parking

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

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

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

@@ -63,7 +63,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/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/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -99,6 +99,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_;
@@ -147,6 +148,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 onExternalApproval(const ApprovalMsg::ConstSharedPtr msg);
   void onForceApproval(const PathChangeModule::ConstSharedPtr msg);
   void onMap(const HADMapBin::ConstSharedPtr map_msg);

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

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

@@ -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> devidePath(
+  const PathWithLaneId & path, const std::vector<size_t> indices);
+
+void correctDevidedPathVelocity(std::vector<PathWithLaneId> & devided_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,58 @@
+// 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/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::PlannerCommonParam;
+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/scene_module/pull_over/pull_over_module.hpp

@@ -15,6 +15,7 @@
 #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/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"
@@ -23,6 +24,8 @@
 #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};
   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/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

@@ -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

@@ -90,6 +90,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) {
@@ -1037,6 +1040,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::onExternalApproval(const ApprovalMsg::ConstSharedPtr msg)
 {
   const std::lock_guard<std::mutex> lock(mutex_pd_);