use new error handling API from rcutils (#577)
Signed-off-by: William Woodall <william@osrfoundation.org>
This commit is contained in:
parent
ec834d321b
commit
33a755c535
18 changed files with 81 additions and 87 deletions
|
@ -102,7 +102,7 @@ public:
|
||||||
if (ret != RCL_RET_OK) {
|
if (ret != RCL_RET_OK) {
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"failed to destroy serialized message: %s", rcl_get_error_string_safe());
|
"failed to destroy serialized message: %s", rcl_get_error_string().str);
|
||||||
}
|
}
|
||||||
});
|
});
|
||||||
|
|
||||||
|
|
|
@ -121,7 +121,7 @@ public:
|
||||||
RCLCPP_ERROR(
|
RCLCPP_ERROR(
|
||||||
rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp"),
|
rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp"),
|
||||||
"Error in destruction of rcl service handle: %s",
|
"Error in destruction of rcl service handle: %s",
|
||||||
rcl_get_error_string_safe());
|
rcl_get_error_string().str);
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
|
@ -164,7 +164,7 @@ public:
|
||||||
any_callback_(any_callback)
|
any_callback_(any_callback)
|
||||||
{
|
{
|
||||||
// check if service handle was initialized
|
// check if service handle was initialized
|
||||||
if (!rcl_service_is_valid(service_handle.get(), nullptr)) {
|
if (!rcl_service_is_valid(service_handle.get())) {
|
||||||
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
|
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string("rcl_service_t in constructor argument must be initialized beforehand."));
|
std::string("rcl_service_t in constructor argument must be initialized beforehand."));
|
||||||
|
@ -182,7 +182,7 @@ public:
|
||||||
any_callback_(any_callback)
|
any_callback_(any_callback)
|
||||||
{
|
{
|
||||||
// check if service handle was initialized
|
// check if service handle was initialized
|
||||||
if (!rcl_service_is_valid(service_handle, nullptr)) {
|
if (!rcl_service_is_valid(service_handle)) {
|
||||||
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
|
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string("rcl_service_t in constructor argument must be initialized beforehand."));
|
std::string("rcl_service_t in constructor argument must be initialized beforehand."));
|
||||||
|
|
|
@ -186,7 +186,7 @@ public:
|
||||||
if (rcl_wait_set_add_subscription(wait_set, subscription.get()) != RCL_RET_OK) {
|
if (rcl_wait_set_add_subscription(wait_set, subscription.get()) != RCL_RET_OK) {
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"Couldn't add subscription to wait set: %s", rcl_get_error_string_safe());
|
"Couldn't add subscription to wait set: %s", rcl_get_error_string().str);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -195,7 +195,7 @@ public:
|
||||||
if (rcl_wait_set_add_client(wait_set, client.get()) != RCL_RET_OK) {
|
if (rcl_wait_set_add_client(wait_set, client.get()) != RCL_RET_OK) {
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"Couldn't add client to wait set: %s", rcl_get_error_string_safe());
|
"Couldn't add client to wait set: %s", rcl_get_error_string().str);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -204,7 +204,7 @@ public:
|
||||||
if (rcl_wait_set_add_service(wait_set, service.get()) != RCL_RET_OK) {
|
if (rcl_wait_set_add_service(wait_set, service.get()) != RCL_RET_OK) {
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"Couldn't add service to wait set: %s", rcl_get_error_string_safe());
|
"Couldn't add service to wait set: %s", rcl_get_error_string().str);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -213,7 +213,7 @@ public:
|
||||||
if (rcl_wait_set_add_timer(wait_set, timer.get()) != RCL_RET_OK) {
|
if (rcl_wait_set_add_timer(wait_set, timer.get()) != RCL_RET_OK) {
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"Couldn't add timer to wait set: %s", rcl_get_error_string_safe());
|
"Couldn't add timer to wait set: %s", rcl_get_error_string().str);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -223,7 +223,7 @@ public:
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"Couldn't add guard_condition to wait set: %s",
|
"Couldn't add guard_condition to wait set: %s",
|
||||||
rcl_get_error_string_safe());
|
rcl_get_error_string().str);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -48,7 +48,7 @@ ClientBase::ClientBase(
|
||||||
if (rcl_client_fini(client, handle.get()) != RCL_RET_OK) {
|
if (rcl_client_fini(client, handle.get()) != RCL_RET_OK) {
|
||||||
RCLCPP_ERROR(
|
RCLCPP_ERROR(
|
||||||
rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp"),
|
rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp"),
|
||||||
"Error in destruction of rcl client handle: %s", rcl_get_error_string_safe());
|
"Error in destruction of rcl client handle: %s", rcl_get_error_string().str);
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
|
|
|
@ -75,7 +75,7 @@ throw_from_rcl_error(
|
||||||
|
|
||||||
RCLErrorBase::RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state)
|
RCLErrorBase::RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state)
|
||||||
: ret(ret), message(error_state->message), file(error_state->file), line(error_state->line_number),
|
: ret(ret), message(error_state->message), file(error_state->file), line(error_state->line_number),
|
||||||
formatted_message(rcl_get_error_string_safe())
|
formatted_message(rcl_get_error_string().str)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
RCLError::RCLError(
|
RCLError::RCLError(
|
||||||
|
|
|
@ -45,7 +45,7 @@ Executor::Executor(const ExecutorArgs & args)
|
||||||
{
|
{
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string("Failed to create interrupt guard condition in Executor constructor: ") +
|
std::string("Failed to create interrupt guard condition in Executor constructor: ") +
|
||||||
rcl_get_error_string_safe());
|
rcl_get_error_string().str);
|
||||||
}
|
}
|
||||||
|
|
||||||
// The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond,
|
// The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond,
|
||||||
|
@ -63,12 +63,12 @@ Executor::Executor(const ExecutorArgs & args)
|
||||||
{
|
{
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"failed to create wait set: %s", rcl_get_error_string_safe());
|
"failed to create wait set: %s", rcl_get_error_string().str);
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
|
if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"failed to destroy guard condition: %s", rcl_get_error_string_safe());
|
"failed to destroy guard condition: %s", rcl_get_error_string().str);
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
throw std::runtime_error("Failed to create wait set in Executor constructor");
|
throw std::runtime_error("Failed to create wait set in Executor constructor");
|
||||||
|
@ -91,14 +91,14 @@ Executor::~Executor()
|
||||||
if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) {
|
if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) {
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"failed to destroy wait set: %s", rcl_get_error_string_safe());
|
"failed to destroy wait set: %s", rcl_get_error_string().str);
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
// Finalize the interrupt guard condition.
|
// Finalize the interrupt guard condition.
|
||||||
if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
|
if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"failed to destroy guard condition: %s", rcl_get_error_string_safe());
|
"failed to destroy guard condition: %s", rcl_get_error_string().str);
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
// Remove and release the sigint guard condition
|
// Remove and release the sigint guard condition
|
||||||
|
@ -127,7 +127,7 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
|
||||||
if (notify) {
|
if (notify) {
|
||||||
// Interrupt waiting to handle new node
|
// Interrupt waiting to handle new node
|
||||||
if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
|
if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
|
||||||
throw std::runtime_error(rcl_get_error_string_safe());
|
throw std::runtime_error(rcl_get_error_string().str);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// Add the node's notify condition to the guard condition handles
|
// Add the node's notify condition to the guard condition handles
|
||||||
|
@ -161,7 +161,7 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node
|
||||||
// If the node was matched and removed, interrupt waiting
|
// If the node was matched and removed, interrupt waiting
|
||||||
if (node_removed) {
|
if (node_removed) {
|
||||||
if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
|
if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
|
||||||
throw std::runtime_error(rcl_get_error_string_safe());
|
throw std::runtime_error(rcl_get_error_string().str);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -247,7 +247,7 @@ Executor::cancel()
|
||||||
{
|
{
|
||||||
spinning.store(false);
|
spinning.store(false);
|
||||||
if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
|
if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
|
||||||
throw std::runtime_error(rcl_get_error_string_safe());
|
throw std::runtime_error(rcl_get_error_string().str);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -286,7 +286,7 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
|
||||||
// Wake the wait, because it may need to be recalculated or work that
|
// Wake the wait, because it may need to be recalculated or work that
|
||||||
// was previously blocked is now available.
|
// was previously blocked is now available.
|
||||||
if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
|
if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
|
||||||
throw std::runtime_error(rcl_get_error_string_safe());
|
throw std::runtime_error(rcl_get_error_string().str);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -309,7 +309,7 @@ Executor::execute_subscription(
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"take_serialized failed for subscription on topic '%s': %s",
|
"take_serialized failed for subscription on topic '%s': %s",
|
||||||
subscription->get_topic_name(), rcl_get_error_string_safe());
|
subscription->get_topic_name(), rcl_get_error_string().str);
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
subscription->return_serialized_message(serialized_msg);
|
subscription->return_serialized_message(serialized_msg);
|
||||||
|
@ -324,7 +324,7 @@ Executor::execute_subscription(
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"could not deserialize serialized message on topic '%s': %s",
|
"could not deserialize serialized message on topic '%s': %s",
|
||||||
subscription->get_topic_name(), rcl_get_error_string_safe());
|
subscription->get_topic_name(), rcl_get_error_string().str);
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
subscription->return_message(message);
|
subscription->return_message(message);
|
||||||
|
@ -349,7 +349,7 @@ Executor::execute_intra_process_subscription(
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"take failed for intra process subscription on topic '%s': %s",
|
"take failed for intra process subscription on topic '%s': %s",
|
||||||
subscription->get_topic_name(), rcl_get_error_string_safe());
|
subscription->get_topic_name(), rcl_get_error_string().str);
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -377,7 +377,7 @@ Executor::execute_service(
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"take request failed for server of service '%s': %s",
|
"take request failed for server of service '%s': %s",
|
||||||
service->get_service_name(), rcl_get_error_string_safe());
|
service->get_service_name(), rcl_get_error_string().str);
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -398,7 +398,7 @@ Executor::execute_client(
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"take response failed for client of service '%s': %s",
|
"take response failed for client of service '%s': %s",
|
||||||
client->get_service_name(), rcl_get_error_string_safe());
|
client->get_service_name(), rcl_get_error_string().str);
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -433,7 +433,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
|
||||||
memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services());
|
memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services());
|
||||||
if (RCL_RET_OK != ret) {
|
if (RCL_RET_OK != ret) {
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string("Couldn't resize the wait set : ") + rcl_get_error_string_safe());
|
std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) {
|
if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) {
|
||||||
|
|
|
@ -51,14 +51,7 @@ rclcpp::expand_topic_or_service_name(
|
||||||
}
|
}
|
||||||
rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map);
|
rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map);
|
||||||
if (ret != RCL_RET_OK) {
|
if (ret != RCL_RET_OK) {
|
||||||
rcutils_error_state_t error_state;
|
const rcutils_error_state_t * error_state = rcl_get_error_state();
|
||||||
if (rcutils_error_state_copy(rcl_get_error_state(), &error_state) != RCUTILS_RET_OK) {
|
|
||||||
throw std::bad_alloc();
|
|
||||||
}
|
|
||||||
auto error_state_scope_exit = rclcpp::make_scope_exit(
|
|
||||||
[&error_state]() {
|
|
||||||
rcutils_error_state_fini(&error_state);
|
|
||||||
});
|
|
||||||
// finalize the string map before throwing
|
// finalize the string map before throwing
|
||||||
rcutils_ret = rcutils_string_map_fini(&substitutions_map);
|
rcutils_ret = rcutils_string_map_fini(&substitutions_map);
|
||||||
if (rcutils_ret != RCUTILS_RET_OK) {
|
if (rcutils_ret != RCUTILS_RET_OK) {
|
||||||
|
@ -66,10 +59,10 @@ rclcpp::expand_topic_or_service_name(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"failed to fini string_map (%d) during error handling: %s",
|
"failed to fini string_map (%d) during error handling: %s",
|
||||||
rcutils_ret,
|
rcutils_ret,
|
||||||
rcutils_get_error_string_safe());
|
rcutils_get_error_string().str);
|
||||||
rcutils_reset_error();
|
rcutils_reset_error();
|
||||||
}
|
}
|
||||||
throw_from_rcl_error(ret, "", &error_state);
|
throw_from_rcl_error(ret, "", error_state);
|
||||||
}
|
}
|
||||||
|
|
||||||
ret = rcl_expand_topic_name(
|
ret = rcl_expand_topic_name(
|
||||||
|
|
|
@ -54,7 +54,7 @@ NodeBase::NodeBase(
|
||||||
if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) {
|
if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) {
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"failed to destroy guard condition: %s", rcl_get_error_string_safe());
|
"failed to destroy guard condition: %s", rcl_get_error_string().str);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -122,7 +122,7 @@ NodeBase::NodeBase(
|
||||||
// Print message because exception will be thrown later in this code block
|
// Print message because exception will be thrown later in this code block
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"Failed to fini arguments during error handling: %s", rcl_get_error_string_safe());
|
"Failed to fini arguments during error handling: %s", rcl_get_error_string().str);
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -180,7 +180,7 @@ NodeBase::NodeBase(
|
||||||
if (rcl_node_fini(node) != RCL_RET_OK) {
|
if (rcl_node_fini(node) != RCL_RET_OK) {
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"Error in destruction of rcl node handle: %s", rcl_get_error_string_safe());
|
"Error in destruction of rcl node handle: %s", rcl_get_error_string().str);
|
||||||
}
|
}
|
||||||
delete node;
|
delete node;
|
||||||
});
|
});
|
||||||
|
@ -197,7 +197,7 @@ NodeBase::NodeBase(
|
||||||
// print message because throwing would prevent the destructor from being called
|
// print message because throwing would prevent the destructor from being called
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"Failed to fini arguments: %s", rcl_get_error_string_safe());
|
"Failed to fini arguments: %s", rcl_get_error_string().str);
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -211,7 +211,7 @@ NodeBase::~NodeBase()
|
||||||
if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) {
|
if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) {
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"failed to destroy guard condition: %s", rcl_get_error_string_safe());
|
"failed to destroy guard condition: %s", rcl_get_error_string().str);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -59,13 +59,13 @@ NodeGraph::get_topic_names_and_types(bool no_demangle) const
|
||||||
&topic_names_and_types);
|
&topic_names_and_types);
|
||||||
if (ret != RCL_RET_OK) {
|
if (ret != RCL_RET_OK) {
|
||||||
auto error_msg = std::string("failed to get topic names and types: ") +
|
auto error_msg = std::string("failed to get topic names and types: ") +
|
||||||
rcl_get_error_string_safe();
|
rcl_get_error_string().str;
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) {
|
if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) {
|
||||||
error_msg += std::string(", failed also to cleanup topic names and types, leaking memory: ") +
|
error_msg += std::string(", failed also to cleanup topic names and types, leaking memory: ") +
|
||||||
rcl_get_error_string_safe();
|
rcl_get_error_string().str;
|
||||||
}
|
}
|
||||||
throw std::runtime_error(error_msg + rcl_get_error_string_safe());
|
throw std::runtime_error(error_msg + rcl_get_error_string().str);
|
||||||
}
|
}
|
||||||
|
|
||||||
std::map<std::string, std::vector<std::string>> topics_and_types;
|
std::map<std::string, std::vector<std::string>> topics_and_types;
|
||||||
|
@ -80,7 +80,7 @@ NodeGraph::get_topic_names_and_types(bool no_demangle) const
|
||||||
if (ret != RCL_RET_OK) {
|
if (ret != RCL_RET_OK) {
|
||||||
// *INDENT-OFF*
|
// *INDENT-OFF*
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string("could not destroy topic names and types: ") + rcl_get_error_string_safe());
|
std::string("could not destroy topic names and types: ") + rcl_get_error_string().str);
|
||||||
// *INDENT-ON*
|
// *INDENT-ON*
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -99,14 +99,14 @@ NodeGraph::get_service_names_and_types() const
|
||||||
&service_names_and_types);
|
&service_names_and_types);
|
||||||
if (ret != RCL_RET_OK) {
|
if (ret != RCL_RET_OK) {
|
||||||
auto error_msg = std::string("failed to get service names and types: ") +
|
auto error_msg = std::string("failed to get service names and types: ") +
|
||||||
rcl_get_error_string_safe();
|
rcl_get_error_string().str;
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
if (rcl_names_and_types_fini(&service_names_and_types) != RCL_RET_OK) {
|
if (rcl_names_and_types_fini(&service_names_and_types) != RCL_RET_OK) {
|
||||||
error_msg +=
|
error_msg +=
|
||||||
std::string(", failed also to cleanup service names and types, leaking memory: ") +
|
std::string(", failed also to cleanup service names and types, leaking memory: ") +
|
||||||
rcl_get_error_string_safe();
|
rcl_get_error_string().str;
|
||||||
}
|
}
|
||||||
throw std::runtime_error(error_msg + rcl_get_error_string_safe());
|
throw std::runtime_error(error_msg + rcl_get_error_string().str);
|
||||||
}
|
}
|
||||||
|
|
||||||
std::map<std::string, std::vector<std::string>> services_and_types;
|
std::map<std::string, std::vector<std::string>> services_and_types;
|
||||||
|
@ -121,7 +121,7 @@ NodeGraph::get_service_names_and_types() const
|
||||||
if (ret != RCL_RET_OK) {
|
if (ret != RCL_RET_OK) {
|
||||||
// *INDENT-OFF*
|
// *INDENT-OFF*
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string("could not destroy service names and types: ") + rcl_get_error_string_safe());
|
std::string("could not destroy service names and types: ") + rcl_get_error_string().str);
|
||||||
// *INDENT-ON*
|
// *INDENT-ON*
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -155,16 +155,16 @@ NodeGraph::get_node_names_and_namespaces() const
|
||||||
&node_names_c,
|
&node_names_c,
|
||||||
&node_namespaces_c);
|
&node_namespaces_c);
|
||||||
if (ret != RCL_RET_OK) {
|
if (ret != RCL_RET_OK) {
|
||||||
auto error_msg = std::string("failed to get node names: ") + rcl_get_error_string_safe();
|
auto error_msg = std::string("failed to get node names: ") + rcl_get_error_string().str;
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
if (rcutils_string_array_fini(&node_names_c) != RCUTILS_RET_OK) {
|
if (rcutils_string_array_fini(&node_names_c) != RCUTILS_RET_OK) {
|
||||||
error_msg += std::string(", failed also to cleanup node names, leaking memory: ") +
|
error_msg += std::string(", failed also to cleanup node names, leaking memory: ") +
|
||||||
rcl_get_error_string_safe();
|
rcl_get_error_string().str;
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
if (rcutils_string_array_fini(&node_namespaces_c) != RCUTILS_RET_OK) {
|
if (rcutils_string_array_fini(&node_namespaces_c) != RCUTILS_RET_OK) {
|
||||||
error_msg += std::string(", failed also to cleanup node namespaces, leaking memory: ") +
|
error_msg += std::string(", failed also to cleanup node namespaces, leaking memory: ") +
|
||||||
rcl_get_error_string_safe();
|
rcl_get_error_string().str;
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
// TODO(karsten1987): Append rcutils_error_message once it's in master
|
// TODO(karsten1987): Append rcutils_error_message once it's in master
|
||||||
|
@ -217,7 +217,7 @@ NodeGraph::count_publishers(const std::string & topic_name) const
|
||||||
if (ret != RMW_RET_OK) {
|
if (ret != RMW_RET_OK) {
|
||||||
// *INDENT-OFF*
|
// *INDENT-OFF*
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string("could not count publishers: ") + rmw_get_error_string_safe());
|
std::string("could not count publishers: ") + rmw_get_error_string().str);
|
||||||
// *INDENT-ON*
|
// *INDENT-ON*
|
||||||
}
|
}
|
||||||
return count;
|
return count;
|
||||||
|
@ -239,7 +239,7 @@ NodeGraph::count_subscribers(const std::string & topic_name) const
|
||||||
if (ret != RMW_RET_OK) {
|
if (ret != RMW_RET_OK) {
|
||||||
// *INDENT-OFF*
|
// *INDENT-OFF*
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string("could not count subscribers: ") + rmw_get_error_string_safe());
|
std::string("could not count subscribers: ") + rmw_get_error_string().str);
|
||||||
// *INDENT-ON*
|
// *INDENT-ON*
|
||||||
}
|
}
|
||||||
return count;
|
return count;
|
||||||
|
|
|
@ -123,7 +123,7 @@ NodeParameters::NodeParameters(
|
||||||
if (!rcl_parse_yaml_file(yaml_path.c_str(), yaml_params)) {
|
if (!rcl_parse_yaml_file(yaml_path.c_str(), yaml_params)) {
|
||||||
std::ostringstream ss;
|
std::ostringstream ss;
|
||||||
ss << "Failed to parse parameters from file '" << yaml_path << "': " <<
|
ss << "Failed to parse parameters from file '" << yaml_path << "': " <<
|
||||||
rcl_get_error_string_safe();
|
rcl_get_error_string().str;
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
throw std::runtime_error(ss.str());
|
throw std::runtime_error(ss.str());
|
||||||
}
|
}
|
||||||
|
|
|
@ -45,8 +45,8 @@ NodeServices::add_service(
|
||||||
auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
|
auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
|
||||||
if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
|
if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string(
|
std::string("Failed to notify wait set on service creation: ") +
|
||||||
"Failed to notify wait set on service creation: ") + rmw_get_error_string()
|
rmw_get_error_string().str
|
||||||
);
|
);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -72,7 +72,8 @@ NodeServices::add_client(
|
||||||
auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
|
auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
|
||||||
if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
|
if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string("Failed to notify wait set on client creation: ") + rmw_get_error_string()
|
std::string("Failed to notify wait set on client creation: ") +
|
||||||
|
rmw_get_error_string().str
|
||||||
);
|
);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -41,7 +41,7 @@ NodeTimers::add_timer(
|
||||||
}
|
}
|
||||||
if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
|
if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string(
|
std::string("Failed to notify wait set on timer creation: ") +
|
||||||
"Failed to notify wait set on timer creation: ") + rmw_get_error_string());
|
rmw_get_error_string().str);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -73,8 +73,8 @@ NodeTopics::add_publisher(
|
||||||
auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
|
auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
|
||||||
if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
|
if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string(
|
std::string("Failed to notify wait set on publisher creation: ") +
|
||||||
"Failed to notify wait set on publisher creation: ") + rmw_get_error_string());
|
rmw_get_error_string().str);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -123,8 +123,8 @@ NodeTopics::add_subscription(
|
||||||
auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
|
auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
|
||||||
if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
|
if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string(
|
std::string("Failed to notify wait set on subscription creation: ") +
|
||||||
"Failed to notify wait set on subscription creation: ") + rmw_get_error_string()
|
rmw_get_error_string().str
|
||||||
);
|
);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -66,12 +66,12 @@ PublisherBase::PublisherBase(
|
||||||
// Life time of this object is tied to the publisher handle.
|
// Life time of this object is tied to the publisher handle.
|
||||||
rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&publisher_handle_);
|
rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&publisher_handle_);
|
||||||
if (!publisher_rmw_handle) {
|
if (!publisher_rmw_handle) {
|
||||||
auto msg = std::string("failed to get rmw handle: ") + rcl_get_error_string_safe();
|
auto msg = std::string("failed to get rmw handle: ") + rcl_get_error_string().str;
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
throw std::runtime_error(msg);
|
throw std::runtime_error(msg);
|
||||||
}
|
}
|
||||||
if (rmw_get_gid_for_publisher(publisher_rmw_handle, &rmw_gid_) != RMW_RET_OK) {
|
if (rmw_get_gid_for_publisher(publisher_rmw_handle, &rmw_gid_) != RMW_RET_OK) {
|
||||||
auto msg = std::string("failed to get publisher gid: ") + rmw_get_error_string_safe();
|
auto msg = std::string("failed to get publisher gid: ") + rmw_get_error_string().str;
|
||||||
rmw_reset_error();
|
rmw_reset_error();
|
||||||
throw std::runtime_error(msg);
|
throw std::runtime_error(msg);
|
||||||
}
|
}
|
||||||
|
@ -83,7 +83,7 @@ PublisherBase::~PublisherBase()
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"Error in destruction of intra process rcl publisher handle: %s",
|
"Error in destruction of intra process rcl publisher handle: %s",
|
||||||
rcl_get_error_string_safe());
|
rcl_get_error_string().str);
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -91,7 +91,7 @@ PublisherBase::~PublisherBase()
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"Error in destruction of rcl publisher handle: %s",
|
"Error in destruction of rcl publisher handle: %s",
|
||||||
rcl_get_error_string_safe());
|
rcl_get_error_string().str);
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -107,7 +107,7 @@ PublisherBase::get_queue_size() const
|
||||||
{
|
{
|
||||||
const rcl_publisher_options_t * publisher_options = rcl_publisher_get_options(&publisher_handle_);
|
const rcl_publisher_options_t * publisher_options = rcl_publisher_get_options(&publisher_handle_);
|
||||||
if (!publisher_options) {
|
if (!publisher_options) {
|
||||||
auto msg = std::string("failed to get publisher options: ") + rcl_get_error_string_safe();
|
auto msg = std::string("failed to get publisher options: ") + rcl_get_error_string().str;
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
throw std::runtime_error(msg);
|
throw std::runtime_error(msg);
|
||||||
}
|
}
|
||||||
|
@ -150,14 +150,14 @@ PublisherBase::operator==(const rmw_gid_t * gid) const
|
||||||
bool result = false;
|
bool result = false;
|
||||||
auto ret = rmw_compare_gids_equal(gid, &this->get_gid(), &result);
|
auto ret = rmw_compare_gids_equal(gid, &this->get_gid(), &result);
|
||||||
if (ret != RMW_RET_OK) {
|
if (ret != RMW_RET_OK) {
|
||||||
auto msg = std::string("failed to compare gids: ") + rmw_get_error_string_safe();
|
auto msg = std::string("failed to compare gids: ") + rmw_get_error_string().str;
|
||||||
rmw_reset_error();
|
rmw_reset_error();
|
||||||
throw std::runtime_error(msg);
|
throw std::runtime_error(msg);
|
||||||
}
|
}
|
||||||
if (!result) {
|
if (!result) {
|
||||||
ret = rmw_compare_gids_equal(gid, &this->get_intra_process_gid(), &result);
|
ret = rmw_compare_gids_equal(gid, &this->get_intra_process_gid(), &result);
|
||||||
if (ret != RMW_RET_OK) {
|
if (ret != RMW_RET_OK) {
|
||||||
auto msg = std::string("failed to compare gids: ") + rmw_get_error_string_safe();
|
auto msg = std::string("failed to compare gids: ") + rmw_get_error_string().str;
|
||||||
rmw_reset_error();
|
rmw_reset_error();
|
||||||
throw std::runtime_error(msg);
|
throw std::runtime_error(msg);
|
||||||
}
|
}
|
||||||
|
@ -204,7 +204,7 @@ PublisherBase::setup_intra_process(
|
||||||
rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(
|
rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(
|
||||||
&intra_process_publisher_handle_);
|
&intra_process_publisher_handle_);
|
||||||
if (publisher_rmw_handle == nullptr) {
|
if (publisher_rmw_handle == nullptr) {
|
||||||
auto msg = std::string("Failed to get rmw publisher handle") + rcl_get_error_string_safe();
|
auto msg = std::string("Failed to get rmw publisher handle") + rcl_get_error_string().str;
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
throw std::runtime_error(msg);
|
throw std::runtime_error(msg);
|
||||||
}
|
}
|
||||||
|
@ -212,7 +212,7 @@ PublisherBase::setup_intra_process(
|
||||||
publisher_rmw_handle, &intra_process_rmw_gid_);
|
publisher_rmw_handle, &intra_process_rmw_gid_);
|
||||||
if (rmw_ret != RMW_RET_OK) {
|
if (rmw_ret != RMW_RET_OK) {
|
||||||
auto msg =
|
auto msg =
|
||||||
std::string("failed to create intra process publisher gid: ") + rmw_get_error_string_safe();
|
std::string("failed to create intra process publisher gid: ") + rmw_get_error_string().str;
|
||||||
rmw_reset_error();
|
rmw_reset_error();
|
||||||
throw std::runtime_error(msg);
|
throw std::runtime_error(msg);
|
||||||
}
|
}
|
||||||
|
|
|
@ -43,7 +43,7 @@ SubscriptionBase::SubscriptionBase(
|
||||||
RCLCPP_ERROR(
|
RCLCPP_ERROR(
|
||||||
rclcpp::get_logger(rcl_node_get_logger_name(node_handle.get())).get_child("rclcpp"),
|
rclcpp::get_logger(rcl_node_get_logger_name(node_handle.get())).get_child("rclcpp"),
|
||||||
"Error in destruction of rcl subscription handle: %s",
|
"Error in destruction of rcl subscription handle: %s",
|
||||||
rcl_get_error_string_safe());
|
rcl_get_error_string().str);
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
delete rcl_subs;
|
delete rcl_subs;
|
||||||
|
|
|
@ -32,7 +32,7 @@ TimerBase::TimerBase(rclcpp::Clock::SharedPtr clock, std::chrono::nanoseconds pe
|
||||||
if (rcl_timer_fini(timer) != RCL_RET_OK) {
|
if (rcl_timer_fini(timer) != RCL_RET_OK) {
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"Failed to clean up rcl timer handle: %s", rcl_get_error_string_safe());
|
"Failed to clean up rcl timer handle: %s", rcl_get_error_string().str);
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
delete timer;
|
delete timer;
|
||||||
|
@ -49,7 +49,7 @@ TimerBase::TimerBase(rclcpp::Clock::SharedPtr clock, std::chrono::nanoseconds pe
|
||||||
{
|
{
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string_safe());
|
"Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string().str);
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -61,7 +61,7 @@ void
|
||||||
TimerBase::cancel()
|
TimerBase::cancel()
|
||||||
{
|
{
|
||||||
if (rcl_timer_cancel(timer_handle_.get()) != RCL_RET_OK) {
|
if (rcl_timer_cancel(timer_handle_.get()) != RCL_RET_OK) {
|
||||||
throw std::runtime_error(std::string("Couldn't cancel timer: ") + rcl_get_error_string_safe());
|
throw std::runtime_error(std::string("Couldn't cancel timer: ") + rcl_get_error_string().str);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -69,7 +69,7 @@ void
|
||||||
TimerBase::reset()
|
TimerBase::reset()
|
||||||
{
|
{
|
||||||
if (rcl_timer_reset(timer_handle_.get()) != RCL_RET_OK) {
|
if (rcl_timer_reset(timer_handle_.get()) != RCL_RET_OK) {
|
||||||
throw std::runtime_error(std::string("Couldn't reset timer: ") + rcl_get_error_string_safe());
|
throw std::runtime_error(std::string("Couldn't reset timer: ") + rcl_get_error_string().str);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -78,7 +78,7 @@ TimerBase::is_ready()
|
||||||
{
|
{
|
||||||
bool ready = false;
|
bool ready = false;
|
||||||
if (rcl_timer_is_ready(timer_handle_.get(), &ready) != RCL_RET_OK) {
|
if (rcl_timer_is_ready(timer_handle_.get(), &ready) != RCL_RET_OK) {
|
||||||
throw std::runtime_error(std::string("Failed to check timer: ") + rcl_get_error_string_safe());
|
throw std::runtime_error(std::string("Failed to check timer: ") + rcl_get_error_string().str);
|
||||||
}
|
}
|
||||||
return ready;
|
return ready;
|
||||||
}
|
}
|
||||||
|
@ -92,7 +92,7 @@ TimerBase::time_until_trigger()
|
||||||
{
|
{
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string("Timer could not get time until next call: ") +
|
std::string("Timer could not get time until next call: ") +
|
||||||
rcl_get_error_string_safe());
|
rcl_get_error_string().str);
|
||||||
}
|
}
|
||||||
return std::chrono::nanoseconds(time_until_next_call);
|
return std::chrono::nanoseconds(time_until_next_call);
|
||||||
}
|
}
|
||||||
|
|
|
@ -120,7 +120,7 @@ trigger_interrupt_guard_condition(int signal_value)
|
||||||
if (status != RCL_RET_OK) {
|
if (status != RCL_RET_OK) {
|
||||||
RCUTILS_LOG_ERROR_NAMED(
|
RCUTILS_LOG_ERROR_NAMED(
|
||||||
"rclcpp",
|
"rclcpp",
|
||||||
"failed to trigger guard condition: %s", rcl_get_error_string_safe());
|
"failed to trigger guard condition: %s", rcl_get_error_string().str);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -167,7 +167,7 @@ rclcpp::init(int argc, char const * const argv[])
|
||||||
g_is_interrupted.store(false);
|
g_is_interrupted.store(false);
|
||||||
if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) {
|
if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) {
|
||||||
std::string msg = "failed to initialize rmw implementation: ";
|
std::string msg = "failed to initialize rmw implementation: ";
|
||||||
msg += rcl_get_error_string_safe();
|
msg += rcl_get_error_string().str;
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
throw std::runtime_error(msg);
|
throw std::runtime_error(msg);
|
||||||
}
|
}
|
||||||
|
@ -233,7 +233,7 @@ rclcpp::remove_ros_arguments(int argc, char const * const argv[])
|
||||||
if (RCL_RET_OK != rcl_arguments_fini(&parsed_args)) {
|
if (RCL_RET_OK != rcl_arguments_fini(&parsed_args)) {
|
||||||
base_exc.formatted_message += std::string(
|
base_exc.formatted_message += std::string(
|
||||||
", failed also to cleanup parsed arguments, leaking memory: ") +
|
", failed also to cleanup parsed arguments, leaking memory: ") +
|
||||||
rcl_get_error_string_safe();
|
rcl_get_error_string().str;
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
}
|
}
|
||||||
throw exceptions::RCLError(base_exc, "");
|
throw exceptions::RCLError(base_exc, "");
|
||||||
|
@ -307,7 +307,7 @@ rclcpp::get_sigint_guard_condition(rcl_wait_set_t * wait_set)
|
||||||
if (rcl_guard_condition_init(&handle, options) != RCL_RET_OK) {
|
if (rcl_guard_condition_init(&handle, options) != RCL_RET_OK) {
|
||||||
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
|
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
|
||||||
throw std::runtime_error(std::string(
|
throw std::runtime_error(std::string(
|
||||||
"Couldn't initialize guard condition: ") + rcl_get_error_string_safe());
|
"Couldn't initialize guard condition: ") + rcl_get_error_string().str);
|
||||||
// *INDENT-ON*
|
// *INDENT-ON*
|
||||||
}
|
}
|
||||||
g_sigint_guard_cond_handles[wait_set] = handle;
|
g_sigint_guard_cond_handles[wait_set] = handle;
|
||||||
|
@ -325,7 +325,7 @@ rclcpp::release_sigint_guard_condition(rcl_wait_set_t * wait_set)
|
||||||
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
|
// *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
|
||||||
throw std::runtime_error(std::string(
|
throw std::runtime_error(std::string(
|
||||||
"Failed to destroy sigint guard condition: ") +
|
"Failed to destroy sigint guard condition: ") +
|
||||||
rcl_get_error_string_safe());
|
rcl_get_error_string().str);
|
||||||
// *INDENT-ON*
|
// *INDENT-ON*
|
||||||
}
|
}
|
||||||
g_sigint_guard_cond_handles.erase(kv);
|
g_sigint_guard_cond_handles.erase(kv);
|
||||||
|
|
|
@ -356,7 +356,7 @@ public:
|
||||||
{
|
{
|
||||||
if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
|
if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
|
||||||
RCUTILS_LOG_ERROR("Unable to change state for state machine for %s: %s",
|
RCUTILS_LOG_ERROR("Unable to change state for state machine for %s: %s",
|
||||||
node_base_interface_->get_name(), rcl_get_error_string_safe());
|
node_base_interface_->get_name(), rcl_get_error_string().str);
|
||||||
return RCL_RET_ERROR;
|
return RCL_RET_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -368,7 +368,7 @@ public:
|
||||||
&state_machine_, transition_id, publish_update) != RCL_RET_OK)
|
&state_machine_, transition_id, publish_update) != RCL_RET_OK)
|
||||||
{
|
{
|
||||||
RCUTILS_LOG_ERROR("Unable to start transition %u from current state %s: %s",
|
RCUTILS_LOG_ERROR("Unable to start transition %u from current state %s: %s",
|
||||||
transition_id, state_machine_.current_state->label, rcl_get_error_string_safe());
|
transition_id, state_machine_.current_state->label, rcl_get_error_string().str);
|
||||||
return RCL_RET_ERROR;
|
return RCL_RET_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue