feat(behavior_path_planner): update behavior path panner for v0.20.1

planning/behavior_path_dynamic_avoidance_module/src/scene.cpp

@@ -397,6 +397,7 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan()
     current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
   output.reference_path = getPreviousModuleOutput().reference_path;
   output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
+  output.modified_goal = getPreviousModuleOutput().modified_goal;
 
   return output;
 }

planning/behavior_path_goal_planner_module/README.md

@@ -155,6 +155,7 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio
 | 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 | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0           |
+| detection_bound_offset                                       | [m]  | double | expand pull over lane with this offset to make detection area for collision check of path generation     | 15.0          |
 
 ## **Goal Search**
 

planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml

@@ -38,6 +38,7 @@
         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
+        detection_bound_offset: 15.0
 
       # pull over
       pull_over:

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -230,6 +230,13 @@ struct LastApprovalData
   Pose pose{};
 };
 
+enum class DecidingPathStatus {
+  NOT_DECIDED,
+  DECIDING,
+  DECIDED,
+};
+using DecidingPathStatusWithStamp = std::pair<DecidingPathStatus, rclcpp::Time>;
+
 struct PreviousPullOverData
 {
   struct SafetyStatus
@@ -240,18 +247,37 @@ struct PreviousPullOverData
 
   void reset()
   {
-    stop_path = nullptr;
-    stop_path_after_approval = nullptr;
     found_path = false;
     safety_status = SafetyStatus{};
-    has_decided_path = false;
+    deciding_path_status = DecidingPathStatusWithStamp{};
   }
 
-  std::shared_ptr<PathWithLaneId> stop_path{nullptr};
-  std::shared_ptr<PathWithLaneId> stop_path_after_approval{nullptr};
   bool found_path{false};
   SafetyStatus safety_status{};
-  bool has_decided_path{false};
+  DecidingPathStatusWithStamp deciding_path_status{};
+};
+
+// store stop_pose_ pointer with reason string
+struct PoseWithString
+{
+  std::optional<Pose> * pose;
+  std::string string;
+
+  explicit PoseWithString(std::optional<Pose> * shared_pose) : pose(shared_pose), string("") {}
+
+  void set(const Pose & new_pose, const std::string & new_string)
+  {
+    *pose = new_pose;
+    string = new_string;
+  }
+
+  void set(const std::string & new_string) { string = new_string; }
+
+  void clear()
+  {
+    pose->reset();
+    string = "";
+  }
 };
 
 class GoalPlannerModule : public SceneModuleInterface
@@ -364,7 +390,7 @@ class GoalPlannerModule : public SceneModuleInterface
   ThreadSafeData thread_safe_data_;
 
   std::unique_ptr<LastApprovalData> last_approval_data_{nullptr};
-  PreviousPullOverData prev_data_{nullptr};
+  PreviousPullOverData prev_data_{};
 
   // approximate distance from the start point to the end point of pull_over.
   // this is used as an assumed value to decelerate, etc., before generating the actual path.
@@ -385,14 +411,15 @@ class GoalPlannerModule : public SceneModuleInterface
 
   // debug
   mutable GoalPlannerDebugData debug_data_;
+  mutable PoseWithString debug_stop_pose_with_info_;
 
   // collision check
   void initializeOccupancyGridMap();
   void updateOccupancyGrid();
   bool checkOccupancyGridCollision(const PathWithLaneId & path) const;
   bool checkObjectsCollision(
     const PathWithLaneId & path, const double collision_check_margin,
-    const bool update_debug_data = false) const;
+    const bool extract_static_objects, const bool update_debug_data = false) const;
 
   // goal seach
   Pose calcRefinedGoal(const Pose & goal_pose) const;
@@ -404,7 +431,7 @@ class GoalPlannerModule : public SceneModuleInterface
   void decelerateBeforeSearchStart(
     const Pose & search_start_offset_pose, PathWithLaneId & path) const;
   PathWithLaneId generateStopPath() const;
-  PathWithLaneId generateFeasibleStopPath() const;
+  PathWithLaneId generateFeasibleStopPath(const PathWithLaneId & path) const;
 
   void keepStoppedWithCurrentPath(PathWithLaneId & path) const;
   double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const;
