Skip to content

Commit

Permalink
address feedback part 1: static get_parameter method, remove register…
Browse files Browse the repository at this point in the history 
…_parameter_update, mutex for thread-safety

Signed-off-by: bpwilcox <bpwilcox@eng.ucsd.edu>
  • Loading branch information
bpwilcox committed Sep 24, 2019
1 parent 83753c5 commit a2536fb
Show file tree
Hide file tree
Showing 3 changed files with 40 additions and 80 deletions.
53 changes: 10 additions & 43 deletions rclcpp/include/rclcpp/parameter_events_subscriber.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,57 +79,22 @@ class ParameterEventsSubscriber
std::function<void(const rclcpp::Parameter &)> callback,
const std::string & node_name = "");

/// Add a callback to assign the value of a changed parameter to a reference variable.
/**
* If a node_name is not provided, defaults to the current node
*
* \param[in] parameter_name Name of parameter.
* \param[in] value Reference to variable receiving update.
* \param[in] node_name Name of node which hosts the parameter.
*/
template<typename ParameterT>
void
register_parameter_update(
const std::string & parameter_name, ParameterT & value, const std::string & node_name = "")
{
auto callback =
[&value, this](const rclcpp::Parameter & param) {
this->get_parameter_update<ParameterT>(param, value);
};

register_parameter_callback(parameter_name, callback, node_name);
}

RCLCPP_PUBLIC
bool
static bool
get_parameter_from_event(
const rcl_interfaces::msg::ParameterEvent::SharedPtr event,
rclcpp::Parameter & parameter,
const std::string parameter_name,
const std::string node_name = "");

protected:
/// Get value of specified parameter from an a rclcpp::Parameter.
/**
* If the parameter does not appear in the event, no value will be assigned.
* \param[in] parameter_name Name of parameter.
* \param[in] value Reference to variable receiving updates.
*/
template<typename ParameterT>
void
get_parameter_update(
const rclcpp::Parameter & param, ParameterT & value)
{
try {
value = param.get_value<ParameterT>();
RCLCPP_DEBUG(node_logging_->get_logger(), "Updating parameter: %s", param.get_name().c_str());
} catch (...) {
RCLCPP_WARN(node_logging_->get_logger(),
"Parameter '%s' has different type (%s), cannot update registered parameter",
param.get_name().c_str(), param.get_type_name().c_str());
}
}
RCLCPP_PUBLIC
static rclcpp::Parameter
get_parameter_from_event(
const rcl_interfaces::msg::ParameterEvent::SharedPtr event,
const std::string parameter_name,
const std::string node_name = "");

protected:
/// Add a subscription (if unique) to a namespace parameter events topic.
void
add_namespace_event_subscriber(const std::string & node_namespace);
Expand Down Expand Up @@ -187,6 +152,8 @@ class ParameterEventsSubscriber
<rcl_interfaces::msg::ParameterEvent>::SharedPtr> event_subscriptions_;

std::function<void(const rcl_interfaces::msg::ParameterEvent::SharedPtr &)> user_callback_;

