feat(goal_planner): divide Planners to isolated threads (#9514)

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

Author: soblin

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt

@@ -19,6 +19,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/util.cpp
   src/goal_planner_module.cpp
   src/manager.cpp
+  src/thread_data.cpp
   src/decision_state.cpp
 )
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -18,9 +18,7 @@
 #include "autoware/behavior_path_goal_planner_module/decision_state.hpp"
 #include "autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp"
 #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp"
-#include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp"
 #include "autoware/behavior_path_goal_planner_module/thread_data.hpp"
-#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp"
 #include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp"
 #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 
@@ -89,27 +87,44 @@ struct PullOverContextData
   PullOverContextData() = delete;
   explicit PullOverContextData(
     const bool is_stable_safe_path, const PredictedObjects & static_objects,
-    const PredictedObjects & dynamic_objects, std::optional<PullOverPath> && pull_over_path_opt,
-    const std::vector<PullOverPath> & pull_over_path_candidates,
-    const PathDecisionState & prev_state)
+    const PredictedObjects & dynamic_objects, const PathDecisionState & prev_state,
+    const bool is_stopped, const LaneParkingResponse & lane_parking_response,
+    const FreespaceParkingResponse & freespace_parking_response)
   : is_stable_safe_path(is_stable_safe_path),
     static_target_objects(static_objects),
     dynamic_target_objects(dynamic_objects),
-    pull_over_path_opt(pull_over_path_opt),
-    pull_over_path_candidates(pull_over_path_candidates),
     prev_state_for_debug(prev_state),
-    last_path_idx_increment_time(std::nullopt)
+    is_stopped(is_stopped),
+    lane_parking_response(lane_parking_response),
+    freespace_parking_response(freespace_parking_response)
   {
   }
-  const bool is_stable_safe_path;
-  const PredictedObjects static_target_objects;
-  const PredictedObjects dynamic_target_objects;
-  // TODO(soblin): due to deceleratePath(), this cannot be const member(velocity is modified by it)
+  // TODO(soblin): make following variables private
+  bool is_stable_safe_path;
+  PredictedObjects static_target_objects;
+  PredictedObjects dynamic_target_objects;
+  PathDecisionState prev_state_for_debug;
+  bool is_stopped;
+  LaneParkingResponse lane_parking_response;
+  FreespaceParkingResponse freespace_parking_response;
   std::optional<PullOverPath> pull_over_path_opt;
-  const std::vector<PullOverPath> pull_over_path_candidates;
-  // TODO(soblin): goal_candidate_from_thread, modifed_goal_pose_from_thread, closest_start_pose
-  const PathDecisionState prev_state_for_debug;
+  std::optional<rclcpp::Time> last_path_update_time;
   std::optional<rclcpp::Time> last_path_idx_increment_time;
