Merge pull request #794 from tier4/feat/beta/v0.10.0/update-start-goa…

…l-planner

Feat/beta/v0.10.0/update start goal planner

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/goal_planner/goal_planner.param.yaml

@@ -31,7 +31,8 @@
       # object recognition
       object_recognition:
         use_object_recognition: true
-        object_recognition_collision_check_margin: 1.0
+        object_recognition_collision_check_margin: 0.6 # this should be larger than `surround_check_distance` of surround_obstacle_checker
+        object_recognition_collision_check_max_extra_stopping_margin: 1.0
 
       # pull over
       pull_over:

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/docs/behavior_path_planner_goal_planner_design.md

@@ -150,10 +150,11 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio
 
 #### Parameters for object recognition based collision check
 
-| Name                                      | Unit | Type   | Description                                                | Default value |
-| :---------------------------------------- | :--- | :----- | :--------------------------------------------------------- | :------------ |
-| use_object_recognition                    | [-]  | bool   | flag whether to use object recognition for collision check | true          |
-| object_recognition_collision_check_margin | [m]  | double | margin to calculate ego-vehicle cells from footprint.      | 1.0           |
+| Name                                                         | Unit | Type   | Description                                                | Default value |
+| :----------------------------------------------------------- | :--- | :----- | :--------------------------------------------------------- | :------------ | ---------------------------------------------------------------------------------------------------------- |
+| use_object_recognition                                       | [-]  | bool   | flag whether to use object recognition for collision check | true          |
+| object_recognition_collision_check_margin                    | [m]  | double | margin to calculate ego-vehicle cells from footprint.      | 0.6           |
+| object_recognition_collision_check_max_extra_stopping_margin | [m]  | double |                                                            | 1.0           | 　maximum value when adding longitudinal distance margin for collision check considering stopping distance |
 
 ## **Goal Search**
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

@@ -170,7 +170,6 @@ class GoalPlannerModule : public SceneModuleInterface
 
   // for parking policy
   bool left_side_parking_{true};
-  mutable bool allow_goal_modification_{false};  // need to be set in isExecutionRequested
 
   // pre-generate lane parking paths in a separate thread
   rclcpp::TimerBase::SharedPtr lane_parking_timer_;

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"
@@ -94,7 +95,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:
   std::shared_ptr<StartPlannerParameters> parameters_;
@@ -109,7 +112,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();
 
@@ -133,14 +144,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/goal_planner/freespace_pull_over.hpp

@@ -47,6 +47,7 @@ class FreespacePullOver : public PullOverPlannerBase
   std::unique_ptr<AbstractPlanningAlgorithm> planner_;
   double velocity_;
   bool use_back_;
+  bool left_side_parking_;
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp

@@ -68,6 +68,7 @@ struct GoalPlannerParameters
   // object recognition
   bool use_object_recognition;
   double object_recognition_collision_check_margin;
+  double object_recognition_collision_check_max_extra_stopping_margin;
 
   // pull over general params
   double pull_over_velocity;

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp

@@ -101,6 +101,15 @@ bool checkCollision(
   const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration,
   const double rear_object_deceleration, CollisionCheckDebug & debug);
 
+/**
+ * @brief Check collision between ego path footprints with extra longitudinal stopping margin and
+ * objects.
+ * @return Has collision or not
+ */
+bool checkCollisionWithExtraStoppingMargin(
+  const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects,
+  const double base_to_front, const double base_to_rear, const double width,
+  const double maximum_deceleration, const double margin, const double max_stopping_margin);
 }  // namespace behavior_path_planner::utils::path_safety_checker
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_

planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp

@@ -75,7 +75,8 @@ std::pair<TurnIndicatorsCommand, double> getPathTurnSignal(
   const BehaviorPathPlannerParameters & common_parameter);
 
 PathWithLaneId convertWayPointsToPathWithLaneId(
-  const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity);
+  const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity,
+  const lanelet::ConstLanelets & lanelets);
 
 std::vector<size_t> getReversingIndices(const PathWithLaneId & path);
 

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/include/behavior_path_planner/utils/utils.hpp

@@ -164,6 +164,16 @@ double getDistanceBetweenPredictedPathAndObject(
   const PredictedObject & object, const PredictedPath & path, const double start_time,
   const double end_time, const double resolution);
 
+/**
+ * @brief Check collision between ego path footprints with extra longitudinal stopping margin and
+ * objects.
+ * @return Has collision or not
+ */
+bool checkCollisionWithExtraStoppingMargin(
+  const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects,
+  const double base_to_front, const double base_to_rear, const double width,
+  const double maximum_deceleration, const double margin, const double max_stopping_margin);
+
 /**
  * @brief Check collision between ego path footprints and objects.
  * @return Has collision or not
@@ -399,6 +409,9 @@ void makeBoundLongitudinallyMonotonic(
 std::optional<lanelet::Polygon3d> getPolygonByPoint(
   const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstPoint3d & point,
   const std::string & polygon_name);
+
+lanelet::ConstLanelets combineLanelets(
+  const lanelet::ConstLanelets & base_lanes, const lanelet::ConstLanelets & added_lanes);
 }  // namespace behavior_path_planner::utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -273,10 +273,6 @@ bool GoalPlannerModule::isExecutionRequested() const
   const Pose & current_pose = planner_data_->self_odometry->pose.pose;
   const Pose & goal_pose = route_handler->getGoalPose();
 
-  // if goal is shoulder lane, allow goal modification
-  allow_goal_modification_ =
-    route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder();
-
   // check if goal_pose is in current_lanes.
   lanelet::ConstLanelet current_lane{};
   const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_);
@@ -553,7 +549,9 @@ void GoalPlannerModule::selectSafePullOverPath()
     }
 
     // check if path is valid and safe
-    if (!hasEnoughDistance(pull_over_path) || checkCollision(pull_over_path.getParkingPath())) {
+    if (
+      !hasEnoughDistance(pull_over_path) ||
+      checkCollision(utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5))) {
       continue;
     }
 
@@ -1169,9 +1167,14 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const
   }
 
   if (parameters_->use_object_recognition) {
-    if (utils::checkCollisionBetweenPathFootprintsAndObjects(
-          vehicle_footprint_, path, *(planner_data_->dynamic_object),
-          parameters_->object_recognition_collision_check_margin)) {
+    const auto common_parameters = planner_data_->parameters;
+    const double base_link2front = common_parameters.base_link2front;
+    const double base_link2rear = common_parameters.base_link2rear;
+    const double vehicle_width = common_parameters.vehicle_width;
+    if (utils::path_safety_checker::checkCollisionWithExtraStoppingMargin(
+          path, *(planner_data_->dynamic_object), base_link2front, base_link2rear, vehicle_width,
+          parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_margin,
+          parameters_->object_recognition_collision_check_max_extra_stopping_margin)) {
       return true;
     }
   }

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

@@ -88,6 +88,9 @@ GoalPlannerModuleManager::GoalPlannerModuleManager(
     p.use_object_recognition = node->declare_parameter<bool>(ns + "use_object_recognition");
     p.object_recognition_collision_check_margin =
       node->declare_parameter<double>(ns + "object_recognition_collision_check_margin");
+    p.object_recognition_collision_check_max_extra_stopping_margin =
+      node->declare_parameter<double>(
+        ns + "object_recognition_collision_check_max_extra_stopping_margin");
   }
 
   // pull over general params