{

RCLCPP_SMART_PTR_DEFINITIONS(WeakContextsWrapper)

void

add_context(const Context::SharedPtr & context)

{

std::lock_guard<std::mutex> guard(mutex_);

weak_contexts_.push_back(context);

}

void

remove_context(const Context * context)

{

std::lock_guard<std::mutex> guard(mutex_);

weak_contexts_.erase(

std::remove_if(

weak_contexts_.begin(),

weak_contexts_.end(),

[context](const Context::WeakPtr weak_context) {

auto locked_context = weak_context.lock();

if (!locked_context) {

// take advantage and removed expired contexts

return true;

}

return locked_context.get() == context;

}

),

weak_contexts_.end());

}

std::vector<Context::SharedPtr>

get_contexts()

{

std::lock_guard<std::mutex> lock(mutex_);

std::vector<Context::SharedPtr> shared_contexts;

for (auto it = weak_contexts_.begin(); it != weak_contexts_.end(); /* noop */) {

auto context_ptr = it->lock();

if (!context_ptr) {

// remove invalid weak context pointers

it = weak_contexts_.erase(it);

} else {

++it;

shared_contexts.push_back(context_ptr);

}

}

return shared_contexts;

}

std::vector<std::weak_ptr<rclcpp::Context>> weak_contexts_;

std::mutex mutex_;

{

static WeakContextsWrapper::SharedPtr weak_contexts = WeakContextsWrapper::make_shared();

if (!weak_contexts) {

throw std::runtime_error("weak contexts vector is not valid");

}

return weak_contexts;

const rcutils_log_location_t * location,

int severity, const char * name, rcutils_time_point_value_t timestamp,

const char * format, va_list * args)

{

try {

std::shared_ptr<std::recursive_mutex> logging_mutex;

logging_mutex = get_global_logging_mutex();

std::lock_guard<std::recursive_mutex> guard(*logging_mutex);

return rcl_logging_multiple_output_handler(

location, severity, name, timestamp, format, args);

} catch (std::exception & ex) {

RCUTILS_SAFE_FWRITE_TO_STDERR(ex.what());

RCUTILS_SAFE_FWRITE_TO_STDERR("\n");

} catch (...) {

RCUTILS_SAFE_FWRITE_TO_STDERR("failed to take global rclcpp logging mutex\n");

}

: rcl_context_(nullptr),

shutdown_reason_(""),

logging_mutex_(nullptr)

{

if (context) {

if (rcl_context_is_valid(context)) {

RCLCPP_ERROR(

rclcpp::get_logger("rclcpp"), "rcl context unexpectedly not shutdown during cleanup");

} else {

// if context pointer is not null and is shutdown, then it's ready for fini

rcl_ret_t ret = rcl_context_fini(context);

if (RCL_RET_OK != ret) {

RCLCPP_ERROR(

rclcpp::get_logger("rclcpp"),

"failed to finalize context: %s", rcl_get_error_string().str);

rcl_reset_error();

}

}

delete context;

}

int argc,

char const * const * argv,

const rclcpp::InitOptions & init_options)

{

std::lock_guard<std::recursive_mutex> init_lock(init_mutex_);

if (this->is_valid()) {

throw rclcpp::ContextAlreadyInitialized();

}

this->clean_up();

rcl_context_t * context = new rcl_context_t;

if (!context) {

throw std::runtime_error("failed to allocate memory for rcl context");

}

*context = rcl_get_zero_initialized_context();

rcl_ret_t ret = rcl_init(argc, argv, init_options.get_rcl_init_options(), context);

if (RCL_RET_OK != ret) {

delete context;

rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl");

}

rcl_context_.reset(context, __delete_context);

try {

if (init_options.auto_initialize_logging()) {

logging_mutex_ = get_global_logging_mutex();

std::lock_guard<std::recursive_mutex> guard(*logging_mutex_);

size_t & count = get_logging_reference_count();

if (0u == count) {

ret = rcl_logging_configure_with_output_handler(

&rcl_context_->global_arguments,

rcl_init_options_get_allocator(init_options.get_rcl_init_options()),

rclcpp_logging_output_handler);

if (RCL_RET_OK != ret) {

rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure logging");

}

} else {

RCLCPP_WARN(

rclcpp::get_logger("rclcpp"),

"logging was initialized more than once");

}

++count;

}

std::vector<std::string> unparsed_ros_arguments = detail::get_unparsed_ros_arguments(

argc, argv, &(rcl_context_->global_arguments), rcl_get_default_allocator());

if (!unparsed_ros_arguments.empty()) {

throw exceptions::UnknownROSArgsError(std::move(unparsed_ros_arguments));

}

init_options_ = init_options;

weak_contexts_ = get_weak_contexts();

weak_contexts_->add_context(this->shared_from_this());

} catch (const std::exception & e) {

ret = rcl_shutdown(rcl_context_.get());

rcl_context_.reset();

if (RCL_RET_OK != ret) {

std::ostringstream oss;

oss << "While handling: " << e.what() << std::endl <<

" another exception was thrown";

rclcpp::exceptions::throw_from_rcl_error(ret, oss.str());

}

throw;

}

{

// Take a local copy of the shared pointer to avoid it getting nulled under our feet.

auto local_rcl_context = rcl_context_;

if (!local_rcl_context) {

return false;

}

return rcl_context_is_valid(local_rcl_context.get());

{

size_t domain_id;

rcl_ret_t ret = rcl_context_get_domain_id(rcl_context_.get(), &domain_id);

if (RCL_RET_OK != ret) {

rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get domain id from context");

}

return domain_id;

{

std::lock_guard<std::recursive_mutex> lock(init_mutex_);

return shutdown_reason_;

{

// prevent races

std::lock_guard<std::recursive_mutex> init_lock(init_mutex_);

// ensure validity

if (!this->is_valid()) {

// if it is not valid, then it cannot be shutdown

return false;

}

// call each pre-shutdown callback

{

std::lock_guard<std::mutex> lock{pre_shutdown_callbacks_mutex_};

for (const auto & callback : pre_shutdown_callbacks_) {

(*callback)();

}

}

// rcl shutdown

rcl_ret_t ret = rcl_shutdown(rcl_context_.get());

if (RCL_RET_OK != ret) {

rclcpp::exceptions::throw_from_rcl_error(ret);

}

// set shutdown reason

shutdown_reason_ = reason;

// call each shutdown callback

{

std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);

for (const auto & callback : on_shutdown_callbacks_) {

(*callback)();

}

}

// interrupt all blocking sleep_for() and all blocking executors or wait sets

this->interrupt_all_sleep_for();

// remove self from the global contexts

weak_contexts_->remove_context(this);

// shutdown logger

if (logging_mutex_) {

// logging was initialized by this context

std::lock_guard<std::recursive_mutex> guard(*logging_mutex_);

size_t & count = get_logging_reference_count();

if (0u == --count) {

rcl_ret_t rcl_ret = rcl_logging_fini();

if (RCL_RET_OK != rcl_ret) {

RCUTILS_SAFE_FWRITE_TO_STDERR(

RCUTILS_STRINGIFY(__file__) ":"

RCUTILS_STRINGIFY(__LINE__)

" failed to fini logging");

rcl_reset_error();

}

}

}

return true;

{

add_on_shutdown_callback(callback);

return callback;

const PreShutdownCallbackHandle & callback_handle)

{

return remove_shutdown_callback<ShutdownType::pre_shutdown>(callback_handle);

ShutdownCallback callback)

{

auto callback_shared_ptr =

std::make_shared<ShutdownCallbackHandle::ShutdownCallbackType>(callback);

static_assert(

shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown);

if constexpr (shutdown_type == ShutdownType::pre_shutdown) {

std::lock_guard<std::mutex> lock(pre_shutdown_callbacks_mutex_);

pre_shutdown_callbacks_.emplace_back(callback_shared_ptr);

} else {

std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);

on_shutdown_callbacks_.emplace_back(callback_shared_ptr);

}

ShutdownCallbackHandle callback_handle;

callback_handle.callback = callback_shared_ptr;

return callback_handle;

const ShutdownCallbackHandle & callback_handle)

{

const auto callback_shared_ptr = callback_handle.callback.lock();

if (callback_shared_ptr == nullptr) {

return false;

}

const auto remove_callback = [&callback_shared_ptr](auto & mutex, auto & callback_vector) {

const std::lock_guard<std::mutex> lock(mutex);

auto iter = callback_vector.begin();

for (; iter != callback_vector.end(); iter++) {

if ((*iter).get() == callback_shared_ptr.get()) {

break;

}

}

if (iter == callback_vector.end()) {

return false;

}

callback_vector.erase(iter);

return true;

};

static_assert(

shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown);

if constexpr (shutdown_type == ShutdownType::pre_shutdown) {

return remove_callback(pre_shutdown_callbacks_mutex_, pre_shutdown_callbacks_);

} else {

return remove_callback(on_shutdown_callbacks_mutex_, on_shutdown_callbacks_);

}

{

const auto get_callback_vector = [](auto & mutex, auto & callback_set) {

const std::lock_guard<std::mutex> lock(mutex);

std::vector<rclcpp::Context::ShutdownCallback> callbacks;

for (auto & callback : callback_set) {

callbacks.push_back(*callback);

}

return callbacks;

};

static_assert(

shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown);

if constexpr (shutdown_type == ShutdownType::pre_shutdown) {

return get_callback_vector(pre_shutdown_callbacks_mutex_, pre_shutdown_callbacks_);

} else {

return get_callback_vector(on_shutdown_callbacks_mutex_, on_shutdown_callbacks_);

}

{

std::chrono::nanoseconds time_left = nanoseconds;

do {

{

std::unique_lock<std::mutex> lock(interrupt_mutex_);

auto start = std::chrono::steady_clock::now();

// this will release the lock while waiting

interrupt_condition_variable_.wait_for(lock, time_left);

time_left -= std::chrono::steady_clock::now() - start;

}

} while (time_left > std::chrono::nanoseconds::zero() && this->is_valid());

// Return true if the timeout elapsed successfully, otherwise false.

return this->is_valid();

{

shutdown_reason_ = "";

rcl_context_.reset();

sub_contexts_.clear();

{

WeakContextsWrapper::SharedPtr weak_contexts = get_weak_contexts();

return weak_contexts->get_contexts();