Skip to content

Commit 980f563

Browse files
jonbinneyjonbinney
authored
Merge pull request #83 from eurogroep/feat/reconfigure-parameters
Handle dynamic reconfiguration of parameters
2 parents 67b4ad6 + 383237c commit 980f563

File tree

2 files changed

+88
-12
lines changed

2 files changed

+88
-12
lines changed

‎include/filters/filter_base.hpp‎

Lines changed: 42 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -114,14 +114,34 @@ class FilterBase
114114
*/
115115
virtual bool update(const T & data_in, T & data_out) = 0;
116116

117+
/**
118+
* \brief reconfigureCB
119+
* can be overridden in the derived class
120+
* \param parameters A vector parameters to be reconfigured
121+
*/
122+
virtual rcl_interfaces::msg::SetParametersResult reconfigureCB(
123+
std::vector<rclcpp::Parameter> parameters)
124+
{
125+
auto result = rcl_interfaces::msg::SetParametersResult();
126+
result.successful = true;
127+
return result;
128+
}
129+
117130
/**
118131
* \brief Get the name of the filter as a string
119132
*/
120133
inline const std::string & getName() {return filter_name_;}
121134

135+
/**
136+
* \brief Get the parameter_prefix of the filter as a string
137+
*/
138+
inline const std::string & getParamPrefix() {return param_prefix_;}
139+
122140
private:
123141
template<typename PT>
124-
bool getParamImpl(const std::string & name, const uint8_t type, PT default_value, PT & value_out)
142+
bool getParamImpl(
143+
const std::string & name, const uint8_t type, PT default_value, PT & value_out,
144+
bool read_only)
125145
{
126146
std::string param_name = param_prefix_ + name;
127147

@@ -131,7 +151,7 @@ class FilterBase
131151
rcl_interfaces::msg::ParameterDescriptor desc;
132152
desc.name = name;
133153
desc.type = type;
134-
desc.read_only = true;
154+
desc.read_only = read_only;
135155

136156
if (name.empty()) {
137157
throw std::runtime_error("Parameter must have a name");
@@ -159,11 +179,11 @@ class FilterBase
159179
* \param default_value The default value to use if the parameter is not set
160180
* \return Whether or not the parameter of name/type was set */
161181
bool getParam(
162-
const std::string & name, std::string & value,
182+
const std::string & name, std::string & value, bool read_only = true,
163183
std::string default_value = std::string())
164184
{
165185
return getParamImpl(
166-
name, rcl_interfaces::msg::ParameterType::PARAMETER_STRING, default_value, value);
186+
name, rcl_interfaces::msg::ParameterType::PARAMETER_STRING, default_value, value, read_only);
167187
}
168188

169189
/**
@@ -172,11 +192,13 @@ class FilterBase
172192
* \param value The boolean to set with the value
173193
* \param default_value The default value to use if the parameter is not set
174194
* \return Whether or not the parameter of name/type was set */
175-
bool getParam(const std::string & name, bool & value, bool default_value = false)
195+
bool getParam(
196+
const std::string & name, bool & value,
197+
bool read_only = true, bool default_value = false)
176198
{
177199
return getParamImpl(
178200
name, rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, default_value,
179-
value);
201+
value, read_only);
180202
}
181203

182204
/**
@@ -185,11 +207,13 @@ class FilterBase
185207
* \param value The double to set with the value
186208
* \param default_value The default value to use if the parameter is not set
187209
* \return Whether or not the parameter of name/type was set */
188-
bool getParam(const std::string & name, double & value, double default_value = 0.0)
210+
bool getParam(
211+
const std::string & name, double & value,
212+
bool read_only = true, double default_value = 0.0)
189213
{
190214
return getParamImpl(
191215
name, rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, default_value,
192-
value);
216+
value, read_only);
193217
}
194218

195219
/**
@@ -198,11 +222,13 @@ class FilterBase
198222
* \param value The int to set with the value
199223
* \param default_value The default value to use if the parameter is not set
200224
* \return Whether or not the parameter of name/type was set */
201-
bool getParam(const std::string & name, int & value, int default_value = 0)
225+
bool getParam(
226+
const std::string & name, int & value,
227+
bool read_only = true, int default_value = 0)
202228
{
203229
return getParamImpl(
204230
name, rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, default_value,
205-
value);
231+
value, read_only);
206232
}
207233

