fix for old archi

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · Jun 13, 2023 · e99095e · e99095e
1 parent fe9c796
commit e99095e
Show file tree

Hide file tree

Showing 2 changed files with 25 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
@@ -152,7 +152,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
@@ -488,8 +488,27 @@ void GoalPlannerModule::returnToLaneParking()
 
 void GoalPlannerModule::generateGoalCandidates()
 {
+  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) {
+    // this process causes twice reset when receiving new route.
+    resetStatus();
+  }
+  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 = planner_data_->route_handler->getGoalPose();
+  const Pose goal_pose = route_handler->getGoalPose();
   refined_goal_pose_ = calcRefinedGoal(goal_pose);
   if (allow_goal_modification_) {
     goal_searcher_->setPlannerData(planner_data_);
@@ -504,9 +523,7 @@ void GoalPlannerModule::generateGoalCandidates()
 
 BehaviorModuleOutput GoalPlannerModule::plan()
 {
-  if (goal_candidates_.empty()) {
-    generateGoalCandidates();
-  }
+  generateGoalCandidates();
 
   if (allow_goal_modification_) {
     return planWithGoalModification();