feat(planner_manager): output module add/delete infomation (autowaref…

…oundation#4006) Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
tier4 · Jun 19, 2023 · db19060 · db19060
1 parent 30c8de9
commit db19060
Show file tree

Hide file tree

Showing 2 changed files with 60 additions and 5 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp
@@ -42,6 +42,39 @@ using tier4_planning_msgs::msg::StopReasonArray;
 using SceneModulePtr = std::shared_ptr<SceneModuleInterface>;
 using SceneModuleManagerPtr = std::shared_ptr<SceneModuleManagerInterface>;
 
+enum Action {
+  ADD = 0,
+  DELETE,
+  MOVE,
+};
+
+struct ModuleUpdateInfo
+{
+  explicit ModuleUpdateInfo(
+    const SceneModulePtr & module_ptr, const Action & action, const std::string & description)
+  : status(module_ptr->getCurrentStatus()),
+    action(action),
+    module_name(module_ptr->name()),
+    description(description)
+  {
+  }
+
+  explicit ModuleUpdateInfo(
+    const std::string & name, const Action & action, const ModuleStatus & status,
+    const std::string & description)
+  : status(status), action(action), module_name(name), description(description)
+  {
+  }
+
+  ModuleStatus status;
+
+  Action action;
+
+  std::string module_name;
+
+  std::string description;
+};
+
 struct SceneModuleStatus
 {
   explicit SceneModuleStatus(const std::string & n) : module_name(n) {}
@@ -400,6 +433,8 @@ class PlannerManager
 
   mutable std::unordered_map<std::string, double> processing_time_;
 
+  mutable std::vector<ModuleUpdateInfo> debug_info_;
+
   bool verbose_{false};
 };
 }  // namespace behavior_path_planner

diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp
@@ -38,6 +38,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
 {
   resetProcessingTime();
   stop_watch_.tic("total_time");
+  debug_info_.clear();
 
   if (!root_lanelet_) {
     root_lanelet_ = updateRootLanelet(data);
@@ -112,6 +113,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
        */
       addApprovedModule(highest_priority_module);
       clearCandidateModules();
+      debug_info_.emplace_back(highest_priority_module, Action::ADD, "To Approval");
     }
     return BehaviorModuleOutput{};
   }();
@@ -451,6 +453,8 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
 
       std::for_each(
         std::next(itr), approved_module_ptrs_.end(), [this](auto & m) { deleteExpiredModules(m); });
+
+      debug_info_.emplace_back(*itr, Action::MOVE, "Back To Waiting Approval");
     }
 
     approved_module_ptrs_.erase(itr, approved_module_ptrs_.end());
@@ -464,7 +468,10 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
       approved_module_ptrs_.begin(), approved_module_ptrs_.end(),
       [](const auto & m) { return m->getCurrentStatus() == ModuleStatus::FAILURE; });
 
-    std::for_each(itr, approved_module_ptrs_.end(), [this](auto & m) { deleteExpiredModules(m); });
+    std::for_each(itr, approved_module_ptrs_.end(), [this](auto & m) {
+      debug_info_.emplace_back(m, Action::DELETE, "From Approved");
+      deleteExpiredModules(m);
+    });
 
     if (itr != approved_module_ptrs_.end()) {
       clearCandidateModules();
@@ -512,8 +519,10 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
    * ModuleStatus::SUCCESS.
    */
   if (lane_change_itr == approved_module_ptrs_.end()) {
-    std::for_each(
-      success_itr, approved_module_ptrs_.end(), [this](auto & m) { deleteExpiredModules(m); });
+    std::for_each(success_itr, approved_module_ptrs_.end(), [this](auto & m) {
+      debug_info_.emplace_back(m, Action::DELETE, "From Approved");
+      deleteExpiredModules(m);
+    });
 
     approved_module_ptrs_.erase(success_itr, approved_module_ptrs_.end());
     clearCandidateModules();
@@ -528,8 +537,10 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
    * change First In module's input.
    */
   if (not_success_itr == approved_module_ptrs_.rend()) {
-    std::for_each(
-      success_itr, approved_module_ptrs_.end(), [this](auto & m) { deleteExpiredModules(m); });
+    std::for_each(success_itr, approved_module_ptrs_.end(), [this](auto & m) {
+      debug_info_.emplace_back(m, Action::DELETE, "From Approved");
+      deleteExpiredModules(m);
+    });
 
     approved_module_ptrs_.erase(success_itr, approved_module_ptrs_.end());
     clearCandidateModules();
@@ -664,6 +675,15 @@ void PlannerManager::print() const
     string_stream << "[" << m->name() << "]->";
   }
 
+  string_stream << "\n";
+  string_stream << "update module info: ";
+  for (const auto & i : debug_info_) {
+    string_stream << "[Module:" << i.module_name << " Status:" << magic_enum::enum_name(i.status)
+                  << " Action:" << magic_enum::enum_name(i.action)
+                  << " Description:" << i.description << "]\n"
+                  << std::setw(28);
+  }
+
   string_stream << "\n" << std::fixed << std::setprecision(1);
   string_stream << "processing time   : ";
   for (const auto & t : processing_time_) {