std::recursive_mutex mutex_;
};

} // namespace rclcpp
Expand Down
17 changes: 16 additions & 1 deletion rclcpp/src/rclcpp/parameter_events_subscriber.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@ ParameterEventsSubscriber::ParameterEventsSubscriber(
void
ParameterEventsSubscriber::add_namespace_event_subscriber(const std::string & node_namespace)
{
std::lock_guard<std::recursive_mutex> lock(mutex_);
if (std::find(node_namespaces_.begin(), node_namespaces_.end(),
node_namespace) == node_namespaces_.end())
{
Expand All @@ -56,6 +57,7 @@ ParameterEventsSubscriber::set_event_callback(
std::function<void(const rcl_interfaces::msg::ParameterEvent::SharedPtr &)> callback,
const std::string & node_namespace)
{
std::lock_guard<std::recursive_mutex> lock(mutex_);
std::string full_namespace;
if (node_namespace == "") {
full_namespace = node_base_->get_namespace();
Expand All @@ -72,6 +74,7 @@ ParameterEventsSubscriber::register_parameter_callback(
std::function<void(const rclcpp::Parameter &)> callback,
const std::string & node_name)
{
std::lock_guard<std::recursive_mutex> lock(mutex_);
auto full_node_name = resolve_path(node_name);
add_namespace_event_subscriber(split_path(full_node_name).first);
parameter_callbacks_[{parameter_name, full_node_name}] = callback;
Expand All @@ -84,7 +87,7 @@ ParameterEventsSubscriber::get_parameter_from_event(
const std::string parameter_name,
const std::string node_name)
{
if (event->node == resolve_path(node_name)) {
if (event->node == node_name) {
rclcpp::ParameterEventsFilter filter(event, {parameter_name},
{rclcpp::ParameterEventsFilter::EventType::NEW,
rclcpp::ParameterEventsFilter::EventType::CHANGED});
Expand All @@ -98,10 +101,22 @@ ParameterEventsSubscriber::get_parameter_from_event(
return false;
}

rclcpp::Parameter
ParameterEventsSubscriber::get_parameter_from_event(
const rcl_interfaces::msg::ParameterEvent::SharedPtr event,
const std::string parameter_name,
const std::string node_name)
{
rclcpp::Parameter p(parameter_name);
get_parameter_from_event(event, p, parameter_name, node_name);
return p;
}

void
ParameterEventsSubscriber::event_callback(
const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
{
std::lock_guard<std::recursive_mutex> lock(mutex_);
const std::string & node_name = event->node;
RCLCPP_DEBUG(node_logging_->get_logger(), "Parameter event received for node: %s",
node_name.c_str());
Expand Down
50 changes: 14 additions & 36 deletions rclcpp/test/test_parameter_events_subscriber.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,47 +138,21 @@ TEST_F(TestNode, RegisterParameterCallback)
EXPECT_EQ(received, true);
}

TEST_F(TestNode, RegisterParameterUpdate)
{
double double_param;
int int_param;
bool bool_param;
std::string string_param;

// Set individual parameters
ParamSubscriber->register_parameter_update("my_double", double_param);
ParamSubscriber->register_parameter_update("my_int", int_param);
ParamSubscriber->register_parameter_update("my_string", string_param, remote_node_name);
ParamSubscriber->register_parameter_update("my_bool", bool_param, diff_ns_name);

ParamSubscriber->test_event(same_node_double);
ParamSubscriber->test_event(same_node_int);
ParamSubscriber->test_event(remote_node_string);
ParamSubscriber->test_event(diff_ns_bool);

EXPECT_EQ(double_param, 1.0);
EXPECT_EQ(int_param, 1);
EXPECT_EQ(string_param, "test");
EXPECT_EQ(bool_param, true);

// Set multiple parameters atomically
double_param = 0;
int_param = 0;

ParamSubscriber->test_event(multiple);
EXPECT_EQ(double_param, 1.0);
EXPECT_EQ(int_param, 1);
}

TEST_F(TestNode, SameParameterDifferentNode)
{
int int_param_node1;
int int_param_node2;

auto cb1 = [&int_param_node1](const rclcpp::Parameter & p) {
int_param_node1 = p.get_value<int>();
};
auto cb2 = [&int_param_node2](const rclcpp::Parameter & p) {
int_param_node2 = p.get_value<int>();
};

// Set individual parameters
ParamSubscriber->register_parameter_update("my_int", int_param_node1);
ParamSubscriber->register_parameter_update("my_int", int_param_node2, remote_node_name);
ParamSubscriber->register_parameter_callback("my_int", cb1);
ParamSubscriber->register_parameter_callback("my_int", cb2, remote_node_name);

ParamSubscriber->test_event(same_node_int);
EXPECT_EQ(int_param_node1, 1);
Expand All @@ -194,6 +168,8 @@ TEST_F(TestNode, SameParameterDifferentNode)

TEST_F(TestNode, UserCallback)
{
using rclcpp::ParameterEventsSubscriber;

double double_param = 0.0;
int int_param = 0;
bool received = false;
Expand All @@ -210,12 +186,14 @@ TEST_F(TestNode, UserCallback)
}

rclcpp::Parameter p;
if (ParamSubscriber->get_parameter_from_event(event, p, "my_int")) {
if (ParameterEventsSubscriber::get_parameter_from_event(event, p, "my_int", node_name)) {
int_param = p.get_value<int>();
}

if (ParamSubscriber->get_parameter_from_event(event, p, "my_double")) {
p = ParameterEventsSubscriber::get_parameter_from_event(event, "my_double", node_name);
try {
double_param = p.get_value<double>();
} catch (...) {
}

product = int_param * double_param;
Expand Down

0 comments on commit a2536fb

Please sign in to comment.