Merge pull request #587 from ami-iit/ros/text_logging

Implement the ROS2 sink for the TextLogging

CHANGELOG.md

@@ -5,6 +5,7 @@ All notable changes to this project are documented in this file.
 ### Added
 - Add the possibility to attach all the multiple analog sensor clients  (https://github.com/ami-iit/bipedal-locomotion-framework/pull/569)
 - Add a tutorial for the inverse kinematics  (https://github.com/ami-iit/bipedal-locomotion-framework/pull/596)
+- Implement the ROS2 sink for the `TextLogging` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/587)
 
 ### Changed
 - Ask for `toml++ v3.0.1` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/581)

bindings/python/TextLogging/CMakeLists.txt

@@ -15,3 +15,13 @@ add_bipedal_locomotion_python_module(
   LINK_LIBRARIES
   BipedalLocomotion::TextLogging
   )
+
+if(FRAMEWORK_COMPILE_RosImplementation)
+
+  add_bipedal_locomotion_python_module(
+    NAME  TextLoggingRosImplementation
+    SOURCES src/RosLogger.cpp src/RosModule.cpp
+    HEADERS ${H_PREFIX}/RosLogger.h ${H_PREFIX}/RosModule.h
+    LINK_LIBRARIES BipedalLocomotion::TextLoggingRosImplementation)
+
+endif()

bindings/python/TextLogging/include/BipedalLocomotion/bindings/TextLogging/RosLogger.h

@@ -0,0 +1,26 @@
+/**
+ * @file RosLogger.h
+ * @authors Giulio Romualdi
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_ROS_LOGGER_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_ROS_LOGGER_H
+
+#include <pybind11/pybind11.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace TextLogging
+{
+
+void CreateRosLoggerFactory(pybind11::module& module);
+
+} // namespace TextLogging
+} // namespace bindings
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_ROS_LOGGER_H

bindings/python/TextLogging/include/BipedalLocomotion/bindings/TextLogging/RosModule.h

@@ -0,0 +1,18 @@
+/**
+ * @file RosModule.h
+ * @authors Giulio Romualdi
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_BINDINGS_TEXT_LOGGING_ROS_MODULE_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_TEXT_LOGGING_ROS_MODULE_H
+
+#include <pybind11/pybind11.h>
+
+namespace BipedalLocomotion::bindings::TextLogging
+{
+void CreateRosModule(pybind11::module& module);
+} // namespace BipedalLocomotion::bindings::TextLogging
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_TEXT_LOGGING_ROS_MODULE_H

bindings/python/TextLogging/include/BipedalLocomotion/bindings/TextLogging/TextLogging.h

@@ -15,4 +15,9 @@ namespace BipedalLocomotion::bindings::TextLogging
 void CreateTextLogging(pybind11::module& module);
 } // namespace BipedalLocomotion::bindings::TextLogging
 
+namespace BipedalLocomotion::bindings
+{
+void CreateLogger(pybind11::module& module);
+}
+
 #endif // BIPEDAL_LOCOMOTION_BINDINGS_TEXT_LOGGING_H

bindings/python/TextLogging/src/RosLogger.cpp

@@ -0,0 +1,39 @@
+/**
+ * @file RosLoggerFactory.cpp
+ * @authors Giulio Romualdi
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#include <pybind11/cast.h>
+#include <pybind11/chrono.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <BipedalLocomotion/TextLogging/RosLogger.h>
+#include <BipedalLocomotion/bindings/TextLogging/RosLogger.h>
+
+#include <string_view>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace TextLogging
+{
+
+void CreateRosLoggerFactory(pybind11::module& module)
+{
+    namespace py = ::pybind11;
+    using namespace BipedalLocomotion::TextLogging;
+
+    py::class_<RosLoggerFactory, //
+               LoggerFactory,
+               std::shared_ptr<RosLoggerFactory>>(module,
+                                                  "RosLoggerFactory")
+        .def(py::init<const std::string_view&>(), py::arg("name"));
+}
+
+} // namespace TextLogging
+} // namespace bindings
+} // namespace BipedalLocomotion

bindings/python/TextLogging/src/RosModule.cpp

@@ -0,0 +1,19 @@
+/**
+ * @file RosModule.cpp
+ * @authors Giulio Romualdi
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#include <pybind11/pybind11.h>
+
+#include <BipedalLocomotion/bindings/TextLogging/RosModule.h>
+#include <BipedalLocomotion/bindings/TextLogging/RosLogger.h>
+
+namespace BipedalLocomotion::bindings::TextLogging
+{
+void CreateRosModule(pybind11::module& module)
+{
+    CreateRosLoggerFactory(module);
+}
+} // namespace BipedalLocomotion::bindings::TextLogging

bindings/python/TextLogging/src/TextLogging.cpp

@@ -5,8 +5,13 @@
  * distributed under the terms of the BSD-3-Clause license.
  */
 
