feat(goal_planner): fix non-thread-safe access in goal_planner (revert of #6809 and minor fix) (#6965)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp

@@ -16,7 +16,6 @@
 #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_
 
 #include "behavior_path_goal_planner_module/pull_over_planner_base.hpp"
-#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
 #include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp"
 
 #include <lane_departure_checker/lane_departure_checker.hpp>
@@ -34,9 +33,7 @@ class GeometricPullOver : public PullOverPlannerBase
 public:
   GeometricPullOver(
     rclcpp::Node & node, const GoalPlannerParameters & parameters,
-    const LaneDepartureChecker & lane_departure_checker,
-    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const bool is_forward);
+    const LaneDepartureChecker & lane_departure_checker, const bool is_forward);
 
   PullOverPlannerType getPlannerType() const override
   {
@@ -61,7 +58,6 @@ class GeometricPullOver : public PullOverPlannerBase
 protected:
   ParallelParkingParameters parallel_parking_parameters_;
   LaneDepartureChecker lane_departure_checker_{};
-  std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_;
   bool is_forward_{true};
   bool left_side_parking_{true};
 

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -91,6 +91,33 @@ public:                                                       \
   DEFINE_SETTER_WITH_MUTEX(TYPE, NAME)              \
   DEFINE_GETTER_WITH_MUTEX(TYPE, NAME)
 
+enum class DecidingPathStatus {
+  NOT_DECIDED,
+  DECIDING,
+  DECIDED,
+};
+using DecidingPathStatusWithStamp = std::pair<DecidingPathStatus, rclcpp::Time>;
+
+struct PreviousPullOverData
+{
+  struct SafetyStatus
+  {
+    std::optional<rclcpp::Time> safe_start_time{};
+    bool is_safe{false};
+  };
+
+  void reset()
+  {
+    found_path = false;
+    safety_status = SafetyStatus{};
+    deciding_path_status = DecidingPathStatusWithStamp{};
+  }
+
+  bool found_path{false};
+  SafetyStatus safety_status{};
+  DecidingPathStatusWithStamp deciding_path_status{};
+};
+
 class ThreadSafeData
 {
 public:
@@ -145,6 +172,9 @@ class ThreadSafeData
   void set(const std::shared_ptr<PullOverPath> & arg) { set_pull_over_path(arg); }
   void set(const PullOverPath & arg) { set_pull_over_path(arg); }
   void set(const GoalCandidate & arg) { set_modified_goal_pose(arg); }
+  void set(const BehaviorModuleOutput & arg) { set_last_previous_module_output(arg); }
+  void set(const PreviousPullOverData & arg) { set_prev_data(arg); }
+  void set(const CollisionCheckDebugMap & arg) { set_collision_check(arg); }
 
   void clearPullOverPath()
   {
@@ -182,6 +212,8 @@ class ThreadSafeData
     last_path_update_time_ = std::nullopt;
     last_path_idx_increment_time_ = std::nullopt;
     closest_start_pose_ = std::nullopt;
+    last_previous_module_output_ = std::nullopt;
+    prev_data_.reset();
   }
 
   DEFINE_GETTER_WITH_MUTEX(std::shared_ptr<PullOverPath>, pull_over_path)
@@ -193,6 +225,9 @@ class ThreadSafeData
   DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates)
   DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional<GoalCandidate>, modified_goal_pose)
   DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional<Pose>, closest_start_pose)
+  DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional<BehaviorModuleOutput>, last_previous_module_output)
+  DEFINE_SETTER_GETTER_WITH_MUTEX(PreviousPullOverData, prev_data)
+  DEFINE_SETTER_GETTER_WITH_MUTEX(CollisionCheckDebugMap, collision_check)
 
 private:
   std::shared_ptr<PullOverPath> pull_over_path_{nullptr};
@@ -203,6 +238,9 @@ class ThreadSafeData
   std::optional<rclcpp::Time> last_path_update_time_;
   std::optional<rclcpp::Time> last_path_idx_increment_time_;
   std::optional<Pose> closest_start_pose_{};
