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/include/mc_rtc/log/Logger.h b/include/mc_rtc/log/Logger.h index bcaf5a8107..98ec0b5839 100644 --- a/include/mc_rtc/log/Logger.h +++ b/include/mc_rtc/log/Logger.h @@ -195,19 +195,25 @@ struct MC_RTC_UTILS_DLLAPI Logger * * \param get_fn A function that provides data that should be logged * + * \param overwrite If true, overwrite the previous entry of the same name (if any), otherwise log and return + * */ template::value, int>::type = 0> - void addLogEntry(const std::string & name, const SourceT * source, CallbackT && get_fn) + void addLogEntry(const std::string & name, const SourceT * source, CallbackT && get_fn, bool overwrite = false) { using ret_t = decltype(get_fn()); using base_t = typename std::decay::type; auto it = find_entry(name); if(it != log_entries_.end()) { - log::error("Already logging an entry named {}", name); - return; + if(!overwrite) + { + log::error("Already logging an entry named {}", name); + return; + } + else { log_entries_.erase(it); } } auto log_type = log::callback_is_serializable::log_type; log_events_.push_back(KeyAddedEvent{log_type, name}); @@ -228,15 +234,18 @@ struct MC_RTC_UTILS_DLLAPI Logger * * \param source Source of the log entry * + * \param overwrite If true, overwrite the previous entry of the same name (if any), otherwise log and return + * */ template::value, int>::type = 0> - void addLogEntry(const std::string & name, const SourceT * source) + void addLogEntry(const std::string & name, const SourceT * source, bool overwrite = false) { using MemberT = decltype(source->*member); - addLogEntry(name, source, [source]() -> const MemberT & { return source->*member; }); + addLogEntry( + name, source, [source]() -> const MemberT & { return source->*member; }, overwrite); } /** Add a log entry from a source and a compile-time pointer to method @@ -251,15 +260,18 @@ struct MC_RTC_UTILS_DLLAPI Logger * * \param source Source of the log entry * + * \param overwrite If true, overwrite the previous entry of the same name (if any), otherwise log and return + * */ template::value, int>::type = 0> - void addLogEntry(const std::string & name, const SourceT * source) + void addLogEntry(const std::string & name, const SourceT * source, bool overwrite = false) { using MethodRetT = decltype((source->*method)()); - addLogEntry(name, source, [source]() -> MethodRetT { return (source->*method)(); }); + addLogEntry( + name, source, [source]() -> MethodRetT { return (source->*method)(); }, overwrite); } /** Add a log entry into the log with no source @@ -274,11 +286,13 @@ struct MC_RTC_UTILS_DLLAPI Logger * * \param get_fn A function that provides data that should be logged * + * \param overwrite If true, overwrite the previous entry of the same name (if any), otherwise log and return + * */ template::value, int>::type = 0> - void addLogEntry(const std::string & name, T && get_fn) + void addLogEntry(const std::string & name, T && get_fn, bool overwrite = false) { - addLogEntry(name, static_cast(nullptr), std::forward(get_fn)); + addLogEntry(name, static_cast(nullptr), std::forward(get_fn), overwrite); } /** Add multiple entries at once with the same entry 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