Added log handler for handling log messages from client library with … (#398)

* Added log handler for handling log messages from client library with ROS logging

Author: urmahp

ur_calibration/CMakeLists.txt

@@ -6,6 +6,7 @@ add_compile_options(-std=c++11)
 
 find_package(catkin REQUIRED COMPONENTS
   roscpp
+  ur_robot_driver
 )
 find_package(Eigen3 REQUIRED)
 find_package(yaml-cpp REQUIRED)
@@ -17,6 +18,7 @@ set(YAML_CPP_INCLUDE_DIRS ${YAML_CPP_INCLUDE_DIR})
 catkin_package(
   CATKIN_DEPENDS
     roscpp
+    ur_robot_driver
   DEPENDS
     YAML_CPP
     ur_client_library

ur_calibration/package.xml

@@ -53,6 +53,7 @@
   <depend>roscpp</depend>
   <depend>ur_client_library</depend>
   <depend>yaml-cpp</depend>
+  <depend>ur_robot_driver</depend>
 
 
   <test_depend>rosunit</test_depend>

ur_calibration/src/calibration_correction.cpp

@@ -34,6 +34,8 @@
 #include <ur_client_library/primary/package_header.h>
 #include <ur_client_library/primary/primary_parser.h>
 
+#include <ur_robot_driver/urcl_log_handler.h>
+
 #include <sensor_msgs/JointState.h>
 #include <tf/transform_listener.h>
 #include <ros/package.h>
@@ -162,6 +164,8 @@ int main(int argc, char* argv[])
   ros::init(argc, argv, "ur_calibration");
   ros::NodeHandle nh("~");
 
+  ur_driver::registerUrclLogHandler();
+
   try
   {
     CalibrationCorrection my_calibration_correction(nh);

ur_robot_driver/CMakeLists.txt

@@ -36,6 +36,7 @@ catkin_package(
     include
   LIBRARIES
     ur_robot_driver_plugin
+    urcl_log_handler
   CATKIN_DEPENDS
     actionlib
     control_msgs
@@ -88,19 +89,24 @@ add_library(ur_robot_driver_plugin
 target_link_libraries(ur_robot_driver_plugin ur_client_library::urcl ${catkin_LIBRARIES})
 add_dependencies(ur_robot_driver_plugin ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
+add_library(urcl_log_handler
+  src/urcl_log_handler.cpp
+)
+target_link_libraries(urcl_log_handler ${catkin_LIBRARIES} ur_client_library::urcl)
+
 add_executable(ur_robot_driver_node
   src/dashboard_client_ros.cpp
   src/hardware_interface.cpp
   src/hardware_interface_node.cpp
 )
-target_link_libraries(ur_robot_driver_node ${catkin_LIBRARIES} ur_client_library::urcl)
+target_link_libraries(ur_robot_driver_node ${catkin_LIBRARIES} ur_client_library::urcl urcl_log_handler)
 add_dependencies(ur_robot_driver_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 add_executable(dashboard_client
   src/dashboard_client_ros.cpp
   src/dashboard_client_node.cpp
 )
-target_link_libraries(dashboard_client ${catkin_LIBRARIES} ur_client_library::urcl)
+target_link_libraries(dashboard_client ${catkin_LIBRARIES} ur_client_library::urcl urcl_log_handler)
 add_dependencies(dashboard_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 add_executable(robot_state_helper
@@ -117,7 +123,7 @@ if(CATKIN_ENABLE_TESTING)
 endif()
 
 
-install(TARGETS ur_robot_driver_plugin ur_robot_driver_node robot_state_helper dashboard_client
+install(TARGETS ur_robot_driver_plugin urcl_log_handler ur_robot_driver_node robot_state_helper dashboard_client
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

ur_robot_driver/include/ur_robot_driver/urcl_log_handler.h

@@ -0,0 +1,80 @@
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// 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 [email protected].
+// -- END LICENSE BLOCK ------------------------------------------------
+
+#ifndef UR_DRIVER_URCL_LOG_HANDLER_H_INCLUDED
+#define UR_DRIVER_URCL_LOG_HANDLER_H_INCLUDED
+
+#include <ur_client_library/log.h>
+
+namespace ur_driver
+{
+/*!
+ * \brief Loghandler for handling messages logged with the C++ client library. This loghandler will log the messages
+ * from the client library with ROS logging.
+ * Use registerLogHandler to register this class. 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;
+
+private:
+  std::string log_name_;
+
+  void logMessage(const char* file, int line, ros::console::Level level, const char* message);
+};
+
+/*!
+ * \brief Register the UrclLoghHandler, this will start logging messages from the client library with RPS2 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_driver
+
+#endif  // UR_DRIVER_URCL_LOG_HANDLER_H_INCLUDED

ur_robot_driver/src/dashboard_client_node.cpp

@@ -28,12 +28,16 @@
 #include <ros/ros.h>
 #include <ur_robot_driver/dashboard_client_ros.h>
 
+#include <ur_robot_driver/urcl_log_handler.h>
+
 int main(int argc, char** argv)
 {
   // Set up ROS.
   ros::init(argc, argv, "dashboard_client");
   ros::NodeHandle priv_nh("~");
 
+  ur_driver::registerUrclLogHandler();
+
   // The IP address under which the robot is reachable.
   std::string robot_ip = priv_nh.param<std::string>("robot_ip", "192.168.56.101");
 

ur_robot_driver/src/hardware_interface_node.cpp

@@ -29,6 +29,7 @@
 
 #include <csignal>
 #include <ur_robot_driver/hardware_interface.h>
+#include <ur_robot_driver/urcl_log_handler.h>
 
 std::unique_ptr<ur_driver::HardwareInterface> g_hw_interface;
 
@@ -56,6 +57,8 @@ int main(int argc, char** argv)
   // register signal SIGINT and signal handler
   signal(SIGINT, signalHandler);
 
+  ur_driver::registerUrclLogHandler();
+
   std::ifstream realtime_file("/sys/kernel/realtime", std::ios::in);
   bool has_realtime = false;
   if (realtime_file.is_open())

ur_robot_driver/src/urcl_log_handler.cpp

@@ -0,0 +1,96 @@
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// 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 [email protected].
+// -- END LICENSE BLOCK ------------------------------------------------
+
+#include <ros/console.h>
+#include <ur_robot_driver/urcl_log_handler.h>
+
+namespace ur_driver
+{
+bool g_registered = false;
+std::unique_ptr<UrclLogHandler> g_log_handler(new UrclLogHandler);
+
+UrclLogHandler::UrclLogHandler() : log_name_(std::string(ROSCONSOLE_NAME_PREFIX) + ".ur_client_library")
+{
+}
+
+void UrclLogHandler::log(const char* file, int line, urcl::LogLevel loglevel, const char* message)
+{
+  switch (loglevel)
+  {
+    case urcl::LogLevel::DEBUG:
+      logMessage(file, line, ros::console::levels::Debug, message);
+      break;
+    case urcl::LogLevel::INFO:
+      logMessage(file, line, ros::console::levels::Info, message);
+      break;
+    case urcl::LogLevel::WARN:
+      logMessage(file, line, ros::console::levels::Warn, message);
+      break;
+    case urcl::LogLevel::ERROR:
+      logMessage(file, line, ros::console::levels::Error, message);
+      break;
+    case urcl::LogLevel::FATAL:
+      logMessage(file, line, ros::console::levels::Fatal, message);
+      break;
+    default:
+      break;
+  }
+}
+
+void UrclLogHandler::logMessage(const char* file, int line, ros::console::Level level, const char* message)
+{
+  ROSCONSOLE_DEFINE_LOCATION(true, level, log_name_);
+  if (ROS_UNLIKELY(__rosconsole_define_location__enabled))
+  {
+    ros::console::print(NULL, __rosconsole_define_location__loc.logger_, level, file, line, "", "%s", message);
+  }
+}
+
+void registerUrclLogHandler()
+{
+  if (g_registered == false)
+  {
+    // Log level is decided by ROS 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_driver