Go to the documentation of this file.00001 #pragma once
00002
00003 #include <ros/ros.h>
00004 #include <boost/algorithm/string.hpp>
00005
00006 namespace rosparam_handler {
00007
00015 inline void setLoggerLevel(const ros::NodeHandle& nodeHandle) {
00016
00017 std::string verbosity;
00018 if (!nodeHandle.getParam("verbosity", verbosity)) {
00019 verbosity = "warning";
00020 }
00021
00022 ros::console::Level level_ros;
00023 bool valid_verbosity {true};
00024 if (verbosity == "debug") {
00025 level_ros = ros::console::levels::Debug;
00026 } else if (verbosity == "info") {
00027 level_ros = ros::console::levels::Info;
00028 } else if (verbosity == "warning") {
00029 level_ros = ros::console::levels::Warn;
00030 } else if (verbosity == "error") {
00031 level_ros = ros::console::levels::Error;
00032 } else if (verbosity == "fatal") {
00033 level_ros = ros::console::levels::Fatal;
00034 } else {
00035 ROS_WARN_STREAM(
00036 "Invalid verbosity level specified: " << verbosity << "! Falling back to INFO.");
00037 valid_verbosity = false;
00038 }
00039 if (valid_verbosity) {
00040 if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, level_ros)) {
00041 ros::console::notifyLoggerLevelsChanged();
00042 ROS_DEBUG_STREAM("Verbosity set to " << verbosity);
00043 }
00044 }
00045 }
00046
00051 inline void showNodeInfo() {
00052
00053 using namespace ros::this_node;
00054
00055 std::vector<std::string> subscribed_topics, advertised_topics;
00056 getSubscribedTopics(subscribed_topics);
00057 getAdvertisedTopics(advertised_topics);
00058
00059 std::ostringstream msg_subscr, msg_advert;
00060 for (auto const& t : subscribed_topics) {
00061 msg_subscr << t << std::endl;
00062 }
00063 for (auto const& t : advertised_topics) {
00064 msg_advert << t << std::endl;
00065 }
00066
00067 ROS_INFO_STREAM(
00068 "Started '" << getName() << "' in namespace '" << getNamespace() << "'." << std::endl << "Subscribed topics: " << std::endl << msg_subscr.str() << "Advertised topics: " << std::endl << msg_advert.str());
00069 }
00070
00071 }