@@ -419,6 +446,8 @@ class GoalPlannerModule : public SceneModuleInterface
   bool needPathUpdate(const double path_update_duration) const;
   bool isStuck();
   bool hasDecidedPath() const;
+  bool hasNotDecidedPath() const;
+  DecidingPathStatusWithStamp checkDecidingPathStatus() const;
   void decideVelocity();
   bool foundPullOverPath() const;
   void updateStatus(const BehaviorModuleOutput & output);
@@ -453,19 +482,8 @@ class GoalPlannerModule : public SceneModuleInterface
 
   // output setter
   void setOutput(BehaviorModuleOutput & output) const;
-  void setStopPath(BehaviorModuleOutput & output) const;
-  void updatePreviousData(const BehaviorModuleOutput & output);
+  void updatePreviousData();
 
-  /**
-   * @brief Sets a stop path in the current path based on safety conditions and previous paths.
-   *
-   * This function sets a stop path in the current path. Depending on whether the previous safety
-   * judgement against dynamic objects were safe or if a previous stop path existed, it either
-   * generates a new stop path or uses the previous stop path.
-   *
-   * @param output BehaviorModuleOutput
-   */
-  void setStopPathFromCurrentPath(BehaviorModuleOutput & output) const;
   void setModifiedGoal(BehaviorModuleOutput & output) const;
   void setTurnSignalInfo(BehaviorModuleOutput & output) const;
 

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp

@@ -74,6 +74,7 @@ struct GoalPlannerParameters
   double object_recognition_collision_check_margin{0.0};
   double object_recognition_collision_check_max_extra_stopping_margin{0.0};
   double th_moving_object_velocity{0.0};
+  double detection_bound_offset{0.0};
 
   // pull over general params
   double pull_over_minimum_request_length{0.0};

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp

@@ -35,8 +35,13 @@ class GoalSearcher : public GoalSearcherBase
 
   GoalCandidates search() override;
   void update(GoalCandidates & goal_candidates) const override;
+
+  // todo(kosuke55):　Functions for this specific use should not be in the interface,
+  // so it is better to consider interface design when we implement other goal searchers.
   GoalCandidate getClosetGoalCandidateAlongLanes(
     const GoalCandidates & goal_candidates) const override;
+  bool isSafeGoalWithMarginScaleFactor(
+    const GoalCandidate & goal_candidate, const double margin_scale_factor) const override;
 
 private:
   void countObjectsToAvoid(

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp

@@ -60,6 +60,8 @@ class GoalSearcherBase
   virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; }
   virtual GoalCandidate getClosetGoalCandidateAlongLanes(
     const GoalCandidates & goal_candidates) const = 0;
+  virtual bool isSafeGoalWithMarginScaleFactor(
+    const GoalCandidate & goal_candidate, const double margin_scale_factor) const = 0;
 
 protected:
   GoalPlannerParameters parameters_{};

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp

@@ -45,9 +45,19 @@ using visualization_msgs::msg::MarkerArray;
 lanelet::ConstLanelets getPullOverLanes(
   const RouteHandler & route_handler, const bool left_side, const double backward_distance,
   const double forward_distance);
+lanelet::ConstLanelets generateExpandedPullOverLanes(
+  const RouteHandler & route_handler, const bool left_side, const double backward_distance,
+  const double forward_distance, double bound_offset);
+PredictedObjects extractObjectsInExpandedPullOverLanes(
+  const RouteHandler & route_handler, const bool left_side, const double backward_distance,
+  const double forward_distance, double bound_offset, const PredictedObjects & objects);
 PredictedObjects filterObjectsByLateralDistance(
   const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects,
   const double distance_thresh, const bool filter_inside);
+PredictedObjects extractStaticObjectsInExpandedPullOverLanes(
+  const RouteHandler & route_handler, const bool left_side, const double backward_distance,
+  const double forward_distance, double bound_offset, const PredictedObjects & objects,
+  const double velocity_thresh);
 
 double calcLateralDeviationBetweenPaths(
   const PathWithLaneId & reference_path, const PathWithLaneId & target_path);