-#include "BipedalLocomotion/bindings/TextLogging/TextLogging.h"
-#include "BipedalLocomotion/TextLogging/Logger.h"
+#include <BipedalLocomotion/TextLogging/Logger.h>
+#include <BipedalLocomotion/TextLogging/LoggerBuilder.h>
+
+#include <BipedalLocomotion/bindings/TextLogging/TextLogging.h>
+
+#include <pybind11/cast.h>
+#include <pybind11/pybind11.h>
 
 namespace BipedalLocomotion::bindings::TextLogging
 {
@@ -24,6 +29,29 @@ void CreateTextLogging(pybind11::module& module)
         .value("Critical", Verbosity::Critical)
         .value("Off", Verbosity::Off);
 
+    py::class_<Logger, std::shared_ptr<Logger>>(module, "Logger")
+        .def("info", [](Logger& logger, const std::string& msg) { logger.info(msg); })
+        .def("warn", [](Logger& logger, const std::string& msg) { logger.warn(msg); })
+        .def("error", [](Logger& logger, const std::string& msg) { logger.error(msg); })
+        .def("critical", [](Logger& logger, const std::string& msg) { logger.critical(msg); })
+        .def("trace", [](Logger& logger, const std::string& msg) { logger.trace(msg); })
+        .def("debug", [](Logger& logger, const std::string& msg) { logger.debug(msg); });
+
     module.def("set_verbosity", &setVerbosity, py::arg("verbosity"));
+
+    py::class_<LoggerFactory, std::shared_ptr<LoggerFactory>>(module, "LoggerFactory");
+    py::class_<LoggerBuilder>(module, "LoggerBuilder")
+        .def_static("set_factory", &LoggerBuilder::setFactory, py::arg("factory"));
+
 }
 } // namespace BipedalLocomotion::bindings::TextLogging
+
+namespace BipedalLocomotion::bindings
+{
+void CreateLogger(pybind11::module& module)
+{
+    namespace py = ::pybind11;
+
+    module.def("log", &::BipedalLocomotion::log);
+}
+} // namespace BipedalLocomotion::bindings

bindings/python/bipedal_locomotion_framework.cpp.in

@@ -11,6 +11,11 @@
 #include <BipedalLocomotion/bindings/Math/Module.h>
 #include <BipedalLocomotion/bindings/ParametersHandler/Module.h>
 #include <BipedalLocomotion/bindings/TextLogging/Module.h>
+#include <BipedalLocomotion/bindings/TextLogging/TextLogging.h>
+
+@cmakeif FRAMEWORK_USE_rclcpp
+#include <BipedalLocomotion/bindings/TextLogging/RosModule.h>
+@endcmakeif FRAMEWORK_USE_rclcpp
 
 @cmakeif FRAMEWORK_COMPILE_YarpImplementation
 #include <BipedalLocomotion/bindings/ParametersHandler/YarpModule.h>
@@ -81,8 +86,13 @@ PYBIND11_MODULE(bindings, m)
 
     m.doc() = "BipedalLocomotionFramework bindings";
 
-    py::module textLoggingModule = m.def_submodule("logging");
+    py::module textLoggingModule = m.def_submodule("text_logging");
     bindings::TextLogging::CreateModule(textLoggingModule);
+    bindings::CreateLogger(m);
+
+    @cmakeif FRAMEWORK_USE_rclcpp
+    bindings::TextLogging::CreateRosModule(textLoggingModule);
+    @endcmakeif FRAMEWORK_USE_rclcpp
 
     py::module parametersHandlerModule = m.def_submodule("parameters_handler");
     bindings::ParametersHandler::CreateModule(parametersHandlerModule);

src/TextLogging/CMakeLists.txt

@@ -9,4 +9,4 @@ add_bipedal_locomotion_library(
                          include/BipedalLocomotion/TextLogging/DefaultLogger.h
   SOURCES                src/Logger.cpp src/LoggerBuilder.cpp src/DefaultLogger.cpp
   PUBLIC_LINK_LIBRARIES  spdlog::spdlog Eigen3::Eigen
-  SUBDIRECTORIES         YarpImplementation)
+  SUBDIRECTORIES         YarpImplementation RosImplementation)

src/TextLogging/RosImplementation/CMakeLists.txt