+  std::optional<BehaviorModuleOutput> last_previous_module_output_{};
+  PreviousPullOverData prev_data_{};
+  CollisionCheckDebugMap collision_check_{};
 
   std::recursive_mutex & mutex_;
   rclcpp::Clock::SharedPtr clock_;
@@ -234,33 +272,6 @@ struct LastApprovalData
   Pose pose{};
 };
 
-enum class DecidingPathStatus {
-  NOT_DECIDED,
-  DECIDING,
-  DECIDED,
-};
-using DecidingPathStatusWithStamp = std::pair<DecidingPathStatus, rclcpp::Time>;
-
-struct PreviousPullOverData
-{
-  struct SafetyStatus
-  {
-    std::optional<rclcpp::Time> safe_start_time{};
-    bool is_safe{false};
-  };
-
-  void reset()
-  {
-    found_path = false;
-    safety_status = SafetyStatus{};
-    deciding_path_status = DecidingPathStatusWithStamp{};
-  }
-
-  bool found_path{false};
-  SafetyStatus safety_status{};
-  DecidingPathStatusWithStamp deciding_path_status{};
-};
-
 // store stop_pose_ pointer with reason string
 struct PoseWithString
 {
@@ -363,6 +374,50 @@ class GoalPlannerModule : public SceneModuleInterface
   CandidateOutput planCandidate() const override { return CandidateOutput{}; }
 
 private:
+  /**
+   * @brief shared data for onTimer(onTimer/onFreespaceParkingTimer just read this)
+   */
+  struct GoalPlannerData
+  {
+    GoalPlannerData(const PlannerData & planner_data, const GoalPlannerParameters & parameters)
+    {
+      initializeOccupancyGridMap(planner_data, parameters);
+    };
+    GoalPlannerParameters parameters;
+    std::shared_ptr<EgoPredictedPathParams> ego_predicted_path_params;
+    std::shared_ptr<ObjectsFilteringParams> objects_filtering_params;
+    std::shared_ptr<SafetyCheckParams> safety_check_params;
+    tier4_autoware_utils::LinearRing2d vehicle_footprint;
+
+    PlannerData planner_data;
+    ModuleStatus current_status;
+    BehaviorModuleOutput previous_module_output;
+    // collision detector
+    // need to be shared_ptr to be used in planner and goal searcher
+    std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map;
+    std::shared_ptr<GoalSearcherBase> goal_searcher;
+
+    const BehaviorModuleOutput & getPreviousModuleOutput() const { return previous_module_output; }
+    const ModuleStatus & getCurrentStatus() const { return current_status; }
+    void updateOccupancyGrid();
+    GoalPlannerData clone() const;
+    void update(
+      const GoalPlannerParameters & parameters,
+      const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params_,
+      const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params_,
+      const std::shared_ptr<SafetyCheckParams> & safety_check_params_,
+      const PlannerData & planner_data, const ModuleStatus & current_status,
+      const BehaviorModuleOutput & previous_module_output,
+      const std::shared_ptr<GoalSearcherBase> goal_searcher_,
+      const tier4_autoware_utils::LinearRing2d & vehicle_footprint);
+
+  private:
+    void initializeOccupancyGridMap(
+      const PlannerData & planner_data, const GoalPlannerParameters & parameters);
+  };
+  std::optional<GoalPlannerData> gp_planner_data_{std::nullopt};
+  std::mutex gp_planner_data_mutex_;
+
   // Flag class for managing whether a certain callback is running in multi-threading
   class ScopedFlag
   {
@@ -420,9 +475,10 @@ class GoalPlannerModule : public SceneModuleInterface
   // goal searcher
   std::shared_ptr<GoalSearcherBase> goal_searcher_;
 
-  // collision detector
-  // need to be shared_ptr to be used in planner and goal searcher
-  std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_;
+  // NOTE: this is latest occupancy_grid_map pointer which the local planner_data on
+  // onFreespaceParkingTimer thread storage may point to while calculation.
+  // onTimer/onFreespaceParkingTimer and their callees MUST NOT use this
+  std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_{nullptr};
 
   // check stopped and stuck state
   std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_stopped_;
@@ -431,10 +487,10 @@ class GoalPlannerModule : public SceneModuleInterface
   tier4_autoware_utils::LinearRing2d vehicle_footprint_;
 
   std::recursive_mutex mutex_;
-  ThreadSafeData thread_safe_data_;
+  // TODO(Mamoru Sobue): isSafePath() modifies ThreadSafeData::check_collision, avoid this mutable
+  mutable ThreadSafeData thread_safe_data_;
 
   std::unique_ptr<LastApprovalData> last_approval_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.
@@ -460,11 +516,12 @@ class GoalPlannerModule : public SceneModuleInterface
   mutable PoseWithString debug_stop_pose_with_info_;
 
   // collision check
-  void initializeOccupancyGridMap();
-  void updateOccupancyGrid();
-  bool checkOccupancyGridCollision(const PathWithLaneId & path) const;
+  bool checkOccupancyGridCollision(
+    const PathWithLaneId & path,
+    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map) const;
   bool checkObjectsCollision(
-    const PathWithLaneId & path, const double collision_check_margin,
+    const PathWithLaneId & path, const std::shared_ptr<const PlannerData> planner_data,
+    const GoalPlannerParameters & parameters, const double collision_check_margin,
     const bool extract_static_objects, const bool update_debug_data = false) const;
 
   // goal seach
@@ -487,13 +544,39 @@ class GoalPlannerModule : public SceneModuleInterface
   bool isStopped(
     std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> & odometry_buffer, const double time);
   bool hasFinishedCurrentPath();
-  bool isOnModifiedGoal() const;
+  bool isOnModifiedGoal(const Pose & current_pose, const GoalPlannerParameters & parameters) const;
   double calcModuleRequestLength() const;
-  bool needPathUpdate(const double path_update_duration) const;
-  bool isStuck();
-  bool hasDecidedPath() const;
-  bool hasNotDecidedPath() const;
-  DecidingPathStatusWithStamp checkDecidingPathStatus() const;
+  bool needPathUpdate(
+    const Pose & current_pose, const double path_update_duration,
+    const GoalPlannerParameters & parameters) const;
+  bool isStuck(
+    const std::shared_ptr<const PlannerData> planner_data,
+    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
+    const GoalPlannerParameters & parameters);
+  bool hasDecidedPath(
+    const std::shared_ptr<const PlannerData> planner_data,
+    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
+    const GoalPlannerParameters & parameters,
+    const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
+    const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
+    const std::shared_ptr<SafetyCheckParams> & safety_check_params,
+    const std::shared_ptr<GoalSearcherBase> goal_searcher) const;
+  bool hasNotDecidedPath(
+    const std::shared_ptr<const PlannerData> planner_data,
+    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
+    const GoalPlannerParameters & parameters,
+    const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
+    const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
+    const std::shared_ptr<SafetyCheckParams> & safety_check_params,
+    const std::shared_ptr<GoalSearcherBase> goal_searcher) const;
+  DecidingPathStatusWithStamp checkDecidingPathStatus(
+    const std::shared_ptr<const PlannerData> planner_data,
+    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
+    const GoalPlannerParameters & parameters,
+    const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
+    const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
+    const std::shared_ptr<SafetyCheckParams> & safety_check_params,
+    const std::shared_ptr<GoalSearcherBase> goal_searcher) const;
   void decideVelocity();
   bool foundPullOverPath() const;
   void updateStatus(const BehaviorModuleOutput & output);
@@ -508,7 +591,10 @@ class GoalPlannerModule : public SceneModuleInterface
   bool hasEnoughTimePassedSincePathUpdate(const double duration) const;
 
   // freespace parking
-  bool planFreespacePath();
+  bool planFreespacePath(
+    std::shared_ptr<const PlannerData> planner_data,
+    const std::shared_ptr<GoalSearcherBase> goal_searcher,
+    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map);
   bool canReturnToLaneParking();
 
   // plan pull over path
@@ -537,10 +623,12 @@ class GoalPlannerModule : public SceneModuleInterface
   TurnSignalInfo calcTurnSignalInfo();
   std::optional<lanelet::Id> ignore_signal_{std::nullopt};
 
-  std::optional<BehaviorModuleOutput> last_previous_module_output_{};
-  bool hasPreviousModulePathShapeChanged() const;
-  bool hasDeviatedFromLastPreviousModulePath() const;
-  bool hasDeviatedFromCurrentPreviousModulePath() const;
+  bool hasPreviousModulePathShapeChanged(const BehaviorModuleOutput & previous_module_output) const;
+  bool hasDeviatedFromLastPreviousModulePath(
+    const std::shared_ptr<const PlannerData> planner_data) const;
+  bool hasDeviatedFromCurrentPreviousModulePath(
+    const std::shared_ptr<const PlannerData> planner_data,
+    const BehaviorModuleOutput & previous_module_output) const;
 
   // timer for generating pull over path candidates in a separate thread
   void onTimer();
@@ -556,16 +644,22 @@ class GoalPlannerModule : public SceneModuleInterface
   // safety check
   void initializeSafetyCheckParameters();
   SafetyCheckParams createSafetyCheckParams() const;
+  /*
   void updateSafetyCheckTargetObjectsData(
     const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane,
     const std::vector<PoseWithVelocityStamped> & ego_predicted_path) const;
+  */
   /**
    * @brief Checks if the current path is safe.
    * @return std::pair<bool, bool>
    *         first: If the path is safe for a certain period of time, true.
    *         second: If the path is safe in the current state, true.
    */
-  std::pair<bool, bool> isSafePath() const;
+  std::pair<bool, bool> isSafePath(
+    const std::shared_ptr<const PlannerData> planner_data, const GoalPlannerParameters & parameters,
+    const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
+    const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
+    const std::shared_ptr<SafetyCheckParams> & safety_check_params) const;
 
   // debug
   void setDebugData();

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp

@@ -29,33 +29,45 @@ using BasicPolygons2d = std::vector<lanelet::BasicPolygon2d>;
 class GoalSearcher : public GoalSearcherBase
 {
 public:
-  GoalSearcher(
-    const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint,
-    const std::shared_ptr<OccupancyGridBasedCollisionDetector> & occupancy_grid_map);
+  GoalSearcher(const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint);
 
-  GoalCandidates search() override;
-  void update(GoalCandidates & goal_candidates) const override;
+  GoalCandidates search(
+    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
+    const std::shared_ptr<const PlannerData> & planner_data) override;
+  void update(
+    GoalCandidates & goal_candidates,
+    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
+    const std::shared_ptr<const PlannerData> & planner_data) 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;
+    const GoalCandidates & goal_candidates,
+    const std::shared_ptr<const PlannerData> & planner_data) const override;
   bool isSafeGoalWithMarginScaleFactor(
-    const GoalCandidate & goal_candidate, const double margin_scale_factor) const override;
+    const GoalCandidate & goal_candidate, const double margin_scale_factor,
+    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
+    const std::shared_ptr<const PlannerData> & planner_data) const override;
 
 private:
   void countObjectsToAvoid(
-    GoalCandidates & goal_candidates, const PredictedObjects & objects) const;
-  void createAreaPolygons(std::vector<Pose> original_search_poses);
-  bool checkCollision(const Pose & pose, const PredictedObjects & objects) const;
+    GoalCandidates & goal_candidates, const PredictedObjects & objects,
+    const std::shared_ptr<const PlannerData> & planner_data) const;
+  void createAreaPolygons(
+    std::vector<Pose> original_search_poses,
+    const std::shared_ptr<const PlannerData> & planner_data);
+  bool checkCollision(
+    const Pose & pose, const PredictedObjects & objects,
+    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map) const;
   bool checkCollisionWithLongitudinalDistance(
-    const Pose & ego_pose, const PredictedObjects & objects) const;
+    const Pose & ego_pose, const PredictedObjects & objects,
+    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
+    const std::shared_ptr<const PlannerData> & planner_data) const;
   BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
   BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
   bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const;
 
   LinearRing2d vehicle_footprint_{};
-  std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_{};
   bool left_side_parking_{true};
 };
 }  // namespace behavior_path_planner