From 51583ed088cac69e3144210d3addba6440c1cb0f Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Mon, 24 Jul 2023 11:00:15 +0200 Subject: [PATCH] Introduced tf_prefix into log handler (#713) * Introduced tf_prefix into log handler * added default argument to prefix --------- Co-authored-by: Lennart Nachtigall Co-authored-by: Felix Exner Co-authored-by: Lennart Nachtigall (cherry picked from commit d7e0d6d95788298a9d59a5f3b287c946c0edefde) --- .../ur_robot_driver/urcl_log_handler.hpp | 47 ++++++++++++++----- ur_robot_driver/src/dashboard_client_node.cpp | 2 +- ur_robot_driver/src/hardware_interface.cpp | 5 +- ur_robot_driver/src/urcl_log_handler.cpp | 15 +++--- 4 files changed, 50 insertions(+), 19 deletions(-) diff --git a/ur_robot_driver/include/ur_robot_driver/urcl_log_handler.hpp b/ur_robot_driver/include/ur_robot_driver/urcl_log_handler.hpp index ddc5242d9..a0eb8e2d4 100644 --- a/ur_robot_driver/include/ur_robot_driver/urcl_log_handler.hpp +++ b/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 #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 diff --git a/ur_robot_driver/src/dashboard_client_node.cpp b/ur_robot_driver/src/dashboard_client_node.cpp index c6e24c182..91cdf9f97 100644 --- a/ur_robot_driver/src/dashboard_client_node.cpp +++ b/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("robot_ip", "192.168.56.101"); node->get_parameter("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); diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index ddbf8b91a..2434e5248 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/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( diff --git a/ur_robot_driver/src/urcl_log_handler.cpp b/ur_robot_driver/src/urcl_log_handler.cpp index 8c4a4d48a..809313d19 100644 --- a/ur_robot_driver/src/urcl_log_handler.cpp +++ b/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(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));