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

Rcl consolidate wait set functions (#540)

Browse source 
* Use consolidated rcl_wait_set_clear()

* Use consolidated rcl_wait_set_resize()
This commit is contained in:
Shane Loretz 2018-08-27 11:55:04 -07:00 • committed by GitHub
parent 18ad26e654
commit 4653bfcce6
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 11 additions and 54 deletions
Download patch file Download diff file Expand all files Collapse all files

59
rclcpp/src/rclcpp/executor.cpp
View file

@ -410,60 +410,17 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
); );
} }
// clear wait set // clear wait set
if (rcl_wait_set_clear_subscriptions(&wait_set_) != RCL_RET_OK) { if (rcl_wait_set_clear(&wait_set_) != RCL_RET_OK) {
throw std::runtime_error("Couldn't clear subscriptions from wait set"); throw std::runtime_error("Couldn't clear 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( rcl_ret_t ret = rcl_wait_set_resize(
&wait_set_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK) &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( throw std::runtime_error(
std::string("Couldn't resize the number of subscriptions in wait set : ") + std::string("Couldn't resize the wait set : ") + rcl_get_error_string_safe());
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_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)
{
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());
} }
if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) { if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) {

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

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