@@ -0,0 +1,14 @@
+# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# BSD-3-Clause license.
+
+if(FRAMEWORK_COMPILE_RosImplementation)
+
+  add_bipedal_locomotion_library(
+    NAME                   TextLoggingRosImplementation
+    SOURCES                src/RosLogger.cpp
+    PUBLIC_HEADERS         include/BipedalLocomotion/TextLogging/RosLogger.h
+    PUBLIC_LINK_LIBRARIES  BipedalLocomotion::TextLogging rclcpp::rclcpp
+    INSTALLATION_FOLDER    TextLogging)
+
+endif()

src/TextLogging/RosImplementation/include/BipedalLocomotion/TextLogging/RosLogger.h

@@ -0,0 +1,123 @@
+/**
+ * @file RosLogger.h
+ * @authors Giulio Romualdi
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_TEXT_LOGGING_ROS_LOGGER_H
+#define BIPEDAL_LOCOMOTION_TEXT_LOGGING_ROS_LOGGER_H
+
+#include <memory>
+#include <mutex>
+#include <string>
+
+#include <spdlog/common.h>
+#include <spdlog/sinks/base_sink.h>
+
+#include <BipedalLocomotion/TextLogging/Logger.h>
+
+#include <rclcpp/logging.hpp>
+
+namespace BipedalLocomotion
+{
+namespace TextLogging
+{
+
+namespace sinks
+{
+
+template <typename Mutex> class RosSink : public spdlog::sinks::base_sink<Mutex>
+{
+public:
+    RosSink(const rclcpp::Logger& rosLogger)
+        : m_rosLogger(rosLogger)
+    {
+    }
+
+private:
+    rclcpp::Logger m_rosLogger;
+
+protected:
+    void sink_it_(const spdlog::details::log_msg& msg) override
+    {
+        // log_msg is a struct containing the log entry info like level, timestamp, thread id etc.
+        // msg.raw contains pre formatted log
+        spdlog::memory_buf_t formatted;
+
+        spdlog::sinks::base_sink<Mutex>::formatter_->format(msg, formatted);
+
+        if (msg.level == spdlog::level::level_enum::trace)
+        {
+            RCLCPP_INFO(m_rosLogger, formatted.data());
+
+        } else if (msg.level == spdlog::level::level_enum::debug)
+        {
+            RCLCPP_DEBUG(m_rosLogger, formatted.data());
+        } else if (msg.level == spdlog::level::level_enum::info)
+        {
+            RCLCPP_INFO(m_rosLogger, formatted.data());
+        } else if (msg.level == spdlog::level::level_enum::warn)
+        {
+            RCLCPP_WARN(m_rosLogger, formatted.data());
+        } else if (msg.level == spdlog::level::level_enum::err)
+        {
+            RCLCPP_ERROR(m_rosLogger, formatted.data());
+        } else
+        {
+            RCLCPP_FATAL(m_rosLogger, formatted.data());
+        }
+    }
+
+    void flush_() override
+    {
+    }
+};
+
+using RosSink_mt = RosSink<std::mutex>;
+} // namespace sinks
+
+/**
+ * RosLoggerFactory implements the factory you should use to enable the sink using ros.
+ * The ROS logger can be easily used as follows
+ * \code{.cpp}
+ * #include <BipedalLocomotion/TextLogging/Logger.h>
+ * #include <BipedalLocomotion/TextLogging/RosLogger.h>
+ * #include <BipedalLocomotion/TextLogging/LoggerBuilder.h>
+ *
+ * // Change the logger
+ * BipedalLocomotion::TextLogging::LoggerBuilder::setFactory(std::make_shared<BipedalLocomotion::TextLogging::RosLoggerFactory>()));
+ *
+ * BipedalLocomotion::log()->info("My info");
+ * \endcode
+ */
+class RosLoggerFactory final : public LoggerFactory
+{
+public:
+    /**
+     * Construct a new RosLoggerFactory object
+     * @param name the name of the logger which will be used inside the formatted messages
+     */
+    RosLoggerFactory(const std::string_view& name = "blf");
+
+    /**
+     * Construct a new RosLoggerFactory object
+     * @param name the name of the logger which will be used inside the formatted messages
+     */
+    RosLoggerFactory(const rclcpp::Logger& logger);
+
+    /**
+     * Create the ROSLogger as a singleton
+     * @return the pointer to TextLogging::Logger that streams the output using ROS
+     */
+    std::shared_ptr<TextLogging::Logger> const createLogger() final;
+
+private:
+    const std::string m_name; /** The name of the logger */
+    const rclcpp::Logger m_rosLogger; /** Associated ros logger */
+};
+
+} // namespace TextLogging
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_TEXT_LOGGING_ROS_LOGGER_H