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 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