Skip to content

Commit

Permalink
Allow registering multiple on_parameters_set_callback (ros2#772)
Browse files Browse the repository at this point in the history
Signed-off-by: ivanpauno <ivanpauno@ekumenlabs.com>
  • Loading branch information
ivanpauno authored and Joshua Hampp committed Aug 2, 2019
1 parent 05d8b51 commit 41f9f0f
Show file tree
Hide file tree
Showing 6 changed files with 288 additions and 18 deletions.
63 changes: 58 additions & 5 deletions rclcpp/include/rclcpp/node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -889,10 +889,12 @@ class Node : public std::enable_shared_from_this<Node>
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
Expand Down Expand Up @@ -936,17 +938,68 @@ class Node : public std::enable_shared_from_this<Node>
* 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.
* \return The previous callback that was registered, if there was one,
* otherwise nullptr.
*/
RCLCPP_PUBLIC
rclcpp::Node::OnParametersSetCallbackType
OnParametersSetCallbackType
set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback);

/// Register the callback for parameter changes
Expand Down
13 changes: 13 additions & 0 deletions rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

#include <map>
#include <memory>
#include <list>
#include <string>
#include <vector>

Expand Down Expand Up @@ -157,6 +158,14 @@ class NodeParameters : public NodeParametersInterface
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;
Expand All @@ -170,6 +179,8 @@ class NodeParameters : public NodeParametersInterface
const std::map<std::string, rclcpp::ParameterValue> &
get_parameter_overrides() const override;

using CallbacksContainerType = std::list<OnSetParametersCallbackHandle::WeakPtr>;

private:
RCLCPP_DISABLE_COPY(NodeParameters)

Expand All @@ -182,6 +193,8 @@ class NodeParameters : public NodeParametersInterface

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_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_

#include <map>
#include <memory>
#include <string>
#include <vector>

Expand All @@ -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
{
Expand Down Expand Up @@ -158,14 +171,29 @@ class NodeParametersInterface
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
Expand Down
12 changes: 12 additions & 0 deletions rclcpp/src/rclcpp/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
105 changes: 96 additions & 9 deletions rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@

#include <cmath>
#include <cstdlib>
#include <functional>
#include <limits>
#include <map>
#include <memory>
Expand Down Expand Up @@ -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
__set_parameters_atomically_common(
__call_on_parameters_set_callbacks(
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);
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,
CallbacksContainerType & callback_container,
const OnParametersSetCallbackType & callback)
{
// Call the user callback to see if the new value(s) are allowed.
rcl_interfaces::msg::SetParametersResult result =
__call_on_parameters_set_callbacks(parameters, callback_container, callback);
if (!result.successful) {
return result;
}
Expand Down Expand Up @@ -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)
{
Expand All @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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.
Expand All @@ -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) {
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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)
{
Expand Down
Loading

0 comments on commit 41f9f0f

Please sign in to comment.