208234
/**
@@ -249,10 +275,12 @@ class FilterBase
249275
* \return Whether or not the parameter of name/type was set */
250276
bool getParam(
251277
const std::string & name, std::vector<double> & value,
278+
bool read_only = true,
252279
std::vector<double> default_value = {})
253280
{
254281
return getParamImpl(
255-
name, rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, default_value, value);
282+
name, rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, default_value, value,
283+
read_only);
256284
}
257285

258286
/**
@@ -263,10 +291,12 @@ class FilterBase
263291
* \return Whether or not the parameter of name/type was set */
264292
bool getParam(
265293
const std::string & name, std::vector<std::string> & value,
294+
bool read_only = true,
266295
std::vector<std::string> default_value = {})
267296
{
268297
return getParamImpl(
269-
name, rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, default_value, value);
298+
name, rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, default_value, value,
299+
read_only);
270300
}
271301

272302
/// The name of the filter

‎include/filters/filter_chain.hpp‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -247,6 +247,8 @@ class FilterChain
247247
}
248248
logging_interface_ = node_logger;
249249
params_interface_ = node_params;
250+
on_set_parameters_callback_handle_ = params_interface_->add_on_set_parameters_callback(
251+
std::bind(&FilterChain<T>::reconfigureCB, this, std::placeholders::_1));
250252

251253
std::vector<struct impl::FoundFilter> found_filters;
252254
if (!impl::load_chain_config(
@@ -292,6 +294,25 @@ class FilterChain
292294
return true;
293295
}
294296

297+
rcl_interfaces::msg::SetParametersResult reconfigureCB(std::vector<rclcpp::Parameter> parameters)
298+
{
299+
auto result = rcl_interfaces::msg::SetParametersResult();
300+
result.successful = true;
301+
302+
for (auto & ref_ptr : reference_pointers_) {
303+
std::vector<rclcpp::Parameter> parameters_subset;
304+
for (auto parameter : parameters) {
305+
if (parameter.get_name().find(ref_ptr->getParamPrefix()) != std::string::npos) {
306+
parameters_subset.push_back(parameter);
307+
}
308+
}
309+
if (!parameters_subset.empty() && !ref_ptr->reconfigureCB(parameters_subset).successful) {
310+
result.successful = false;
311+
}
312+
}
313+
return result;
314+
}
315+
295316
private:
296317
pluginlib::ClassLoader<filters::FilterBase<T>> loader_;
297318

@@ -304,6 +325,8 @@ class FilterChain
304325

305326
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr params_interface_;
306327
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_;
328+
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr
329+
on_set_parameters_callback_handle_;
307330
};
308331

309332
/**
@@ -438,13 +461,34 @@ class MultiChannelFilterChain
438461

439462
// Everything went ok!
440463
reference_pointers_ = std::move(loaded_filters);
464+
on_set_parameters_callback_handle_ = params_interface_->add_on_set_parameters_callback(
465+
std::bind(&MultiChannelFilterChain<T>::reconfigureCB, this, std::placeholders::_1));
441466
// Allocate ahead of time
442467
buffer0_.resize(number_of_channels);
443468
buffer1_.resize(number_of_channels);
444469
configured_ = true;
445470
return true;
446471
}
447472

473+
rcl_interfaces::msg::SetParametersResult reconfigureCB(std::vector<rclcpp::Parameter> parameters)
474+
{
475+
auto result = rcl_interfaces::msg::SetParametersResult();
476+
result.successful = true;
477+
478+
for (auto & ref_ptr : reference_pointers_) {
479+
std::vector<rclcpp::Parameter> parameters_subset;
480+
for (auto parameter : parameters) {
481+
if (parameter.get_name().find(ref_ptr->getParamPrefix()) != std::string::npos) {
482+
parameters_subset.push_back(parameter);
483+
}
484+
}
485+
if (!parameters_subset.empty() && !ref_ptr->reconfigureCB(parameters_subset).successful) {
486+
result.successful = false;
487+
}
488+
}
489+
return result;
490+
}
491+
448492
private:
449493
pluginlib::ClassLoader<filters::MultiChannelFilterBase<T>> loader_;
450494

@@ -457,6 +501,8 @@ class MultiChannelFilterChain
457501

458502
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr params_interface_;
459503
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_;
504+
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr
505+
on_set_parameters_callback_handle_;
460506
};
461507

462508
} // namespace filters

0 commit comments

Comments
 (0)