Rcl consolidate wait set functions (#540)

* Use consolidated rcl_wait_set_clear() * Use consolidated rcl_wait_set_resize()
ros2 · Aug 27, 2018 · 4653bfc · 4653bfc
1 parent 18ad26e
commit 4653bfc
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 54 deletions.
diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp
@@ -410,60 +410,17 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
     );
   }
   // clear wait set
-  if (rcl_wait_set_clear_subscriptions(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear subscriptions from wait set");
-  }
-  if (rcl_wait_set_clear_services(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear servicess from wait set");
-  }
-  if (rcl_wait_set_clear_clients(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear clients from wait set");
-  }
-  if (rcl_wait_set_clear_guard_conditions(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear guard conditions from wait set");
-  }
-  if (rcl_wait_set_clear_timers(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear timers from wait set");
-  }
-
-  if (rcl_wait_set_resize_subscriptions(
-      &wait_set_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("Couldn't resize the number of subscriptions in wait set : ") +
-            rcl_get_error_string_safe());
-  }
-
-  if (rcl_wait_set_resize_services(
-      &wait_set_, memory_strategy_->number_of_ready_services()) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("Couldn't resize the number of services in wait set : ") +
-            rcl_get_error_string_safe());
+  if (rcl_wait_set_clear(&wait_set_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear wait set");
   }
 
-  if (rcl_wait_set_resize_clients(
-      &wait_set_, memory_strategy_->number_of_ready_clients()) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("Couldn't resize the number of clients in wait set : ") +
-            rcl_get_error_string_safe());
-  }
-
-  if (rcl_wait_set_resize_guard_conditions(
-      &wait_set_, memory_strategy_->number_of_guard_conditions()) != RCL_RET_OK)
-  {
+  rcl_ret_t ret = rcl_wait_set_resize(
+    &wait_set_, memory_strategy_->number_of_ready_subscriptions(),
+    memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(),
+    memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services());
+  if (RCL_RET_OK != ret) {
     throw std::runtime_error(
-            std::string("Couldn't resize the number of guard_conditions in wait set : ") +
-            rcl_get_error_string_safe());
-  }
-
-  if (rcl_wait_set_resize_timers(
-      &wait_set_, memory_strategy_->number_of_ready_timers()) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("Couldn't resize the number of timers in wait set : ") +
-            rcl_get_error_string_safe());
+            std::string("Couldn't resize the wait set : ") + rcl_get_error_string_safe());
   }
 
   if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) {

diff --git a/rclcpp/src/rclcpp/graph_listener.cpp b/rclcpp/src/rclcpp/graph_listener.cpp
@@ -131,13 +131,13 @@ GraphListener::run_loop()
 
     // Resize the wait set if necessary.
     if (wait_set_.size_of_guard_conditions < (node_graph_interfaces_.size() + 2)) {
-      ret = rcl_wait_set_resize_guard_conditions(&wait_set_, 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");
       }
     }
-    // Clear the wait set's guard conditions.
-    ret = rcl_wait_set_clear_guard_conditions(&wait_set_);
+    // Clear the wait set.
+    ret = rcl_wait_set_clear(&wait_set_);
     if (RCL_RET_OK != ret) {
       throw_from_rcl_error(ret, "failed to clear wait set");
     }