fix(goal_planner): fix publishing new modified goal (#4302)

* fix(goal_planner): fix publishing new modified goal Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * style(pre-commit): autofix --------- Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Jul 18, 2023 · 77a7616 · 77a7616
1 parent 2dbb2a6
commit 77a7616
Show file tree

Hide file tree

Showing 3 changed files with 3 additions and 5 deletions.
diff --git a/...h_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/...h_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp
@@ -128,7 +128,6 @@ class GoalPlannerModule : public SceneModuleInterface
   // goal searcher
   std::shared_ptr<GoalSearcherBase> goal_searcher_;
   std::optional<GoalCandidate> modified_goal_pose_;
-  std::optional<size_t> prev_goal_id_;
   Pose refined_goal_pose_;
   GoalCandidates goal_candidates_;
 

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -575,8 +575,9 @@ void BehaviorPathPlannerNode::run()
   if (
     output.modified_goal &&
     /* has changed modified goal */ (
-      !planner_data_->prev_modified_goal ||
-      planner_data_->prev_modified_goal->uuid != output.modified_goal->uuid)) {
+      !planner_data_->prev_modified_goal || tier4_autoware_utils::calcDistance2d(
+                                              planner_data_->prev_modified_goal->pose.position,
+                                              output.modified_goal->pose.position) > 0.01)) {
     PoseWithUuidStamped modified_goal = *(output.modified_goal);
     modified_goal.header.stamp = path->header.stamp;
     planner_data_->prev_modified_goal = modified_goal;

diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
@@ -122,7 +122,6 @@ void GoalPlannerModule::resetStatus()
   pull_over_path_candidates_.clear();
   closest_start_pose_.reset();
   goal_candidates_.clear();
-  prev_goal_id_.reset();
 }
 
 // This function is needed for waiting for planner_data_
@@ -621,7 +620,6 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output)
   setDrivableAreaInfo(output);
 
   setModifiedGoal(output);
-  prev_goal_id_ = modified_goal_pose_->id;
 
   // set hazard and turn signal
   if (status_.has_decided_path) {