refactor(goal_planner): remove modified_goal in ThreadDafeData (#9010)

Signed-off-by: Mamoru Sobue <[email protected]>
autowarefoundation · Oct 9, 2024 · 41e08dc · 41e08dc
1 parent 3eafb9b
commit 41e08dc
Show file tree

Hide file tree

Showing 4 changed files with 80 additions and 79 deletions.
diff --git a/...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
@@ -372,10 +372,13 @@ class GoalPlannerModule : public SceneModuleInterface
   bool isStopped(
     std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> & odometry_buffer, const double time);
   bool hasFinishedCurrentPath(const PullOverContextData & ctx_data);
-  bool isOnModifiedGoal(const Pose & current_pose, const GoalPlannerParameters & parameters) const;
+  bool isOnModifiedGoal(
+    const Pose & current_pose, const std::optional<GoalCandidate> & modified_goal_opt,
+    const GoalPlannerParameters & parameters) const;
   double calcModuleRequestLength() const;
   bool needPathUpdate(
     const Pose & current_pose, const double path_update_duration,
+    const std::optional<GoalCandidate> & modified_goal_opt,
     const GoalPlannerParameters & parameters) const;
   bool isStuck(
     const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects,
@@ -406,7 +409,7 @@ class GoalPlannerModule : public SceneModuleInterface
   BehaviorModuleOutput planPullOver(const PullOverContextData & context_data);
   BehaviorModuleOutput planPullOverAsOutput(const PullOverContextData & context_data);
   BehaviorModuleOutput planPullOverAsCandidate(const PullOverContextData & context_data);
-  std::optional<std::pair<PullOverPath, GoalCandidate>> selectPullOverPath(
+  std::optional<PullOverPath> selectPullOverPath(
     const PullOverContextData & context_data,
     const std::vector<PullOverPath> & pull_over_path_candidates,
     const GoalCandidates & goal_candidates) const;

diff --git a/...e/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/...e/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp
@@ -54,8 +54,8 @@ struct PullOverPath
   size_t goal_id() const { return modified_goal_pose_.id; }
   size_t id() const { return id_; }
   Pose start_pose() const { return start_pose_; }
-  Pose end_pose() const { return modified_goal_pose_.goal_pose; }
-  GoalCandidate modified_goal_pose() const { return modified_goal_pose_; }
+  Pose modified_goal_pose() const { return modified_goal_pose_.goal_pose; }
+  GoalCandidate modified_goal() const { return modified_goal_pose_; }
 
   std::vector<PathWithLaneId> & partial_paths() { return partial_paths_; }
   const std::vector<PathWithLaneId> & partial_paths() const { return partial_paths_; }

diff --git a/...th_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp b/...th_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp
@@ -105,22 +105,12 @@ class ThreadSafeData
     return true;
   }
 
-  std::optional<PullOverPlannerType> getPullOverPlannerType() const
-  {
-    const std::lock_guard<std::recursive_mutex> lock(mutex_);
-    if (!pull_over_path_) {
-      return std::nullopt;
-    }
-    return pull_over_path_->type();
-  };
-
   void reset()
   {
     const std::lock_guard<std::recursive_mutex> lock(mutex_);
     pull_over_path_ = nullptr;
     pull_over_path_candidates_.clear();
     goal_candidates_.clear();
-    modified_goal_pose_ = std::nullopt;
     last_path_update_time_ = std::nullopt;
     last_path_idx_increment_time_ = std::nullopt;
     closest_start_pose_ = std::nullopt;
@@ -135,7 +125,6 @@ class ThreadSafeData
 
   DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector<PullOverPath>, pull_over_path_candidates)
   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(
@@ -168,7 +157,6 @@ class ThreadSafeData
   void set_no_lock(const std::vector<PullOverPath> & arg) { pull_over_path_candidates_ = arg; }
   void set_no_lock(const std::shared_ptr<PullOverPath> & arg) { set_pull_over_path_no_lock(arg); }
   void set_no_lock(const PullOverPath & arg) { set_pull_over_path_no_lock(arg); }
-  void set_no_lock(const GoalCandidate & arg) { modified_goal_pose_ = arg; }
   void set_no_lock(const BehaviorModuleOutput & arg) { last_previous_module_output_ = arg; }
   void set_no_lock(const utils::path_safety_checker::CollisionCheckDebugMap & arg)
   {
@@ -179,7 +167,6 @@ class ThreadSafeData
   std::shared_ptr<PullOverPath> lane_parking_pull_over_path_{nullptr};
   std::vector<PullOverPath> pull_over_path_candidates_;
   GoalCandidates goal_candidates_{};
-  std::optional<GoalCandidate> modified_goal_pose_;
   std::optional<rclcpp::Time> last_path_update_time_;
   std::optional<rclcpp::Time> last_path_idx_increment_time_;
   std::optional<Pose> closest_start_pose_{};