utils.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

/*
 * File: utils.h
 * Author: Vladislav Tananaev
 *
 */

#pragma once

#include <ros/console.h>
#include <mrpt/system/datetime.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/version.h>
#include <log4cxx/logger.h>

#if MRPT_VERSION >= 0x199
#include <mrpt/system/COutputLogger.h>
#include <mrpt/math/TPose3D.h>
using namespace mrpt::system;
#else
#include <mrpt/utils/COutputLogger.h>
using namespace mrpt::utils;
#endif

namespace mrpt_bridge
{
inline VerbosityLevel rosLoggerLvlToMRPTLoggerLvl(log4cxx::LevelPtr lvl)
{
        using namespace log4cxx;

        // determine on the corresponding VerbosityLevel
        VerbosityLevel mrpt_lvl;

        if (lvl == Level::getFatal() || lvl == Level::getError())
        {
                mrpt_lvl = LVL_ERROR;
        }
        else if (lvl == Level::getWarn())
        {
                mrpt_lvl = LVL_WARN;
        }
        else if (lvl == Level::getInfo())
        {
                mrpt_lvl = LVL_INFO;
        }
        else if (lvl == Level::getDebug())
        {
                mrpt_lvl = LVL_DEBUG;
        }
        else
        {
                THROW_EXCEPTION("Unknown log4cxx::Level is given.");
        }

        return mrpt_lvl;

}  // end of rosLoggerLvlToMRPTLoggerLvl

inline void mrptToROSLoggerCallback(
        const std::string& msg, const VerbosityLevel level,
        const std::string& loggerName, const mrpt::system::TTimeStamp timestamp)
{
        // Remove trailing \n if present
        std::string tmsg = msg;
        if (!tmsg.empty() &&
                tmsg.compare(tmsg.length() - 1, tmsg.length(), "\n") == 0)
        {
                tmsg.erase(tmsg.end() - 1);
        }

        if (level == LVL_DEBUG)
        {
                ROS_DEBUG("%s", tmsg.c_str());
        }
        else if (level == LVL_INFO)
        {
                ROS_INFO("%s", tmsg.c_str());
        }
        else if (level == LVL_WARN)
        {
                ROS_WARN("%s", tmsg.c_str());
        }
        else if (level == LVL_ERROR)
        {
                ROS_ERROR("%s", tmsg.c_str());
        }
}
inline void mrptToROSLoggerCallback_mrpt_15(
        const std::string& msg, const VerbosityLevel level,
        const std::string& loggerName, const mrpt::system::TTimeStamp timestamp,
        void* userParam)
{
        mrptToROSLoggerCallback(msg, level, loggerName, timestamp);
}

inline mrpt::math::TPose3D p2t(const mrpt::poses::CPose3D& p)
{
#if MRPT_VERSION >= 0x199
        return p.asTPose();
#else
        return mrpt::math::TPose3D(p);
#endif
}

}  // namespace mrpt_bridge