utilities.hpp
Go to the documentation of this file.
1 #pragma once
2 
3 #include <limits>
4 #include <stdlib.h>
5 #include <string>
6 #include <ros/ros.h>
7 #include <boost/algorithm/string.hpp>
8 
10 template <typename T>
11 using is_vector = std::is_same<T, std::vector<typename T::value_type, typename T::allocator_type>>;
12 
14 template <typename T>
15 using is_map = std::is_same<
16  T, std::map<typename T::key_type, typename T::mapped_type, typename T::key_compare, typename T::allocator_type>>;
17 
18 namespace rosparam_handler {
19 
20 template <typename T>
21 inline std::string to_string(const std::vector<T>& v)
22 {
23  std::string s;
24  if (!v.empty()) {
25  std::stringstream ss;
26  ss << '[';
27  std::copy(v.begin(), v.end(), std::ostream_iterator<T>(ss, ", "));
28  ss << "\b\b]";
29  s = ss.str();
30  }
31  return s;
32 }
33 
34 template <typename... T>
35 inline std::string to_string(const std::map<T...>& map)
36 {
37  std::stringstream ss;
38  ss << '{';
39  for (typename std::map<T...>::const_iterator it = map.begin(); it != map.end(); ++it) {
40  ss << (*it).first << " --> " << (*it).second << ", ";
41  }
42  ss << '}';
43  return ss.str();
44 }
45 
49 inline void setLoggerLevel(const ros::NodeHandle& nodeHandle) {
50 
51  std::string verbosity;
52  if (!nodeHandle.getParam("verbosity", verbosity)) {
53  verbosity = "warning";
54  }
55 
56  ros::console::Level level_ros;
57  bool valid_verbosity{true};
58  if (verbosity == "debug") {
59  level_ros = ros::console::levels::Debug;
60  } else if (verbosity == "info") {
61  level_ros = ros::console::levels::Info;
62  } else if (verbosity == "warning") {
63  level_ros = ros::console::levels::Warn;
64  } else if (verbosity == "error") {
65  level_ros = ros::console::levels::Error;
66  } else if (verbosity == "fatal") {
67  level_ros = ros::console::levels::Fatal;
68  } else {
69  ROS_WARN_STREAM("Invalid verbosity level specified: " << verbosity << "! Falling back to INFO.");
70  valid_verbosity = false;
71  }
72  if (valid_verbosity) {
75  ROS_DEBUG_STREAM("Verbosity set to " << verbosity);
76  }
77  }
78 }
79 
81 inline void showNodeInfo() {
82 
83  using namespace ros::this_node;
84 
85  std::vector<std::string> subscribed_topics, advertised_topics;
86  getSubscribedTopics(subscribed_topics);
87  getAdvertisedTopics(advertised_topics);
88 
89  std::ostringstream msg_subscr, msg_advert;
90  for (auto const& t : subscribed_topics) {
91  msg_subscr << t << "\n";
92  }
93  for (auto const& t : advertised_topics) {
94  msg_advert << t << "\n";
95  }
96 
97  ROS_INFO_STREAM("Started '" << getName() << "' in namespace '" << getNamespace() << "'.\n"
98  << "Subscribed topics:\n"
99  << msg_subscr.str() << "Advertised topics:\n"
100  << msg_advert.str());
101 }
102 
108 inline std::string getNodeName(const ros::NodeHandle& privateNodeHandle) {
109  std::string name_space = privateNodeHandle.getNamespace();
110  std::stringstream tempString(name_space);
111  std::string name;
112  while (std::getline(tempString, name, '/')) {
113  ;
114  }
115  return name;
116 }
117 
119 inline void exit(const std::string msg = "Runtime Error in rosparam handler.") {
120  // std::exit(EXIT_FAILURE);
121  throw std::runtime_error(msg);
122 }
123 
128 template <typename T>
129 inline void setParam(const std::string key, T val) {
130  ros::param::set(key, val);
131 }
132 
137 template <typename T>
138 inline bool getParam(const std::string key, T& val) {
139  if (!ros::param::has(key)) {
140  ROS_ERROR_STREAM("Parameter '" << key << "' is not defined.");
141  return false;
142  } else if (!ros::param::get(key, val)) {
143  ROS_ERROR_STREAM("Could not retrieve parameter'" << key << "'. Does it have a different type?");
144  return false;
145  } else {
146  return true;
147  }
148 }
149 
156 template <typename T>
157 inline bool getParam(const std::string key, T& val, const T& defaultValue) {
158  if (!ros::param::has(key) || !ros::param::get(key, val)) {
159  val = defaultValue;
160  ros::param::set(key, defaultValue);
161  ROS_INFO_STREAM("Setting default value for parameter '" << key << "'.");
162  return true;
163  } else {
164  // Param was already retrieved with last if statement.
165  return true;
166  }
167 }
168 
170 inline bool testConstParam(const std::string key) {
171  if (ros::param::has(key)) {
172  ROS_WARN_STREAM("Parameter " << key
173  << "' was set on the parameter server eventhough it was defined to be constant.");
174  return false;
175  } else {
176  return true;
177  }
178 }
179 
185 template <typename T>
186 inline void testMin(const std::string key, T& val, T min = std::numeric_limits<T>::min()) {
187  if (val < min) {
188  ROS_WARN_STREAM("Value of " << val << " for " << key
189  << " is smaller than minimal allowed value. Correcting value to min=" << min);
190  val = min;
191  }
192 }
193 
199 template <typename T>
200 inline void testMin(const std::string key, std::vector<T>& val, T min = std::numeric_limits<T>::min()) {
201  for (auto& v : val)
202  testMin(key, v, min);
203 }
204 
210 template <typename K, typename T>
211 inline void testMin(const std::string key, std::map<K, T>& val, T min = std::numeric_limits<T>::min()) {
212  for (auto& v : val)
213  testMin(key, v.second, min);
214 }
215 
221 template <typename T>
222 inline void testMax(const std::string key, T& val, T max = std::numeric_limits<T>::max()) {
223  if (val > max) {
224  ROS_WARN_STREAM("Value of " << val << " for " << key
225  << " is greater than maximal allowed. Correcting value to max=" << max);
226  val = max;
227  }
228 }
229 
235 template <typename T>
236 inline void testMax(const std::string key, std::vector<T>& val, T max = std::numeric_limits<T>::max()) {
237  for (auto& v : val)
238  testMax(key, v, max);
239 }
240 
246 template <typename K, typename T>
247 inline void testMax(const std::string key, std::map<K, T>& val, T max = std::numeric_limits<T>::max()) {
248  for (auto& v : val)
249  testMax(key, v.second, max);
250 }
251 
252 } // namespace rosparam_handler
void exit(const std::string msg="Runtime Error in rosparam handler.")
ExitFunction for rosparam_handler.
Definition: utilities.hpp:119
ROSCONSOLE_DECL void notifyLoggerLevelsChanged()
void testMin(const std::string key, T &val, T min=std::numeric_limits< T >::min())
Limit parameter to lower bound if parameter is a scalar.
Definition: utilities.hpp:186
void setParam(const std::string key, T val)
Set parameter on ROS parameter server.
Definition: utilities.hpp:129
std::string getNodeName(const ros::NodeHandle &privateNodeHandle)
Retrieve node name.
Definition: utilities.hpp:108
std::string to_string(const std::vector< T > &v)
Definition: utilities.hpp:21
XmlRpcServer s
std::is_same< T, std::vector< typename T::value_type, typename T::allocator_type >> is_vector
Helper function to test for std::vector.
Definition: utilities.hpp:11
std::string getName(void *handle)
ROSCPP_DECL void set(const std::string &key, const XmlRpc::XmlRpcValue &v)
bool testConstParam(const std::string key)
Tests that parameter is not set on the parameter server.
Definition: utilities.hpp:170
ROSCPP_DECL const std::string & getNamespace()
void showNodeInfo()
Show summary about node containing name, namespace, subscribed and advertised topics.
Definition: utilities.hpp:81
ROSCPP_DECL bool get(const std::string &key, std::string &s)
ROSCPP_DECL void getSubscribedTopics(V_string &topics)
const std::string & getNamespace() const
ROSCPP_DECL bool has(const std::string &key)
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
void testMax(const std::string key, T &val, T max=std::numeric_limits< T >::max())
Limit parameter to upper bound if parameter is a scalar.
Definition: utilities.hpp:222
ROSCPP_DECL void getAdvertisedTopics(V_string &topics)
std::is_same< T, std::map< typename T::key_type, typename T::mapped_type, typename T::key_compare, typename T::allocator_type >> is_map
Helper function to test for std::map.
Definition: utilities.hpp:16
bool getParam(const std::string key, T &val)
Get parameter from ROS parameter server.
Definition: utilities.hpp:138
ROSCONSOLE_DECL bool set_logger_level(const std::string &name, levels::Level level)
#define ROS_ERROR_STREAM(args)
#define ROSCONSOLE_DEFAULT_NAME
void setLoggerLevel(const ros::NodeHandle &nodeHandle)
Sets the logger level according to a standardized parameter name &#39;verbosity&#39;.
Definition: utilities.hpp:49


rosparam_handler
Author(s): Claudio Bandera
autogenerated on Wed Jun 5 2019 19:50:36