Add a callback function to stream MRPT logs to ROS.

mrpt_bridge/include/mrpt_bridge/mrpt_bridge.h

@@ -10,4 +10,5 @@
 #include "point_cloud2.h"
 #include "beacon.h"
 #include "landmark.h"
+#include "utils.h"
 #endif

mrpt_bridge/include/mrpt_bridge/utils.h

@@ -0,0 +1,61 @@
+/*
+ * File: utils.h
+ * Author: Vladislav Tananaev
+ *
+ */
+
+#ifndef MRPT_BRIDGE_UTILS_H
+#define MRPT_BRIDGE_UTILS_H
+#include <ros/console.h>
+#include <mrpt/system/datetime.h>
+#include <mrpt/utils/COutputLogger.h>
+#include <log4cxx/logger.h>
+
+namespace mrpt_bridge {
+
+	/**
+   * @brief function that converts ROS verbosity level log4cxx::Level to MRPT equivalent mrpt::utils::VerbosityLevel
+   */
+	inline mrpt::utils::VerbosityLevel rosLoggerLvlToMRPTLoggerLvl(log4cxx::LevelPtr lvl){
+		if (lvl == log4cxx::Level::getFatal())
+	  {
+	    return mrpt::utils::LVL_ERROR;
+	  }
+	  else if (lvl == log4cxx::Level::getError())
+	  {
+	    return mrpt::utils::LVL_ERROR;
+	  }
+	  else if (lvl == log4cxx::Level::getWarn())
+	  {
+	    return mrpt::utils::LVL_WARN;
+	  }
+	  else if (lvl == log4cxx::Level::getInfo())
+	  {
+	    return mrpt::utils::LVL_INFO;
+	  }
+	  else if (lvl == log4cxx::Level::getDebug())
+	  {
+	    return mrpt::utils::LVL_DEBUG;
+	  }
+	}
+
+	/**
+   * @brief callback that is called by MRPT mrpt::utils::COuputLogger to refirect log messages to ROS logger. 
+   * 	This function has to be inline, otherwise option log4j.logger.ros.package_name will be taken from mrpt_bridge
+   *  instead of the package from which macro is actually called.
+   */
+	inline void mrptToROSLoggerCallback(const std::string &msg, const mrpt::utils::VerbosityLevel level, const std::string &loggerName, const mrpt::system::TTimeStamp timestamp, void *userParam){
+		if (level == mrpt::utils::LVL_DEBUG){
+	    ROS_DEBUG("%s", msg.c_str());
+	  } else if (level == mrpt::utils::LVL_INFO) {
+	    ROS_INFO("%s", msg.c_str());
+	  } else if (level == mrpt::utils::LVL_WARN) {
+	    ROS_WARN("%s", msg.c_str());
+	  } else if (level == mrpt::utils::LVL_ERROR) {
+	    ROS_ERROR("%s", msg.c_str());
+	  }
+	}
+
+} //namespace mrpt_bridge
+
+#endif //MRPT_BRIDGE_UTILS_H