feat: update start goal planner v0.10.1

planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml

@@ -2,7 +2,6 @@
   ros__parameters:
     goal_planner:
       # general params
-      minimum_request_length: 100.0
       th_arrived_distance: 1.0
       th_stopped_velocity: 0.01
       th_stopped_time: 2.0  # It must be greater than the state_machine's.
@@ -17,25 +16,28 @@
         longitudinal_margin: 3.0
         max_lateral_offset: 0.5
         lateral_offset_interval: 0.25
-        ignore_distance_from_lane_start: 15.0
+        ignore_distance_from_lane_start: 0.0
         margin_from_boundary: 0.5
 
       # occupancy grid map
       occupancy_grid:
-        use_occupancy_grid: true
-        use_occupancy_grid_for_longitudinal_margin: false
+        use_occupancy_grid_for_goal_search: true
+        use_occupancy_grid_for_goal_longitudinal_margin: false
+        use_occupancy_grid_for_path_collision_check: false
         occupancy_grid_collision_check_margin: 0.0
         theta_size: 360
         obstacle_threshold: 60
 
       # object recognition
       object_recognition:
-        use_object_recognition: true
+        use_object_recognition: false
         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
+        th_moving_object_velocity: 1.0
 
       # pull over
       pull_over:
+        minimum_request_length: 100.0
         pull_over_velocity: 3.0
         pull_over_minimum_velocity: 1.38
         decide_path_distance: 10.0

planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md

@@ -86,16 +86,16 @@ Either one is activated when all conditions are met.
 
 ### fixed_goal_planner
 
-- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`.
 - Route is set with `allow_goal_modification=false` by default.
+- ego-vehicle is in the same lane as the goal.
 
 <img src="https://user-images.githubusercontent.com/39142679/237929955-c0adf01b-9e3c-45e3-848d-98cf11e52b65.png" width="600">
 
 ### rough_goal_planner
 
 #### pull over on road lane
 
-- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`.
+- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`.
 - Route is set with `allow_goal_modification=true` .
   - We can set this option with [SetRoute](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/routing/srv/SetRoute.srv#L2) api service.
   - We support `2D Rough Goal Pose` with the key bind `r` in RViz, but in the future there will be a panel of tools to manipulate various Route API from RViz.
@@ -105,7 +105,7 @@ Either one is activated when all conditions are met.
 
 #### pull over on shoulder lane
 
-- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`.
+- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`.
 - Goal is set in the `road_shoulder`.
 
 <img src="https://user-images.githubusercontent.com/39142679/237929941-2ce26ea5-c84d-4d17-8cdc-103f5246db90.png" width="600">
@@ -118,17 +118,11 @@ Either one is activated when all conditions are met.
 
 ## General parameters for goal_planner
 
-| Name                       | Unit   | Type   | Description                                                                                                                             | Default value |
-| :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
-| minimum_request_length     | [m]    | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, the module is activated.                          | 100.0         |
-| th_arrived_distance        | [m]    | double | distance threshold for arrival of path termination                                                                                      | 1.0           |
-| th_stopped_velocity        | [m/s]  | double | velocity threshold for arrival of path termination                                                                                      | 0.01          |
-| th_stopped_time            | [s]    | double | time threshold for arrival of path termination                                                                                          | 2.0           |
-| pull_over_velocity         | [m/s]  | double | decelerate to this speed by the goal search area                                                                                        | 3.0           |
-| pull_over_minimum_velocity | [m/s]  | double | speed of pull_over after stopping once. this prevents excessive acceleration.                                                           | 1.38          |
-| margin_from_boundary       | [m]    | double | distance margin from edge of the shoulder lane                                                                                          | 0.5           |
-| decide_path_distance       | [m]    | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0          |
-| maximum_deceleration       | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly                                      | 1.0           |
+| Name                | Unit  | Type   | Description                                        | Default value |
+| :------------------ | :---- | :----- | :------------------------------------------------- | :------------ |
+| th_arrived_distance | [m]   | double | distance threshold for arrival of path termination | 1.0           |
+| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01          |
+| th_stopped_time     | [s]   | double | time threshold for arrival of path termination     | 2.0           |
 
 ## **collision check**
 
@@ -138,13 +132,14 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio
 
 #### Parameters for occupancy grid based collision check
 
-| Name                                       | Unit | Type   | Description                                                                                                     | Default value |
-| :----------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ |
-| use_occupancy_grid                         | [-]  | bool   | flag whether to use occupancy grid for collision check                                                          | true          |
-| use_occupancy_grid_for_longitudinal_margin | [-]  | bool   | flag whether to use occupancy grid for keeping longitudinal margin                                              | false         |
-| occupancy_grid_collision_check_margin      | [m]  | double | margin to calculate ego-vehicle cells from footprint.                                                           | 0.0           |
-| theta_size                                 | [-]  | int    | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360           |
-| obstacle_threshold                         | [-]  | int    | threshold of cell values to be considered as obstacles                                                          | 60            |
+| Name                                            | Unit | Type   | Description                                                                                                     | Default value |
+| :---------------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ |
+| use_occupancy_grid_for_goal_search              | [-]  | bool   | flag whether to use occupancy grid for goal search collision check                                              | true          |
+| use_occupancy_grid_for_goal_longitudinal_margin | [-]  | bool   | flag whether to use occupancy grid for keeping longitudinal margin                                              | false         |
+| use_occupancy_grid_for_path_collision_check     | [-]  | bool   | flag whether to use occupancy grid for collision check                                                          | false         |
+| occupancy_grid_collision_check_margin           | [m]  | double | margin to calculate ego-vehicle cells from footprint.                                                           | 0.0           |
+| theta_size                                      | [-]  | int    | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360           |
+| obstacle_threshold                              | [-]  | int    | threshold of cell values to be considered as obstacles                                                          | 60            |
 
 ### **object recognition based collision check**
 
@@ -174,12 +169,22 @@ searched for in certain range of the shoulder lane.
 | longitudinal_margin             | [m]  | double | margin between ego-vehicle at the goal position and obstacles                                                                                                                                                            | 3.0            |
 | max_lateral_offset              | [m]  | double | maximum offset of goal search in the lateral direction                                                                                                                                                                   | 0.5            |
 | lateral_offset_interval         | [m]  | double | distance interval of goal search in the lateral direction                                                                                                                                                                | 0.25           |
-| ignore_distance_from_lane_start | [m]  | double | distance from start of pull over lanes for ignoring goal candidates                                                                                                                                                      | 15.0           |
+| ignore_distance_from_lane_start | [m]  | double | distance from start of pull over lanes for ignoring goal candidates                                                                                                                                                      | 0.0            |
+| ignore_distance_from_lane_start | [m]  | double | distance from start of pull over lanes for ignoring goal candidates                                                                                                                                                      | 0.0            |
+| margin_from_boundary            | [m]  | double | distance margin from edge of the shoulder lane                                                                                                                                                                           | 0.5            |
 
-## **Path Generation**
+## **Pull Over**
 
 There are three path generation methods.
-The path is generated with a certain margin (default: `0.5 m`) from left boundary of shoulder lane.
+The path is generated with a certain margin (default: `0.5 m`) from the boundary of shoulder lane.
+
+| Name                             | Unit   | Type   | Description                                                                                                                             | Default value |
+| :------------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| pull_over_minimum_request_length | [m]    | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, pull over is activated.                           | 100.0         |
+| pull_over_velocity               | [m/s]  | double | decelerate to this speed by the goal search area                                                                                        | 3.0           |
+| pull_over_minimum_velocity       | [m/s]  | double | speed of pull_over after stopping once. this prevents excessive acceleration.                                                           | 1.38          |
+| decide_path_distance             | [m]    | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0          |
+| maximum_deceleration             | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly                                      | 1.0           |
 
 ### **shift parking**
 

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

@@ -48,7 +48,6 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface
   std::shared_ptr<GoalPlannerParameters> parameters_;
 
   std::vector<std::shared_ptr<GoalPlannerModule>> registered_modules_;
-  bool left_side_parking_;
 };
 
 }  // namespace behavior_path_planner

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

@@ -54,7 +54,8 @@ struct PullOutStatus
   PathWithLaneId backward_path{};
   lanelet::ConstLanelets pull_out_lanes{};
   bool is_safe{false};
-  bool back_finished{false};
+  bool back_finished{false};  // if backward driving is not required, this is also set to true
+                              // todo: rename to clear variable name.
   Pose pull_out_start_pose{};
 
   PullOutStatus() {}
@@ -122,7 +123,8 @@ class StartPlannerModule : public SceneModuleInterface
   std::mutex mutex_;
 
   PathWithLaneId getFullPath() const;
-  std::vector<Pose> searchPullOutStartPoses();
+  PathWithLaneId calcStartPoseCandidatesBackwardPath() const;
+  std::vector<Pose> searchPullOutStartPoses(const PathWithLaneId & start_pose_candidates) const;
 
   std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;
 
@@ -132,8 +134,8 @@ class StartPlannerModule : public SceneModuleInterface
   void incrementPathIndex();
   PathWithLaneId getCurrentPath() const;
   void planWithPriority(
-    const std::vector<Pose> & start_pose_candidates, const Pose & goal_pose,
-    const std::string search_priority);
+    const std::vector<Pose> & start_pose_candidates, const Pose & refined_start_pose,
+    const Pose & goal_pose, const std::string search_priority);
   PathWithLaneId generateStopPath() const;
   lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const;
   std::vector<DrivableLanes> generateDrivableLanes(const PathWithLaneId & path) const;

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

@@ -40,7 +40,6 @@ enum class ParkingPolicy {
 struct GoalPlannerParameters
 {
   // general  params
-  double minimum_request_length;
   double th_arrived_distance;
   double th_stopped_velocity;
   double th_stopped_time;
@@ -59,8 +58,9 @@ struct GoalPlannerParameters
   double margin_from_boundary;
 
   // occupancy grid map
-  bool use_occupancy_grid;
-  bool use_occupancy_grid_for_longitudinal_margin;
+  bool use_occupancy_grid_for_goal_search;
+  bool use_occupancy_grid_for_goal_longitudinal_margin;
+  bool use_occupancy_grid_for_path_collision_check;
   double occupancy_grid_collision_check_margin;
   int theta_size;
   int obstacle_threshold;
@@ -69,8 +69,10 @@ struct GoalPlannerParameters
   bool use_object_recognition;
   double object_recognition_collision_check_margin;
   double object_recognition_collision_check_max_extra_stopping_margin;
+  double th_moving_object_velocity;
 
   // pull over general params
+  double pull_over_minimum_request_length;
   double pull_over_velocity;
   double pull_over_minimum_velocity;
   double decide_path_distance;

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

@@ -46,15 +46,15 @@ using visualization_msgs::msg::MarkerArray;
 
 // TODO(sugahara) move to util
 PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2);
-lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side);
+lanelet::ConstLanelets getPullOverLanes(
+  const RouteHandler & route_handler, const bool left_side, const double backward_distance,
+  const double forward_distance);
 PredictedObjects filterObjectsByLateralDistance(
   const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects,
   const double distance_thresh, const bool filter_inside);
 
-bool isAllowedGoalModification(
-  const std::shared_ptr<RouteHandler> & route_handler, const bool left_side_parking);
-bool checkOriginalGoalIsInShoulder(
-  const std::shared_ptr<RouteHandler> & route_handler, const bool left_side_parking);
+bool isAllowedGoalModification(const std::shared_ptr<RouteHandler> & route_handler);
+bool checkOriginalGoalIsInShoulder(const std::shared_ptr<RouteHandler> & route_handler);
 
 // debug
 MarkerArray createPullOverAreaMarkerArray(

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

@@ -41,7 +41,7 @@ class FreespacePullOut : public PullOutPlannerBase
 
   PlannerType getPlannerType() override { return PlannerType::FREESPACE; }
 
-  boost::optional<PullOutPath> plan(Pose start_pose, Pose end_pose) override;
+  boost::optional<PullOutPath> plan(const Pose & start_pose, const Pose & end_pose) override;
 
 protected:
   std::unique_ptr<AbstractPlanningAlgorithm> planner_;

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

@@ -29,7 +29,7 @@ class GeometricPullOut : public PullOutPlannerBase
   explicit GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters);
 
   PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; };
-  boost::optional<PullOutPath> plan(Pose start_pose, Pose goal_pose) override;
+  boost::optional<PullOutPath> plan(const Pose & start_pose, const Pose & goal_pose) override;
 
   GeometricParallelParking planner_;
   ParallelParkingParameters parallel_parking_parameters_;

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

@@ -60,7 +60,7 @@ class PullOutPlannerBase
   }
 
   virtual PlannerType getPlannerType() = 0;
-  virtual boost::optional<PullOutPath> plan(Pose start_pose, Pose goal_pose) = 0;
+  virtual boost::optional<PullOutPath> plan(const Pose & start_pose, const Pose & goal_pose) = 0;
 
 protected:
   std::shared_ptr<const PlannerData> planner_data_;

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

@@ -37,7 +37,7 @@ class ShiftPullOut : public PullOutPlannerBase
     std::shared_ptr<LaneDepartureChecker> & lane_departure_checker);
 
   PlannerType getPlannerType() override { return PlannerType::SHIFT; };
-  boost::optional<PullOutPath> plan(Pose start_pose, Pose goal_pose) override;
+  boost::optional<PullOutPath> plan(const Pose & start_pose, const Pose & goal_pose) override;
 
   std::vector<PullOutPath> calcPullOutPaths(
     const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes,