mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
Loading...
Searching...
No Matches
param_provider.hpp
Go to the documentation of this file.
1// clang: MatousFormat
7#pragma once
8
10
11namespace mrs_lib
12{
13
14 template <typename T>
15 std::ostream& operator<<(std::ostream& os, const std::vector<T>& var)
16 {
17 for (size_t it = 0; it < var.size(); it++)
18 {
19 os << var.at(it);
20 if (it < var.size() - 1)
21 os << ", ";
22 }
23 return os;
24 }
25
26 template <typename Key, typename Value>
27 std::ostream& operator<<(std::ostream& os, const std::map<Key, Value>& var)
28 {
29 size_t it = 0;
30 for (const auto& pair : var) {
31 os << pair.first << ": " << pair.second;
32 if (it < var.size() - 1)
33 os << std::endl;
34 it++;
35 }
36 return os;
37 }
38
39 /* ParamProvider::resolved_name_t //{ */
40
42 {
43 std::string str;
44
45 resolved_name_t() = default;
46 resolved_name_t(std::string&& s) : str(s) {}
47 resolved_name_t(const std::string& s) : str(s) {}
48
49 // Explicit conversion
50 explicit operator std::string() const
51 {
52 return str;
53 }
54
55 friend std::ostream& operator<<(std::ostream& os, const resolved_name_t& r)
56 {
57 return os << r.str;
58 }
59
60 friend bool operator==(const resolved_name_t& lhs, const resolved_name_t& rhs)
61 {
62 return lhs.str == rhs.str;
63 }
64 };
65
66 //}
67
68 /* to_param_type() method //{ */
69 template <typename T>
70 rclcpp::ParameterType to_param_type()
71 {
72 return rclcpp::ParameterValue{T{}}.get_type();
73 }
74 //}
75
76 /* getParam() method and overloads //{ */
77 template <typename T>
78 bool ParamProvider::getParam(const std::string& param_name, T& value_out) const
79 {
80 return getParam(resolveName(param_name), value_out, {});
81 }
82
83 template <typename T>
84 bool ParamProvider::getParam(const resolved_name_t& resolved_name, T& value_out, const get_options_t<T>& opts) const
85 {
86 bool use_rosparam = m_use_rosparam;
87 // the options structure always has precedence if the parameter is set
88 if (opts.use_rosparam.has_value())
89 use_rosparam = opts.use_rosparam.value();
90
91 // first, try to load from YAML, if enabled
92 if (opts.use_yaml && loadFromYaml(resolved_name, value_out, opts))
93 return true;
94
95 // then, try to load from ROS, if enabled
96 if (use_rosparam && loadFromROS(resolved_name, value_out, opts))
97 return true;
98
99 // if both fail, check for a default value
100 if (opts.declare_options.default_value.has_value())
101 {
102 const auto& default_value = opts.declare_options.default_value.value();
103
104 if (opts.always_declare && !declareParam<T>(resolved_name, opts.declare_options))
105 {
106 RCLCPP_ERROR_STREAM(m_node->get_logger(), "Failed to declare parameter \"" << resolved_name << "\".");
107 return false;
108 }
109
110 value_out = default_value;
111 return true;
112 }
113
114 // if all options fail, return false
115 std::stringstream ss;
116 ss << "Param '" << resolved_name << "' not found:";
117 if (opts.use_yaml)
118 ss << " in YAML files,";
119 if (use_rosparam)
120 ss << " in ROS,";
121 ss << " no default value provided.";
122 RCLCPP_ERROR_STREAM(m_node->get_logger(), ss.str());
123 return false;
124 }
125 //}
126
127 /* setParam() method //{ */
128 template <typename T>
129 bool ParamProvider::setParam(const std::string& param_name, const T& value) const
130 {
131 return setParam(resolveName(param_name), value);
132 }
133
134 template <typename T>
135 bool ParamProvider::setParam(const resolved_name_t& resolved_name, const T& value, const set_options_t<T>& opts) const
136 {
137 // if the parameter is not yet declared, declare and set it together
138 if (!m_node->has_parameter(resolved_name.str))
139 {
140 auto declare_opts_local = opts.declare_options;
141 declare_opts_local.default_value = value;
142 const auto result = declareParam<T>(resolved_name, declare_opts_local);
143 if (!result)
144 RCLCPP_ERROR_STREAM(m_node->get_logger(), "Could not declare and set param '" << resolved_name << "'!");
145 return result;
146 }
147
148 // otherwise, try setting it
149 try
150 {
151 /* RCLCPP_INFO_STREAM(m_node->get_logger(), "Setting param '" << resolved_name << "'"); */
152 rcl_interfaces::msg::SetParametersResult res = m_node->set_parameter({resolved_name.str, value});
153 if (!res.successful)
154 {
155 RCLCPP_ERROR_STREAM(m_node->get_logger(), "Could not set param '" << resolved_name << "': " << res.reason);
156 return false;
157 }
158 }
159 // if the parameter has a wrong value, return failure
160 catch (const rclcpp::exceptions::ParameterNotDeclaredException& e)
161 {
162 RCLCPP_ERROR_STREAM(m_node->get_logger(), "Could not set param '" << resolved_name << "': " << e.what());
163 return false;
164 }
165 return true;
166 }
167 //}
168
169 /* declareParam() method and overloads //{ */
170 template <typename T>
171 bool ParamProvider::declareParam(const std::string& param_name) const
172 {
173 return declareParam<T>(resolveName(param_name), {});
174 }
175
176 template <typename T>
177 bool ParamProvider::declareParam(const std::string& param_name, const T& default_value) const
178 {
179 return declareParam<T>(resolveName(param_name), {.default_value = default_value});
180 }
181
182 template <typename T>
183 bool ParamProvider::declareParam(const resolved_name_t& resolved_name, const declare_options_t<T>& opts) const
184 {
185 try
186 {
187 rcl_interfaces::msg::ParameterDescriptor descriptor;
188 descriptor.read_only = !opts.reconfigurable;
189
190 // if the parameter range is specified, set it if applicable
191 if (opts.range.has_value())
192 {
193 const auto& opt_range = opts.range.value();
194 // set the range approprately according to the parameter type
195 if constexpr (!numeric<T>)
196 {
197 // if the type is not numerical, print an error to let the user know and fail
198 RCLCPP_ERROR_STREAM(m_node->get_logger(), "Error when declaring parameter \"" << resolved_name << "\": Range cannot be set for non-numerical values! Ignoring range.");
199 return false;
200 }
201 else if constexpr (std::integral<T>)
202 {
203 // integer range for integral types
204 rcl_interfaces::msg::IntegerRange range;
205 range.from_value = opt_range.minimum;
206 range.to_value = opt_range.maximum;
207 descriptor.integer_range.push_back(range);
208 }
209 else if constexpr (std::floating_point<T>)
210 {
211 // floating-point range for floating-point types
212 rcl_interfaces::msg::FloatingPointRange range;
213 range.from_value = opt_range.minimum;
214 range.to_value = opt_range.maximum;
215 descriptor.floating_point_range.push_back(range);
216 }
217 }
218
219 // actually declare the parameter
220 if (opts.default_value.has_value())
221 {
222 m_node->declare_parameter(resolved_name.str, opts.default_value.value(), descriptor);
223 }
224 else
225 {
226 // a hack to allow declaring a strong-typed parameter without a default value
227 // because ROS2 apparently fell on its head when it was a litle baby
228 // this is actually not documented in the API documentation, so see https://github.com/ros2/rclcpp/issues/1691
229 const rclcpp::ParameterType type = to_param_type<T>();
230 descriptor.type = type;
231 m_node->declare_parameter(resolved_name.str, type, descriptor);
232 }
233 }
234 catch (const std::exception& e)
235 {
236 // this can happen if (see
237 // http://docs.ros.org/en/humble/p/rclcpp/generated/classrclcpp_1_1Node.html#_CPPv4N6rclcpp4Node17declare_parameterERKNSt6stringERKN6rclcpp14ParameterValueERKN14rcl_interfaces3msg19ParameterDescriptorEb):
238 // * parameter has already been declared (rclcpp::exceptions::ParameterAlreadyDeclaredException)
239 // * parameter name is invalid (rclcpp::exceptions::InvalidParametersException)
240 // * initial value fails to be set (rclcpp::exceptions::InvalidParameterValueException, not sure what exactly this means)
241 // * type of the default value or override is wrong (rclcpp::exceptions::InvalidParameterTypeException, the most common one)
242 RCLCPP_ERROR_STREAM(m_node->get_logger(), "Could not declare param '" << resolved_name << "': " << e.what());
243
244 return false;
245 }
246 return true;
247 }
248 //}
249
250 /* loadFromYaml() method //{ */
251 template <typename T>
252 bool ParamProvider::loadFromYaml(const resolved_name_t& resolved_name, T& value_out, const get_options_t<T>& opts) const
253 {
254 T loaded_value;
255
256 const auto found_node = findYamlNode(resolved_name);
257 if (!found_node.has_value())
258 {
259 return false;
260 }
261
262 try
263 {
264 // try catch is the only type-generic option...
265 loaded_value = found_node.value().as<T>();
266 }
267 catch (const YAML::BadConversion& e)
268 {
269 RCLCPP_ERROR_STREAM(m_node->get_logger(), "[" << m_node_name << "]: The YAML-loaded parameter has a wrong type: " << e.what());
270 return false;
271 }
272 catch (const YAML::Exception& e)
273 {
274 RCLCPP_ERROR_STREAM(m_node->get_logger(), "[" << m_node_name << "]: YAML-CPP threw an unknown exception: " << e.what());
275 return false;
276 }
277
278 // declare the parameter if the options specify to always define it
279 if (opts.always_declare)
280 {
281 auto declare_opts_local = opts.declare_options;
282 declare_opts_local.default_value = loaded_value;
283
284 // see https://docs.ros.org/en/jazzy/Concepts/Basic/About-Parameters.html#parameters
285 if (!m_node->has_parameter(resolved_name.str) && !declareParam<T>(resolved_name, declare_opts_local))
286 {
287 RCLCPP_ERROR_STREAM(m_node->get_logger(), "Failed to declare parameter \"" << resolved_name << "\".");
288 return false;
289 }
290 }
291
292 // if all is OK, set the output value
293 value_out = loaded_value;
294 // the parameter value was successfully loaded and the parameter was declared if required, everything is done, return true
295 return true;
296 }
297 //}
298
299 /* ranges_match() method //{ */
300 template <typename T>
301 bool ParamProvider::ranges_match(const rcl_interfaces::msg::ParameterDescriptor& descriptor, const range_t<T>& declare_range) const
302 {
303 if constexpr (numeric<T>)
304 {
305 if (!descriptor.floating_point_range.empty())
306 {
307 const auto& desc_range = descriptor.floating_point_range.front();
308 if (desc_range.from_value != declare_range.minimum || desc_range.to_value != declare_range.maximum)
309 return false;
310 }
311
312 if (!descriptor.integer_range.empty())
313 {
314 const auto& desc_range = descriptor.integer_range.front();
315 if (desc_range.from_value != declare_range.minimum || desc_range.to_value != declare_range.maximum)
316 return false;
317 }
318 }
319
320 // trying to specify a range for a non-numeric type...
321 return false;
322 }
323
324//}
325
326 /* loadFromROS() method //{ */
327 template <typename T>
328 bool ParamProvider::loadFromROS(const resolved_name_t& resolved_name, T& value_out, const get_options_t<T>& opts) const
329 {
330 std::optional<T> loaded_value;
331 const bool was_declared = m_node->has_parameter(resolved_name.str);
332
333 // check if the current declaration is compatible with the desired declaration
334 if (was_declared)
335 {
336 const auto descriptor = m_node->describe_parameter(resolved_name.str);
337 if (descriptor.read_only && opts.declare_options.reconfigurable)
338 {
339 RCLCPP_ERROR_STREAM(m_node->get_logger(), "Parameter \"" << resolved_name << "\" already declared as read-only, cannot re-declare as reconfigurable!");
340 return false;
341 }
342
343 if (opts.declare_options.range.has_value() && !ranges_match(descriptor, opts.declare_options.range.value()))
344 {
345 RCLCPP_ERROR_STREAM(m_node->get_logger(), "Parameter \"" << resolved_name << "\" already declared with a range, cannot re-declare with a different one!");
346 return false;
347 }
348 }
349
350 // if the parameter is not declared yet, check if it is available in ROS by declaring it as dynamically-typed and read-writable
351 if (!was_declared)
352 {
353 rcl_interfaces::msg::ParameterDescriptor descriptor;
354 descriptor.read_only = false;
355 descriptor.dynamic_typing = true;
356 try
357 {
358 m_node->declare_parameter(resolved_name.str, rclcpp::ParameterValue(), descriptor);
359 }
360 catch (const std::exception& e)
361 {
362 // if the declaration already fails, something is wrong
363 RCLCPP_ERROR_STREAM(m_node->get_logger(), "Failed to declare parameter \"" << resolved_name << "\": " << e.what());
364 return false;
365 }
366 }
367
368 // now try loading the parameter
369 try
370 {
371 /* RCLCPP_INFO_STREAM(m_node->get_logger(), "Getting param '" << resolved_name << "'"); */
372 rclcpp::Parameter param;
373 if (m_node->get_parameter(resolved_name.str, param))
374 loaded_value = param.get_value<T>();
375 }
376 // if the parameter has a wrong value, return failure
377 catch (const rclcpp::exceptions::InvalidParameterTypeException& e)
378 {
379 RCLCPP_ERROR_STREAM(m_node->get_logger(), "Could not get param '" << resolved_name << "' from ROS: " << e.what());
380 }
381
382 // if the parameter was not declared before, undecalre it:
383 // 1. either it was loaded successfully and has to be re-declared with the correct options
384 // 2. or it was not loaded and has to be un-declared
385 if (!was_declared)
386 m_node->undeclare_parameter(resolved_name.str);
387
388 // loading from ROS was not successful, just return false
389 if (!loaded_value.has_value())
390 return false;
391
392 // if the parameter was not declared before, redeclare it with the correct parameters
393 if (!was_declared)
394 {
395 auto declare_opts_local = opts.declare_options;
396 declare_opts_local.default_value = value_out;
397 if (!declareParam<T>(resolved_name, declare_opts_local))
398 {
399 RCLCPP_ERROR_STREAM(m_node->get_logger(), "Failed to declare parameter \"" << resolved_name << "\".");
400 return false;
401 }
402 }
403
404 // if all went good, set the value and return true
405 value_out = std::move(loaded_value.value());
406 return true;
407 }
408 //}
409
410} // namespace mrs_lib
411
412namespace std
413{
414 template <>
415 struct hash<mrs_lib::ParamProvider::resolved_name_t>
416 {
417 std::size_t operator()(const mrs_lib::ParamProvider::resolved_name_t& r) const noexcept
418 {
419 return std::hash<std::string>{}(r.str);
420 }
421 };
422}
resolved_name_t resolveName(const std::string &param_name) const
Returns the parameter name with prefixes and subnode namespaces.
Definition param_provider.cpp:148
bool getParam(const std::string &param_name, T &value_out) const
Gets the value of a parameter.
Definition param_provider.hpp:78
bool declareParam(const std::string &param_name) const
Defines a parameter.
Definition param_provider.hpp:171
bool setParam(const std::string &param_name, const T &value) const
Sets the value of a parameter to ROS.
Definition param_provider.hpp:129
Convenience concept of a numeric value (i.e. either integral or floating point, and not bool).
Definition param_provider.h:58
All mrs_lib functions, classes, variables and definitions are contained in this namespace.
Definition attitude_converter.h:24
std::ostream & operator<<(std::ostream &os, const Eigen::MatrixX< T > &var)
Helper overload for printing of Eigen matrices.
Definition param_loader.hpp:21
rclcpp::ParameterType to_param_type()
Convenince function to get the corresponding rclcpp::ParamType from a C++ type.
Definition param_provider.hpp:70
Defines ParamProvider - a convenience class for seamlessly loading parameters from YAML or ROS.
Struct of options when declaring a parameter to ROS.
Definition param_provider.h:102
bool reconfigurable
If true, the parameter will be dynamically reconfigurable, otherwise it will be declared as read-only...
Definition param_provider.h:104
std::optional< T > default_value
An optional default value to initialize the parameter with.
Definition param_provider.h:106
std::optional< range_t< T > > range
An optional range of valid values of the parameter (only for numerical types).
Definition param_provider.h:108
Struct of options when getting a parameter from ROS.
Definition param_provider.h:118
declare_options_t< T > declare_options
Options when declaring a parameter to ROS (see the declare_options_t<T> documentation).
Definition param_provider.h:128
std::optional< bool > use_rosparam
Specifies whether the parameter should be attempted to be loaded from ROS if it cannot be loaded from...
Definition param_provider.h:124
bool always_declare
Iff true, the parameter will be declared to ROS even if it's value was loaded from a YAML.
Definition param_provider.h:120
bool use_yaml
Iff false, loading from YAML will be skipped even if some YAML files were specified.
Definition param_provider.h:122
Helper struct for a numeric range with named members to make the code a bit more readable.
Definition param_provider.h:87
T maximum
Maximal value of a parameter.
Definition param_provider.h:91
T minimum
Minimal value of a parameter.
Definition param_provider.h:89
Definition param_provider.hpp:42
Struct of options when setting a parameter to ROS.
Definition param_provider.h:138
declare_options_t< T > declare_options
Options when declaring a parameter to ROS (see the declare_options_t<T> documentation).
Definition param_provider.h:142