Skip to content

Commit

Permalink
pass rclcpp::Parameter object to callback, rename functions, get para…
Browse files Browse the repository at this point in the history 
…m from event
  • Loading branch information
bpwilcox committed Aug 23, 2019
1 parent 654751f commit 94dfc1f
Show file tree
Hide file tree
Showing 3 changed files with 71 additions and 53 deletions.
51 changes: 24 additions & 27 deletions rclcpp/include/rclcpp/parameter_events_subscriber.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,9 +70,9 @@ class ParameterEventsSubscriber
* \param[in] callback Function callback to be evaluated upon parameter event
* \param[in] node_name Name of node which hosts the parameter
*/
void register_param_callback(
void register_parameter_callback(
const std::string & parameter_name,
std::function<void()> callback,
std::function<void(const rclcpp::Parameter &)> callback,
const std::string & node_name = "");

/// adds a callback to assign the value of a changed parameter to a reference variable
Expand All @@ -84,44 +84,44 @@ class ParameterEventsSubscriber
* \param[in] node_name Name of node which hosts the parameter
*/
template<typename ParameterT>
void register_param_update(
void register_parameter_update(
const std::string & parameter_name, ParameterT & value, const std::string & node_name = "")
{
auto callback =
[parameter_name, &value, this]() {
get_param_update<ParameterT>(parameter_name, value);
[&value, this](const rclcpp::Parameter & param) {
get_parameter_update<ParameterT>(param, value);
};

register_param_callback(parameter_name, callback, node_name);
register_parameter_callback(parameter_name, callback, node_name);
}

/// Gets value of specified parameter from an event
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:
/// Gets 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_param_update(const std::string & parameter_name, ParameterT & value)
void get_parameter_update(
const rclcpp::Parameter & param, ParameterT & value)
{
rclcpp::ParameterEventsFilter filter(last_event_, {parameter_name},
{rclcpp::ParameterEventsFilter::EventType::NEW,
rclcpp::ParameterEventsFilter::EventType::CHANGED});
if (!filter.get_events().empty()) {
RCLCPP_DEBUG(node_logging_->get_logger(), "Updating parameter: %s", parameter_name.c_str());
auto param_msg = filter.get_events()[0].second;
auto param = rclcpp::Parameter::from_parameter_msg(*param_msg);
try {
value = param.get_value<ParameterT>();
} catch (...) {
RCLCPP_WARN(node_logging_->get_logger(),
"Parameter '%s' has different type (%s), cannot update registered parameter",
parameter_name.c_str(), param.get_type_name().c_str());
}
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());
}
}

protected:
/// Adds a subscription (if unique) to a namespace parameter events topic
void add_namespace_event_subscriber(const std::string & node_namespace);

Expand All @@ -142,7 +142,7 @@ class ParameterEventsSubscriber

// Map containers for registered parameters
std::map<std::string, std::string> parameter_node_map_;
std::map<std::string, std::function<void()>> parameter_callbacks_;
std::map<std::string, std::function<void(const rclcpp::Parameter &)>> parameter_callbacks_;

// Vector of unique namespaces added
std::vector<std::string> node_namespaces_;
Expand All @@ -152,9 +152,6 @@ class ParameterEventsSubscriber
<rcl_interfaces::msg::ParameterEvent>::SharedPtr> event_subscriptions_;

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

// Pointer to latest event message
rcl_interfaces::msg::ParameterEvent::SharedPtr last_event_;
};

} // namespace rclcpp
Expand Down
40 changes: 26 additions & 14 deletions rclcpp/src/rclcpp/parameter_events_subscriber.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,8 +30,7 @@ ParameterEventsSubscriber::ParameterEventsSubscriber(
: node_base_(node_base),
node_topics_(node_topics),
node_logging_(node_logging),
qos_(qos),
last_event_(std::make_shared<rcl_interfaces::msg::ParameterEvent>())
qos_(qos)
{}

void ParameterEventsSubscriber::add_namespace_event_subscriber(const std::string & node_namespace)
Expand All @@ -41,7 +40,7 @@ void ParameterEventsSubscriber::add_namespace_event_subscriber(const std::string
{
node_namespaces_.push_back(node_namespace);
auto topic = join_path(node_namespace, "parameter_events");
RCLCPP_INFO(node_logging_->get_logger(), "Subscribing to topic: %s", topic.c_str());
RCLCPP_DEBUG(node_logging_->get_logger(), "Subscribing to topic: %s", topic.c_str());

auto event_sub = rclcpp::create_subscription<rcl_interfaces::msg::ParameterEvent>(
node_topics_, topic, qos_,
Expand All @@ -65,9 +64,9 @@ void ParameterEventsSubscriber::set_event_callback(
user_callback_ = callback;
}

void ParameterEventsSubscriber::register_param_callback(
void ParameterEventsSubscriber::register_parameter_callback(
const std::string & parameter_name,
std::function<void()> callback,
std::function<void(const rclcpp::Parameter &)> callback,
const std::string & node_name)
{
auto full_node_name = resolve_path(node_name);
Expand All @@ -76,25 +75,38 @@ void ParameterEventsSubscriber::register_param_callback(
parameter_callbacks_[parameter_name] = callback;
}

bool ParameterEventsSubscriber::get_parameter_from_event(
const rcl_interfaces::msg::ParameterEvent::SharedPtr event,
rclcpp::Parameter & parameter,
const std::string parameter_name,
const std::string node_name)
{
if (event->node == resolve_path(node_name)) {
rclcpp::ParameterEventsFilter filter(event, {parameter_name},
{rclcpp::ParameterEventsFilter::EventType::NEW,
rclcpp::ParameterEventsFilter::EventType::CHANGED});
if (!filter.get_events().empty()) {
auto param_msg = filter.get_events()[0].second;
parameter = rclcpp::Parameter::from_parameter_msg(*param_msg);
return true;
}
}
return false;
}

void ParameterEventsSubscriber::event_callback(
const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
{
std::string node_name = event->node;
RCLCPP_DEBUG(node_logging_->get_logger(), "Parameter event received for node: %s",
node_name.c_str());

last_event_ = event;

for (std::map<std::string, std::string>::iterator it = parameter_node_map_.begin();
it != parameter_node_map_.end(); ++it)
{
if (node_name == it->second) {
rclcpp::ParameterEventsFilter filter(event, {it->first},
{rclcpp::ParameterEventsFilter::EventType::NEW,
rclcpp::ParameterEventsFilter::EventType::CHANGED});
if (!filter.get_events().empty()) {
parameter_callbacks_[it->first]();
}
rclcpp::Parameter p;
if (get_parameter_from_event(event, p, it->first, it->second)) {
parameter_callbacks_[it->first](p);
}
}

Expand Down
33 changes: 21 additions & 12 deletions rclcpp/test/test_parameter_events_subscriber.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,12 +110,12 @@ class TestNode : public ::testing::Test
TEST_F(TestNode, RegisterParameterCallback)
{
bool received;
auto cb = [&received]() {received = true;};
auto cb = [&received](const rclcpp::Parameter &) {received = true;};

ParamSubscriber->register_param_callback("my_double", cb); // same node
ParamSubscriber->register_param_callback("my_int", cb); // same node
ParamSubscriber->register_param_callback("my_string", cb, remote_node_name); // remote node
ParamSubscriber->register_param_callback("my_bool", cb, diff_ns_name); // different namespace
ParamSubscriber->register_parameter_callback("my_double", cb); // same node
ParamSubscriber->register_parameter_callback("my_int", cb); // same node
ParamSubscriber->register_parameter_callback("my_string", cb, remote_node_name); // remote node
ParamSubscriber->register_parameter_callback("my_bool", cb, diff_ns_name); // different namespace

received = false;
ParamSubscriber->test_event(same_node_double);
Expand All @@ -142,10 +142,10 @@ TEST_F(TestNode, RegisterParameterUpdate)
std::string string_param;

// Set individual parameters
ParamSubscriber->register_param_update("my_double", double_param);
ParamSubscriber->register_param_update("my_int", int_param);
ParamSubscriber->register_param_update("my_string", string_param, remote_node_name);
ParamSubscriber->register_param_update("my_bool", bool_param, diff_ns_name);
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);
Expand Down Expand Up @@ -177,10 +177,19 @@ TEST_F(TestNode, UserCallback)
[&int_param, &double_param, &product, &received,
this](const rcl_interfaces::msg::ParameterEvent::SharedPtr & event)
{
if (event->node == node->get_fully_qualified_name()) {
auto node_name = node->get_fully_qualified_name();

if (event->node == node_name) {
received = true;
ParamSubscriber->get_param_update("my_int", int_param);
ParamSubscriber->get_param_update("my_double", double_param);
}

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

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

product = int_param * double_param;
Expand Down

0 comments on commit 94dfc1f

Please sign in to comment.