utilities.hpp

Go to the documentation of this file.

#pragma once

#include <limits>
#include <stdlib.h>
#include <string>
#include <ros/ros.h>
#include <boost/algorithm/string.hpp>

template <typename T>
using is_vector = std::is_same<T, std::vector<typename T::value_type, typename T::allocator_type>>;

template <typename T>
using is_map = std::is_same<
    T, std::map<typename T::key_type, typename T::mapped_type, typename T::key_compare, typename T::allocator_type>>;

namespace rosparam_handler {

template <typename T>
inline std::string to_string(const std::vector<T>& v)
{
  std::string s;
  if (!v.empty()) {
      std::stringstream ss;
      ss << '[';
      std::copy(v.begin(), v.end(), std::ostream_iterator<T>(ss, ", "));
      ss << "\b\b]";
      s = ss.str();
  }
  return s;
}

template <typename... T>
inline std::string to_string(const std::map<T...>& map)
{
  std::stringstream ss;
  ss << '{';
  for (typename std::map<T...>::const_iterator it = map.begin(); it != map.end(); ++it) {
      ss << (*it).first << " --> " << (*it).second << ", ";
  }
  ss << '}';
  return ss.str();
}

inline void setLoggerLevel(const ros::NodeHandle& nodeHandle) {

    std::string verbosity;
    if (!nodeHandle.getParam("verbosity", verbosity)) {
        verbosity = "warning";
    }

    ros::console::Level level_ros;
    bool valid_verbosity{true};
    if (verbosity == "debug") {
        level_ros = ros::console::levels::Debug;
    } else if (verbosity == "info") {
        level_ros = ros::console::levels::Info;
    } else if (verbosity == "warning") {
        level_ros = ros::console::levels::Warn;
    } else if (verbosity == "error") {
        level_ros = ros::console::levels::Error;
    } else if (verbosity == "fatal") {
        level_ros = ros::console::levels::Fatal;
    } else {
        ROS_WARN_STREAM("Invalid verbosity level specified: " << verbosity << "! Falling back to INFO.");
        valid_verbosity = false;
    }
    if (valid_verbosity) {
        if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, level_ros)) {
            ros::console::notifyLoggerLevelsChanged();
            ROS_DEBUG_STREAM("Verbosity set to " << verbosity);
        }
    }
}

inline void showNodeInfo() {

    using namespace ros::this_node;

    std::vector<std::string> subscribed_topics, advertised_topics;
    getSubscribedTopics(subscribed_topics);
    getAdvertisedTopics(advertised_topics);

    std::ostringstream msg_subscr, msg_advert;
    for (auto const& t : subscribed_topics) {
        msg_subscr << t << "\n";
    }
    for (auto const& t : advertised_topics) {
        msg_advert << t << "\n";
    }

    ROS_INFO_STREAM("Started '" << getName() << "' in namespace '" << getNamespace() << "'.\n"
                                << "Subscribed topics:\n"
                                << msg_subscr.str() << "Advertised topics:\n"
                                << msg_advert.str());
}

inline std::string getNodeName(const ros::NodeHandle& privateNodeHandle) {
    std::string name_space = privateNodeHandle.getNamespace();
    std::stringstream tempString(name_space);
    std::string name;
    while (std::getline(tempString, name, '/')) {
        ;
    }
    return name;
}

inline void exit(const std::string msg = "Runtime Error in rosparam handler.") {
    // std::exit(EXIT_FAILURE);
    throw std::runtime_error(msg);
}

template <typename T>
inline void setParam(const std::string key, T val) {
    ros::param::set(key, val);
}

template <typename T>
inline bool getParam(const std::string key, T& val) {
    if (!ros::param::has(key)) {
        ROS_ERROR_STREAM("Parameter '" << key << "' is not defined.");
        return false;
    } else if (!ros::param::get(key, val)) {
        ROS_ERROR_STREAM("Could not retrieve parameter'" << key << "'. Does it have a different type?");
        return false;
    } else {
        return true;
    }
}

template <typename T>
inline bool getParam(const std::string key, T& val, const T& defaultValue) {
    if (!ros::param::has(key) || !ros::param::get(key, val)) {
        val = defaultValue;
        ros::param::set(key, defaultValue);
        ROS_INFO_STREAM("Setting default value for parameter '" << key << "'.");
        return true;
    } else {
        // Param was already retrieved with last if statement.
        return true;
    }
}

inline bool testConstParam(const std::string key) {
    if (ros::param::has(key)) {
        ROS_WARN_STREAM("Parameter " << key
                                     << "' was set on the parameter server eventhough it was defined to be constant.");
        return false;
    } else {
        return true;
    }
}

template <typename T>
inline void testMin(const std::string key, T& val, T min = std::numeric_limits<T>::min()) {
    if (val < min) {
        ROS_WARN_STREAM("Value of " << val << " for " << key
                                    << " is smaller than minimal allowed value. Correcting value to min=" << min);
        val = min;
    }
}

template <typename T>
inline void testMin(const std::string key, std::vector<T>& val, T min = std::numeric_limits<T>::min()) {
    for (auto& v : val)
        testMin(key, v, min);
}

template <typename K, typename T>
inline void testMin(const std::string key, std::map<K, T>& val, T min = std::numeric_limits<T>::min()) {
    for (auto& v : val)
        testMin(key, v.second, min);
}

template <typename T>
inline void testMax(const std::string key, T& val, T max = std::numeric_limits<T>::max()) {
    if (val > max) {
        ROS_WARN_STREAM("Value of " << val << " for " << key
                                    << " is greater than maximal allowed. Correcting value to max=" << max);
        val = max;
    }
}

template <typename T>
inline void testMax(const std::string key, std::vector<T>& val, T max = std::numeric_limits<T>::max()) {
    for (auto& v : val)
        testMax(key, v, max);
}

template <typename K, typename T>
inline void testMax(const std::string key, std::map<K, T>& val, T max = std::numeric_limits<T>::max()) {
    for (auto& v : val)
        testMax(key, v.second, max);
}

} // namespace rosparam_handler