diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index cf395323..605ad4e3 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -838,7 +838,8 @@ rmw_ret_t GraphCache::publisher_count_matched_subscriptions( *subscription_count = 0; GraphNode::TopicMap::const_iterator topic_it = graph_topics_.find(publisher->topic_name); if (topic_it != graph_topics_.end()) { - rmw_publisher_data_t * pub_data = static_cast(publisher->data); + rmw_zenoh_cpp::rmw_publisher_data_t * pub_data = + static_cast(publisher->data); GraphNode::TopicTypeMap::const_iterator topic_data_it = topic_it->second.find( pub_data->type_support->get_name()); if (topic_data_it != topic_it->second.end()) { @@ -873,7 +874,8 @@ rmw_ret_t GraphCache::subscription_count_matched_publishers( *publisher_count = 0; GraphNode::TopicMap::const_iterator topic_it = graph_topics_.find(subscription->topic_name); if (topic_it != graph_topics_.end()) { - rmw_subscription_data_t * sub_data = static_cast(subscription->data); + rmw_zenoh_cpp::rmw_subscription_data_t * sub_data = + static_cast(subscription->data); GraphNode::TopicTypeMap::const_iterator topic_data_it = topic_it->second.find( sub_data->type_support->get_name()); if (topic_data_it != topic_it->second.end()) { diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp index f1bf8cae..dce43cd5 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp @@ -318,12 +318,12 @@ std::shared_ptr Entity::make( return std::make_shared( Entity{ - zid_to_str(zid), - nid, - id, - std::move(type), - std::move(node_info), - std::move(topic_info)}); + zid_to_str(zid), + nid, + id, + std::move(type), + std::move(node_info), + std::move(topic_info)}); } ///============================================================================= @@ -399,12 +399,12 @@ std::shared_ptr Entity::make(const std::string & keyexpr) return std::make_shared( Entity{ - std::move(zid), - std::move(nid), - std::move(id), - std::move(entity_type), - NodeInfo{std::move(domain_id), std::move(ns), std::move(node_name), ""}, - std::move(topic_info)}); + std::move(zid), + std::move(nid), + std::move(id), + std::move(entity_type), + NodeInfo{std::move(domain_id), std::move(ns), std::move(node_name), ""}, + std::move(topic_info)}); } ///============================================================================= diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index 115f131c..895124b3 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -31,14 +31,14 @@ #include "attachment_helpers.hpp" #include "rmw_data_types.hpp" -namespace rmw_zenoh_cpp -{ ///============================================================================= size_t rmw_context_impl_s::get_next_entity_id() { return next_entity_id_++; } +namespace rmw_zenoh_cpp +{ ///============================================================================= saved_msg_data::saved_msg_data( zc_owned_payload_t p, @@ -104,7 +104,7 @@ std::unique_ptr rmw_subscription_data_t::pop_next_message() return nullptr; } - std::unique_ptr msg_data = std::move(message_queue_.front()); + std::unique_ptr msg_data = std::move(message_queue_.front()); message_queue_.pop_front(); return msg_data; @@ -291,7 +291,7 @@ void rmw_client_data_t::notify() } ///============================================================================= -void rmw_client_data_t::add_new_reply(std::unique_ptr reply) +void rmw_client_data_t::add_new_reply(std::unique_ptr reply) { std::lock_guard lock(reply_queue_mutex_); if (reply_queue_.size() >= adapted_qos_profile.depth) { @@ -336,7 +336,7 @@ void rmw_client_data_t::detach_condition() } ///============================================================================= -std::unique_ptr rmw_client_data_t::pop_next_reply() +std::unique_ptr rmw_client_data_t::pop_next_reply() { std::lock_guard lock(reply_queue_mutex_); @@ -344,7 +344,7 @@ std::unique_ptr rmw_client_data_t::pop_next_reply() return nullptr; } - std::unique_ptr latest_reply = std::move(reply_queue_.front()); + std::unique_ptr latest_reply = std::move(reply_queue_.front()); reply_queue_.pop_front(); return latest_reply; @@ -361,7 +361,7 @@ void sub_data_handler( z_drop(z_move(keystr)); }); - auto sub_data = static_cast(data); + auto sub_data = static_cast(data); if (sub_data == nullptr) { RCUTILS_LOG_ERROR_NAMED( "rmw_zenoh_cpp", @@ -431,7 +431,8 @@ void service_data_handler(const z_query_t * query, void * data) z_drop(z_move(keystr)); }); - rmw_service_data_t * service_data = static_cast(data); + rmw_zenoh_cpp::rmw_service_data_t * service_data = + static_cast(data); if (service_data == nullptr) { RCUTILS_LOG_ERROR_NAMED( "rmw_zenoh_cpp", @@ -477,7 +478,7 @@ size_t rmw_client_data_t::get_next_sequence_number() //============================================================================== void client_data_handler(z_owned_reply_t * reply, void * data) { - auto client_data = static_cast(data); + auto client_data = static_cast(data); if (client_data == nullptr) { RCUTILS_LOG_ERROR_NAMED( "rmw_zenoh_cpp", @@ -506,7 +507,7 @@ void client_data_handler(z_owned_reply_t * reply, void * data) return; } - client_data->add_new_reply(std::make_unique(reply)); + client_data->add_new_reply(std::make_unique(reply)); // Since we took ownership of the reply, null it out here *reply = z_reply_null(); } diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index a4ce40de..285c00a9 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -38,8 +38,7 @@ #include "service_type_support.hpp" /// Structs for various type erased data fields. -namespace rmw_zenoh_cpp -{ + ///============================================================================= class rmw_context_impl_s final { @@ -60,7 +59,7 @@ class rmw_context_impl_s final /// Guard condition that should be triggered when the graph changes. rmw_guard_condition_t * graph_guard_condition; - std::unique_ptr graph_cache; + std::unique_ptr graph_cache; size_t get_next_entity_id(); @@ -69,6 +68,8 @@ class rmw_context_impl_s final size_t next_entity_id_{0}; }; +namespace rmw_zenoh_cpp +{ ///============================================================================= struct rmw_node_data_t { @@ -317,7 +318,7 @@ class rmw_client_data_t final size_t get_next_sequence_number(); - void add_new_reply(std::unique_ptr reply); + void add_new_reply(std::unique_ptr reply); bool reply_queue_is_empty() const; @@ -325,7 +326,7 @@ class rmw_client_data_t final void detach_condition(); - std::unique_ptr pop_next_reply(); + std::unique_ptr pop_next_reply(); DataCallbackManager data_callback_mgr; @@ -338,7 +339,7 @@ class rmw_client_data_t final std::condition_variable * condition_{nullptr}; std::mutex condition_mutex_; - std::deque> reply_queue_; + std::deque> reply_queue_; mutable std::mutex reply_queue_mutex_; }; } // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/rmw_event.cpp b/rmw_zenoh_cpp/src/rmw_event.cpp index b69091fe..33a950aa 100644 --- a/rmw_zenoh_cpp/src/rmw_event.cpp +++ b/rmw_zenoh_cpp/src/rmw_event.cpp @@ -37,7 +37,8 @@ rmw_publisher_event_init( RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(publisher->implementation_identifier, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(publisher->data, RMW_RET_INVALID_ARGUMENT); - rmw_publisher_data_t * pub_data = static_cast(publisher->data); + rmw_zenoh_cpp::rmw_publisher_data_t * pub_data = + static_cast(publisher->data); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data->context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data->entity, RMW_RET_INVALID_ARGUMENT); @@ -47,8 +48,8 @@ rmw_publisher_event_init( return RMW_RET_INCORRECT_RMW_IMPLEMENTATION; } - auto rmw_event_it = event_map.find(event_type); - if (rmw_event_it == event_map.end()) { + auto rmw_event_it = rmw_zenoh_cpp::event_map.find(event_type); + if (rmw_event_it == rmw_zenoh_cpp::event_map.end()) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "provided event_type %d is not supported by rmw_zenoh_cpp", event_type); return RMW_RET_UNSUPPORTED; @@ -63,7 +64,8 @@ rmw_publisher_event_init( pub_data->entity, rmw_event_it->second, [pub_data, - event_id = rmw_event_it->second](std::unique_ptr zenoh_event) + event_id = + rmw_event_it->second](std::unique_ptr zenoh_event) { if (pub_data == nullptr) { return; @@ -89,7 +91,8 @@ rmw_subscription_event_init( RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(subscription->implementation_identifier, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(subscription->data, RMW_RET_INVALID_ARGUMENT); - rmw_subscription_data_t * sub_data = static_cast(subscription->data); + rmw_zenoh_cpp::rmw_subscription_data_t * sub_data = + static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data->context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data->entity, RMW_RET_INVALID_ARGUMENT); @@ -100,8 +103,8 @@ rmw_subscription_event_init( return RMW_RET_INCORRECT_RMW_IMPLEMENTATION; } - auto rmw_event_it = event_map.find(event_type); - if (rmw_event_it == event_map.end()) { + auto rmw_event_it = rmw_zenoh_cpp::event_map.find(event_type); + if (rmw_event_it == rmw_zenoh_cpp::event_map.end()) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "provided event_type %d is not supported by rmw_zenoh_cpp", event_type); return RMW_RET_UNSUPPORTED; @@ -113,7 +116,7 @@ rmw_subscription_event_init( // Register the event with graph cache if the event is not ZENOH_EVENT_MESSAGE_LOST // since this is checked for in the subscription callback. - if (rmw_event_it->second == ZENOH_EVENT_MESSAGE_LOST) { + if (rmw_event_it->second == rmw_zenoh_cpp::ZENOH_EVENT_MESSAGE_LOST) { return RMW_RET_OK; } @@ -121,7 +124,8 @@ rmw_subscription_event_init( sub_data->entity, rmw_event_it->second, [sub_data, - event_id = rmw_event_it->second](std::unique_ptr zenoh_event) + event_id = + rmw_event_it->second](std::unique_ptr zenoh_event) { if (sub_data == nullptr) { return; @@ -146,8 +150,8 @@ rmw_event_set_callback( RMW_CHECK_ARGUMENT_FOR_NULL(rmw_event, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(rmw_event->data, RMW_RET_INVALID_ARGUMENT); - auto zenoh_event_it = event_map.find(rmw_event->event_type); - if (zenoh_event_it == event_map.end()) { + auto zenoh_event_it = rmw_zenoh_cpp::event_map.find(rmw_event->event_type); + if (zenoh_event_it == rmw_zenoh_cpp::event_map.end()) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "RMW Zenoh does not support event [%d]", rmw_event->event_type); @@ -155,7 +159,8 @@ rmw_event_set_callback( } // Both rmw_subscription_data_t and rmw_publisher_data_t inherit EventsBase. - EventsManager * event_data = static_cast(rmw_event->data); + rmw_zenoh_cpp::EventsManager * event_data = + static_cast(rmw_event->data); RMW_CHECK_ARGUMENT_FOR_NULL(event_data, RMW_RET_INVALID_ARGUMENT); event_data->event_set_callback( zenoh_event_it->second, @@ -184,17 +189,18 @@ rmw_take_event( return RMW_RET_INCORRECT_RMW_IMPLEMENTATION; } - auto zenoh_event_it = event_map.find(event_handle->event_type); - if (zenoh_event_it == event_map.end()) { + auto zenoh_event_it = rmw_zenoh_cpp::event_map.find(event_handle->event_type); + if (zenoh_event_it == rmw_zenoh_cpp::event_map.end()) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "RMW Zenoh does not support event [%d]", event_handle->event_type); return RMW_RET_ERROR; } - EventsManager * event_data = static_cast(event_handle->data); + rmw_zenoh_cpp::EventsManager * event_data = + static_cast(event_handle->data); RMW_CHECK_ARGUMENT_FOR_NULL(event_data, RMW_RET_INVALID_ARGUMENT); - std::unique_ptr st = event_data->pop_next_event( + std::unique_ptr st = event_data->pop_next_event( zenoh_event_it->second); if (st == nullptr) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( @@ -205,7 +211,7 @@ rmw_take_event( // Now depending on the event, populate the rmw event status. switch (zenoh_event_it->second) { - case ZENOH_EVENT_REQUESTED_QOS_INCOMPATIBLE: { + case rmw_zenoh_cpp::ZENOH_EVENT_REQUESTED_QOS_INCOMPATIBLE: { auto ei = static_cast(event_info); RMW_CHECK_ARGUMENT_FOR_NULL(ei, RMW_RET_INVALID_ARGUMENT); ei->total_count = st->total_count; @@ -213,7 +219,7 @@ rmw_take_event( *taken = true; return RMW_RET_OK; } - case ZENOH_EVENT_MESSAGE_LOST: { + case rmw_zenoh_cpp::ZENOH_EVENT_MESSAGE_LOST: { auto ei = static_cast(event_info); RMW_CHECK_ARGUMENT_FOR_NULL(ei, RMW_RET_INVALID_ARGUMENT); ei->total_count = st->total_count; @@ -221,8 +227,8 @@ rmw_take_event( *taken = true; return RMW_RET_OK; } - case ZENOH_EVENT_PUBLICATION_MATCHED: - case ZENOH_EVENT_SUBSCRIPTION_MATCHED: { + case rmw_zenoh_cpp::ZENOH_EVENT_PUBLICATION_MATCHED: + case rmw_zenoh_cpp::ZENOH_EVENT_SUBSCRIPTION_MATCHED: { auto ei = static_cast(event_info); RMW_CHECK_ARGUMENT_FOR_NULL(ei, RMW_RET_INVALID_ARGUMENT); ei->total_count = st->total_count; @@ -232,7 +238,7 @@ rmw_take_event( *taken = true; return RMW_RET_OK; } - case ZENOH_EVENT_OFFERED_QOS_INCOMPATIBLE: { + case rmw_zenoh_cpp::ZENOH_EVENT_OFFERED_QOS_INCOMPATIBLE: { auto ei = static_cast(event_info); RMW_CHECK_ARGUMENT_FOR_NULL(ei, RMW_RET_INVALID_ARGUMENT); ei->total_count = st->total_count; diff --git a/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp index efa11ed8..c42295b3 100644 --- a/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp @@ -44,7 +44,7 @@ rmw_get_subscriber_names_and_types_by_node( RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); return node->context->impl->graph_cache->get_entity_names_and_types_by_node( - liveliness::EntityType::Subscription, + rmw_zenoh_cpp::liveliness::EntityType::Subscription, allocator, node_name, node_namespace, @@ -72,7 +72,7 @@ rmw_get_publisher_names_and_types_by_node( RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); return node->context->impl->graph_cache->get_entity_names_and_types_by_node( - liveliness::EntityType::Publisher, + rmw_zenoh_cpp::liveliness::EntityType::Publisher, allocator, node_name, node_namespace, @@ -99,7 +99,7 @@ rmw_get_service_names_and_types_by_node( RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); return node->context->impl->graph_cache->get_entity_names_and_types_by_node( - liveliness::EntityType::Service, + rmw_zenoh_cpp::liveliness::EntityType::Service, allocator, node_name, node_namespace, @@ -126,7 +126,7 @@ rmw_get_client_names_and_types_by_node( RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); return node->context->impl->graph_cache->get_entity_names_and_types_by_node( - liveliness::EntityType::Client, + rmw_zenoh_cpp::liveliness::EntityType::Client, allocator, node_name, node_namespace, diff --git a/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp b/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp index 18250029..a4c4878d 100644 --- a/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -44,7 +44,7 @@ rmw_get_publishers_info_by_topic( RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); return node->context->impl->graph_cache->get_entities_info_by_topic( - liveliness::EntityType::Publisher, + rmw_zenoh_cpp::liveliness::EntityType::Publisher, allocator, topic_name, no_mangle, @@ -70,7 +70,7 @@ rmw_get_subscriptions_info_by_topic( RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT); return node->context->impl->graph_cache->get_entities_info_by_topic( - liveliness::EntityType::Subscription, + rmw_zenoh_cpp::liveliness::EntityType::Subscription, allocator, topic_name, no_mangle, diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index c895dff0..b70c0aa7 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -128,7 +128,7 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) auto impl_destructor = rcpputils::make_scope_exit( [context] { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - context->impl->~rmw_context_impl_t(), rmw_context_impl_t); + context->impl->~rmw_context_impl_t(), rmw_context_impl_t *); }); rmw_ret_t ret; @@ -149,7 +149,11 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) // Initialize the zenoh configuration. z_owned_config_t config; - if ((ret = get_z_config(ConfigurableEntity::Session, &config)) != RMW_RET_OK) { + if ((ret = + rmw_zenoh_cpp::get_z_config( + rmw_zenoh_cpp::ConfigurableEntity::Session, + &config)) != RMW_RET_OK) + { RMW_SET_ERROR_MSG("Error configuring Zenoh session."); return ret; } @@ -174,16 +178,17 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) /// Initialize the graph cache. z_id_t zid = z_info_zid(z_loan(context->impl->session)); - context->impl->graph_cache = std::make_unique(zid); + context->impl->graph_cache = std::make_unique(zid); // Verify if the zenoh router is running if configured. - const std::optional configured_connection_attempts = zenoh_router_check_attempts(); + const std::optional configured_connection_attempts = + rmw_zenoh_cpp::zenoh_router_check_attempts(); if (configured_connection_attempts.has_value()) { ret = RMW_RET_ERROR; uint64_t connection_attempts = 0; // Retry until the connection is successful. while (ret != RMW_RET_OK && connection_attempts < configured_connection_attempts.value()) { - if ((ret = zenoh_router_check(z_loan(context->impl->session))) != RMW_RET_OK) { + if ((ret = rmw_zenoh_cpp::zenoh_router_check(z_loan(context->impl->session))) != RMW_RET_OK) { ++connection_attempts; } std::this_thread::sleep_for(std::chrono::seconds(1)); @@ -240,10 +245,11 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) allocator->deallocate(context->impl->graph_guard_condition, allocator->state); }); - context->impl->graph_guard_condition->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; + context->impl->graph_guard_condition->implementation_identifier = + rmw_zenoh_cpp::rmw_zenoh_identifier; context->impl->graph_guard_condition->data = - allocator->zero_allocate(1, sizeof(GuardCondition), allocator->state); + allocator->zero_allocate(1, sizeof(rmw_zenoh_cpp::GuardCondition), allocator->state); RMW_CHECK_FOR_NULL_WITH_MSG( context->impl->graph_guard_condition->data, "failed to allocate graph guard condition data", @@ -257,15 +263,19 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) context->impl->graph_guard_condition->data, context->impl->graph_guard_condition->data, return RMW_RET_BAD_ALLOC, - GuardCondition); + rmw_zenoh_cpp::GuardCondition); auto destruct_guard_condition_data = rcpputils::make_scope_exit( [context]() { - auto gc_data = static_cast(context->impl->graph_guard_condition->data); - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(gc_data->~GuardCondition(), GuardCondition); + auto gc_data = + static_cast(context->impl->graph_guard_condition->data); + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( + gc_data->~GuardCondition(), + rmw_zenoh_cpp::GuardCondition); }); // Setup liveliness subscriptions for discovery. - const std::string liveliness_str = liveliness::subscription_token(context->actual_domain_id); + const std::string liveliness_str = rmw_zenoh_cpp::liveliness::subscription_token( + context->actual_domain_id); // Query router/liveliness participants to get graph information before this session was started. @@ -405,14 +415,15 @@ rmw_context_fini(rmw_context_t * context) const rcutils_allocator_t * allocator = &context->options.allocator; RMW_TRY_DESTRUCTOR( - static_cast(context->impl->graph_guard_condition->data)->~GuardCondition(), - GuardCondition, ); + static_cast( + context->impl->graph_guard_condition->data)->~GuardCondition(), + rmw_zenoh_cpp::GuardCondition, ); allocator->deallocate(context->impl->graph_guard_condition->data, allocator->state); allocator->deallocate(context->impl->graph_guard_condition, allocator->state); context->impl->graph_guard_condition = nullptr; - RMW_TRY_DESTRUCTOR(context->impl->~rmw_context_impl_t(), rmw_context_impl_t, ); + RMW_TRY_DESTRUCTOR(context->impl->~rmw_context_impl_t(), rmw_context_impl_t *, ); allocator->deallocate(context->impl, allocator->state); diff --git a/rmw_zenoh_cpp/src/rmw_qos.cpp b/rmw_zenoh_cpp/src/rmw_qos.cpp index dd566727..0d2ff238 100644 --- a/rmw_zenoh_cpp/src/rmw_qos.cpp +++ b/rmw_zenoh_cpp/src/rmw_qos.cpp @@ -24,6 +24,7 @@ extern "C" { +///============================================================================= // Copied from rmw_dds_common::qos.cpp. // Returns RMW_RET_OK if successful or no buffer was provided // Returns RMW_RET_ERROR if there as an error copying the message to the buffer @@ -48,6 +49,7 @@ _append_to_buffer(char * buffer, size_t buffer_size, const char * format, ...) return RMW_RET_OK; } +///============================================================================= rmw_ret_t rmw_qos_profile_check_compatible( const rmw_qos_profile_t publisher_profile, diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index b0cac98d..7f207bc8 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -173,7 +173,7 @@ rmw_get_implementation_identifier(void) const char * rmw_get_serialization_format(void) { - return rmw_zenoh_serialization_format; + return rmw_zenoh_cpp::rmw_zenoh_serialization_format; } //============================================================================== @@ -272,7 +272,8 @@ rmw_create_node( }); // Put metadata into node->data. - node->data = allocator->zero_allocate(1, sizeof(rmw_node_data_t), allocator->state); + node->data = + allocator->zero_allocate(1, sizeof(rmw_zenoh_cpp::rmw_node_data_t), allocator->state); RMW_CHECK_FOR_NULL_WITH_MSG( node->data, "unable to allocate memory for node data", @@ -286,14 +287,15 @@ rmw_create_node( node->context = context; // Initialize liveliness token for the node to advertise that a new node is in town. - rmw_node_data_t * node_data = static_cast(node->data); + rmw_zenoh_cpp::rmw_node_data_t * node_data = + static_cast(node->data); node_data->id = context->impl->get_next_entity_id(); - node_data->entity = liveliness::Entity::make( + node_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( z_info_zid(z_loan(context->impl->session)), std::to_string(node_data->id), std::to_string(node_data->id), - liveliness::EntityType::Node, - liveliness::NodeInfo{context->actual_domain_id, namespace_, name, ""}); + rmw_zenoh_cpp::liveliness::EntityType::Node, + rmw_zenoh_cpp::liveliness::NodeInfo{context->actual_domain_id, namespace_, name, ""}); if (node_data->entity == nullptr) { RCUTILS_LOG_ERROR_NAMED( "rmw_zenoh_cpp", @@ -341,7 +343,8 @@ rmw_destroy_node(rmw_node_t * node) // Undeclare liveliness token for the node to advertise that the node has ridden // off into the sunset. - rmw_node_data_t * node_data = static_cast(node->data); + rmw_zenoh_cpp::rmw_node_data_t * node_data = + static_cast(node->data); zc_liveliness_undeclare_token(z_move(node_data->token)); rcutils_allocator_t * allocator = &node->context->options.allocator; @@ -454,7 +457,8 @@ rmw_create_publisher( return nullptr; } RMW_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); - const rmw_node_data_t * node_data = static_cast(node->data); + const rmw_zenoh_cpp::rmw_node_data_t * node_data = + static_cast(node->data); if (node_data == nullptr) { RMW_SET_ERROR_MSG( "Unable to create publisher as node_data is invalid."); @@ -502,8 +506,8 @@ rmw_create_publisher( allocator->deallocate(rmw_publisher, allocator->state); }); - auto publisher_data = static_cast( - allocator->allocate(sizeof(rmw_publisher_data_t), allocator->state)); + auto publisher_data = static_cast( + allocator->allocate(sizeof(rmw_zenoh_cpp::rmw_publisher_data_t), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( publisher_data, "failed to allocate memory for publisher data", @@ -513,11 +517,13 @@ rmw_create_publisher( allocator->deallocate(publisher_data, allocator->state); }); - RMW_TRY_PLACEMENT_NEW(publisher_data, publisher_data, return nullptr, rmw_publisher_data_t); + RMW_TRY_PLACEMENT_NEW( + publisher_data, publisher_data, return nullptr, + rmw_zenoh_cpp::rmw_publisher_data_t); auto destruct_publisher_data = rcpputils::make_scope_exit( [publisher_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - publisher_data->~rmw_publisher_data_t(), rmw_publisher_data_t); + publisher_data->~rmw_publisher_data_t(), rmw_zenoh_cpp::rmw_publisher_data_t); }); generate_random_gid(publisher_data->pub_gid); @@ -538,8 +544,8 @@ rmw_create_publisher( publisher_data->typesupport_identifier = type_support->typesupport_identifier; publisher_data->type_support_impl = type_support->data; auto callbacks = static_cast(type_support->data); - publisher_data->type_support = static_cast( - allocator->allocate(sizeof(MessageTypeSupport), allocator->state)); + publisher_data->type_support = static_cast( + allocator->allocate(sizeof(rmw_zenoh_cpp::MessageTypeSupport), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( publisher_data->type_support, "Failed to allocate MessageTypeSupport", @@ -553,12 +559,12 @@ rmw_create_publisher( publisher_data->type_support, publisher_data->type_support, return nullptr, - MessageTypeSupport, callbacks); + rmw_zenoh_cpp::MessageTypeSupport, callbacks); auto destruct_msg_type_support = rcpputils::make_scope_exit( [publisher_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( publisher_data->type_support->~MessageTypeSupport(), - MessageTypeSupport); + rmw_zenoh_cpp::MessageTypeSupport); }); publisher_data->context = node->context; @@ -633,13 +639,15 @@ rmw_create_publisher( z_undeclare_publisher(z_move(publisher_data->pub)); }); - publisher_data->entity = liveliness::Entity::make( + publisher_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( z_info_zid(z_loan(node->context->impl->session)), std::to_string(node_data->id), - std::to_string(context_impl->get_next_entity_id()), - liveliness::EntityType::Publisher, - liveliness::NodeInfo{node->context->actual_domain_id, node->namespace_, node->name, ""}, - liveliness::TopicInfo{rmw_publisher->topic_name, + std::to_string( + context_impl->get_next_entity_id()), + rmw_zenoh_cpp::liveliness::EntityType::Publisher, + rmw_zenoh_cpp::liveliness::NodeInfo{ + node->context->actual_domain_id, node->namespace_, node->name, ""}, + rmw_zenoh_cpp::liveliness::TopicInfo{rmw_publisher->topic_name, publisher_data->type_support->get_name(), publisher_data->adapted_qos_profile} ); if (publisher_data->entity == nullptr) { @@ -703,7 +711,7 @@ rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) rcutils_allocator_t * allocator = &node->context->options.allocator; - auto publisher_data = static_cast(publisher->data); + auto publisher_data = static_cast(publisher->data); if (publisher_data != nullptr) { zc_liveliness_undeclare_token(z_move(publisher_data->token)); if (publisher_data->pub_cache.has_value()) { @@ -856,7 +864,7 @@ rmw_publish( ros_message, "ros message handle is null", return RMW_RET_INVALID_ARGUMENT); - auto publisher_data = static_cast(publisher->data); + auto publisher_data = static_cast(publisher->data); RMW_CHECK_FOR_NULL_WITH_MSG( publisher_data, "publisher_data is null", return RMW_RET_INVALID_ARGUMENT); @@ -995,7 +1003,8 @@ rmw_publisher_count_matched_subscriptions( return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(subscription_count, RMW_RET_INVALID_ARGUMENT); - rmw_publisher_data_t * pub_data = static_cast(publisher->data); + rmw_zenoh_cpp::rmw_publisher_data_t * pub_data = + static_cast(publisher->data); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data->context, RMW_RET_INVALID_ARGUMENT); rmw_context_impl_t * context_impl = static_cast(pub_data->context->impl); @@ -1020,7 +1029,8 @@ rmw_publisher_get_actual_qos( return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); - rmw_publisher_data_t * pub_data = static_cast(publisher->data); + rmw_zenoh_cpp::rmw_publisher_data_t * pub_data = + static_cast(publisher->data); RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT); *qos = pub_data->adapted_qos_profile; @@ -1046,7 +1056,7 @@ rmw_publish_serialized_message( serialized_message, "serialized message handle is null", return RMW_RET_INVALID_ARGUMENT); - auto publisher_data = static_cast(publisher->data); + auto publisher_data = static_cast(publisher->data); RCUTILS_CHECK_FOR_NULL_WITH_MSG( publisher_data, "publisher data pointer is null", return RMW_RET_ERROR); @@ -1146,7 +1156,7 @@ rmw_serialize( } auto callbacks = static_cast(ts->data); - auto tss = MessageTypeSupport(callbacks); + auto tss = rmw_zenoh_cpp::MessageTypeSupport(callbacks); auto data_length = tss.get_estimated_serialized_size(ros_message, callbacks); if (serialized_message->buffer_capacity < data_length) { if (rmw_serialized_message_resize(serialized_message, data_length) != RMW_RET_OK) { @@ -1180,7 +1190,7 @@ rmw_deserialize( } auto callbacks = static_cast(ts->data); - auto tss = MessageTypeSupport(callbacks); + auto tss = rmw_zenoh_cpp::MessageTypeSupport(callbacks); eprosima::fastcdr::FastBuffer buffer( reinterpret_cast(serialized_message->buffer), serialized_message->buffer_length); rmw_zenoh_cpp::Cdr deser(buffer); @@ -1246,7 +1256,7 @@ rmw_create_subscription( } RMW_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); - auto node_data = static_cast(node->data); + auto node_data = static_cast(node->data); RMW_CHECK_FOR_NULL_WITH_MSG( node_data, "unable to create subscription as node_data is invalid.", return nullptr); @@ -1286,8 +1296,8 @@ rmw_create_subscription( allocator->deallocate(rmw_subscription, allocator->state); }); - auto sub_data = static_cast( - allocator->allocate(sizeof(rmw_subscription_data_t), allocator->state)); + auto sub_data = static_cast( + allocator->allocate(sizeof(rmw_zenoh_cpp::rmw_subscription_data_t), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( sub_data, "failed to allocate memory for subscription data", @@ -1297,12 +1307,12 @@ rmw_create_subscription( allocator->deallocate(sub_data, allocator->state); }); - RMW_TRY_PLACEMENT_NEW(sub_data, sub_data, return nullptr, rmw_subscription_data_t); + RMW_TRY_PLACEMENT_NEW(sub_data, sub_data, return nullptr, rmw_zenoh_cpp::rmw_subscription_data_t); auto destruct_sub_data = rcpputils::make_scope_exit( [sub_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( sub_data->~rmw_subscription_data_t(), - rmw_subscription_data_t); + rmw_zenoh_cpp::rmw_subscription_data_t); }); // Adapt any 'best available' QoS options @@ -1322,8 +1332,8 @@ rmw_create_subscription( sub_data->typesupport_identifier = type_support->typesupport_identifier; sub_data->type_support_impl = type_support->data; auto callbacks = static_cast(type_support->data); - sub_data->type_support = static_cast( - allocator->allocate(sizeof(MessageTypeSupport), allocator->state)); + sub_data->type_support = static_cast( + allocator->allocate(sizeof(rmw_zenoh_cpp::MessageTypeSupport), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( sub_data->type_support, "Failed to allocate MessageTypeSupport", @@ -1337,12 +1347,12 @@ rmw_create_subscription( sub_data->type_support, sub_data->type_support, return nullptr, - MessageTypeSupport, callbacks); + rmw_zenoh_cpp::MessageTypeSupport, callbacks); auto destruct_msg_type_support = rcpputils::make_scope_exit( [sub_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( sub_data->type_support->~MessageTypeSupport(), - MessageTypeSupport); + rmw_zenoh_cpp::MessageTypeSupport); }); sub_data->context = node->context; @@ -1366,7 +1376,7 @@ rmw_create_subscription( // Everything above succeeded and is setup properly. Now declare a subscriber // with Zenoh; after this, callbacks may come in at any time. - z_owned_closure_sample_t callback = z_closure(sub_data_handler, nullptr, sub_data); + z_owned_closure_sample_t callback = z_closure(rmw_zenoh_cpp::sub_data_handler, nullptr, sub_data); z_owned_keyexpr_t keyexpr = ros_topic_name_to_zenoh_key( topic_name, node->context->actual_domain_id); auto always_free_ros_keyexpr = rcpputils::make_scope_exit( @@ -1441,13 +1451,15 @@ rmw_create_subscription( }); // Publish to the graph that a new subscription is in town - sub_data->entity = liveliness::Entity::make( + sub_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( z_info_zid(z_loan(node->context->impl->session)), std::to_string(node_data->id), - std::to_string(context_impl->get_next_entity_id()), - liveliness::EntityType::Subscription, - liveliness::NodeInfo{node->context->actual_domain_id, node->namespace_, node->name, ""}, - liveliness::TopicInfo{rmw_subscription->topic_name, + std::to_string( + context_impl->get_next_entity_id()), + rmw_zenoh_cpp::liveliness::EntityType::Subscription, + rmw_zenoh_cpp::liveliness::NodeInfo{ + node->context->actual_domain_id, node->namespace_, node->name, ""}, + rmw_zenoh_cpp::liveliness::TopicInfo{rmw_subscription->topic_name, sub_data->type_support->get_name(), sub_data->adapted_qos_profile} ); if (sub_data->entity == nullptr) { @@ -1510,7 +1522,7 @@ rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) rcutils_allocator_t * allocator = &node->context->options.allocator; - auto sub_data = static_cast(subscription->data); + auto sub_data = static_cast(subscription->data); if (sub_data != nullptr) { // Publish to the graph that a subscription has ridden off into the sunset zc_liveliness_undeclare_token(z_move(sub_data->token)); @@ -1558,7 +1570,8 @@ rmw_subscription_count_matched_publishers( return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(publisher_count, RMW_RET_INVALID_ARGUMENT); - rmw_subscription_data_t * sub_data = static_cast(subscription->data); + rmw_zenoh_cpp::rmw_subscription_data_t * sub_data = + static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data->context, RMW_RET_INVALID_ARGUMENT); rmw_context_impl_t * context_impl = static_cast(sub_data->context->impl); @@ -1583,7 +1596,7 @@ rmw_subscription_get_actual_qos( return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); - auto sub_data = static_cast(subscription->data); + auto sub_data = static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); *qos = sub_data->adapted_qos_profile; @@ -1635,7 +1648,7 @@ static rmw_ret_t __rmw_take( *taken = false; - auto sub_data = static_cast(subscription->data); + auto sub_data = static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); if (sub_data->context->impl->is_shutdown) { @@ -1644,7 +1657,7 @@ static rmw_ret_t __rmw_take( // RETRIEVE SERIALIZED MESSAGE =============================================== - std::unique_ptr msg_data = sub_data->pop_next_message(); + std::unique_ptr msg_data = sub_data->pop_next_message(); if (msg_data == nullptr) { // This tells rcl that the check for a new message was done, but no messages have come in yet. return RMW_RET_OK; @@ -1750,7 +1763,7 @@ static rmw_ret_t __rmw_take_serialized( *taken = false; - auto sub_data = static_cast(subscription->data); + auto sub_data = static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); if (sub_data->context->impl->is_shutdown) { @@ -1759,7 +1772,7 @@ static rmw_ret_t __rmw_take_serialized( // RETRIEVE SERIALIZED MESSAGE =============================================== - std::unique_ptr msg_data = sub_data->pop_next_message(); + std::unique_ptr msg_data = sub_data->pop_next_message(); if (msg_data == nullptr) { // This tells rcl that the check for a new message was done, but no messages have come in yet. return RMW_RET_OK; @@ -1912,7 +1925,8 @@ rmw_create_client( return nullptr; } RMW_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); - const rmw_node_data_t * node_data = static_cast(node->data); + const rmw_zenoh_cpp::rmw_node_data_t * node_data = + static_cast(node->data); if (node_data == nullptr) { RMW_SET_ERROR_MSG( "Unable to create client as node data is invalid."); @@ -1949,8 +1963,8 @@ rmw_create_client( allocator->deallocate(rmw_client, allocator->state); }); - auto client_data = static_cast( - allocator->allocate(sizeof(rmw_client_data_t), allocator->state)); + auto client_data = static_cast( + allocator->allocate(sizeof(rmw_zenoh_cpp::rmw_client_data_t), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( client_data, "failed to allocate memory for client data", @@ -1960,12 +1974,12 @@ rmw_create_client( allocator->deallocate(client_data, allocator->state); }); - RMW_TRY_PLACEMENT_NEW(client_data, client_data, return nullptr, rmw_client_data_t); + RMW_TRY_PLACEMENT_NEW(client_data, client_data, return nullptr, rmw_zenoh_cpp::rmw_client_data_t); auto destruct_client_data = rcpputils::make_scope_exit( [client_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( client_data->~rmw_client_data_t(), - rmw_client_data_t); + rmw_zenoh_cpp::rmw_client_data_t); }); generate_random_gid(client_data->client_gid); @@ -1998,12 +2012,12 @@ rmw_create_client( client_data->response_type_support_impl = response_members; // Request type support - client_data->request_type_support = static_cast( - allocator->allocate(sizeof(RequestTypeSupport), allocator->state)); + client_data->request_type_support = static_cast( + allocator->allocate(sizeof(rmw_zenoh_cpp::RequestTypeSupport), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( client_data->request_type_support, - "Failed to allocate RequestTypeSupport", + "Failed to allocate rmw_zenoh_cpp::RequestTypeSupport", return nullptr); auto free_request_type_support = rcpputils::make_scope_exit( [client_data, allocator]() { @@ -2014,21 +2028,21 @@ rmw_create_client( client_data->request_type_support, client_data->request_type_support, return nullptr, - RequestTypeSupport, service_members); + rmw_zenoh_cpp::RequestTypeSupport, service_members); auto destruct_request_type_support = rcpputils::make_scope_exit( [client_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( client_data->request_type_support->~RequestTypeSupport(), - RequestTypeSupport); + rmw_zenoh_cpp::RequestTypeSupport); }); // Response type support - client_data->response_type_support = static_cast( - allocator->allocate(sizeof(ResponseTypeSupport), allocator->state)); + client_data->response_type_support = static_cast( + allocator->allocate(sizeof(rmw_zenoh_cpp::ResponseTypeSupport), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( client_data->response_type_support, - "Failed to allocate ResponseTypeSupport", + "Failed to allocate rmw_zenoh_cpp::ResponseTypeSupport", return nullptr); auto free_response_type_support = rcpputils::make_scope_exit( [client_data, allocator]() { @@ -2039,12 +2053,12 @@ rmw_create_client( client_data->response_type_support, client_data->response_type_support, return nullptr, - ResponseTypeSupport, service_members); + rmw_zenoh_cpp::ResponseTypeSupport, service_members); auto destruct_response_type_support = rcpputils::make_scope_exit( [client_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( client_data->response_type_support->~ResponseTypeSupport(), - ResponseTypeSupport); + rmw_zenoh_cpp::ResponseTypeSupport); }); // Populate the rmw_client. @@ -2084,13 +2098,15 @@ rmw_create_client( service_type.c_str(), rmw_client->service_name); return nullptr; } - client_data->entity = liveliness::Entity::make( + client_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( z_info_zid(z_loan(node->context->impl->session)), std::to_string(node_data->id), - std::to_string(context_impl->get_next_entity_id()), - liveliness::EntityType::Client, - liveliness::NodeInfo{node->context->actual_domain_id, node->namespace_, node->name, ""}, - liveliness::TopicInfo{rmw_client->service_name, + std::to_string( + context_impl->get_next_entity_id()), + rmw_zenoh_cpp::liveliness::EntityType::Client, + rmw_zenoh_cpp::liveliness::NodeInfo{ + node->context->actual_domain_id, node->namespace_, node->name, ""}, + rmw_zenoh_cpp::liveliness::TopicInfo{rmw_client->service_name, std::move(service_type), client_data->adapted_qos_profile} ); if (client_data->entity == nullptr) { @@ -2154,7 +2170,8 @@ rmw_destroy_client(rmw_node_t * node, rmw_client_t * client) rcutils_allocator_t * allocator = &node->context->options.allocator; - rmw_client_data_t * client_data = static_cast(client->data); + rmw_zenoh_cpp::rmw_client_data_t * client_data = + static_cast(client->data); RMW_CHECK_FOR_NULL_WITH_MSG( client_data, "client implementation pointer is null.", @@ -2166,11 +2183,12 @@ rmw_destroy_client(rmw_node_t * node, rmw_client_t * client) zc_liveliness_undeclare_token(z_move(client_data->token)); RMW_TRY_DESTRUCTOR( - client_data->request_type_support->~RequestTypeSupport(), RequestTypeSupport, ); + client_data->request_type_support->~RequestTypeSupport(), rmw_zenoh_cpp::RequestTypeSupport, ); allocator->deallocate(client_data->request_type_support, allocator->state); RMW_TRY_DESTRUCTOR( - client_data->response_type_support->~ResponseTypeSupport(), ResponseTypeSupport, ); + client_data->response_type_support->~ResponseTypeSupport(), rmw_zenoh_cpp::ResponseTypeSupport, + ); allocator->deallocate(client_data->response_type_support, allocator->state); RMW_TRY_DESTRUCTOR(client_data->~rmw_client_data_t(), rmw_client_data_t, ); @@ -2200,7 +2218,8 @@ rmw_send_request( rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - rmw_client_data_t * client_data = static_cast(client->data); + rmw_zenoh_cpp::rmw_client_data_t * client_data = + static_cast(client->data); RMW_CHECK_FOR_NULL_WITH_MSG( client_data, "Unable to retrieve client_data from client.", @@ -2274,7 +2293,8 @@ rmw_send_request( opts.consolidation = z_query_consolidation_latest(); opts.value.payload = z_bytes_t{data_length, reinterpret_cast(request_bytes)}; opts.value.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL); - client_data->zn_closure_reply = z_closure(client_data_handler, nullptr, client_data); + client_data->zn_closure_reply = + z_closure(rmw_zenoh_cpp::client_data_handler, nullptr, client_data); z_get( z_loan(context_impl->session), z_loan( client_data->keyexpr), "", &client_data->zn_closure_reply, &opts); @@ -2305,11 +2325,12 @@ rmw_take_response( RMW_CHECK_FOR_NULL_WITH_MSG( client->service_name, "client has no service name", RMW_RET_INVALID_ARGUMENT); - rmw_client_data_t * client_data = static_cast(client->data); + rmw_zenoh_cpp::rmw_client_data_t * client_data = + static_cast(client->data); RMW_CHECK_FOR_NULL_WITH_MSG( client->data, "Unable to retrieve client_data from client.", RMW_RET_INVALID_ARGUMENT); - std::unique_ptr latest_reply = client_data->pop_next_reply(); + std::unique_ptr latest_reply = client_data->pop_next_reply(); if (latest_reply == nullptr) { // This tells rcl that the check for a new message was done, but no messages have come in yet. return RMW_RET_ERROR; @@ -2340,20 +2361,23 @@ rmw_take_response( // Fill in the request_header request_header->request_id.sequence_number = - get_int64_from_attachment(&sample->attachment, "sequence_number"); + rmw_zenoh_cpp::get_int64_from_attachment(&sample->attachment, "sequence_number"); if (request_header->request_id.sequence_number < 0) { RMW_SET_ERROR_MSG("Failed to get sequence_number from client call attachment"); return RMW_RET_ERROR; } request_header->source_timestamp = - get_int64_from_attachment(&sample->attachment, "source_timestamp"); + rmw_zenoh_cpp::get_int64_from_attachment(&sample->attachment, "source_timestamp"); if (request_header->source_timestamp < 0) { RMW_SET_ERROR_MSG("Failed to get source_timestamp from client call attachment"); return RMW_RET_ERROR; } - if (!get_gid_from_attachment(&sample->attachment, request_header->request_id.writer_guid)) { + if (!rmw_zenoh_cpp::get_gid_from_attachment( + &sample->attachment, + request_header->request_id.writer_guid)) + { RMW_SET_ERROR_MSG("Could not get client gid from attachment"); return RMW_RET_ERROR; } @@ -2382,7 +2406,8 @@ rmw_client_request_publisher_get_actual_qos( return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); - rmw_client_data_t * client_data = static_cast(client->data); + rmw_zenoh_cpp::rmw_client_data_t * client_data = + static_cast(client->data); RMW_CHECK_ARGUMENT_FOR_NULL(client_data, RMW_RET_INVALID_ARGUMENT); *qos = client_data->adapted_qos_profile; @@ -2437,7 +2462,8 @@ rmw_create_service( } } RMW_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); - const rmw_node_data_t * node_data = static_cast(node->data); + const rmw_zenoh_cpp::rmw_node_data_t * node_data = + static_cast(node->data); if (node_data == nullptr) { RMW_SET_ERROR_MSG( "Unable to create service as node data is invalid."); @@ -2478,8 +2504,8 @@ rmw_create_service( allocator->deallocate(rmw_service, allocator->state); }); - auto service_data = static_cast( - allocator->allocate(sizeof(rmw_service_data_t), allocator->state)); + auto service_data = static_cast( + allocator->allocate(sizeof(rmw_zenoh_cpp::rmw_service_data_t), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( service_data, "failed to allocate memory for service data", @@ -2489,12 +2515,14 @@ rmw_create_service( allocator->deallocate(service_data, allocator->state); }); - RMW_TRY_PLACEMENT_NEW(service_data, service_data, return nullptr, rmw_service_data_t); + RMW_TRY_PLACEMENT_NEW( + service_data, service_data, return nullptr, + rmw_zenoh_cpp::rmw_service_data_t); auto destruct_service_data = rcpputils::make_scope_exit( [service_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( service_data->~rmw_service_data_t(), - rmw_service_data_t); + rmw_zenoh_cpp::rmw_service_data_t); }); // Adapt any 'best available' QoS options @@ -2525,11 +2553,11 @@ rmw_create_service( service_data->response_type_support_impl = response_members; // Request type support - service_data->request_type_support = static_cast( - allocator->allocate(sizeof(RequestTypeSupport), allocator->state)); + service_data->request_type_support = static_cast( + allocator->allocate(sizeof(rmw_zenoh_cpp::RequestTypeSupport), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( service_data->request_type_support, - "Failed to allocate RequestTypeSupport", + "Failed to allocate rmw_zenoh_cpp::RequestTypeSupport", return nullptr); auto free_request_type_support = rcpputils::make_scope_exit( [request_type_support = service_data->request_type_support, allocator]() { @@ -2539,20 +2567,20 @@ rmw_create_service( service_data->request_type_support, service_data->request_type_support, return nullptr, - RequestTypeSupport, service_members); + rmw_zenoh_cpp::RequestTypeSupport, service_members); auto destruct_request_type_support = rcpputils::make_scope_exit( [service_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( service_data->request_type_support->~RequestTypeSupport(), - RequestTypeSupport); + rmw_zenoh_cpp::RequestTypeSupport); }); // Response type support - service_data->response_type_support = static_cast( - allocator->allocate(sizeof(ResponseTypeSupport), allocator->state)); + service_data->response_type_support = static_cast( + allocator->allocate(sizeof(rmw_zenoh_cpp::ResponseTypeSupport), allocator->state)); RMW_CHECK_FOR_NULL_WITH_MSG( service_data->response_type_support, - "Failed to allocate ResponseTypeSupport", + "Failed to allocate rmw_zenoh_cpp::ResponseTypeSupport", return nullptr); auto free_response_type_support = rcpputils::make_scope_exit( [response_type_support = service_data->response_type_support, allocator]() { @@ -2562,12 +2590,12 @@ rmw_create_service( service_data->response_type_support, service_data->response_type_support, return nullptr, - ResponseTypeSupport, service_members); + rmw_zenoh_cpp::ResponseTypeSupport, service_members); auto destruct_response_type_support = rcpputils::make_scope_exit( [service_data]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( service_data->response_type_support->~ResponseTypeSupport(), - ResponseTypeSupport); + rmw_zenoh_cpp::ResponseTypeSupport); }); // Populate the rmw_service. @@ -2594,7 +2622,9 @@ rmw_create_service( return nullptr; } - z_owned_closure_query_t callback = z_closure(service_data_handler, nullptr, service_data); + z_owned_closure_query_t callback = z_closure( + rmw_zenoh_cpp::service_data_handler, nullptr, + service_data); // Configure the queryable to process complete queries. z_queryable_options_t qable_options = z_queryable_options_default(); qable_options.complete = true; @@ -2627,13 +2657,15 @@ rmw_create_service( service_type.c_str(), rmw_service->service_name); return nullptr; } - service_data->entity = liveliness::Entity::make( + service_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( z_info_zid(z_loan(node->context->impl->session)), std::to_string(node_data->id), - std::to_string(context_impl->get_next_entity_id()), - liveliness::EntityType::Service, - liveliness::NodeInfo{node->context->actual_domain_id, node->namespace_, node->name, ""}, - liveliness::TopicInfo{rmw_service->service_name, + std::to_string( + context_impl->get_next_entity_id()), + rmw_zenoh_cpp::liveliness::EntityType::Service, + rmw_zenoh_cpp::liveliness::NodeInfo{ + node->context->actual_domain_id, node->namespace_, node->name, ""}, + rmw_zenoh_cpp::liveliness::TopicInfo{rmw_service->service_name, std::move(service_type), service_data->adapted_qos_profile} ); if (service_data->entity == nullptr) { @@ -2699,7 +2731,8 @@ rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) rcutils_allocator_t * allocator = &node->context->options.allocator; - rmw_service_data_t * service_data = static_cast(service->data); + rmw_zenoh_cpp::rmw_service_data_t * service_data = + static_cast(service->data); RMW_CHECK_FOR_NULL_WITH_MSG( service_data, "Unable to retrieve service_data from service", @@ -2711,14 +2744,15 @@ rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) zc_liveliness_undeclare_token(z_move(service_data->token)); RMW_TRY_DESTRUCTOR( - service_data->request_type_support->~RequestTypeSupport(), RequestTypeSupport, ); + service_data->request_type_support->~RequestTypeSupport(), rmw_zenoh_cpp::RequestTypeSupport, ); allocator->deallocate(service_data->request_type_support, allocator->state); RMW_TRY_DESTRUCTOR( - service_data->response_type_support->~ResponseTypeSupport(), ResponseTypeSupport, ); + service_data->response_type_support->~ResponseTypeSupport(), rmw_zenoh_cpp::ResponseTypeSupport, + ); allocator->deallocate(service_data->response_type_support, allocator->state); - RMW_TRY_DESTRUCTOR(service_data->~rmw_service_data_t(), rmw_service_data_t, ); + RMW_TRY_DESTRUCTOR(service_data->~rmw_service_data_t(), rmw_zenoh_cpp::rmw_service_data_t, ); allocator->deallocate(service->data, allocator->state); allocator->deallocate(const_cast(service->service_name), allocator->state); @@ -2752,11 +2786,12 @@ rmw_take_request( RMW_CHECK_FOR_NULL_WITH_MSG( service->service_name, "service has no service name", RMW_RET_INVALID_ARGUMENT); - rmw_service_data_t * service_data = static_cast(service->data); + rmw_zenoh_cpp::rmw_service_data_t * service_data = + static_cast(service->data); RMW_CHECK_FOR_NULL_WITH_MSG( service->data, "Unable to retrieve service_data from service", RMW_RET_INVALID_ARGUMENT); - std::unique_ptr query = service_data->pop_next_query(); + std::unique_ptr query = service_data->pop_next_query(); if (query == nullptr) { // This tells rcl that the check for a new message was done, but no messages have come in yet. return RMW_RET_OK; @@ -2789,19 +2824,24 @@ rmw_take_request( z_attachment_t attachment = z_query_attachment(&loaned_query); request_header->request_id.sequence_number = - get_int64_from_attachment(&attachment, "sequence_number"); + rmw_zenoh_cpp::get_int64_from_attachment(&attachment, "sequence_number"); if (request_header->request_id.sequence_number < 0) { RMW_SET_ERROR_MSG("Failed to get sequence_number from client call attachment"); return RMW_RET_ERROR; } - request_header->source_timestamp = get_int64_from_attachment(&attachment, "source_timestamp"); + request_header->source_timestamp = rmw_zenoh_cpp::get_int64_from_attachment( + &attachment, + "source_timestamp"); if (request_header->source_timestamp < 0) { RMW_SET_ERROR_MSG("Failed to get source_timestamp from client call attachment"); return RMW_RET_ERROR; } - if (!get_gid_from_attachment(&attachment, request_header->request_id.writer_guid)) { + if (!rmw_zenoh_cpp::get_gid_from_attachment( + &attachment, + request_header->request_id.writer_guid)) + { RMW_SET_ERROR_MSG("Could not get client GID from attachment"); return RMW_RET_ERROR; } @@ -2845,10 +2885,11 @@ rmw_send_response( "Unable to retrieve service_data from service", RMW_RET_INVALID_ARGUMENT); - rmw_service_data_t * service_data = static_cast(service->data); + rmw_zenoh_cpp::rmw_service_data_t * service_data = + static_cast(service->data); // Create the queryable payload - std::unique_ptr query = + std::unique_ptr query = service_data->take_from_query_map(*request_header); if (query == nullptr) { // If there is no data associated with this request, the higher layers of @@ -2929,7 +2970,8 @@ rmw_service_request_subscription_get_actual_qos( return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); - rmw_service_data_t * service_data = static_cast(service->data); + rmw_zenoh_cpp::rmw_service_data_t * service_data = + static_cast(service->data); RMW_CHECK_ARGUMENT_FOR_NULL(service_data, RMW_RET_INVALID_ARGUMENT); *qos = service_data->adapted_qos_profile; @@ -2969,7 +3011,9 @@ rmw_create_guard_condition(rmw_context_t * context) guard_condition->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; guard_condition->context = context; - guard_condition->data = allocator->zero_allocate(1, sizeof(GuardCondition), allocator->state); + guard_condition->data = allocator->zero_allocate( + 1, sizeof(rmw_zenoh_cpp::GuardCondition), + allocator->state); RMW_CHECK_FOR_NULL_WITH_MSG( guard_condition->data, "unable to allocate memory for guard condition data", @@ -2979,11 +3023,12 @@ rmw_create_guard_condition(rmw_context_t * context) allocator->deallocate(guard_condition->data, allocator->state); }); - new(guard_condition->data) GuardCondition; + new(guard_condition->data) rmw_zenoh_cpp::GuardCondition; auto destruct_guard_condition = rcpputils::make_scope_exit( [guard_condition]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - static_cast(guard_condition->data)->~GuardCondition(), GuardCondition); + static_cast(guard_condition->data)->~GuardCondition(), + rmw_zenoh_cpp::GuardCondition); }); destruct_guard_condition.cancel(); @@ -3002,7 +3047,7 @@ rmw_destroy_guard_condition(rmw_guard_condition_t * guard_condition) rcutils_allocator_t * allocator = &guard_condition->context->options.allocator; if (guard_condition->data) { - static_cast(guard_condition->data)->~GuardCondition(); + static_cast(guard_condition->data)->~GuardCondition(); allocator->deallocate(guard_condition->data, allocator->state); } @@ -3022,7 +3067,7 @@ rmw_trigger_guard_condition(const rmw_guard_condition_t * guard_condition) rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - static_cast(guard_condition->data)->trigger(); + static_cast(guard_condition->data)->trigger(); return RMW_RET_OK; } @@ -3056,7 +3101,9 @@ rmw_create_wait_set(rmw_context_t * context, size_t max_conditions) wait_set->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; - wait_set->data = allocator->zero_allocate(1, sizeof(rmw_wait_set_data_t), allocator->state); + wait_set->data = allocator->zero_allocate( + 1, sizeof(rmw_zenoh_cpp::rmw_wait_set_data_t), + allocator->state); RMW_CHECK_FOR_NULL_WITH_MSG( wait_set->data, "failed to allocate wait set data", @@ -3067,15 +3114,15 @@ rmw_create_wait_set(rmw_context_t * context, size_t max_conditions) }); // Invoke placement new - new(wait_set->data) rmw_wait_set_data_t; + new(wait_set->data) rmw_zenoh_cpp::rmw_wait_set_data_t; auto destruct_rmw_wait_set_data = rcpputils::make_scope_exit( [wait_set]() { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - static_cast(wait_set->data)->~rmw_wait_set_data_t(), + static_cast(wait_set->data)->~rmw_wait_set_data_t(), rmw_wait_set_data); }); - static_cast(wait_set->data)->context = context; + static_cast(wait_set->data)->context = context; destruct_rmw_wait_set_data.cancel(); free_wait_set_data.cancel(); @@ -3097,7 +3144,7 @@ rmw_destroy_wait_set(rmw_wait_set_t * wait_set) rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - auto wait_set_data = static_cast(wait_set->data); + auto wait_set_data = static_cast(wait_set->data); rcutils_allocator_t * allocator = &wait_set_data->context->options.allocator; @@ -3120,7 +3167,8 @@ static bool has_triggered_condition( if (guard_conditions) { for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { - GuardCondition * gc = static_cast(guard_conditions->guard_conditions[i]); + rmw_zenoh_cpp::GuardCondition * gc = + static_cast(guard_conditions->guard_conditions[i]); if (gc != nullptr) { if (gc->get_and_reset_trigger()) { return true; @@ -3134,9 +3182,9 @@ static bool has_triggered_condition( auto event = static_cast(events->events[i]); const rmw_event_type_t & event_type = event->event_type; // Check if the event queue for this event type is empty. - auto zenoh_event_it = event_map.find(event_type); - if (zenoh_event_it != event_map.end()) { - auto event_data = static_cast(event->data); + auto zenoh_event_it = rmw_zenoh_cpp::event_map.find(event_type); + if (zenoh_event_it != rmw_zenoh_cpp::event_map.end()) { + auto event_data = static_cast(event->data); if (event_data != nullptr) { if (!event_data->event_queue_is_empty(zenoh_event_it->second)) { return true; @@ -3151,7 +3199,8 @@ static bool has_triggered_condition( if (subscriptions) { for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { - auto sub_data = static_cast(subscriptions->subscribers[i]); + auto sub_data = + static_cast(subscriptions->subscribers[i]); if (sub_data != nullptr) { if (!sub_data->message_queue_is_empty()) { return true; @@ -3162,7 +3211,7 @@ static bool has_triggered_condition( if (services) { for (size_t i = 0; i < services->service_count; ++i) { - auto serv_data = static_cast(services->services[i]); + auto serv_data = static_cast(services->services[i]); if (serv_data != nullptr) { if (!serv_data->query_queue_is_empty()) { return true; @@ -3173,7 +3222,8 @@ static bool has_triggered_condition( if (clients) { for (size_t i = 0; i < clients->client_count; ++i) { - rmw_client_data_t * client_data = static_cast(clients->clients[i]); + rmw_zenoh_cpp::rmw_client_data_t * client_data = + static_cast(clients->clients[i]); if (client_data != nullptr) { if (!client_data->reply_queue_is_empty()) { return true; @@ -3203,7 +3253,7 @@ rmw_wait( wait_set->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - auto wait_set_data = static_cast(wait_set->data); + auto wait_set_data = static_cast(wait_set->data); RMW_CHECK_FOR_NULL_WITH_MSG( wait_set_data, "waitset data struct is null", @@ -3237,7 +3287,8 @@ rmw_wait( // This is hard to track down, but each of the (void *) pointers in // guard_conditions->guard_conditions points to the data field of the related // rmw_guard_condition_t. So we can directly cast it to GuardCondition. - GuardCondition * gc = static_cast(guard_conditions->guard_conditions[i]); + rmw_zenoh_cpp::GuardCondition * gc = + static_cast(guard_conditions->guard_conditions[i]); if (gc != nullptr) { gc->attach_condition(&wait_set_data->condition_variable); } @@ -3248,7 +3299,8 @@ rmw_wait( // Attach the wait set condition variable to each subscription. // That way they can wake it up if they are triggered while we are waiting. for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { - auto sub_data = static_cast(subscriptions->subscribers[i]); + auto sub_data = + static_cast(subscriptions->subscribers[i]); if (sub_data != nullptr) { sub_data->attach_condition(&wait_set_data->condition_variable); } @@ -3259,7 +3311,7 @@ rmw_wait( // Attach the wait set condition variable to each service. // That way they can wake it up if they are triggered while we are waiting. for (size_t i = 0; i < services->service_count; ++i) { - auto serv_data = static_cast(services->services[i]); + auto serv_data = static_cast(services->services[i]); if (serv_data != nullptr) { serv_data->attach_condition(&wait_set_data->condition_variable); } @@ -3270,7 +3322,8 @@ rmw_wait( // Attach the wait set condition variable to each client. // That way they can wake it up if they are triggered while we are waiting. for (size_t i = 0; i < clients->client_count; ++i) { - rmw_client_data_t * client_data = static_cast(clients->clients[i]); + rmw_zenoh_cpp::rmw_client_data_t * client_data = + static_cast(clients->clients[i]); if (client_data != nullptr) { client_data->attach_condition(&wait_set_data->condition_variable); } @@ -3280,10 +3333,10 @@ rmw_wait( if (events) { for (size_t i = 0; i < events->event_count; ++i) { auto event = static_cast(events->events[i]); - auto event_data = static_cast(event->data); + auto event_data = static_cast(event->data); if (event_data != nullptr) { - auto zenoh_event_it = event_map.find(event->event_type); - if (zenoh_event_it != event_map.end()) { + auto zenoh_event_it = rmw_zenoh_cpp::event_map.find(event->event_type); + if (zenoh_event_it != rmw_zenoh_cpp::event_map.end()) { event_data->attach_event_condition( zenoh_event_it->second, &wait_set_data->condition_variable); @@ -3311,7 +3364,8 @@ rmw_wait( if (guard_conditions) { // Now detach the condition variable and mutex from each of the guard conditions for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { - GuardCondition * gc = static_cast(guard_conditions->guard_conditions[i]); + rmw_zenoh_cpp::GuardCondition * gc = + static_cast(guard_conditions->guard_conditions[i]); if (gc != nullptr) { gc->detach_condition(); // According to the documentation for rmw_wait in rmw.h, entries in the @@ -3327,10 +3381,10 @@ rmw_wait( // Now detach the condition variable and mutex from each of the subscriptions for (size_t i = 0; i < events->event_count; ++i) { auto event = static_cast(events->events[i]); - auto event_data = static_cast(event->data); + auto event_data = static_cast(event->data); if (event_data != nullptr) { - auto zenoh_event_it = event_map.find(event->event_type); - if (zenoh_event_it != event_map.end()) { + auto zenoh_event_it = rmw_zenoh_cpp::event_map.find(event->event_type); + if (zenoh_event_it != rmw_zenoh_cpp::event_map.end()) { event_data->detach_event_condition(zenoh_event_it->second); // According to the documentation for rmw_wait in rmw.h, entries in the // array that have *not* been triggered should be set to NULL @@ -3346,7 +3400,8 @@ rmw_wait( if (subscriptions) { // Now detach the condition variable and mutex from each of the subscriptions for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { - auto sub_data = static_cast(subscriptions->subscribers[i]); + auto sub_data = + static_cast(subscriptions->subscribers[i]); if (sub_data != nullptr) { sub_data->detach_condition(); // According to the documentation for rmw_wait in rmw.h, entries in the @@ -3362,7 +3417,7 @@ rmw_wait( if (services) { // Now detach the condition variable and mutex from each of the services for (size_t i = 0; i < services->service_count; ++i) { - auto serv_data = static_cast(services->services[i]); + auto serv_data = static_cast(services->services[i]); if (serv_data != nullptr) { serv_data->detach_condition(); // According to the documentation for rmw_wait in rmw.h, entries in the @@ -3378,7 +3433,8 @@ rmw_wait( if (clients) { // Now detach the condition variable and mutex from each of the clients for (size_t i = 0; i < clients->client_count; ++i) { - rmw_client_data_t * client_data = static_cast(clients->clients[i]); + rmw_zenoh_cpp::rmw_client_data_t * client_data = + static_cast(clients->clients[i]); if (client_data != nullptr) { client_data->detach_condition(); // According to the documentation for rmw_wait in rmw.h, entries in the @@ -3566,7 +3622,8 @@ rmw_get_gid_for_publisher(const rmw_publisher_t * publisher, rmw_gid_t * gid) RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(gid, RMW_RET_INVALID_ARGUMENT); - rmw_publisher_data_t * pub_data = static_cast(publisher->data); + rmw_zenoh_cpp::rmw_publisher_data_t * pub_data = + static_cast(publisher->data); gid->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; memcpy(gid->data, pub_data->pub_gid, RMW_GID_STORAGE_SIZE); @@ -3582,7 +3639,8 @@ rmw_get_gid_for_client(const rmw_client_t * client, rmw_gid_t * gid) RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(gid, RMW_RET_INVALID_ARGUMENT); - rmw_client_data_t * client_data = static_cast(client->data); + rmw_zenoh_cpp::rmw_client_data_t * client_data = + static_cast(client->data); gid->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; memcpy(gid->data, client_data->client_gid, RMW_GID_STORAGE_SIZE); @@ -3632,7 +3690,8 @@ rmw_service_server_is_available( RMW_CHECK_ARGUMENT_FOR_NULL(client->data, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(is_available, RMW_RET_INVALID_ARGUMENT); - rmw_client_data_t * client_data = static_cast(client->data); + rmw_zenoh_cpp::rmw_client_data_t * client_data = + static_cast(client->data); if (client_data == nullptr) { RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( "Unable to retreive client_data from client for service %s", client->service_name); @@ -3673,8 +3732,8 @@ rmw_subscription_set_on_new_message_callback( const void * user_data) { RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); - rmw_subscription_data_t * sub_data = - static_cast(subscription->data); + rmw_zenoh_cpp::rmw_subscription_data_t * sub_data = + static_cast(subscription->data); RMW_CHECK_ARGUMENT_FOR_NULL(sub_data, RMW_RET_INVALID_ARGUMENT); sub_data->data_callback_mgr.set_callback( user_data, callback); @@ -3690,8 +3749,8 @@ rmw_service_set_on_new_request_callback( const void * user_data) { RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); - rmw_service_data_t * service_data = - static_cast(service->data); + rmw_zenoh_cpp::rmw_service_data_t * service_data = + static_cast(service->data); RMW_CHECK_ARGUMENT_FOR_NULL(service_data, RMW_RET_INVALID_ARGUMENT); service_data->data_callback_mgr.set_callback( user_data, callback); @@ -3707,8 +3766,8 @@ rmw_client_set_on_new_response_callback( const void * user_data) { RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); - rmw_client_data_t * client_data = - static_cast(client->data); + rmw_zenoh_cpp::rmw_client_data_t * client_data = + static_cast(client->data); RMW_CHECK_ARGUMENT_FOR_NULL(client_data, RMW_RET_INVALID_ARGUMENT); client_data->data_callback_mgr.set_callback( user_data, callback); diff --git a/rmw_zenoh_cpp/src/zenohd/main.cpp b/rmw_zenoh_cpp/src/zenohd/main.cpp index c09501f2..785e9664 100644 --- a/rmw_zenoh_cpp/src/zenohd/main.cpp +++ b/rmw_zenoh_cpp/src/zenohd/main.cpp @@ -175,7 +175,10 @@ int main(int argc, char ** argv) // Initialize the zenoh configuration for the router. z_owned_config_t config; - if ((rmw_zenoh_cpp::get_z_config(rmw_zenoh_cpp::ConfigurableEntity::Router, &config)) != RMW_RET_OK) { + if ((rmw_zenoh_cpp::get_z_config( + rmw_zenoh_cpp::ConfigurableEntity::Router, + &config)) != RMW_RET_OK) + { RMW_SET_ERROR_MSG("Error configuring Zenoh router."); return 1; }