[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
jrl-umi3218 · Aug 2, 2023 · b826581 · b826581
1 parent b13fe4c
commit b826581
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 22 deletions.
diff --git 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
@@ -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