fix(goal_planenr): not reset when receving new route

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

Hide file tree

Showing 2 changed files with 14 additions and 21 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
@@ -153,7 +153,6 @@ class GoalPlannerModule : public SceneModuleInterface
   tier4_autoware_utils::LinearRing2d vehicle_footprint_;
 
   // save last time and pose
-  std::unique_ptr<rclcpp::Time> last_received_time_;
   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,31 +488,25 @@ void GoalPlannerModule::returnToLaneParking()
 
 void GoalPlannerModule::generateGoalCandidates()
 {
-  // initialize when receiving new route
-  const auto & route_handler = planner_data_->route_handler;
-  if (!last_received_time_ || *last_received_time_ != route_handler->getRouteHeader().stamp) {
-    // Initialize parallel parking planner status
-    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);
-    }
+  // calculate goal candidates
+  const Pose goal_pose = planner_data_->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);
 }
 
 BehaviorModuleOutput GoalPlannerModule::plan()
 {
-  generateGoalCandidates();
+  if (goal_candidates_.empty()) {
+    generateGoalCandidates();
+  }
 
   if (allow_goal_modification_) {
     return planWithGoalModification();