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

Update rcl_wait_set_add_* calls (#586)

Browse source 
Now the functions take an optional output index argument.
Refactored the graph listener usage of rcl_wait_set_add_guard_condition() to take advantage of the new API.
This commit is contained in:
Jacob Perron 2018-11-20 11:02:13 -08:00 • committed by GitHub
parent c8f3fd3b0e
commit f212d73413
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 23 additions and 22 deletions
Download patch file Download diff file Expand all files Collapse all files

10
rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
View file

@ -183,7 +183,7 @@ public:
bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) bool add_handles_to_wait_set(rcl_wait_set_t * wait_set)
{ {
for (auto subscription : subscription_handles_) { for (auto subscription : subscription_handles_) {
if (rcl_wait_set_add_subscription(wait_set, subscription.get()) != RCL_RET_OK) { if (rcl_wait_set_add_subscription(wait_set, subscription.get(), NULL) != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED( RCUTILS_LOG_ERROR_NAMED(
"rclcpp", "rclcpp",
"Couldn't add subscription to wait set: %s", rcl_get_error_string().str); "Couldn't add subscription to wait set: %s", rcl_get_error_string().str);
@ -192,7 +192,7 @@ public:
} }
for (auto client : client_handles_) { for (auto client : client_handles_) {
if (rcl_wait_set_add_client(wait_set, client.get()) != RCL_RET_OK) { if (rcl_wait_set_add_client(wait_set, client.get(), NULL) != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED( RCUTILS_LOG_ERROR_NAMED(
"rclcpp", "rclcpp",
"Couldn't add client to wait set: %s", rcl_get_error_string().str); "Couldn't add client to wait set: %s", rcl_get_error_string().str);
@ -201,7 +201,7 @@ public:
} }
for (auto service : service_handles_) { for (auto service : service_handles_) {
if (rcl_wait_set_add_service(wait_set, service.get()) != RCL_RET_OK) { if (rcl_wait_set_add_service(wait_set, service.get(), NULL) != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED( RCUTILS_LOG_ERROR_NAMED(
"rclcpp", "rclcpp",
"Couldn't add service to wait set: %s", rcl_get_error_string().str); "Couldn't add service to wait set: %s", rcl_get_error_string().str);
@ -210,7 +210,7 @@ public:
} }
for (auto timer : timer_handles_) { for (auto timer : timer_handles_) {
if (rcl_wait_set_add_timer(wait_set, timer.get()) != RCL_RET_OK) { if (rcl_wait_set_add_timer(wait_set, timer.get(), NULL) != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED( RCUTILS_LOG_ERROR_NAMED(
"rclcpp", "rclcpp",
"Couldn't add timer to wait set: %s", rcl_get_error_string().str); "Couldn't add timer to wait set: %s", rcl_get_error_string().str);
@ -219,7 +219,7 @@ public:
} }
for (auto guard_condition : guard_conditions_) { for (auto guard_condition : guard_conditions_) {
if (rcl_wait_set_add_guard_condition(wait_set, guard_condition) != RCL_RET_OK) { if (rcl_wait_set_add_guard_condition(wait_set, guard_condition, NULL) != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED( RCUTILS_LOG_ERROR_NAMED(
"rclcpp", "rclcpp",
"Couldn't add guard_condition to wait set: %s", "Couldn't add guard_condition to wait set: %s",

35
rclcpp/src/rclcpp/graph_listener.cpp
View file

@ -130,8 +130,10 @@ GraphListener::run_loop()
std::lock_guard<std::mutex> nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock); std::lock_guard<std::mutex> nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock);
// Resize the wait set if necessary. // Resize the wait set if necessary.
if (wait_set_.size_of_guard_conditions < (node_graph_interfaces_.size() + 2)) { const size_t node_graph_interfaces_size = node_graph_interfaces_.size();
ret = rcl_wait_set_resize(&wait_set_, 0, node_graph_interfaces_.size() + 2, 0, 0, 0); // Add 2 for the interrupt and shutdown guard conditions
if (wait_set_.size_of_guard_conditions < (node_graph_interfaces_size + 2)) {
ret = rcl_wait_set_resize(&wait_set_, 0, node_graph_interfaces_size + 2, 0, 0, 0);
if (RCL_RET_OK != ret) { if (RCL_RET_OK != ret) {
throw_from_rcl_error(ret, "failed to resize wait set"); throw_from_rcl_error(ret, "failed to resize wait set");
} }
@ -142,17 +144,21 @@ GraphListener::run_loop()
throw_from_rcl_error(ret, "failed to clear wait set"); throw_from_rcl_error(ret, "failed to clear wait set");
} }
// Put the interrupt guard condition in the wait set. // Put the interrupt guard condition in the wait set.
ret = rcl_wait_set_add_guard_condition(&wait_set_, &interrupt_guard_condition_); ret = rcl_wait_set_add_guard_condition(&wait_set_, &interrupt_guard_condition_, NULL);
if (RCL_RET_OK != ret) { if (RCL_RET_OK != ret) {
throw_from_rcl_error(ret, "failed to add interrupt guard condition to wait set"); throw_from_rcl_error(ret, "failed to add interrupt guard condition to wait set");
} }
// Put the shutdown guard condition in the wait set. // Put the shutdown guard condition in the wait set.
ret = rcl_wait_set_add_guard_condition(&wait_set_, shutdown_guard_condition_); size_t shutdown_guard_condition_index = 0u;
ret = rcl_wait_set_add_guard_condition(
&wait_set_, shutdown_guard_condition_, &shutdown_guard_condition_index);
if (RCL_RET_OK != ret) { if (RCL_RET_OK != ret) {
throw_from_rcl_error(ret, "failed to add shutdown guard condition to wait set"); throw_from_rcl_error(ret, "failed to add shutdown guard condition to wait set");
} }
// Put graph guard conditions for each node into the wait set. // Put graph guard conditions for each node into the wait set.
for (const auto node_ptr : node_graph_interfaces_) { std::vector<size_t> graph_gc_indexes(node_graph_interfaces_size, 0u);
for (size_t i = 0u; i < node_graph_interfaces_size; ++i) {
auto node_ptr = node_graph_interfaces_[i];
// Only wait on graph changes if some user of the node is watching. // Only wait on graph changes if some user of the node is watching.
if (node_ptr->count_graph_users() == 0) { if (node_ptr->count_graph_users() == 0) {
continue; continue;
@ -162,7 +168,7 @@ GraphListener::run_loop()
if (!graph_gc) { if (!graph_gc) {
throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition"); throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition");
} }
ret = rcl_wait_set_add_guard_condition(&wait_set_, graph_gc); ret = rcl_wait_set_add_guard_condition(&wait_set_, graph_gc, &graph_gc_indexes[i]);
if (RCL_RET_OK != ret) { if (RCL_RET_OK != ret) {
throw_from_rcl_error(ret, "failed to add graph guard condition to wait set"); throw_from_rcl_error(ret, "failed to add graph guard condition to wait set");
} }
@ -177,23 +183,18 @@ GraphListener::run_loop()
throw_from_rcl_error(ret, "failed to wait on wait set"); throw_from_rcl_error(ret, "failed to wait on wait set");
} }
bool shutdown_guard_condition_triggered = false;
// Check to see if the shutdown guard condition has been triggered. // Check to see if the shutdown guard condition has been triggered.
for (size_t i = 0; i < wait_set_.size_of_guard_conditions; ++i) { bool shutdown_guard_condition_triggered =
if (shutdown_guard_condition_ == wait_set_.guard_conditions[i]) { (shutdown_guard_condition_ == wait_set_.guard_conditions[shutdown_guard_condition_index]);
shutdown_guard_condition_triggered = true;
}
}
// Notify nodes who's guard conditions are set (triggered). // Notify nodes who's guard conditions are set (triggered).
for (const auto node_ptr : node_graph_interfaces_) { for (size_t i = 0u; i < node_graph_interfaces_size; ++i) {
const auto node_ptr = node_graph_interfaces_[i];
auto graph_gc = node_ptr->get_graph_guard_condition(); auto graph_gc = node_ptr->get_graph_guard_condition();
if (!graph_gc) { if (!graph_gc) {
throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition"); throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition");
} }
for (size_t i = 0; i < wait_set_.size_of_guard_conditions; ++i) { if (graph_gc == wait_set_.guard_conditions[graph_gc_indexes[i]]) {
if (graph_gc == wait_set_.guard_conditions[i]) { node_ptr->notify_graph_change();
node_ptr->notify_graph_change();
}
} }
if (shutdown_guard_condition_triggered) { if (shutdown_guard_condition_triggered) {
// If shutdown, then notify the node of this as well. // If shutdown, then notify the node of this as well.