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

Reset rcl errors (#374)

Browse source
This commit is contained in:
dhood 2017-09-14 11:03:24 -07:00 • committed by GitHub
parent ed26865b71
commit 4a2e9d8af9
1 changed files with 12 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -60,9 +60,11 @@ Executor::Executor(const ExecutorArgs & args)
{
fprintf(stderr,
"[rclcpp::error] failed to create waitset: %s\n", rcl_get_error_string_safe());
rcl_reset_error();
if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
fprintf(stderr,
"[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
rcl_reset_error();
}
throw std::runtime_error("Failed to create waitset in Executor constructor");
}
@ -74,11 +76,13 @@ Executor::~Executor()
if (rcl_wait_set_fini(&waitset_) != RCL_RET_OK) {
fprintf(stderr,
"[rclcpp::error] failed to destroy waitset: %s\n", rcl_get_error_string_safe());
rcl_reset_error();
}
// Finalize the interrupt guard condition.
if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
fprintf(stderr,
"[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
rcl_reset_error();
}
// Remove and release the sigint guard condition
memory_strategy_->remove_guard_condition(
@ -270,6 +274,7 @@ Executor::execute_subscription(
fprintf(stderr,
"[rclcpp::error] take failed for subscription on topic '%s': %s\n",
subscription->get_topic_name(), rcl_get_error_string_safe());
rcl_reset_error();
}
subscription->return_message(message);
}
@ -292,6 +297,7 @@ Executor::execute_intra_process_subscription(
fprintf(stderr,
"[rclcpp::error] take failed for intra process subscription on topic '%s': %s\n",
subscription->get_topic_name(), rcl_get_error_string_safe());
rcl_reset_error();
}
}
@ -318,6 +324,7 @@ Executor::execute_service(
fprintf(stderr,
"[rclcpp::error] take request failed for server of service '%s': %s\n",
service->get_service_name().c_str(), rcl_get_error_string_safe());
rcl_reset_error();
}
}
@ -337,6 +344,7 @@ Executor::execute_client(
fprintf(stderr,
"[rclcpp::error] take response failed for client of service '%s': %s\n",
client->get_service_name().c_str(), rcl_get_error_string_safe());
rcl_reset_error();
}
}
@ -410,7 +418,10 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
if (status == RCL_RET_WAIT_SET_EMPTY) {
fprintf(stderr, "Warning: empty waitset received in rcl_wait(). This should never happen.\n");
} else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) {
throw std::runtime_error(std::string("rcl_wait() failed: ") + rcl_get_error_string_safe());
std::string msg = "rcl_wait() failed: ";
msg += rcl_get_error_string_safe();
rcl_reset_error();
throw std::runtime_error(msg);
}
// check the null handles in the waitset and remove them from the handles in memory strategy