niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Allow registering multiple on_parameters_set_callback (#772)

Browse source 
Signed-off-by: ivanpauno <ivanpauno@ekumenlabs.com>
This commit is contained in:
ivanpauno 2019-07-24 14:01:23 -03:00 • committed by GitHub
parent b214324bf2
commit 94ea26bffb
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
6 changed files with 288 additions and 18 deletions
Download patch file Download diff file Expand all files Collapse all files

63
rclcpp/include/rclcpp/node.hpp
View file

@ -889,10 +889,12 @@ public:
rcl_interfaces::msg::ListParametersResult
list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const;
using OnSetParametersCallbackHandle =
rclcpp::node_interfaces::OnSetParametersCallbackHandle;
using OnParametersSetCallbackType =
rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType;
/// Register a callback to be called anytime a parameter is about to be changed.
/// Add a callback for when parameters are being set.
/**
* The callback signature is designed to allow handling of any of the above
* `set_parameter*` or `declare_parameter*` methods, and so it takes a const
@ -936,9 +938,60 @@ public:
* of the latter things,
* rclcpp::exceptions::ParameterModifiedInCallbackException will be thrown.
*
* There may only be one callback set at a time, so the previously set
* callback is returned when this method is used, or nullptr will be returned
* if no callback was previously set.
* The callback functions must remain valid as long as the
* returned smart pointer is valid.
* The returned smart pointer can be promoted to a shared version.
*
* Resetting or letting the smart pointer go out of scope unregisters the callback.
* `remove_on_set_parameters_callback` can also be used.
*
* The registered callbacks are called when a parameter is set.
* When a callback returns a not successful result, the remaining callbacks aren't called.
* The order of the callback is the reverse from the registration order.
*
* \param callback The callback to register.
* \returns A shared pointer. The callback is valid as long as the smart pointer is alive.
* \throws std::bad_alloc if the allocation of the OnSetParametersCallbackHandle fails.
*/
RCLCPP_PUBLIC
OnSetParametersCallbackHandle::SharedPtr
add_on_set_parameters_callback(OnParametersSetCallbackType callback);
/// Remove a callback registered with `add_on_set_parameters_callback`.
/**
* Delete a handler returned by `add_on_set_parameters_callback`.
*
* e.g.:
*
* `remove_on_set_parameters_callback(scoped_callback.get())`
*
* As an alternative, the smart pointer can be reset:
*
* `scoped_callback.reset()`
*
* Supposing that `scoped_callback` was the only owner.
*
* Calling `remove_on_set_parameters_callback` more than once with the same handler,
* or calling it after the shared pointer has been reset is an error.
* Resetting or letting the smart pointer go out of scope after calling
* `remove_on_set_parameters_callback` is not a problem.
*
* \param handler The callback handler to remove.
* \throws std::runtime_error if the handler was not created with `add_on_set_parameters_callback`,
* or if it has been removed before.
*/
RCLCPP_PUBLIC
void
remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler);
/// Register a callback to be called anytime a parameter is about to be changed.
/**
* With this method, only one callback can be set at a time. The callback that was previously
* set by this method is returned or `nullptr` if no callback was previously set.
*
* The callbacks added with `add_on_set_parameters_callback` are stored in a different place.
* `remove_on_set_parameters_callback` can't be used with the callbacks registered with this
* method. For removing it, use `set_on_parameters_set_callback(nullptr)`.
*
* \param[in] callback The callback to be called when the value for a
* parameter is about to be set.
@ -946,7 +999,7 @@ public:
* otherwise nullptr.
*/
RCLCPP_PUBLIC
rclcpp::Node::OnParametersSetCallbackType
OnParametersSetCallbackType
set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback);
/// Register the callback for parameter changes

13
rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp
View file

