Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix dangerous std::bind capture in ParameterEventHandler implementation #1770

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
51 changes: 30 additions & 21 deletions rclcpp/include/rclcpp/parameter_event_handler.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -173,9 +173,13 @@ class ParameterEventHandler
{
auto node_topics = rclcpp::node_interfaces::get_node_topics_interface(node);

callbacks_ = std::make_shared<Callbacks>();

event_subscription_ = rclcpp::create_subscription<rcl_interfaces::msg::ParameterEvent>(
node_topics, "/parameter_events", qos,
std::bind(&ParameterEventHandler::event_callback, this, std::placeholders::_1));
[callbacks = callbacks_](const rcl_interfaces::msg::ParameterEvent & event) {
callbacks->event_callback(event);
});
}

using ParameterEventCallbackType =
Expand Down Expand Up @@ -285,17 +289,6 @@ class ParameterEventHandler
using CallbacksContainerType = std::list<ParameterCallbackHandle::WeakPtr>;

protected:
/// Callback for parameter events subscriptions.
RCLCPP_PUBLIC
void
event_callback(const rcl_interfaces::msg::ParameterEvent & event);

// Utility function for resolving node path.
std::string resolve_path(const std::string & path);

// Node interface used for base functionality
std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_;

// *INDENT-OFF* Uncrustify doesn't handle indented public/private labels
// Hash function for string pair required in std::unordered_map
// See: https://stackoverflow.com/questions/35985960/c-why-is-boosthash-combine-the-best-way-to-combine-hash-values
Expand All @@ -319,18 +312,34 @@ class ParameterEventHandler
};
// *INDENT-ON*

// Map container for registered parameters
std::unordered_map<
std::pair<std::string, std::string>,
CallbacksContainerType,
StringPairHash
> parameter_callbacks_;
struct Callbacks
{
std::recursive_mutex mutex_;

// Map container for registered parameters
std::unordered_map<
std::pair<std::string, std::string>,
CallbacksContainerType,
StringPairHash
> parameter_callbacks_;

rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr event_subscription_;
std::list<ParameterEventCallbackHandle::WeakPtr> event_callbacks_;

/// Callback for parameter events subscriptions.
RCLCPP_PUBLIC
void
event_callback(const rcl_interfaces::msg::ParameterEvent & event);
};

std::list<ParameterEventCallbackHandle::WeakPtr> event_callbacks_;
std::shared_ptr<Callbacks> callbacks_;

std::recursive_mutex mutex_;
// Utility function for resolving node path.
std::string resolve_path(const std::string & path);

// Node interface used for base functionality
std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_;

rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr event_subscription_;
};

} // namespace rclcpp
Expand Down
26 changes: 13 additions & 13 deletions rclcpp/src/rclcpp/parameter_event_handler.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,10 +29,10 @@ ParameterEventCallbackHandle::SharedPtr
ParameterEventHandler::add_parameter_event_callback(
ParameterEventCallbackType callback)
{
std::lock_guard<std::recursive_mutex> lock(mutex_);
std::lock_guard<std::recursive_mutex> lock(callbacks_->mutex_);
auto handle = std::make_shared<ParameterEventCallbackHandle>();
handle->callback = callback;
event_callbacks_.emplace_front(handle);
callbacks_->event_callbacks_.emplace_front(handle);

return handle;
}
Expand All @@ -41,15 +41,15 @@ void
ParameterEventHandler::remove_parameter_event_callback(
ParameterEventCallbackHandle::SharedPtr callback_handle)
{
std::lock_guard<std::recursive_mutex> lock(mutex_);
std::lock_guard<std::recursive_mutex> lock(callbacks_->mutex_);
auto it = std::find_if(
event_callbacks_.begin(),
event_callbacks_.end(),
callbacks_->event_callbacks_.begin(),
callbacks_->event_callbacks_.end(),
[callback_handle](const auto & weak_handle) {
return callback_handle.get() == weak_handle.lock().get();
});
if (it != event_callbacks_.end()) {
event_callbacks_.erase(it);
if (it != callbacks_->event_callbacks_.end()) {
callbacks_->event_callbacks_.erase(it);
} else {
throw std::runtime_error("Callback doesn't exist");
}
Expand All @@ -61,15 +61,15 @@ ParameterEventHandler::add_parameter_callback(
ParameterCallbackType callback,
const std::string & node_name)
{
std::lock_guard<std::recursive_mutex> lock(mutex_);
std::lock_guard<std::recursive_mutex> lock(callbacks_->mutex_);
auto full_node_name = resolve_path(node_name);

auto handle = std::make_shared<ParameterCallbackHandle>();
handle->callback = callback;
handle->parameter_name = parameter_name;
handle->node_name = full_node_name;
// the last callback registered is executed first.
parameter_callbacks_[{parameter_name, full_node_name}].emplace_front(handle);
callbacks_->parameter_callbacks_[{parameter_name, full_node_name}].emplace_front(handle);

return handle;
}
Expand All @@ -78,9 +78,9 @@ void
ParameterEventHandler::remove_parameter_callback(
ParameterCallbackHandle::SharedPtr callback_handle)
{
std::lock_guard<std::recursive_mutex> lock(mutex_);
std::lock_guard<std::recursive_mutex> lock(callbacks_->mutex_);
auto handle = callback_handle.get();
auto & container = parameter_callbacks_[{handle->parameter_name, handle->node_name}];
auto & container = callbacks_->parameter_callbacks_[{handle->parameter_name, handle->node_name}];
auto it = std::find_if(
container.begin(),
container.end(),
Expand All @@ -90,7 +90,7 @@ ParameterEventHandler::remove_parameter_callback(
if (it != container.end()) {
container.erase(it);
if (container.empty()) {
parameter_callbacks_.erase({handle->parameter_name, handle->node_name});
callbacks_->parameter_callbacks_.erase({handle->parameter_name, handle->node_name});
}
} else {
throw std::runtime_error("Callback doesn't exist");
Expand Down Expand Up @@ -158,7 +158,7 @@ ParameterEventHandler::get_parameters_from_event(
}

void
ParameterEventHandler::event_callback(const rcl_interfaces::msg::ParameterEvent & event)
ParameterEventHandler::Callbacks::event_callback(const rcl_interfaces::msg::ParameterEvent & event)
{
std::lock_guard<std::recursive_mutex> lock(mutex_);

Expand Down
6 changes: 3 additions & 3 deletions rclcpp/test/rclcpp/test_parameter_event_handler.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,17 +29,17 @@ class TestParameterEventHandler : public rclcpp::ParameterEventHandler

void test_event(rcl_interfaces::msg::ParameterEvent::ConstSharedPtr event)
{
event_callback(*event);
callbacks_->event_callback(*event);
}

size_t num_event_callbacks()
{
return event_callbacks_.size();
return callbacks_->event_callbacks_.size();
}

size_t num_parameter_callbacks()
{
return parameter_callbacks_.size();
return callbacks_->parameter_callbacks_.size();
}
};

Expand Down