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)
{
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(
"rclcpp",
"Couldn't add subscription to wait set: %s", rcl_get_error_string().str);
@ -192,7 +192,7 @@ public:
}
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(
"rclcpp",
"Couldn't add client to wait set: %s", rcl_get_error_string().str);
@ -201,7 +201,7 @@ public:
}
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(
"rclcpp",
"Couldn't add service to wait set: %s", rcl_get_error_string().str);
@ -210,7 +210,7 @@ public:
}
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(
"rclcpp",
"Couldn't add timer to wait set: %s", rcl_get_error_string().str);
@ -219,7 +219,7 @@ public:
}
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(
"rclcpp",
"Couldn't add guard_condition to wait set: %s",

33
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);
// Resize the wait set if necessary.
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);
const size_t node_graph_interfaces_size = node_graph_interfaces_.size();
// 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) {
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");
}
// 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) {
throw_from_rcl_error(ret, "failed to add interrupt guard condition to 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) {
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.
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.
if (node_ptr->count_graph_users() == 0) {
continue;
@ -162,7 +168,7 @@ GraphListener::run_loop()
if (!graph_gc) {
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) {
throw_from_rcl_error(ret, "failed to add graph guard condition to wait set");
}
@ -177,24 +183,19 @@ GraphListener::run_loop()
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.
for (size_t i = 0; i < wait_set_.size_of_guard_conditions; ++i) {
if (shutdown_guard_condition_ == wait_set_.guard_conditions[i]) {
shutdown_guard_condition_triggered = true;
}
}
bool shutdown_guard_condition_triggered =
(shutdown_guard_condition_ == wait_set_.guard_conditions[shutdown_guard_condition_index]);
// 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();
if (!graph_gc) {
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[i]) {
if (graph_gc == wait_set_.guard_conditions[graph_gc_indexes[i]]) {
node_ptr->notify_graph_change();
}
}
if (shutdown_guard_condition_triggered) {
// If shutdown, then notify the node of this as well.
node_ptr->notify_shutdown();