@ -17,6 +17,7 @@
#include <map>
#include <memory>
#include <list>
#include <string>
#include <vector>
@ -157,6 +158,14 @@ public:
rcl_interfaces::msg::ListParametersResult
list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const override;
RCLCPP_PUBLIC
OnSetParametersCallbackHandle::SharedPtr
add_on_set_parameters_callback(OnParametersSetCallbackType callback) override;
RCLCPP_PUBLIC
void
remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) override;
RCLCPP_PUBLIC
OnParametersSetCallbackType
set_on_parameters_set_callback(OnParametersSetCallbackType callback) override;
@ -170,6 +179,8 @@ public:
const std::map<std::string, rclcpp::ParameterValue> &
get_parameter_overrides() const override;
using CallbacksContainerType = std::list<OnSetParametersCallbackHandle::WeakPtr>;
private:
RCLCPP_DISABLE_COPY(NodeParameters)
@ -182,6 +193,8 @@ private:
OnParametersSetCallbackType on_parameters_set_callback_ = nullptr;
CallbacksContainerType on_parameters_set_callback_container_;
std::map<std::string, ParameterInfo> parameters_;
std::map<std::string, rclcpp::ParameterValue> parameter_overrides_;

36
rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp
View file

@ -16,6 +16,7 @@
#define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
#include <map>
#include <memory>
#include <string>
#include <vector>
@ -32,6 +33,18 @@ namespace rclcpp
namespace node_interfaces
{
struct OnSetParametersCallbackHandle
{
RCLCPP_SMART_PTR_DEFINITIONS(OnSetParametersCallbackHandle)
using OnParametersSetCallbackType =
std::function<
rcl_interfaces::msg::SetParametersResult(
const std::vector<rclcpp::Parameter> &)>;
OnParametersSetCallbackType callback;
};
/// Pure virtual interface class for the NodeParameters part of the Node API.
class NodeParametersInterface
{
@ -158,14 +171,29 @@ public:
rcl_interfaces::msg::ListParametersResult
list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const = 0;
using OnParametersSetCallbackType =
std::function<
rcl_interfaces::msg::SetParametersResult(const std::vector<rclcpp::Parameter> &)
>;
using OnParametersSetCallbackType = OnSetParametersCallbackHandle::OnParametersSetCallbackType;
using ParametersCallbackFunction [[deprecated("use OnParametersSetCallbackType instead")]] =
OnParametersSetCallbackType;
/// Add a callback for when parameters are being set.
/**
* \sa rclcpp::Node::add_on_set_parameters_callback
*/
RCLCPP_PUBLIC
virtual
OnSetParametersCallbackHandle::SharedPtr
add_on_set_parameters_callback(OnParametersSetCallbackType callback) = 0;
/// Remove a callback registered with `add_on_set_parameters_callback`.
/**
* \sa rclcpp::Node::remove_on_set_parameters_callback
*/
RCLCPP_PUBLIC
virtual
void
remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) = 0;
/// Register a callback for when parameters are being set, return an existing one.
/**
* \sa rclcpp::Node::set_on_parameters_set_callback

12
rclcpp/src/rclcpp/node.cpp
View file

@ -326,6 +326,18 @@ Node::list_parameters(const std::vector<std::string> & prefixes, uint64_t depth)
return node_parameters_->list_parameters(prefixes, depth);
}
rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr
Node::add_on_set_parameters_callback(OnParametersSetCallbackType callback)
{
return node_parameters_->add_on_set_parameters_callback(callback);
}
void
Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const callback)
{
return node_parameters_->remove_on_set_parameters_callback(callback);
}
rclcpp::Node::OnParametersSetCallbackType
Node::set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback)
{

105
rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp
View file

@ -28,6 +28,7 @@
#include <cmath>
#include <cstdlib>
#include <functional>
#include <limits>
#include <map>
#include <memory>
@ -289,20 +290,50 @@ __check_parameters(
using OnParametersSetCallbackType =
rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType;
using CallbacksContainerType =
rclcpp::node_interfaces::NodeParameters::CallbacksContainerType;
using OnSetParametersCallbackHandle =
rclcpp::node_interfaces::OnSetParametersCallbackHandle;
RCLCPP_LOCAL
rcl_interfaces::msg::SetParametersResult
__call_on_parameters_set_callbacks(
const std::vector<rclcpp::Parameter> & parameters,
CallbacksContainerType & callback_container,
const OnParametersSetCallbackType & callback)
{
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
auto it = callback_container.begin();
while (it != callback_container.end()) {
auto shared_handle = it->lock();
if (nullptr != shared_handle) {
result = shared_handle->callback(parameters);
if (!result.successful) {
return result;
}
it++;
} else {
it = callback_container.erase(it);
}
}
if (callback) {
result = callback(parameters);
}
return result;
}
RCLCPP_LOCAL
rcl_interfaces::msg::SetParametersResult
__set_parameters_atomically_common(
const std::vector<rclcpp::Parameter> & parameters,
std::map<std::string, rclcpp::node_interfaces::ParameterInfo> & parameter_infos,
OnParametersSetCallbackType on_set_parameters_callback)
CallbacksContainerType & callback_container,
const OnParametersSetCallbackType & callback)
{
// Call the user callback to see if the new value(s) are allowed.
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
if (on_set_parameters_callback) {
result = on_set_parameters_callback(parameters);
}
rcl_interfaces::msg::SetParametersResult result =
__call_on_parameters_set_callbacks(parameters, callback_container, callback);
if (!result.successful) {
return result;
}
@ -333,7 +364,8 @@ __declare_parameter_common(
const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
std::map<std::string, rclcpp::node_interfaces::ParameterInfo> & parameters_out,
const std::map<std::string, rclcpp::ParameterValue> & overrides,
OnParametersSetCallbackType on_set_parameters_callback,
CallbacksContainerType & callback_container,
const OnParametersSetCallbackType & callback,
rcl_interfaces::msg::ParameterEvent * parameter_event_out,
bool ignore_override = false)
{
@ -354,7 +386,8 @@ __declare_parameter_common(
auto result = __set_parameters_atomically_common(
parameter_wrappers,
parameter_infos,
on_set_parameters_callback);
callback_container,
callback);
// Add declared parameters to storage.
parameters_out[name] = parameter_infos.at(name);
@ -396,6 +429,7 @@ NodeParameters::declare_parameter(
parameter_descriptor,
parameters_,
parameter_overrides_,
on_parameters_set_callback_container_,
on_parameters_set_callback_,
&parameter_event,
ignore_override);
@ -523,6 +557,7 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
std::map<std::string, rclcpp::node_interfaces::ParameterInfo> staged_parameter_changes;
rcl_interfaces::msg::ParameterEvent parameter_event_msg;
parameter_event_msg.node = combined_name_;
CallbacksContainerType empty_callback_container;
for (auto parameter_to_be_declared : parameters_to_be_declared) {
// This should not throw, because we validated the name and checked that
// the parameter was not already declared.
@ -532,7 +567,9 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
rcl_interfaces::msg::ParameterDescriptor(), // Implicit declare uses default descriptor.
staged_parameter_changes,
parameter_overrides_,
nullptr, // callback is explicitly null, so that it is called only once, when setting below.
// Only call callbacks once below
empty_callback_container, // callback_container is explicitly empty
nullptr, // callback is explicitly null.
&parameter_event_msg,
true);
if (!result.successful) {
@ -588,6 +625,9 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
// they are actually set on the official parameter storage
parameters_,
// this will get called once, with all the parameters to be set
on_parameters_set_callback_container_,
// These callbacks are called once. When a callback returns an unsuccessful result,
// the remaining aren't called.
on_parameters_set_callback_);
// If not successful, then stop here.
@ -820,6 +860,53 @@ NodeParameters::list_parameters(const std::vector<std::string> & prefixes, uint6
return result;
}
struct HandleCompare
: public std::unary_function<OnSetParametersCallbackHandle::WeakPtr, bool>
{
explicit HandleCompare(const OnSetParametersCallbackHandle * const base)
: base_(base) {}
bool operator()(const OnSetParametersCallbackHandle::WeakPtr & handle)
{
auto shared_handle = handle.lock();
if (base_ == shared_handle.get()) {
return true;
}
return false;
}
const OnSetParametersCallbackHandle * const base_;
};
void
NodeParameters::remove_on_set_parameters_callback(
const OnSetParametersCallbackHandle * const handle)
{
std::lock_guard<std::recursive_mutex> lock(mutex_);
ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
auto it = std::find_if(
on_parameters_set_callback_container_.begin(),
on_parameters_set_callback_container_.end(),
HandleCompare(handle));
if (it != on_parameters_set_callback_container_.end()) {
on_parameters_set_callback_container_.erase(it);
} else {
throw std::runtime_error("Callback doesn't exist");
}
}
OnSetParametersCallbackHandle::SharedPtr
NodeParameters::add_on_set_parameters_callback(OnParametersSetCallbackType callback)
{
std::lock_guard<std::recursive_mutex> lock(mutex_);
ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
auto handle = std::make_shared<OnSetParametersCallbackHandle>();
handle->callback = callback;
// the last callback registered is executed first.
on_parameters_set_callback_container_.emplace_front(handle);
return handle;
}
NodeParameters::OnParametersSetCallbackType
NodeParameters::set_on_parameters_set_callback(OnParametersSetCallbackType callback)
{

77
rclcpp/test/test_node.cpp
View file

@ -362,6 +362,83 @@ TEST_F(TestNode, declare_parameter_with_no_initial_values) {
}
}
auto get_fixed_on_parameter_set_callback(const std::string & name, bool successful)
{
return
[name, successful](const std::vector<rclcpp::Parameter> & parameters) {
(void)parameters;
rcl_interfaces::msg::SetParametersResult result;
result.successful = successful;
return result;
};
}
TEST_F(TestNode, test_registering_multiple_callbacks_api) {
auto node = std::make_shared<rclcpp::Node>("test_declare_parameter_node"_unq);
{
int64_t default_value{42};
auto name1 = "parameter"_unq;
auto scoped_callback1 = node->add_on_set_parameters_callback(
get_fixed_on_parameter_set_callback(name1, true));
EXPECT_NE(scoped_callback1, nullptr);
int64_t value{node->declare_parameter(name1, default_value)};
EXPECT_EQ(value, default_value);
auto name2 = "parameter"_unq;
auto scoped_callback2 = node->add_on_set_parameters_callback(
get_fixed_on_parameter_set_callback(name2, false));
EXPECT_NE(scoped_callback2, nullptr);
EXPECT_THROW(
{node->declare_parameter(name2, default_value);},
rclcpp::exceptions::InvalidParameterValueException);
auto name3 = "parameter"_unq;
scoped_callback2.reset();
value = node->declare_parameter(name3, default_value);
EXPECT_EQ(value, default_value);
}
{
int64_t default_value{42};
auto name1 = "parameter"_unq;
auto scoped_callback1 = node->add_on_set_parameters_callback(
get_fixed_on_parameter_set_callback(name1, true));
EXPECT_NE(scoped_callback1, nullptr);
int64_t value{node->declare_parameter(name1, default_value)};
EXPECT_EQ(value, default_value);
auto name2 = "parameter"_unq;
auto scoped_callback2 = node->add_on_set_parameters_callback(
get_fixed_on_parameter_set_callback(name2, false));
EXPECT_NE(scoped_callback2, nullptr);
EXPECT_THROW(
{node->declare_parameter(name2, default_value);},
rclcpp::exceptions::InvalidParameterValueException);
auto name3 = "parameter"_unq;
node->remove_on_set_parameters_callback(scoped_callback2.get());
value = node->declare_parameter(name3, default_value);
EXPECT_EQ(value, default_value);
}
{
int64_t default_value{42};
auto name1 = "parameter"_unq;
rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr scoped_callback(
node->add_on_set_parameters_callback(
get_fixed_on_parameter_set_callback(name1, false)));
rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr scoped_callback_copy(scoped_callback);
scoped_callback.reset();
EXPECT_THROW(
{node->declare_parameter("parameter"_unq, default_value);},
rclcpp::exceptions::InvalidParameterValueException);
scoped_callback_copy.reset();
// All the shared_ptr has been reset
int64_t value = node->declare_parameter("parameter"_unq, default_value);
EXPECT_EQ(value, default_value);
}
}
TEST_F(TestNode, declare_parameter_with_overrides) {
// test cases with overrides
rclcpp::NodeOptions no;