Merge pull request #383 from gergondet/topic/FixPluginLoginIssue

jrl-umi3218 · Aug 2, 2023 · c4e2bab · c4e2bab
2 parents 2fdf074 + b826581
commit c4e2bab
Show file tree

Hide file tree

Showing 3 changed files with 51 additions and 31 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/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<typename CallbackT,
            typename SourceT = void,
            typename std::enable_if<mc_rtc::log::callback_is_serializable<CallbackT>::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<ret_t>::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<CallbackT>::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<typename MemberPtrT,
            MemberPtrT member,
            typename SourceT,
            typename std::enable_if<mc_rtc::log::is_serializable_member<MemberPtrT>::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<typename MethodPtrT,
            MethodPtrT method,
            typename SourceT,
            typename std::enable_if<mc_rtc::log::is_serializable_getter<MethodPtrT>::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<typename T, typename std::enable_if<mc_rtc::log::callback_is_serializable<T>::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<const void *>(nullptr), std::forward<T>(get_fn));
+    addLogEntry(name, static_cast<const void *>(nullptr), std::forward<T>(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
@@ -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