From 9c8c88565a4d7d1918e38529c04105c4069ccf99 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 20 Nov 2018 11:02:13 -0800 Subject: [PATCH] Update rcl_wait_set_add_* calls (#586) 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. --- .../strategies/allocator_memory_strategy.hpp | 10 +++--- rclcpp/src/rclcpp/graph_listener.cpp | 35 ++++++++++--------- 2 files changed, 23 insertions(+), 22 deletions(-) diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index e46bb6d185..224a3ac918 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -183,7 +183,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy 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 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } 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 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } 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 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } 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 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } 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", diff --git a/rclcpp/src/rclcpp/graph_listener.cpp b/rclcpp/src/rclcpp/graph_listener.cpp index 7062a3af53..ed9fa9ed22 100644 --- a/rclcpp/src/rclcpp/graph_listener.cpp +++ b/rclcpp/src/rclcpp/graph_listener.cpp @@ -130,8 +130,10 @@ GraphListener::run_loop() std::lock_guard 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 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,23 +183,18 @@ 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]) { - node_ptr->notify_graph_change(); - } + 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.