fix(goal_planenr): not reset when receving new route (autowarefoundat…

…ion#3966) * fix(goal_planenr): not reset when receving new route Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix for old archi Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> --------- Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
tier4 · Aug 22, 2023 · 8a158e4 · 8a158e4
1 parent b9a8043
commit 8a158e4
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 16 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
@@ -162,8 +162,10 @@ class GoalPlannerModule : public SceneModuleInterface
 
   tier4_autoware_utils::LinearRing2d vehicle_footprint_;
 
-  // save last time and pose
+// save last time and pose
+#ifdef USE_OLD_ARCHITECTURE
   std::unique_ptr<rclcpp::Time> last_received_time_;
+#endif
   std::unique_ptr<rclcpp::Time> last_approved_time_;
   std::unique_ptr<rclcpp::Time> last_increment_time_;
   std::unique_ptr<rclcpp::Time> last_path_update_time_;

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
@@ -500,26 +500,37 @@ void GoalPlannerModule::returnToLaneParking()
 
 void GoalPlannerModule::generateGoalCandidates()
 {
-  // initialize when receiving new route
   const auto & route_handler = planner_data_->route_handler;
+
+// with old architecture, module instance is not cleared when new route is received
+// so need to reset status here.
+#ifdef USE_OLD_ARCHITECTURE
+  // initialize when receiving new route
   if (!last_received_time_ || *last_received_time_ != route_handler->getRouteHeader().stamp) {
-    // Initialize parallel parking planner status
+    // this process causes twice reset when receiving first route.
     resetStatus();
-
-    // calculate goal candidates
-    const Pose goal_pose = route_handler->getGoalPose();
-    refined_goal_pose_ = calcRefinedGoal(goal_pose);
-    if (allow_goal_modification_) {
-      goal_searcher_->setPlannerData(planner_data_);
-      goal_candidates_ = goal_searcher_->search(refined_goal_pose_);
-    } else {
-      GoalCandidate goal_candidate{};
-      goal_candidate.goal_pose = goal_pose;
-      goal_candidate.distance_from_original_goal = 0.0;
-      goal_candidates_.push_back(goal_candidate);
-    }
   }
   last_received_time_ = std::make_unique<rclcpp::Time>(route_handler->getRouteHeader().stamp);
+
+#else
+  // todo: move this check out of this function after old architecture is removed
+  if (!goal_candidates_.empty()) {
+    return;
+  }
+#endif
+
+  // calculate goal candidates
+  const Pose goal_pose = route_handler->getGoalPose();
+  refined_goal_pose_ = calcRefinedGoal(goal_pose);
+  if (allow_goal_modification_) {
+    goal_searcher_->setPlannerData(planner_data_);
+    goal_candidates_ = goal_searcher_->search(refined_goal_pose_);
+  } else {
+    GoalCandidate goal_candidate{};
+    goal_candidate.goal_pose = goal_pose;
+    goal_candidate.distance_from_original_goal = 0.0;
+    goal_candidates_.push_back(goal_candidate);
+  }
 }
 
 BehaviorModuleOutput GoalPlannerModule::plan()