Added loghandler to handle log messages from the Client Library with …

ur_robot_driver/CMakeLists.txt

@@ -36,6 +36,7 @@ add_library(ur_robot_driver_plugin
   SHARED
   src/dashboard_client_ros.cpp
   src/hardware_interface.cpp
+  src/urcl_log_handler.cpp
 )
 target_link_libraries(
   ur_robot_driver_plugin
@@ -56,6 +57,7 @@ pluginlib_export_plugin_description_file(hardware_interface hardware_interface_p
 add_executable(dashboard_client
   src/dashboard_client_ros.cpp
   src/dashboard_client_node.cpp
+  src/urcl_log_handler.cpp
 )
 target_link_libraries(dashboard_client ${catkin_LIBRARIES} ur_client_library::urcl)
 ament_target_dependencies(dashboard_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})

ur_robot_driver/include/ur_robot_driver/urcl_log_handler.hpp

@@ -0,0 +1,73 @@
+// Copyright 2021 Universal Robots A/S
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// All source code contained in and/or linked to in this message (the “Source Code”) is subject to the copyright of
+// Universal Robots A/S and/or its licensors. THE SOURCE CODE IS PROVIDED “AS IS” WITHOUT WARRANTY OF ANY KIND, EXPRESS
+// OR IMPLIED, INCLUDING – BUT NOT LIMITED TO – WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, OR
+// NONINFRINGEMENT. USE OF THE SOURCE CODE IS AT YOUR OWN RISK AND UNIVERSAL ROBOTS A/S AND ITS LICENSORS SHALL, TO THE
+// MAXIMUM EXTENT PERMITTED BY LAW, NOT BE LIABLE FOR ANY ERRORS OR MALICIOUS CODE IN THE SOURCE CODE, ANY THIRD-PARTY
+// CLAIMS, OR ANY OTHER CLAIMS AND DAMAGES, INCLUDING INDIRECT, INCIDENTAL, SPECIAL, CONSEQUENTIAL OR PUNITIVE DAMAGES,
+// OR ANY LOSS OF PROFITS, EXPECTED SAVINGS, OR REVENUES, WHETHER INCURRED DIRECTLY OR INDIRECTLY, OR ANY LOSS OF DATA,
+// USE, GOODWILL, OR OTHER INTANGIBLE LOSSES, RESULTING FROM YOUR USE OF THE SOURCE CODE. You may make copies of the
+// Source Code for use in connection with a Universal Robots or UR+ product, provided that you include (i) an
+// appropriate copyright notice (“©  [the year in which you received the Source Code or the Source Code was first
+// published, e.g. “2021”] Universal Robots A/S and/or its licensors”) along with the capitalized section of this notice
+// in all copies of the Source Code. By using the Source Code, you agree to the above terms. For more information,
+// please contact legal@universal-robots.com.
+
+#ifndef UR_ROBOT_DRIVER__URCL_LOG_HANDLER_HPP_
+#define UR_ROBOT_DRIVER__URCL_LOG_HANDLER_HPP_
+
+#include "ur_client_library/log.h"
+
+namespace ur_robot_driver
+{
+/*!
+ * \brief Loghandler for handling messages logged with the C++ client library. This loghandler will log the messages
+ * from the client library with ROS2s logging.
+ * Use registerLogHandler to register this LogHandler. This class shouldn't be instantiated directly.
+ */
+class UrclLogHandler : public urcl::LogHandler
+{
+public:
+  /*!
+   * \brief Default constructor
+   */
+  UrclLogHandler();
+
+  /*!
+   * \brief Function to log a message
+   *
+   * \param file The log message comes from this file
+   * \param line The log message comes from this line
+   * \param loglevel Indicates the severity of the log message
+   * \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 Unregister the UrclLoghHandler, stop logging messages from the client library with ROS2 logging.
+ */
+void unregisterUrclLogHandler();
+
+}  // namespace ur_robot_driver
+
+#endif  // UR_ROBOT_DRIVER__URCL_LOG_HANDLER_HPP_

ur_robot_driver/src/dashboard_client_node.cpp

@@ -26,6 +26,7 @@
 #include <string>
 
 #include "rclcpp/rclcpp.hpp"
+#include "ur_robot_driver/urcl_log_handler.hpp"
 
 int main(int argc, char** argv)
 {
@@ -36,6 +37,8 @@ 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::DashboardClientROS client(node, robot_ip);
 
   rclcpp::spin(node);

ur_robot_driver/src/hardware_interface.cpp

@@ -34,6 +34,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "ur_robot_driver/hardware_interface.hpp"
+#include "ur_robot_driver/urcl_log_handler.hpp"
 
 namespace rtde = urcl::rtde_interface;
 
@@ -335,6 +336,7 @@ return_type URPositionHardwareInterface::start()
   }
 
   RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Initializing driver...");
+  registerUrclLogHandler();
   try {
     ur_driver_ = std::make_unique<urcl::UrDriver>(
         robot_ip, script_filename, output_recipe_filename, input_recipe_filename,

ur_robot_driver/src/urcl_log_handler.cpp

@@ -0,0 +1,84 @@
+// Copyright 2021 Universal Robots A/S
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// All source code contained in and/or linked to in this message (the “Source Code”) is subject to the copyright of
+// Universal Robots A/S and/or its licensors. THE SOURCE CODE IS PROVIDED “AS IS” WITHOUT WARRANTY OF ANY KIND, EXPRESS
+// OR IMPLIED, INCLUDING – BUT NOT LIMITED TO – WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, OR
+// NONINFRINGEMENT. USE OF THE SOURCE CODE IS AT YOUR OWN RISK AND UNIVERSAL ROBOTS A/S AND ITS LICENSORS SHALL, TO THE
+// MAXIMUM EXTENT PERMITTED BY LAW, NOT BE LIABLE FOR ANY ERRORS OR MALICIOUS CODE IN THE SOURCE CODE, ANY THIRD-PARTY
+// CLAIMS, OR ANY OTHER CLAIMS AND DAMAGES, INCLUDING INDIRECT, INCIDENTAL, SPECIAL, CONSEQUENTIAL OR PUNITIVE DAMAGES,
+// OR ANY LOSS OF PROFITS, EXPECTED SAVINGS, OR REVENUES, WHETHER INCURRED DIRECTLY OR INDIRECTLY, OR ANY LOSS OF DATA,
+// USE, GOODWILL, OR OTHER INTANGIBLE LOSSES, RESULTING FROM YOUR USE OF THE SOURCE CODE. You may make copies of the
+// Source Code for use in connection with a Universal Robots or UR+ product, provided that you include (i) an
+// appropriate copyright notice (“©  [the year in which you received the Source Code or the Source Code was first
+// published, e.g. “2021”] Universal Robots A/S and/or its licensors”) along with the capitalized section of this notice
+// in all copies of the Source Code. By using the Source Code, you agree to the above terms. For more information,
+// please contact legal@universal-robots.com.
+
+#include <memory>
+#include <utility>
+
+#include "ur_robot_driver/urcl_log_handler.hpp"
+#include "rclcpp/logging.hpp"
+
+namespace ur_robot_driver
+{
+bool g_registered = false;
+std::unique_ptr<UrclLogHandler> g_log_handler(new UrclLogHandler);
+
+UrclLogHandler::UrclLogHandler() = default;
+
+void UrclLogHandler::log(const char* file, int line, urcl::LogLevel loglevel, const char* message)
+{
+  rcutils_log_location_t location = { "", file, (size_t)line };
+  switch (loglevel) {
+    case urcl::LogLevel::DEBUG:
+      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_DEBUG, "UR_Client_Library", "%s", message);
+      break;
+    case urcl::LogLevel::INFO:
+      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_INFO, "UR_Client_Library", "%s", message);
+      break;
+    case urcl::LogLevel::WARN:
+      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_WARN, "UR_Client_Library", "%s", message);
+      break;
+    case urcl::LogLevel::ERROR:
+      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_ERROR, "UR_Client_Library", "%s", message);
+      break;
+    case urcl::LogLevel::FATAL:
+      rcutils_log(&location, RCUTILS_LOG_SEVERITY::RCUTILS_LOG_SEVERITY_FATAL, "UR_Client_Library", "%s", message);
+      break;
+    default:
+      break;
+  }
+}
+
+void registerUrclLogHandler()
+{
+  if (g_registered == false) {
+    // Log level is decided by ROS2 log level
+    urcl::setLogLevel(urcl::LogLevel::DEBUG);
+    urcl::registerLogHandler(std::move(g_log_handler));
+    g_registered = true;
+  }
+}
+
+void unregisterUrclLogHandler()
+{
+  if (g_registered == true) {
+    urcl::unregisterLogHandler();
+    g_registered = false;
+  }
+}
+
+}  // namespace ur_robot_driver