+
+  void update(
+    const bool is_stable_safe_path_, const PredictedObjects static_target_objects_,
+    const PredictedObjects dynamic_target_objects_, const PathDecisionState prev_state_for_debug_,
+    const bool is_stopped_, const LaneParkingResponse & lane_parking_response_,
+    const FreespaceParkingResponse & freespace_parking_response_)
+  {
+    is_stable_safe_path = is_stable_safe_path_;
+    static_target_objects = static_target_objects_;
+    dynamic_target_objects = dynamic_target_objects_;
+    prev_state_for_debug = prev_state_for_debug_;
+    is_stopped = is_stopped_;
+    lane_parking_response = lane_parking_response_;
+    freespace_parking_response = freespace_parking_response_;
+  }
 };
 
 bool isOnModifiedGoal(
@@ -133,6 +148,11 @@ bool checkOccupancyGridCollision(
   const PathWithLaneId & path,
   const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map);
 
+// freespace parking
+std::optional<PullOverPath> planFreespacePath(
+  const FreespaceParkingRequest & req, const PredictedObjects & static_target_objects,
+  std::shared_ptr<PullOverPlannerBase> freespace_planner);
+
 bool isStopped(
   std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> & odometry_buffer,
   const nav_msgs::msg::Odometry::ConstSharedPtr self_odometry, const double duration_lower,
@@ -150,6 +170,61 @@ class ScopedFlag
   std::atomic<bool> & flag_;
 };
 
+class LaneParkingPlanner
+{
+public:
+  LaneParkingPlanner(
+    rclcpp::Node & node, std::mutex & lane_parking_mutex,
+    const std::optional<LaneParkingRequest> & request, LaneParkingResponse & response,
+    std::atomic<bool> & is_lane_parking_cb_running, const rclcpp::Logger & logger,
+    const GoalPlannerParameters & parameters);
+  rclcpp::Logger getLogger() const { return logger_; }
+  void onTimer();
+
+private:
+  std::mutex & mutex_;
+  const std::optional<LaneParkingRequest> & request_;
+  LaneParkingResponse & response_;
+  std::atomic<bool> & is_lane_parking_cb_running_;
+  rclcpp::Logger logger_;
+
+  std::vector<std::shared_ptr<PullOverPlannerBase>> pull_over_planners_;
+};
+
+class FreespaceParkingPlanner
+{
+public:
+  FreespaceParkingPlanner(
+    std::mutex & freespace_parking_mutex, const std::optional<FreespaceParkingRequest> & request,
+    FreespaceParkingResponse & response, std::atomic<bool> & is_freespace_parking_cb_running,
+    const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock,
+    const std::shared_ptr<PullOverPlannerBase> freespace_planner)
+  : mutex_(freespace_parking_mutex),
+    request_(request),
+    response_(response),
+    is_freespace_parking_cb_running_(is_freespace_parking_cb_running),
+    logger_(logger),
+    clock_(clock),
+    freespace_planner_(freespace_planner)
+  {
+  }
+  void onTimer();
+
+private:
+  std::mutex & mutex_;
+  const std::optional<FreespaceParkingRequest> & request_;
+  FreespaceParkingResponse & response_;
+  std::atomic<bool> & is_freespace_parking_cb_running_;
+  rclcpp::Logger logger_;
+  rclcpp::Clock::SharedPtr clock_;
+
+  std::shared_ptr<PullOverPlannerBase> freespace_planner_;
+
+  bool isStuck(
+    const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects,
+    const FreespaceParkingRequest & req) const;
+};
+
 class GoalPlannerModule : public SceneModuleInterface
 {
 public:
@@ -217,48 +292,16 @@ 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,
-      const BehaviorModuleOutput & previous_module_output_)
-    {
-      initializeOccupancyGridMap(planner_data, parameters);
-      previous_module_output = previous_module_output_;
-      last_previous_module_output = previous_module_output_;
-    };
-    GoalPlannerParameters parameters;
-    autoware::universe_utils::LinearRing2d vehicle_footprint;
-
-    PlannerData planner_data;
-    ModuleStatus current_status;
-    BehaviorModuleOutput previous_module_output;
-    BehaviorModuleOutput last_previous_module_output;  //<! previous "previous_module_output"
-    GoalCandidates goal_candidates;  //<! only the positional information of goal_candidates
-
-    // collision detector
-    // need to be shared_ptr to be used in planner and goal searcher
-    std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map;
-
-    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 PlannerData & planner_data,
-      const ModuleStatus & current_status, const BehaviorModuleOutput & previous_module_output,
-      const autoware::universe_utils::LinearRing2d & vehicle_footprint,
-      const GoalCandidates & goal_candidates);
-
-  private:
-    void initializeOccupancyGridMap(
-      const PlannerData & planner_data, const GoalPlannerParameters & parameters);
-  };
-  std::optional<GoalPlannerData> gp_planner_data_{std::nullopt};
-  std::mutex gp_planner_data_mutex_;
+  std::pair<LaneParkingResponse, FreespaceParkingResponse> syncWithThreads();
+
+  // NOTE: never access to following variables except in updateData()!!!
+  std::mutex lane_parking_mutex_;
+  std::optional<LaneParkingRequest> lane_parking_request_;
+  LaneParkingResponse lane_parking_response_;
+
+  std::mutex freespace_parking_mutex_;
+  std::optional<FreespaceParkingRequest> freespace_parking_request_;
+  FreespaceParkingResponse freespace_parking_response_;
 
   /*
    * state transitions and plan function used in each state
@@ -297,9 +340,6 @@ class GoalPlannerModule : public SceneModuleInterface
 
   autoware::vehicle_info_utils::VehicleInfo vehicle_info_{};
 
-  // planner
-  std::vector<std::shared_ptr<PullOverPlannerBase>> pull_over_planners_;
-  std::unique_ptr<PullOverPlannerBase> freespace_planner_;
   std::unique_ptr<FixedGoalPlannerBase> fixed_goal_planner_;
 
   // goal searcher
@@ -317,11 +357,6 @@ class GoalPlannerModule : public SceneModuleInterface
 
   autoware::universe_utils::LinearRing2d vehicle_footprint_;
 
-  std::recursive_mutex mutex_;
-  mutable ThreadSafeData thread_safe_data_;
-
-  // TODO(soblin): organize part of thread_safe_data and previous data to PullOverContextData
-  // context_data_ is initialized in updateData(), used in plan() and refreshed in postProcess()
   std::optional<PullOverContextData> context_data_{std::nullopt};
   // path_decision_controller is updated in updateData(), and used in plan()
   PathDecisionStateController path_decision_controller_{getLogger()};
@@ -369,12 +404,7 @@ class GoalPlannerModule : public SceneModuleInterface
   // status
   bool hasFinishedCurrentPath(const PullOverContextData & ctx_data);
   double calcModuleRequestLength() const;
-  bool isStuck(
-    const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects,
-    const std::shared_ptr<const PlannerData> planner_data,
-    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const GoalPlannerParameters & parameters);
-  void decideVelocity();
+  void decideVelocity(PullOverPath & pull_over_path);
   void updateStatus(const BehaviorModuleOutput & output);
 
   // validation
@@ -385,21 +415,13 @@ class GoalPlannerModule : public SceneModuleInterface
   bool isCrossingPossible(
     const Pose & start_pose, const Pose & end_pose, const lanelet::ConstLanelets lanes) const;
   bool isCrossingPossible(const PullOverPath & pull_over_path) const;
-  bool hasEnoughTimePassedSincePathUpdate(const double duration) const;
-
-  // freespace parking
-  bool planFreespacePath(
-    std::shared_ptr<const PlannerData> planner_data,
-    const std::shared_ptr<GoalSearcherBase> goal_searcher, const GoalCandidates & goal_candidates,
-    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const PredictedObjects & static_target_objects);
   bool canReturnToLaneParking(const PullOverContextData & context_data);
 
   // plan pull over path
-  BehaviorModuleOutput planPullOver(const PullOverContextData & context_data);
-  BehaviorModuleOutput planPullOverAsOutput(const PullOverContextData & context_data);
+  BehaviorModuleOutput planPullOver(PullOverContextData & context_data);
+  BehaviorModuleOutput planPullOverAsOutput(PullOverContextData & context_data);
   BehaviorModuleOutput planPullOverAsCandidate(
-    const PullOverContextData & context_data, const std::string & detail);
+    PullOverContextData & context_data, const std::string & detail);
   std::optional<PullOverPath> selectPullOverPath(
     const PullOverContextData & context_data,
     const std::vector<PullOverPath> & pull_over_path_candidates,
@@ -411,7 +433,9 @@ class GoalPlannerModule : public SceneModuleInterface
     const PullOverContextData & context_data, BehaviorModuleOutput & output) const;
 
   // output setter
-  void setOutput(const PullOverContextData & context_data, BehaviorModuleOutput & output);
+  void setOutput(
+    const std::optional<PullOverPath> selected_pull_over_path_with_velocity_opt,
+    const PullOverContextData & context_data, BehaviorModuleOutput & output);
 
   void setModifiedGoal(
     const PullOverContextData & context_data, BehaviorModuleOutput & output) const;
@@ -421,10 +445,6 @@ class GoalPlannerModule : public SceneModuleInterface
   TurnSignalInfo calcTurnSignalInfo(const PullOverContextData & context_data);
   std::optional<lanelet::Id> ignore_signal_{std::nullopt};
 
-  // timer for generating pull over path candidates in a separate thread
-  void onTimer();
-  void onFreespaceParkingTimer();
-
   // steering factor
   void updateSteeringFactor(
     const PullOverContextData & context_data, const std::array<Pose, 2> & pose,