15 std::ostream&
operator<<(std::ostream& os,
const std::vector<T>& var)
17 for (
size_t it = 0; it < var.size(); it++)
20 if (it < var.size() - 1)
26 template <
typename Key,
typename Value>
27 std::ostream&
operator<<(std::ostream& os,
const std::map<Key, Value>& var)
30 for (
const auto& pair : var) {
31 os << pair.first <<
": " << pair.second;
32 if (it < var.size() - 1)
50 explicit operator std::string()
const
55 friend std::ostream& operator<<(std::ostream& os,
const resolved_name_t& r)
62 return lhs.str == rhs.str;
72 return rclcpp::ParameterValue{T{}}.get_type();
86 bool use_rosparam = m_use_rosparam;
92 if (opts.
use_yaml && loadFromYaml(resolved_name, value_out, opts))
96 if (use_rosparam && loadFromROS(resolved_name, value_out, opts))
102 const auto& default_value = opts.
declare_options.default_value.value();
106 RCLCPP_ERROR_STREAM(m_node->get_logger(),
"Failed to declare parameter \"" << resolved_name <<
"\".");
110 value_out = default_value;
115 std::stringstream ss;
116 ss <<
"Param '" << resolved_name <<
"' not found:";
118 ss <<
" in YAML files,";
121 ss <<
" no default value provided.";
122 RCLCPP_ERROR_STREAM(m_node->get_logger(), ss.str());
128 template <
typename T>
134 template <
typename T>
138 if (!m_node->has_parameter(resolved_name.str))
141 declare_opts_local.default_value = value;
142 const auto result = declareParam<T>(resolved_name, declare_opts_local);
144 RCLCPP_ERROR_STREAM(m_node->get_logger(),
"Could not declare and set param '" << resolved_name <<
"'!");
152 rcl_interfaces::msg::SetParametersResult res = m_node->set_parameter({resolved_name.str, value});
155 RCLCPP_ERROR_STREAM(m_node->get_logger(),
"Could not set param '" << resolved_name <<
"': " << res.reason);
160 catch (
const rclcpp::exceptions::ParameterNotDeclaredException& e)
162 RCLCPP_ERROR_STREAM(m_node->get_logger(),
"Could not set param '" << resolved_name <<
"': " << e.what());
170 template <
typename T>
173 return declareParam<T>(
resolveName(param_name), {});
176 template <
typename T>
179 return declareParam<T>(
resolveName(param_name), {.default_value = default_value});
182 template <
typename T>
187 rcl_interfaces::msg::ParameterDescriptor descriptor;
191 if (opts.
range.has_value())
193 const auto& opt_range = opts.
range.value();
198 RCLCPP_ERROR_STREAM(m_node->get_logger(),
"Error when declaring parameter \"" << resolved_name <<
"\": Range cannot be set for non-numerical values! Ignoring range.");
201 else if constexpr (std::integral<T>)
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);
209 else if constexpr (std::floating_point<T>)
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);
222 m_node->declare_parameter(resolved_name.str, opts.
default_value.value(), descriptor);
229 const rclcpp::ParameterType type = to_param_type<T>();
230 descriptor.type = type;
231 m_node->declare_parameter(resolved_name.str, type, descriptor);
234 catch (
const std::exception& e)
242 RCLCPP_ERROR_STREAM(m_node->get_logger(),
"Could not declare param '" << resolved_name <<
"': " << e.what());
251 template <
typename T>
252 bool ParamProvider::loadFromYaml(
const resolved_name_t& resolved_name, T& value_out,
const get_options_t<T>& opts)
const
256 const auto found_node = findYamlNode(resolved_name);
257 if (!found_node.has_value())
265 loaded_value = found_node.value().as<T>();
267 catch (
const YAML::BadConversion& e)
269 RCLCPP_ERROR_STREAM(m_node->get_logger(),
"[" << m_node_name <<
"]: The YAML-loaded parameter has a wrong type: " << e.what());
272 catch (
const YAML::Exception& e)
274 RCLCPP_ERROR_STREAM(m_node->get_logger(),
"[" << m_node_name <<
"]: YAML-CPP threw an unknown exception: " << e.what());
279 if (opts.always_declare)
281 auto declare_opts_local = opts.declare_options;
282 declare_opts_local.default_value = loaded_value;
285 if (!m_node->has_parameter(resolved_name.str) && !declareParam<T>(resolved_name, declare_opts_local))
287 RCLCPP_ERROR_STREAM(m_node->get_logger(),
"Failed to declare parameter \"" << resolved_name <<
"\".");
293 value_out = loaded_value;
300 template <
typename T>
301 bool ParamProvider::ranges_match(
const rcl_interfaces::msg::ParameterDescriptor& descriptor,
const range_t<T>& declare_range)
const
303 if constexpr (numeric<T>)
305 if (!descriptor.floating_point_range.empty())
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)
312 if (!descriptor.integer_range.empty())
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)
327 template <
typename T>
328 bool ParamProvider::loadFromROS(
const resolved_name_t& resolved_name, T& value_out,
const get_options_t<T>& opts)
const
330 std::optional<T> loaded_value;
331 const bool was_declared = m_node->has_parameter(resolved_name.str);
336 const auto descriptor = m_node->describe_parameter(resolved_name.str);
337 if (descriptor.read_only && opts.declare_options.reconfigurable)
339 RCLCPP_ERROR_STREAM(m_node->get_logger(),
"Parameter \"" << resolved_name <<
"\" already declared as read-only, cannot re-declare as reconfigurable!");
343 if (opts.declare_options.range.has_value() && !ranges_match(descriptor, opts.declare_options.range.value()))
345 RCLCPP_ERROR_STREAM(m_node->get_logger(),
"Parameter \"" << resolved_name <<
"\" already declared with a range, cannot re-declare with a different one!");
353 rcl_interfaces::msg::ParameterDescriptor descriptor;
354 descriptor.read_only =
false;
355 descriptor.dynamic_typing =
true;
358 m_node->declare_parameter(resolved_name.str, rclcpp::ParameterValue(), descriptor);
360 catch (
const std::exception& e)
363 RCLCPP_ERROR_STREAM(m_node->get_logger(),
"Failed to declare parameter \"" << resolved_name <<
"\": " << e.what());
372 rclcpp::Parameter param;
373 if (m_node->get_parameter(resolved_name.str, param))
374 loaded_value = param.get_value<T>();
377 catch (
const rclcpp::exceptions::InvalidParameterTypeException& e)
379 RCLCPP_ERROR_STREAM(m_node->get_logger(),
"Could not get param '" << resolved_name <<
"' from ROS: " << e.what());
386 m_node->undeclare_parameter(resolved_name.str);
389 if (!loaded_value.has_value())
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))
399 RCLCPP_ERROR_STREAM(m_node->get_logger(),
"Failed to declare parameter \"" << resolved_name <<
"\".");
405 value_out = std::move(loaded_value.value());
415 struct hash<
mrs_lib::ParamProvider::resolved_name_t>
419 return std::hash<std::string>{}(r.str);
resolved_name_t resolveName(const std::string ¶m_name) const
Returns the parameter name with prefixes and subnode namespaces.
Definition param_provider.cpp:148
bool getParam(const std::string ¶m_name, T &value_out) const
Gets the value of a parameter.
Definition param_provider.hpp:78
bool declareParam(const std::string ¶m_name) const
Defines a parameter.
Definition param_provider.hpp:171
bool setParam(const std::string ¶m_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