Introduced tf_prefix into log handler (#713)

* Introduced tf_prefix into log handler
* added default argument to prefix

---------

Co-authored-by: Lennart Nachtigall <[email protected]>
Co-authored-by: Felix Exner <[email protected]>
Co-authored-by: Lennart Nachtigall <[email protected]>
(cherry picked from commit d7e0d6d)

ur_robot_driver/include/ur_robot_driver/urcl_log_handler.hpp

@@ -43,10 +43,22 @@
 #ifndef UR_ROBOT_DRIVER__URCL_LOG_HANDLER_HPP_
 #define UR_ROBOT_DRIVER__URCL_LOG_HANDLER_HPP_
 
+#include <string>
 #include "ur_client_library/log.h"
-
 namespace ur_robot_driver
 {
+
+/*!
+ * \brief Register the UrclLoghHandler, this will start logging messages from the client library with ROS2 logging.
+ * This function has to be called inside your node, to enable the log handler.
+ */
+void registerUrclLogHandler(const std::string& tf_prefix = "");
+
+/*!
+ * \brief Unregister the UrclLoghHandler, stop logging messages from the client library with ROS2 logging.
+ */
+void unregisterUrclLogHandler();
+
 /*!
  * \brief Loghandler for handling messages logged with the C++ client library. This loghandler will log the messages
  * from the client library with ROS2s logging.
@@ -69,18 +81,31 @@ class UrclLogHandler : public urcl::LogHandler
    * \param log Log message
    */
   void log(const char* file, int line, urcl::LogLevel loglevel, const char* message) override;
-};
 
-/*!
- * \brief Register the UrclLoghHandler, this will start logging messages from the client library with ROS2 logging.
- * This function has to be called inside your node, to enable the log handler.
- */
-void registerUrclLogHandler();
+  /**
+   * @brief getTFPrefix - obtain the currently set tf_prefix
+   * @return
+   */
+  const std::string& getTFPrefix() const
+  {
+    return tf_prefix_;
+  }
 
-/*!
- * \brief Unregister the UrclLoghHandler, stop logging messages from the client library with ROS2 logging.
- */
-void unregisterUrclLogHandler();
+private:
+  std::string tf_prefix_{};
+
+  /**
+   * @brief setTFPrefix - set the tf_prefix the logger will append to the node name
+   * @param tf_prefix
+   */
+  void setTFPrefix(const std::string& tf_prefix)
+  {
+    tf_prefix_ = tf_prefix;
+  }
+
+  // Declare the register method as a friend so that we can access setTFPrefix from it
+  friend void registerUrclLogHandler(const std::string& tf_prefix);
+};
 
 }  // namespace ur_robot_driver
 

ur_robot_driver/src/dashboard_client_node.cpp

@@ -51,7 +51,7 @@ int main(int argc, char** argv)
   std::string robot_ip = node->declare_parameter<std::string>("robot_ip", "192.168.56.101");
   node->get_parameter<std::string>("robot_ip", robot_ip);
 
-  ur_robot_driver::registerUrclLogHandler();
+  ur_robot_driver::registerUrclLogHandler("");  // Set empty tf_prefix at the moment
 
   ur_robot_driver::DashboardClientROS client(node, robot_ip);
 

ur_robot_driver/src/hardware_interface.cpp

@@ -417,8 +417,11 @@ URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous
   // Amount of allowed timed out reads before the reverse interface will be dropped
   const int keep_alive_count = std::stoi(info_.hardware_parameters["keep_alive_count"]);
 
+  // Obtain the tf_prefix which is needed for the logging handler so that log messages from different arms are
+  // distiguishable in the log
+  const std::string tf_prefix = info_.hardware_parameters.at("tf_prefix");
   RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Initializing driver...");
-  registerUrclLogHandler();
+  registerUrclLogHandler(tf_prefix);
   try {
     rtde_comm_has_been_started_ = false;
     ur_driver_ = std::make_unique<urcl::UrDriver>(

ur_robot_driver/src/urcl_log_handler.cpp

@@ -56,30 +56,33 @@ UrclLogHandler::UrclLogHandler() = default;
 void UrclLogHandler::log(const char* file, int line, urcl::LogLevel loglevel, const char* message)
 {
   rcutils_log_location_t location = { "", file, static_cast<size_t>(line) };
+
+  const auto logger_name = "UR_Client_Library:" + tf_prefix_;
   switch (loglevel) {
     case urcl::LogLevel::DEBUG:
-      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_DEBUG, "UR_Client_Library", "%s", message);
+      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_DEBUG, logger_name.c_str(), "%s", message);
       break;
     case urcl::LogLevel::INFO:
-      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_INFO, "UR_Client_Library", "%s", message);
+      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_INFO, logger_name.c_str(), "%s", message);
       break;
     case urcl::LogLevel::WARN:
-      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_WARN, "UR_Client_Library", "%s", message);
+      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_WARN, logger_name.c_str(), "%s", message);
       break;
     case urcl::LogLevel::ERROR:
-      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_ERROR, "UR_Client_Library", "%s", message);
+      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_ERROR, logger_name.c_str(), "%s", message);
       break;
     case urcl::LogLevel::FATAL:
-      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_FATAL, "UR_Client_Library", "%s", message);
+      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_FATAL, logger_name.c_str(), "%s", message);
       break;
     default:
       break;
   }
 }
 
-void registerUrclLogHandler()
+void registerUrclLogHandler(const std::string& tf_prefix)
 {
   if (g_registered == false) {
+    g_log_handler->setTFPrefix(tf_prefix);
     // Log level is decided by ROS2 log level
     urcl::setLogLevel(urcl::LogLevel::DEBUG);
     urcl::registerLogHandler(std::move(g_log_handler));