From 8a84611d85022f37b80d8bdcb7acaa1910669fc1 Mon Sep 17 00:00:00 2001
From: Jose Luis Blanco-Claraco <joseluisblancoc@gmail.com>
Date: Tue, 28 Jan 2025 11:42:32 +0100
Subject: [PATCH] Feature: all MOLA modules got its MRPT logger to ROS console
 for easier debugging

---
 .../include/mola_bridge_ros2/BridgeROS2.h     |  8 +++
 mola_bridge_ros2/src/BridgeROS2.cpp           | 67 +++++++++++++++++++
 2 files changed, 75 insertions(+)

diff --git a/mola_bridge_ros2/include/mola_bridge_ros2/BridgeROS2.h b/mola_bridge_ros2/include/mola_bridge_ros2/BridgeROS2.h
index 00d2c33..199c20c 100644
--- a/mola_bridge_ros2/include/mola_bridge_ros2/BridgeROS2.h
+++ b/mola_bridge_ros2/include/mola_bridge_ros2/BridgeROS2.h
@@ -322,6 +322,14 @@ class BridgeROS2 : public RawDataSourceBase, public mola::RawDataConsumer
     void internalAnalyzeTopicsToSubscribe(const mrpt::containers::yaml& ds_subscribe);
 
     void publishStaticTFs();
+
+    void                  redirectMolaModuleOutputToROSConsole(mola::ExecutableBase::Ptr& module);
+    std::set<std::string> already_handled_modules_;
+    std::vector<std::weak_ptr<mola::ExecutableBase>>      already_handled_consoles_;
+    std::optional<mrpt::system::output_logger_callback_t> mrpt2ros_log_cb_;
+
+    // Undo redirectMolaModuleOutputToROSConsole(), before the node is destructed
+    void unredirectMolaModuleOutput();
 };
 
 }  // namespace mola
diff --git a/mola_bridge_ros2/src/BridgeROS2.cpp b/mola_bridge_ros2/src/BridgeROS2.cpp
index 3e8dd0a..5e3e4f0 100644
--- a/mola_bridge_ros2/src/BridgeROS2.cpp
+++ b/mola_bridge_ros2/src/BridgeROS2.cpp
@@ -159,6 +159,9 @@ void BridgeROS2::ros_node_thread_main(Yaml cfg)
         // Spin:
         rclcpp::spin(rosNode_);
 
+        // Unregister MRPT -> ROS console callbacks:
+        unredirectMolaModuleOutput();
+
         rclcpp::shutdown();
     }
     catch (const std::exception& e)
@@ -924,6 +927,10 @@ void BridgeROS2::doLookForNewMolaSubs()
 
     auto lck = mrpt::lockHelper(molaSubsMtx_);
 
+    // All of them: redirect output to ros console:
+    auto listAll = this->findService<mola::ExecutableBase>();
+    for (auto& module : listAll) redirectMolaModuleOutputToROSConsole(module);
+
     // RawDataSourceBase:
     auto listRDS = this->findService<mola::RawDataSourceBase>();
     for (auto& module : listRDS)
@@ -1568,3 +1575,63 @@ void BridgeROS2::internalPublishGridMap(
         pubMeta->publish(gridMsg.info);
     }
 }
+
+void BridgeROS2::redirectMolaModuleOutputToROSConsole(mola::ExecutableBase::Ptr& module)
+{
+    ASSERT_(module);
+
+    if (already_handled_modules_.count(module->getModuleInstanceName()) != 0)
+        return;  // already done
+
+    already_handled_modules_.insert(module->getModuleInstanceName());  // mark as done
+    already_handled_consoles_.push_back(module);
+
+    // Redirect MRPT logger to ROS logger:
+    module->logging_enable_console_output = false;  // No console, go thru ROS
+
+    if (!mrpt2ros_log_cb_)
+    {
+        auto node        = rosNode();
+        mrpt2ros_log_cb_ = [node](
+                               std::string_view msg, const mrpt::system::VerbosityLevel level,
+                               [[maybe_unused]] std::string_view              loggerName,
+                               [[maybe_unused]] const mrpt::Clock::time_point timestamp)
+        {
+            switch (level)
+            {
+                case mrpt::system::LVL_DEBUG:
+                    RCLCPP_DEBUG_STREAM(node->get_logger(), msg);
+                    break;
+                case mrpt::system::LVL_INFO:
+                    RCLCPP_INFO_STREAM(node->get_logger(), msg);
+                    break;
+                case mrpt::system::LVL_WARN:
+                    RCLCPP_WARN_STREAM(node->get_logger(), msg);
+                    break;
+                case mrpt::system::LVL_ERROR:
+                    RCLCPP_ERROR_STREAM(node->get_logger(), msg);
+                    break;
+                default:
+                    break;
+            };
+        };
+    }
+
+    module->logRegisterCallback(*mrpt2ros_log_cb_);
+}
+
+void BridgeROS2::unredirectMolaModuleOutput()
+{
+    if (!mrpt2ros_log_cb_) return;
+
+    for (const auto& weakPtrToModule : already_handled_consoles_)
+    {
+        auto mod = weakPtrToModule.lock();
+        if (!mod) continue;
+
+        mod->setVerbosityLevelForCallbacks(mrpt::system::LVL_ERROR);
+        mod->logging_enable_console_output = true;
+        // mod->logDeregisterCallback(*mrpt2ros_log_cb_);
+    }
+    already_handled_consoles_.clear();
+}