Merge branch 'main' into feat/keep_distance_against_front_objects

Signed-off-by: kyoichi-sugahara <[email protected]>

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -555,9 +555,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
     return;
   };
 
-  const auto info_throttle = [this](const auto & s) {
-    RCLCPP_INFO_SKIPFIRST_THROTTLE(logger_, *clock_, 5000, "%s", s);
-  };
+  const auto info_once = [this](const auto & s) { RCLCPP_INFO_ONCE(logger_, "%s", s); };
 
   const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled &&
                                 m_current_operation_mode.mode == OperationModeState::AUTONOMOUS;
@@ -623,10 +621,10 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
   if (m_control_state == ControlState::STOPPED) {
     // -- debug print --
     if (has_nonzero_target_vel && !departure_condition_from_stopped) {
-      info_throttle("target speed > 0, but departure condition is not met. Keep STOPPED.");
+      info_once("target speed > 0, but departure condition is not met. Keep STOPPED.");
     }
     if (has_nonzero_target_vel && keep_stopped_condition) {
-      info_throttle("target speed > 0, but keep stop condition is met. Keep STOPPED.");
+      info_once("target speed > 0, but keep stop condition is met. Keep STOPPED.");
     }
     // ---------------
 

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -435,6 +435,15 @@ class PlannerManager
     const std::vector<SceneModulePtr> & request_modules, const std::shared_ptr<PlannerData> & data,
     const BehaviorModuleOutput & previous_module_output);
 
+  /**
+   * @brief run keep last approved modules
+   * @param planner data.
+   * @param previous module output.
+   * @return planning result.
+   */
+  BehaviorModuleOutput runKeepLastModules(
+    const std::shared_ptr<PlannerData> & data, const BehaviorModuleOutput & previous_output) const;
+
   static std::string getNames(const std::vector<SceneModulePtr> & modules);
 
   std::optional<lanelet::ConstLanelet> root_lanelet_{std::nullopt};

planning/behavior_path_planner/src/planner_manager.cpp

@@ -141,33 +141,42 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
        * STEP3: if there is no module that need to be launched, return approved modules' output
        */
       if (request_modules.empty()) {
+        const auto output = runKeepLastModules(data, approved_modules_output);
         processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
-        return approved_modules_output;
+        return output;
       }
 
       /**
        * STEP4: if there is module that should be launched, execute the module
        */
       const auto [highest_priority_module, candidate_modules_output] =
         runRequestModules(request_modules, data, approved_modules_output);
+
+      /**
+       * STEP5: run keep last approved modules after running candidate modules.
+       * NOTE: if no candidate module is launched, approved_modules_output used as input for keep
+       * last modules and return the result immediately.
+       */
+      const auto output = runKeepLastModules(
+        data, highest_priority_module ? candidate_modules_output : approved_modules_output);
       if (!highest_priority_module) {
         processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
-        return approved_modules_output;
+        return output;
       }
 
       /**
-       * STEP5: if the candidate module's modification is NOT approved yet, return the result.
+       * STEP6: if the candidate module's modification is NOT approved yet, return the result.
        * NOTE: the result is output of the candidate module, but the output path don't contains path
        * shape modification that needs approval. On the other hand, it could include velocity
        * profile modification.
        */
       if (highest_priority_module->isWaitingApproval()) {
         processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
-        return candidate_modules_output;
+        return output;
       }
 
       /**
-       * STEP6: if the candidate module is approved, push the module into approved_module_ptrs_
+       * STEP7: if the candidate module is approved, push the module into approved_module_ptrs_
        */
       addApprovedModule(highest_priority_module);
       clearCandidateModules();
@@ -178,7 +187,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
           logger_, clock_, 1000, "Reach iteration limit (max: %ld). Output current result.",
           max_iteration_num_);
         processing_time_.at("total_time") = stop_watch_.toc("total_time", true);
-        return candidate_modules_output;
+        return output;
       }
     }
 
@@ -394,6 +403,19 @@ std::vector<SceneModulePtr> PlannerManager::getRequestModules(
   return request_modules;
 }
 
+BehaviorModuleOutput PlannerManager::runKeepLastModules(
+  const std::shared_ptr<PlannerData> & data, const BehaviorModuleOutput & previous_output) const
+{
+  auto output = previous_output;
+  std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) {
+    if (getManager(m)->isKeepLast()) {
+      output = run(m, data, output);
+    }
+  });
+
+  return output;
+}
+
 BehaviorModuleOutput PlannerManager::getReferencePath(
   const std::shared_ptr<PlannerData> & data) const
 {
@@ -652,11 +674,13 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
   }
 
   /**
-   * execute all approved modules.
+   * execute approved modules except keep last modules.
    */
   std::for_each(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [&](const auto & m) {
-    output = run(m, data, output);
-    results.emplace(m->name(), output);
+    if (!getManager(m)->isKeepLast()) {
+      output = run(m, data, output);
+      results.emplace(m->name(), output);
+    }
   });
 
   /**