From b82658125122abc7d1af6bf7c5503887a001bf73 Mon Sep 17 00:00:00 2001
From: Pierre Gergondet <pierre.gergondet@gmail.com>
Date: Wed, 2 Aug 2023 10:33:30 +0900
Subject: [PATCH] [mc_control] Correctly log plugin performances

- Setup the plugin performance log after all plugins have been loaded
- Avoid dangling references across resets by forcing the log entry to be
  re-added
---
 include/mc_control/mc_global_controller.h |  1 +
 src/mc_control/mc_global_controller.cpp   | 49 +++++++++++++----------
 2 files changed, 28 insertions(+), 22 deletions(-)

diff --git a/include/mc_control/mc_global_controller.h b/include/mc_control/mc_global_controller.h
index 40fd5df555..340b400b2a 100644
--- a/include/mc_control/mc_global_controller.h
+++ b/include/mc_control/mc_global_controller.h
@@ -944,6 +944,7 @@ struct MC_CONTROL_DLLAPI MCGlobalController
 
   void start_log();
   void setup_log();
+  void setup_plugin_log();
   std::map<std::string, bool> setup_logger_ = {};
 
   /** Timers and performance measure */
diff --git a/src/mc_control/mc_global_controller.cpp b/src/mc_control/mc_global_controller.cpp
index 3974d47fc6..0ce9bb2bf0 100644
--- a/src/mc_control/mc_global_controller.cpp
+++ b/src/mc_control/mc_global_controller.cpp
@@ -765,7 +765,7 @@ bool MCGlobalController::run()
       next_controller_->reset({controller_->robot().mbc().q});
       next_controller_->resetObserverPipelines();
       controller_ = next_controller_;
-      /** Reset plugins */
+      /** Reset global plugins */
       for(auto & plugin : plugins_) { plugin.plugin->reset(*this); }
       resetControllerPlugins();
     }
@@ -1034,27 +1034,6 @@ void MCGlobalController::setup_log()
   controller->logger().addLogEntry("perf_Log", [this]() { return log_dt.count(); });
   controller->logger().addLogEntry("perf_Gui", [this]() { return gui_dt.count(); });
   controller->logger().addLogEntry("perf_FrameworkCost", [this]() { return framework_cost; });
-  auto getPluginName = [this](GlobalPlugin * plugin) -> const std::string &
-  {
-    for(auto & p : plugins_)
-    {
-      if(p.plugin.get() == plugin) { return p.name; }
-    }
-    mc_rtc::log::error_and_throw(
-        "Impossible error, searched for a plugin name from a pointer to a plugin that was not loaded");
-  };
-  for(const auto & plugin : plugins_before_)
-  {
-    const auto & name = getPluginName(plugin.plugin);
-    controller->logger().addLogEntry(fmt::format("perf_Plugins_{}_before", name),
-                                     [&plugin]() { return plugin.plugin_before_dt.count(); });
-  }
-  for(const auto & plugin : plugins_after_)
-  {
-    const auto & name = getPluginName(plugin.plugin);
-    controller->logger().addLogEntry(fmt::format("perf_Plugins_{}_after", name),
-                                     [&plugin]() { return plugin.plugin_after_dt.count(); });
-  }
   // Log system wall time as nanoseconds since epoch (can be used to manage synchronization with ros)
   controller->logger().addLogEntry("timeWall",
                                    []() -> int64_t
@@ -1145,6 +1124,32 @@ void MCGlobalController::resetControllerPlugins()
     auto plugin = loadPlugin(name, next_ctrl.c_str());
     if(plugin) { plugin->init(*this, config.global_plugin_configs[name]); }
   }
+  setup_plugin_log();
+}
+
+void MCGlobalController::setup_plugin_log()
+{
+  auto getPluginName = [this](GlobalPlugin * plugin) -> const std::string &
+  {
+    for(auto & p : plugins_)
+    {
+      if(p.plugin.get() == plugin) { return p.name; }
+    }
+    mc_rtc::log::error_and_throw(
+        "Impossible error, searched for a plugin name from a pointer to a plugin that was not loaded");
+  };
+  for(const auto & plugin : plugins_before_)
+  {
+    const auto & name = getPluginName(plugin.plugin);
+    controller_->logger().addLogEntry(
+        fmt::format("perf_Plugins_{}_before", name), [&plugin]() { return plugin.plugin_before_dt.count(); }, true);
+  }
+  for(const auto & plugin : plugins_after_)
+  {
+    const auto & name = getPluginName(plugin.plugin);
+    controller_->logger().addLogEntry(
+        fmt::format("perf_Plugins_{}_after", name), [&plugin]() { return plugin.plugin_after_dt.count(); }, true);
+  }
 }
 
 } // namespace mc_control