From 5ff0ff33f28a647739a42c5bccbd5d8a909d7f97 Mon Sep 17 00:00:00 2001
From: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Date: Mon, 17 Apr 2023 23:13:52 +0900
Subject: [PATCH] fix(behavior_path_planner): fix updating pull out status for
 new architecture (#3447)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
---
 .../scene_module/pull_out/pull_out_module.hpp |  1 -
 .../scene_module/pull_out/pull_out_module.cpp | 52 +++++++++----------
 2 files changed, 24 insertions(+), 29 deletions(-)

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp
index 78cc37b0a9d68..1546015d5479c 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp
@@ -89,7 +89,6 @@ class PullOutModule : public SceneModuleInterface
   BehaviorModuleOutput plan() override;
   BehaviorModuleOutput planWaitingApproval() override;
   CandidateOutput planCandidate() const override;
-  void processOnEntry() override;
   void processOnExit() override;
 
   void setParameters(const std::shared_ptr<PullOutParameters> & parameters)
diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp
index 18acee33cc521..a5b6944ee5b5d 100644
--- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp
@@ -98,34 +98,6 @@ BehaviorModuleOutput PullOutModule::run()
   return plan();
 }
 
-void PullOutModule::processOnEntry()
-{
-  // initialize when receiving new route
-  if (
-    last_route_received_time_ == nullptr ||
-    *last_route_received_time_ != planner_data_->route_handler->getRouteHeader().stamp) {
-    RCLCPP_INFO(getLogger(), "Receive new route, so reset status");
-    resetStatus();
-    updatePullOutStatus();
-  }
-  last_route_received_time_ =
-    std::make_unique<rclcpp::Time>(planner_data_->route_handler->getRouteHeader().stamp);
-
-  // for preventing chattering between back and pull_out
-  if (!status_.back_finished) {
-    if (last_pull_out_start_update_time_ == nullptr) {
-      last_pull_out_start_update_time_ = std::make_unique<rclcpp::Time>(clock_->now());
-    }
-    const auto elapsed_time = (clock_->now() - *last_pull_out_start_update_time_).seconds();
-    if (elapsed_time < parameters_->backward_path_update_duration) {
-      return;
-    }
-    last_pull_out_start_update_time_ = std::make_unique<rclcpp::Time>(clock_->now());
-  }
-
-  updatePullOutStatus();
-}
-
 void PullOutModule::processOnExit()
 {
   resetPathCandidate();
@@ -320,6 +292,7 @@ PathWithLaneId PullOutModule::getFullPath() const
 
 BehaviorModuleOutput PullOutModule::planWaitingApproval()
 {
+  updatePullOutStatus();
   waitApproval();
 
   BehaviorModuleOutput output;
@@ -563,6 +536,29 @@ PathWithLaneId PullOutModule::generateStopPath() const
 
 void PullOutModule::updatePullOutStatus()
 {
+  // if new route is received, reset status
+  const bool has_received_new_route =
+    last_route_received_time_ == nullptr ||
+    *last_route_received_time_ != planner_data_->route_handler->getRouteHeader().stamp;
+  if (has_received_new_route) {
+    RCLCPP_INFO(getLogger(), "Receive new route, so reset status");
+    resetStatus();
+  }
+  last_route_received_time_ =
+    std::make_unique<rclcpp::Time>(planner_data_->route_handler->getRouteHeader().stamp);
+
+  // skip updating if enough time has not passed for preventing chattering between back and pull_out
+  if (!has_received_new_route && !status_.back_finished) {
+    if (last_pull_out_start_update_time_ == nullptr) {
+      last_pull_out_start_update_time_ = std::make_unique<rclcpp::Time>(clock_->now());
+    }
+    const auto elapsed_time = (clock_->now() - *last_pull_out_start_update_time_).seconds();
+    if (elapsed_time < parameters_->backward_path_update_duration) {
+      return;
+    }
+    last_pull_out_start_update_time_ = std::make_unique<rclcpp::Time>(clock_->now());
+  }
+
   const auto & route_handler = planner_data_->route_handler;
   const auto & current_pose = planner_data_->self_odometry->pose.pose;
   const auto & goal_pose = planner_data_->route_handler->getGoalPose();