diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp index 5fadb413..46191ecc 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp @@ -22,6 +22,8 @@ #include "attachment_helpers.hpp" +namespace rmw_zenoh_cpp +{ bool get_gid_from_attachment( const z_attachment_t * const attachment, uint8_t gid[RMW_GID_STORAGE_SIZE]) { @@ -91,3 +93,4 @@ int64_t get_int64_from_attachment( return num; } +} // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp index 0a9498b9..e27dc886 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp @@ -21,10 +21,13 @@ #include "rmw/types.h" +namespace rmw_zenoh_cpp +{ bool get_gid_from_attachment( const z_attachment_t * const attachment, uint8_t gid[RMW_GID_STORAGE_SIZE]); int64_t get_int64_from_attachment( const z_attachment_t * const attachment, const std::string & name); +} // namespace rmw_zenoh_cpp #endif // DETAIL__ATTACHMENT_HELPERS_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/cdr.cpp b/rmw_zenoh_cpp/src/detail/cdr.cpp index 0d8ec267..3aea5df0 100644 --- a/rmw_zenoh_cpp/src/detail/cdr.cpp +++ b/rmw_zenoh_cpp/src/detail/cdr.cpp @@ -18,7 +18,9 @@ #include "cdr.hpp" -rmw_zenoh_cpp::Cdr::Cdr(eprosima::fastcdr::FastBuffer & fastbuffer) +namespace rmw_zenoh_cpp +{ +Cdr::Cdr(eprosima::fastcdr::FastBuffer & fastbuffer) #if FASTCDR_VERSION_MAJOR == 1 : cdr_(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR) #else @@ -27,7 +29,7 @@ rmw_zenoh_cpp::Cdr::Cdr(eprosima::fastcdr::FastBuffer & fastbuffer) { } -size_t rmw_zenoh_cpp::Cdr::get_serialized_data_length() const +size_t Cdr::get_serialized_data_length() const { #if FASTCDR_VERSION_MAJOR == 1 return cdr_.getSerializedDataLength(); @@ -36,7 +38,8 @@ size_t rmw_zenoh_cpp::Cdr::get_serialized_data_length() const #endif } -eprosima::fastcdr::Cdr & rmw_zenoh_cpp::Cdr::get_cdr() +eprosima::fastcdr::Cdr & Cdr::get_cdr() { return cdr_; } +} // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/cdr.hpp b/rmw_zenoh_cpp/src/detail/cdr.hpp index 22ac942d..2c16fc75 100644 --- a/rmw_zenoh_cpp/src/detail/cdr.hpp +++ b/rmw_zenoh_cpp/src/detail/cdr.hpp @@ -33,7 +33,6 @@ class Cdr final private: eprosima::fastcdr::Cdr cdr_; }; - } // namespace rmw_zenoh_cpp #endif // DETAIL__CDR_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/event.cpp b/rmw_zenoh_cpp/src/detail/event.cpp index 6cb83f62..b40f7048 100644 --- a/rmw_zenoh_cpp/src/detail/event.cpp +++ b/rmw_zenoh_cpp/src/detail/event.cpp @@ -20,7 +20,8 @@ #include "rmw/error_handling.h" - +namespace rmw_zenoh_cpp +{ ///============================================================================= void DataCallbackManager::set_callback( const void * user_data, rmw_event_callback_t callback) @@ -229,3 +230,4 @@ void EventsManager::notify_event(rmw_zenoh_event_type_t event_id) event_conditions_[event_id]->notify_one(); } } +} // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/event.hpp b/rmw_zenoh_cpp/src/detail/event.hpp index a8246e97..ca18ca30 100644 --- a/rmw_zenoh_cpp/src/detail/event.hpp +++ b/rmw_zenoh_cpp/src/detail/event.hpp @@ -25,7 +25,8 @@ #include "rmw/event.h" #include "rmw/event_callback_type.h" - +namespace rmw_zenoh_cpp +{ ///============================================================================= // A struct that represents an event status in rmw_zenoh. enum rmw_zenoh_event_type_t @@ -157,12 +158,7 @@ class EventsManager mutable std::mutex event_condition_mutex_; /// Condition variable to attach for event notifications. std::condition_variable * event_conditions_[ZENOH_EVENT_ID_MAX + 1]{nullptr}; - /// User callback that can be set via data_callback_mgr.set_callback(). - rmw_event_callback_t callback_ {nullptr}; - /// User data that should be passed to the user callback. - const void * user_data_ {nullptr}; - /// Count for - size_t unread_count_ {0}; + rmw_event_callback_t event_callback_[ZENOH_EVENT_ID_MAX + 1] {nullptr}; const void * event_data_[ZENOH_EVENT_ID_MAX + 1] {nullptr}; size_t event_unread_count_[ZENOH_EVENT_ID_MAX + 1] {0}; @@ -170,5 +166,6 @@ class EventsManager std::deque> event_queues_[ZENOH_EVENT_ID_MAX + 1] {}; const std::size_t event_queue_depth_ = 10; }; +} // namespace rmw_zenoh_cpp #endif // DETAIL__EVENT_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index 8df25418..63cca99e 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -36,6 +36,8 @@ #include "graph_cache.hpp" #include "rmw_data_types.hpp" +namespace rmw_zenoh_cpp +{ ///============================================================================= using Entity = liveliness::Entity; using ConstEntityPtr = liveliness::ConstEntityPtr; @@ -836,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_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()) { @@ -871,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_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()) { @@ -1297,3 +1301,4 @@ std::unique_ptr GraphCache::take_event_status( status_to_take.current_count_change = 0; return result; } +} // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp index 614880b4..f3331e8b 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -35,6 +35,8 @@ #include "rmw/names_and_types.h" +namespace rmw_zenoh_cpp +{ ///============================================================================= // TODO(Yadunund): Consider changing this to an array of unordered_set where the index of the // array corresponds to the EntityType enum. This way we don't need to mix @@ -284,5 +286,6 @@ class GraphCache final // Mutex to lock before modifying the members above. mutable std::mutex graph_mutex_; }; +} // namespace rmw_zenoh_cpp #endif // DETAIL__GRAPH_CACHE_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/guard_condition.cpp b/rmw_zenoh_cpp/src/detail/guard_condition.cpp index b850095f..feb6ea48 100644 --- a/rmw_zenoh_cpp/src/detail/guard_condition.cpp +++ b/rmw_zenoh_cpp/src/detail/guard_condition.cpp @@ -15,14 +15,16 @@ #include "guard_condition.hpp" -///============================================================================== +namespace rmw_zenoh_cpp +{ +///============================================================================= GuardCondition::GuardCondition() : has_triggered_(false), condition_variable_(nullptr) { } -///============================================================================== +///============================================================================= void GuardCondition::trigger() { std::lock_guard lock(internal_mutex_); @@ -37,21 +39,21 @@ void GuardCondition::trigger() } } -///============================================================================== +///============================================================================= void GuardCondition::attach_condition(std::condition_variable * condition_variable) { std::lock_guard lock(internal_mutex_); condition_variable_ = condition_variable; } -///============================================================================== +///============================================================================= void GuardCondition::detach_condition() { std::lock_guard lock(internal_mutex_); condition_variable_ = nullptr; } -///============================================================================== +///============================================================================= bool GuardCondition::get_and_reset_trigger() { std::lock_guard lock(internal_mutex_); @@ -63,3 +65,4 @@ bool GuardCondition::get_and_reset_trigger() return ret; } +} // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/guard_condition.hpp b/rmw_zenoh_cpp/src/detail/guard_condition.hpp index b556c5f7..07e1e6b0 100644 --- a/rmw_zenoh_cpp/src/detail/guard_condition.hpp +++ b/rmw_zenoh_cpp/src/detail/guard_condition.hpp @@ -20,7 +20,9 @@ #include #include -///============================================================================== +namespace rmw_zenoh_cpp +{ +///============================================================================= class GuardCondition final { public: @@ -40,5 +42,6 @@ class GuardCondition final std::atomic_bool has_triggered_; std::condition_variable * condition_variable_; }; +} // namespace rmw_zenoh_cpp #endif // DETAIL__GUARD_CONDITION_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/identifier.cpp b/rmw_zenoh_cpp/src/detail/identifier.cpp index ce5f86d9..d0ddd65a 100644 --- a/rmw_zenoh_cpp/src/detail/identifier.cpp +++ b/rmw_zenoh_cpp/src/detail/identifier.cpp @@ -14,4 +14,7 @@ #include "identifier.hpp" +namespace rmw_zenoh_cpp +{ const char * const rmw_zenoh_identifier = "rmw_zenoh_cpp"; +} // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/identifier.hpp b/rmw_zenoh_cpp/src/detail/identifier.hpp index f7e73df9..1061829b 100644 --- a/rmw_zenoh_cpp/src/detail/identifier.hpp +++ b/rmw_zenoh_cpp/src/detail/identifier.hpp @@ -15,6 +15,9 @@ #ifndef DETAIL__IDENTIFIER_HPP_ #define DETAIL__IDENTIFIER_HPP_ +namespace rmw_zenoh_cpp +{ extern const char * const rmw_zenoh_identifier; +} // namespace rmw_zenoh_cpp #endif // DETAIL__IDENTIFIER_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp index a4b1e5ce..78d5b412 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp @@ -28,10 +28,10 @@ #include "rmw/error_handling.h" - +namespace rmw_zenoh_cpp +{ namespace liveliness { - ///============================================================================= NodeInfo::NodeInfo( std::size_t domain_id, @@ -88,7 +88,6 @@ static const char PUB_STR[] = "MP"; static const char SUB_STR[] = "MS"; static const char SRV_STR[] = "SS"; static const char CLI_STR[] = "SC"; -static const char EMPTY_NAMESPACE_REPLACEMENT = '_'; static const char KEYEXPR_DELIMITER = '/'; static const char SLASH_REPLACEMENT = '%'; static const char QOS_DELIMITER = ':'; @@ -261,7 +260,7 @@ Entity::Entity( keyexpr_parts[KeyexprIndex::Id] = id_; keyexpr_parts[KeyexprIndex::EntityStr] = entity_to_str.at(type_); // An empty namespace from rcl will contain "/" but zenoh does not allow keys with "//". - // Hence we add an "_" to denote an empty namespace such that splitting the key + // Hence we mangle the empty namespace such that splitting the key // will always result in 5 parts. keyexpr_parts[KeyexprIndex::Namespace] = mangle_name(node_info_.ns_); keyexpr_parts[KeyexprIndex::NodeName] = mangle_name(node_info_.name_); @@ -318,12 +317,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 +398,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)}); } ///============================================================================= @@ -499,5 +498,5 @@ std::string demangle_name(const std::string & input) } return output; } - } // namespace liveliness +} // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp index e1999fdb..f3db1d9e 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp @@ -25,9 +25,10 @@ #include "rmw/types.h" +namespace rmw_zenoh_cpp +{ namespace liveliness { - ///============================================================================= struct NodeInfo { @@ -213,42 +214,41 @@ std::optional keyexpr_to_qos(const std::string & keyexpr); ///============================================================================= /// Convert a Zenoh id to a string. std::string zid_to_str(const z_id_t & id); - } // namespace liveliness +} // namespace rmw_zenoh_cpp ///============================================================================= // Allow Entity to be hashed and used as a key in unordered_maps/sets namespace std { template<> -struct hash +struct hash { - auto operator()(const liveliness::Entity & entity) const -> size_t + auto operator()(const rmw_zenoh_cpp::liveliness::Entity & entity) const -> size_t { return entity.guid(); } }; template<> -struct hash +struct hash { - auto operator()(const liveliness::ConstEntityPtr & entity) const -> size_t + auto operator()(const rmw_zenoh_cpp::liveliness::ConstEntityPtr & entity) const -> size_t { return entity->guid(); } }; template<> -struct equal_to +struct equal_to { auto operator()( - const liveliness::ConstEntityPtr & lhs, - const liveliness::ConstEntityPtr & rhs) const -> bool + const rmw_zenoh_cpp::liveliness::ConstEntityPtr & lhs, + const rmw_zenoh_cpp::liveliness::ConstEntityPtr & rhs) const -> bool { return lhs->guid() == rhs->guid(); } }; - } // namespace std #endif // DETAIL__LIVELINESS_UTILS_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/message_type_support.cpp b/rmw_zenoh_cpp/src/detail/message_type_support.cpp index e270a12a..ff92bb27 100644 --- a/rmw_zenoh_cpp/src/detail/message_type_support.cpp +++ b/rmw_zenoh_cpp/src/detail/message_type_support.cpp @@ -24,6 +24,8 @@ #include "message_type_support.hpp" #include "type_support_common.hpp" +namespace rmw_zenoh_cpp +{ MessageTypeSupport::MessageTypeSupport(const message_type_support_callbacks_t * members) { assert(members); @@ -33,3 +35,4 @@ MessageTypeSupport::MessageTypeSupport(const message_type_support_callbacks_t * set_members(members); } +} // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/message_type_support.hpp b/rmw_zenoh_cpp/src/detail/message_type_support.hpp index 538064f5..55cdbf55 100644 --- a/rmw_zenoh_cpp/src/detail/message_type_support.hpp +++ b/rmw_zenoh_cpp/src/detail/message_type_support.hpp @@ -22,11 +22,14 @@ #include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" #include "type_support.hpp" -///============================================================================== +namespace rmw_zenoh_cpp +{ +///============================================================================= class MessageTypeSupport final : public TypeSupport { public: explicit MessageTypeSupport(const message_type_support_callbacks_t * members); }; +} // namespace rmw_zenoh_cpp #endif // DETAIL__MESSAGE_TYPE_SUPPORT_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp index 770863b4..0f14b9af 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp @@ -31,12 +31,32 @@ #include "attachment_helpers.hpp" #include "rmw_data_types.hpp" +///============================================================================= +static size_t hash_gid(const uint8_t * gid) +{ + std::stringstream hash_str; + hash_str << std::hex; + size_t i = 0; + for (; i < (RMW_GID_STORAGE_SIZE - 1); i++) { + hash_str << static_cast(gid[i]); + } + return std::hash{}(hash_str.str()); +} + +///============================================================================= +static size_t hash_gid(const rmw_request_id_t & request_id) +{ + return hash_gid(request_id.writer_guid); +} + ///============================================================================= 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, @@ -102,7 +122,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; @@ -133,7 +153,24 @@ void rmw_subscription_data_t::add_new_message( } } - // TODO(Yadunund): Check for ZENOH_EVENT_MESSAGE_LOST. + // Check for messages lost if the new sequence number is not monotonically increasing. + const size_t gid_hash = hash_gid(msg->publisher_gid); + auto last_known_pub_it = last_known_published_msg_.find(gid_hash); + if (last_known_pub_it != last_known_published_msg_.end()) { + const int64_t seq_increment = std::abs(msg->sequence_number - last_known_pub_it->second); + if (seq_increment > 1) { + const size_t num_msg_lost = seq_increment - 1; + total_messages_lost_ += num_msg_lost; + auto event_status = std::make_unique(); + event_status->total_count_change = num_msg_lost; + event_status->total_count = total_messages_lost_; + events_mgr.add_new_event( + ZENOH_EVENT_MESSAGE_LOST, + std::move(event_status)); + } + } + // Always update the last known sequence number for the publisher + last_known_published_msg_[gid_hash] = msg->sequence_number; message_queue_.emplace_back(std::move(msg)); @@ -209,17 +246,6 @@ void rmw_service_data_t::add_new_query(std::unique_ptr query) notify(); } -static size_t hash_gid(const rmw_request_id_t & request_id) -{ - std::stringstream hash_str; - hash_str << std::hex; - size_t i = 0; - for (; i < (RMW_GID_STORAGE_SIZE - 1); i++) { - hash_str << static_cast(request_id.writer_guid[i]); - } - return std::hash{}(hash_str.str()); -} - ///============================================================================= bool rmw_service_data_t::add_to_query_map( const rmw_request_id_t & request_id, std::unique_ptr query) @@ -429,7 +455,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_service_data_t * service_data = + static_cast(data); if (service_data == nullptr) { RCUTILS_LOG_ERROR_NAMED( "rmw_zenoh_cpp", @@ -508,3 +535,4 @@ void client_data_handler(z_owned_reply_t * reply, void * data) // Since we took ownership of the reply, null it out here *reply = z_reply_null(); } +} // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 6ea2564a..a43af5d3 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -59,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(); @@ -68,6 +68,8 @@ class rmw_context_impl_s final size_t next_entity_id_{0}; }; +namespace rmw_zenoh_cpp +{ ///============================================================================= struct rmw_node_data_t { @@ -189,6 +191,10 @@ class rmw_subscription_data_t final std::deque> message_queue_; mutable std::mutex message_queue_mutex_; + // Map GID of a publisher to the sequence number of the message it published. + std::unordered_map last_known_published_msg_; + size_t total_messages_lost_{0}; + void notify(); std::condition_variable * condition_{nullptr}; @@ -316,7 +322,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; @@ -324,7 +330,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; @@ -337,8 +343,9 @@ 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 #endif // DETAIL__RMW_DATA_TYPES_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp b/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp index f6ec5881..1574abc5 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_init_options_impl.hpp @@ -17,11 +17,14 @@ #include +namespace rmw_zenoh_cpp +{ // TODO(YV): Consider using this again. struct rmw_init_options_impl_s { // An owned config. z_owned_config_t config; }; +} // namespace rmw_zenoh_cpp #endif // DETAIL__RMW_INIT_OPTIONS_IMPL_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/serialization_format.hpp b/rmw_zenoh_cpp/src/detail/serialization_format.hpp index f6f90269..f65dde6a 100644 --- a/rmw_zenoh_cpp/src/detail/serialization_format.hpp +++ b/rmw_zenoh_cpp/src/detail/serialization_format.hpp @@ -15,6 +15,9 @@ #ifndef DETAIL__SERIALIZATION_FORMAT_HPP_ #define DETAIL__SERIALIZATION_FORMAT_HPP_ +namespace rmw_zenoh_cpp +{ extern inline const char * const rmw_zenoh_serialization_format = "cdr"; +} // namespace rmw_zenoh_cpp #endif // DETAIL__SERIALIZATION_FORMAT_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/service_type_support.cpp b/rmw_zenoh_cpp/src/detail/service_type_support.cpp index 4cdee389..6101a208 100644 --- a/rmw_zenoh_cpp/src/detail/service_type_support.cpp +++ b/rmw_zenoh_cpp/src/detail/service_type_support.cpp @@ -23,10 +23,14 @@ #include "service_type_support.hpp" #include "type_support_common.hpp" +namespace rmw_zenoh_cpp +{ +///============================================================================= ServiceTypeSupport::ServiceTypeSupport() { } +///============================================================================= RequestTypeSupport::RequestTypeSupport(const service_type_support_callbacks_t * members) { assert(members); @@ -39,6 +43,7 @@ RequestTypeSupport::RequestTypeSupport(const service_type_support_callbacks_t * set_members(msg); } +///============================================================================= ResponseTypeSupport::ResponseTypeSupport(const service_type_support_callbacks_t * members) { assert(members); @@ -50,3 +55,4 @@ ResponseTypeSupport::ResponseTypeSupport(const service_type_support_callbacks_t set_members(msg); } +} // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/service_type_support.hpp b/rmw_zenoh_cpp/src/detail/service_type_support.hpp index 0e87bfca..1e5808f7 100644 --- a/rmw_zenoh_cpp/src/detail/service_type_support.hpp +++ b/rmw_zenoh_cpp/src/detail/service_type_support.hpp @@ -22,23 +22,28 @@ #include "rosidl_typesupport_fastrtps_cpp/service_type_support.h" #include "type_support.hpp" -///============================================================================== +namespace rmw_zenoh_cpp +{ +///============================================================================= class ServiceTypeSupport : public TypeSupport { protected: ServiceTypeSupport(); }; +///============================================================================= class RequestTypeSupport final : public ServiceTypeSupport { public: explicit RequestTypeSupport(const service_type_support_callbacks_t * members); }; +///============================================================================= class ResponseTypeSupport final : public ServiceTypeSupport { public: explicit ResponseTypeSupport(const service_type_support_callbacks_t * members); }; +} // namespace rmw_zenoh_cpp #endif // DETAIL__SERVICE_TYPE_SUPPORT_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/type_support.cpp b/rmw_zenoh_cpp/src/detail/type_support.cpp index dadd94fa..fff22474 100644 --- a/rmw_zenoh_cpp/src/detail/type_support.cpp +++ b/rmw_zenoh_cpp/src/detail/type_support.cpp @@ -24,6 +24,9 @@ #include "type_support.hpp" +namespace rmw_zenoh_cpp +{ +///============================================================================= TypeSupport::TypeSupport() : max_size_bound_(false), is_plain_(false), type_size_(0) {} @@ -66,6 +69,7 @@ void TypeSupport::set_members(const message_type_support_callbacks_t * members) type_size_ = (type_size_ + 3) & ~3; } +///============================================================================= size_t TypeSupport::get_estimated_serialized_size(const void * ros_message, const void * impl) const { if (is_plain_) { @@ -81,6 +85,7 @@ size_t TypeSupport::get_estimated_serialized_size(const void * ros_message, cons return 4 + callbacks->get_serialized_size(ros_message); } +///============================================================================= bool TypeSupport::serialize_ros_message( const void * ros_message, eprosima::fastcdr::Cdr & ser, @@ -103,6 +108,7 @@ bool TypeSupport::serialize_ros_message( return true; } +///============================================================================= bool TypeSupport::deserialize_ros_message( eprosima::fastcdr::Cdr & deser, void * ros_message, @@ -134,3 +140,4 @@ bool TypeSupport::deserialize_ros_message( return true; } +} // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/type_support.hpp b/rmw_zenoh_cpp/src/detail/type_support.hpp index d90dcdcf..a4714863 100644 --- a/rmw_zenoh_cpp/src/detail/type_support.hpp +++ b/rmw_zenoh_cpp/src/detail/type_support.hpp @@ -26,6 +26,9 @@ #include "rosidl_typesupport_fastrtps_cpp/message_type_support.h" +namespace rmw_zenoh_cpp +{ +///============================================================================= enum SerializedDataType { FASTRTPS_SERIALIZED_DATA_TYPE_CDR_BUFFER, @@ -33,6 +36,7 @@ enum SerializedDataType FASTRTPS_SERIALIZED_DATA_TYPE_ROS_MESSAGE }; +///============================================================================= // Publishers write method will receive a pointer to this struct struct SerializedData { @@ -41,6 +45,7 @@ struct SerializedData const void * impl; // RMW implementation specific data }; +///============================================================================= class TypeSupport { public: @@ -74,5 +79,6 @@ class TypeSupport std::string topic_data_type_name_; }; +} // namespace rmw_zenoh_cpp #endif // DETAIL__TYPE_SUPPORT_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/type_support_common.cpp b/rmw_zenoh_cpp/src/detail/type_support_common.cpp index 0186f4e5..0ace875a 100644 --- a/rmw_zenoh_cpp/src/detail/type_support_common.cpp +++ b/rmw_zenoh_cpp/src/detail/type_support_common.cpp @@ -25,6 +25,9 @@ #include "type_support_common.hpp" +namespace rmw_zenoh_cpp +{ +///============================================================================= std::string _create_type_name( const message_type_support_callbacks_t * members) @@ -44,3 +47,4 @@ _create_type_name( return ss.str(); } +} // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/type_support_common.hpp b/rmw_zenoh_cpp/src/detail/type_support_common.hpp index b80f732f..a5ca2d8e 100644 --- a/rmw_zenoh_cpp/src/detail/type_support_common.hpp +++ b/rmw_zenoh_cpp/src/detail/type_support_common.hpp @@ -28,6 +28,10 @@ #define RMW_ZENOH_CPP_TYPESUPPORT_C rosidl_typesupport_fastrtps_c__identifier #define RMW_ZENOH_CPP_TYPESUPPORT_CPP rosidl_typesupport_fastrtps_cpp::typesupport_identifier +namespace rmw_zenoh_cpp +{ +///============================================================================= std::string _create_type_name(const message_type_support_callbacks_t * members); +} // namespace rmw_zenoh_cpp #endif // DETAIL__TYPE_SUPPORT_COMMON_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp index de0b5a0c..dcf2048c 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp @@ -23,7 +23,9 @@ #include #include -///============================================================================== +///============================================================================= +namespace rmw_zenoh_cpp +{ namespace { /// Map the configurable entity to a pair of environment variable name that @@ -76,7 +78,7 @@ rmw_ret_t _get_z_config( } } // namespace -///============================================================================== +///============================================================================= rmw_ret_t get_z_config(const ConfigurableEntity & entity, z_owned_config_t * config) { auto envar_map_it = envar_map.find(entity); @@ -93,7 +95,7 @@ rmw_ret_t get_z_config(const ConfigurableEntity & entity, z_owned_config_t * con return _get_z_config(envar_map_it->second.first, default_config_path.c_str(), config); } -///============================================================================== +///============================================================================= std::optional zenoh_router_check_attempts() { const char * envar_value; @@ -123,3 +125,4 @@ std::optional zenoh_router_check_attempts() // If unset, check indefinitely. return default_value; } +} // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.hpp b/rmw_zenoh_cpp/src/detail/zenoh_config.hpp index 1a2c6a59..221d726b 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.hpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.hpp @@ -23,7 +23,9 @@ #include "rmw/ret_types.h" -///============================================================================== +namespace rmw_zenoh_cpp +{ +///============================================================================= enum class ConfigurableEntity : uint8_t { Invalid = 0, @@ -31,7 +33,7 @@ enum class ConfigurableEntity : uint8_t Router }; -///============================================================================== +///============================================================================= /// Get the zenoh configuration for a configurable entity. /// @details The behavior is as follows: /// - If the environment variable for the entity is set, the z_owned_config_t @@ -44,7 +46,7 @@ enum class ConfigurableEntity : uint8_t [[nodiscard]] rmw_ret_t get_z_config(const ConfigurableEntity & entity, z_owned_config_t * config); -///============================================================================== +///============================================================================= /// Get the number of times rmw_init should try to connect to a zenoh router /// based on the environment variable ZENOH_ROUTER_CHECK_ATTEMPTS. /// @details The behavior is as follows: @@ -54,5 +56,6 @@ rmw_ret_t get_z_config(const ConfigurableEntity & entity, z_owned_config_t * con /// @return The number of times to try connecting to a zenoh router and /// std::nullopt if establishing a connection to a router is not required. std::optional zenoh_router_check_attempts(); +} // namespace rmw_zenoh_cpp #endif // DETAIL__ZENOH_CONFIG_HPP_ diff --git a/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp b/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp index 5f12e89d..b23ccb1a 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_router_check.cpp @@ -23,6 +23,8 @@ #include "liveliness_utils.hpp" +namespace rmw_zenoh_cpp +{ ///============================================================================= rmw_ret_t zenoh_router_check(z_session_t session) { @@ -59,3 +61,4 @@ rmw_ret_t zenoh_router_check(z_session_t session) return ret; } +} // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/zenoh_router_check.hpp b/rmw_zenoh_cpp/src/detail/zenoh_router_check.hpp index 652b2706..f53bc2f5 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_router_check.hpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_router_check.hpp @@ -19,9 +19,12 @@ #include "rmw/ret_types.h" +namespace rmw_zenoh_cpp +{ /// Check if a Zenoh router is connected to the session. /// @param session Zenoh session to check. /// @return RMW_RET_OK if a Zenoh router is connected to the session. rmw_ret_t zenoh_router_check(z_session_t session); +} // namespace rmw_zenoh_cpp #endif // DETAIL__ZENOH_ROUTER_CHECK_HPP_ diff --git a/rmw_zenoh_cpp/src/rmw_event.cpp b/rmw_zenoh_cpp/src/rmw_event.cpp index ef286b73..33a950aa 100644 --- a/rmw_zenoh_cpp/src/rmw_event.cpp +++ b/rmw_zenoh_cpp/src/rmw_event.cpp @@ -25,7 +25,7 @@ extern "C" { -///============================================================================== +///============================================================================= /// Initialize a rmw publisher event rmw_ret_t rmw_publisher_event_init( @@ -37,18 +37,19 @@ 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); - if (publisher->implementation_identifier != rmw_zenoh_identifier) { + if (publisher->implementation_identifier != rmw_zenoh_cpp::rmw_zenoh_identifier) { RMW_SET_ERROR_MSG("Publisher implementation identifier not from this implementation"); 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; @@ -77,7 +79,7 @@ rmw_publisher_event_init( return RMW_RET_OK; } -///============================================================================== +///============================================================================= /// Take an event from the event handle. rmw_ret_t rmw_subscription_event_init( @@ -89,19 +91,20 @@ 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); - if (subscription->implementation_identifier != rmw_zenoh_identifier) { + if (subscription->implementation_identifier != rmw_zenoh_cpp::rmw_zenoh_identifier) { RMW_SET_ERROR_MSG( "Subscription implementation identifier not from this implementation"); 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, @@ -165,7 +170,7 @@ rmw_event_set_callback( return RMW_RET_OK; } -///============================================================================== +///============================================================================= rmw_ret_t rmw_take_event( const rmw_event_t * event_handle, @@ -178,23 +183,24 @@ rmw_take_event( *taken = false; - if (event_handle->implementation_identifier != rmw_zenoh_identifier) { + if (event_handle->implementation_identifier != rmw_zenoh_cpp::rmw_zenoh_identifier) { RMW_SET_ERROR_MSG( "Event implementation identifier not from this implementation"); 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_network_flow_endpoints.cpp b/rmw_zenoh_cpp/src/rmw_get_network_flow_endpoints.cpp index 6ad3f52d..64006c10 100644 --- a/rmw_zenoh_cpp/src/rmw_get_network_flow_endpoints.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_network_flow_endpoints.cpp @@ -19,7 +19,7 @@ extern "C" { -///============================================================================== +///============================================================================= /// Get network flow endpoints of a publisher rmw_ret_t rmw_publisher_get_network_flow_endpoints( @@ -33,7 +33,7 @@ rmw_publisher_get_network_flow_endpoints( return RMW_RET_UNSUPPORTED; } -///============================================================================== +///============================================================================= /// Get network flow endpoints of a subscription rmw_ret_t rmw_subscription_get_network_flow_endpoints( 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 bac35e45..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 @@ -24,7 +24,7 @@ extern "C" { -///============================================================================== +///============================================================================= // Return all topic names and types for which a given remote node has subscriptions. rmw_ret_t rmw_get_subscriber_names_and_types_by_node( @@ -39,12 +39,12 @@ rmw_get_subscriber_names_and_types_by_node( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 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, @@ -52,7 +52,7 @@ rmw_get_subscriber_names_and_types_by_node( topic_names_and_types); } -///============================================================================== +///============================================================================= /// Return all topic names and types for which a given remote node has publishers. rmw_ret_t rmw_get_publisher_names_and_types_by_node( @@ -67,12 +67,12 @@ rmw_get_publisher_names_and_types_by_node( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 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, @@ -80,7 +80,7 @@ rmw_get_publisher_names_and_types_by_node( topic_names_and_types); } -///============================================================================== +///============================================================================= /// Return all service names and types for which a given remote node has servers. rmw_ret_t rmw_get_service_names_and_types_by_node( @@ -94,12 +94,12 @@ rmw_get_service_names_and_types_by_node( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 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, @@ -107,7 +107,7 @@ rmw_get_service_names_and_types_by_node( service_names_and_types); } -///============================================================================== +///============================================================================= /// Return all service names and types for which a given remote node has clients. rmw_ret_t rmw_get_client_names_and_types_by_node( @@ -121,12 +121,12 @@ rmw_get_client_names_and_types_by_node( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 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_service_names_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp index acda0b19..12415ebe 100644 --- a/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_service_names_and_types.cpp @@ -22,7 +22,7 @@ extern "C" { -///============================================================================== +///============================================================================= /// Return all service names and types in the ROS graph. rmw_ret_t rmw_get_service_names_and_types( 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 9dcc0d65..a4c4878d 100644 --- a/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -25,7 +25,7 @@ extern "C" { -///============================================================================== +///============================================================================= /// Retrieve endpoint information for each known publisher of a given topic. rmw_ret_t rmw_get_publishers_info_by_topic( @@ -39,19 +39,19 @@ rmw_get_publishers_info_by_topic( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 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, publishers_info); } -///============================================================================== +///============================================================================= /// Retrieve endpoint information for each known subscription of a given topic. rmw_ret_t rmw_get_subscriptions_info_by_topic( @@ -65,12 +65,12 @@ rmw_get_subscriptions_info_by_topic( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 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_get_topic_names_and_types.cpp b/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp index 673f56d5..db7d972c 100644 --- a/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp +++ b/rmw_zenoh_cpp/src/rmw_get_topic_names_and_types.cpp @@ -22,7 +22,7 @@ extern "C" { -///============================================================================== +///============================================================================= /// Return all topic names and types in the ROS graph. rmw_ret_t rmw_get_topic_names_and_types( diff --git a/rmw_zenoh_cpp/src/rmw_init.cpp b/rmw_zenoh_cpp/src/rmw_init.cpp index edd95455..b70c0aa7 100644 --- a/rmw_zenoh_cpp/src/rmw_init.cpp +++ b/rmw_zenoh_cpp/src/rmw_init.cpp @@ -91,7 +91,7 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) RMW_CHECK_TYPE_IDENTIFIERS_MATCH( options, options->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_FOR_NULL_WITH_MSG( options->enclave, @@ -106,7 +106,7 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) [context]() {*context = rmw_get_zero_initialized_context();}); context->instance_id = options->instance_id; - context->implementation_identifier = rmw_zenoh_identifier; + context->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; // No custom handling of RMW_DEFAULT_DOMAIN_ID. Simply use a reasonable domain id. context->actual_domain_id = RMW_DEFAULT_DOMAIN_ID != options->domain_id ? options->domain_id : 0u; @@ -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_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. @@ -364,7 +374,7 @@ rmw_shutdown(rmw_context_t * context) RMW_CHECK_TYPE_IDENTIFIERS_MATCH( context, context->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); z_undeclare_subscriber(z_move(context->impl->graph_subscriber)); @@ -395,7 +405,7 @@ rmw_context_fini(rmw_context_t * context) RMW_CHECK_TYPE_IDENTIFIERS_MATCH( context, context->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); if (!context->impl->is_shutdown) { RCUTILS_SET_ERROR_MSG("context has not been shutdown"); @@ -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_init_options.cpp b/rmw_zenoh_cpp/src/rmw_init_options.cpp index 9eb146a4..e5ca732c 100644 --- a/rmw_zenoh_cpp/src/rmw_init_options.cpp +++ b/rmw_zenoh_cpp/src/rmw_init_options.cpp @@ -39,7 +39,7 @@ rmw_init_options_init(rmw_init_options_t * init_options, rcutils_allocator_t all } init_options->instance_id = 0; - init_options->implementation_identifier = rmw_zenoh_identifier; + init_options->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; init_options->allocator = allocator; init_options->impl = nullptr; init_options->enclave = NULL; @@ -64,7 +64,7 @@ rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst) RMW_CHECK_TYPE_IDENTIFIERS_MATCH( src, src->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); if (NULL != dst->implementation_identifier) { RMW_SET_ERROR_MSG("expected zero-initialized dst"); @@ -106,7 +106,7 @@ rmw_init_options_fini(rmw_init_options_t * init_options) RMW_CHECK_TYPE_IDENTIFIERS_MATCH( init_options, init_options->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); rcutils_allocator_t * allocator = &init_options->allocator; RCUTILS_CHECK_ALLOCATOR(allocator, return RMW_RET_INVALID_ARGUMENT); 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 baf31baa..43341974 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -81,8 +81,9 @@ namespace // careful about who owns the string. z_owned_keyexpr_t ros_topic_name_to_zenoh_key(const char * const topic_name, size_t domain_id) { - const std::string keyexpr_str = std::to_string(domain_id) + "/" + liveliness::mangle_name( - topic_name); + const std::string keyexpr_str = std::to_string(domain_id) + + "/" + + rmw_zenoh_cpp::liveliness::mangle_name(topic_name); return z_keyexpr_new(keyexpr_str.c_str()); } @@ -148,7 +149,7 @@ extern "C" const char * rmw_get_implementation_identifier(void) { - return rmw_zenoh_identifier; + return rmw_zenoh_cpp::rmw_zenoh_identifier; } //============================================================================== @@ -156,7 +157,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; } //============================================================================== @@ -188,7 +189,7 @@ rmw_create_node( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( context, context->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return nullptr); RMW_CHECK_FOR_NULL_WITH_MSG( context->impl, @@ -255,7 +256,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", @@ -265,18 +267,19 @@ rmw_create_node( allocator->deallocate(node->data, allocator->state); }); - node->implementation_identifier = rmw_zenoh_identifier; + node->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; 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", @@ -319,12 +322,13 @@ rmw_destroy_node(rmw_node_t * node) RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); // 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; @@ -346,7 +350,7 @@ rmw_node_get_graph_guard_condition(const rmw_node_t * node) RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(node->context, nullptr); @@ -407,7 +411,7 @@ rmw_create_publisher( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); @@ -437,7 +441,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."); @@ -485,8 +490,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", @@ -496,11 +501,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); @@ -521,8 +528,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", @@ -536,17 +543,17 @@ 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; rmw_publisher->data = publisher_data; - rmw_publisher->implementation_identifier = rmw_zenoh_identifier; + rmw_publisher->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; rmw_publisher->options = *publisher_options; rmw_publisher->can_loan_messages = false; @@ -616,13 +623,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) { @@ -674,19 +683,19 @@ rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( publisher, publisher->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); rmw_ret_t ret = RMW_RET_OK; 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()) { @@ -833,13 +842,13 @@ rmw_publish( publisher, "publisher handle is null", return RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - publisher, publisher->implementation_identifier, rmw_zenoh_identifier, + publisher, publisher->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_FOR_NULL_WITH_MSG( 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); @@ -974,11 +983,12 @@ rmw_publisher_count_matched_subscriptions( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( publisher, publisher->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, 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); @@ -999,11 +1009,12 @@ rmw_publisher_get_actual_qos( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( publisher, publisher->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, 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; @@ -1023,13 +1034,13 @@ rmw_publish_serialized_message( publisher, "publisher handle is null", return RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - publisher, publisher->implementation_identifier, rmw_zenoh_identifier, + publisher, publisher->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_FOR_NULL_WITH_MSG( 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); @@ -1129,7 +1140,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) { @@ -1163,7 +1174,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); @@ -1211,7 +1222,7 @@ rmw_create_subscription( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); @@ -1229,7 +1240,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); @@ -1269,8 +1280,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", @@ -1280,12 +1291,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 @@ -1305,8 +1316,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", @@ -1320,17 +1331,17 @@ 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; - rmw_subscription->implementation_identifier = rmw_zenoh_identifier; + rmw_subscription->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; rmw_subscription->topic_name = rcutils_strdup(topic_name, *allocator); RMW_CHECK_FOR_NULL_WITH_MSG( @@ -1349,7 +1360,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( @@ -1424,13 +1435,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) { @@ -1481,19 +1494,19 @@ rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( subscription, subscription->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); rmw_ret_t ret = RMW_RET_OK; 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)); @@ -1537,11 +1550,12 @@ rmw_subscription_count_matched_publishers( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( subscription, subscription->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, 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); @@ -1562,11 +1576,11 @@ rmw_subscription_get_actual_qos( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( subscription, subscription->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, 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; @@ -1613,12 +1627,12 @@ static rmw_ret_t __rmw_take( RMW_CHECK_ARGUMENT_FOR_NULL(message_info, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( subscription handle, - subscription->implementation_identifier, rmw_zenoh_identifier, + subscription->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); *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) { @@ -1627,7 +1641,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; @@ -1656,7 +1670,7 @@ static rmw_ret_t __rmw_take( message_info->publication_sequence_number = msg_data->sequence_number; // TODO(clalancette): fill in reception_sequence_number message_info->reception_sequence_number = 0; - message_info->publisher_gid.implementation_identifier = rmw_zenoh_identifier; + message_info->publisher_gid.implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; memcpy(message_info->publisher_gid.data, msg_data->publisher_gid, RMW_GID_STORAGE_SIZE); message_info->from_intra_process = false; @@ -1728,12 +1742,12 @@ static rmw_ret_t __rmw_take_serialized( RMW_CHECK_ARGUMENT_FOR_NULL(message_info, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( subscription handle, - subscription->implementation_identifier, rmw_zenoh_identifier, + subscription->implementation_identifier, rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); *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) { @@ -1742,7 +1756,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; @@ -1767,7 +1781,7 @@ static rmw_ret_t __rmw_take_serialized( message_info->publication_sequence_number = msg_data->sequence_number; // TODO(clalancette): fill in reception_sequence_number message_info->reception_sequence_number = 0; - message_info->publisher_gid.implementation_identifier = rmw_zenoh_identifier; + message_info->publisher_gid.implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; memcpy(message_info->publisher_gid.data, msg_data->publisher_gid, RMW_GID_STORAGE_SIZE); message_info->from_intra_process = false; @@ -1865,7 +1879,7 @@ rmw_create_client( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(service_name, nullptr); @@ -1895,7 +1909,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."); @@ -1932,8 +1947,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", @@ -1943,12 +1958,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); @@ -1981,12 +1996,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]() { @@ -1997,21 +2012,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]() { @@ -2022,16 +2037,16 @@ 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. - rmw_client->implementation_identifier = rmw_zenoh_identifier; + rmw_client->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; rmw_client->service_name = rcutils_strdup(service_name, *allocator); RMW_CHECK_FOR_NULL_WITH_MSG( rmw_client->service_name, @@ -2067,13 +2082,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) { @@ -2127,17 +2144,18 @@ rmw_destroy_client(rmw_node_t * node, rmw_client_t * client) RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( client, client->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 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.", @@ -2149,11 +2167,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, ); @@ -2180,10 +2199,11 @@ rmw_send_request( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( client, client->implementation_identifier, - rmw_zenoh_identifier, + 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.", @@ -2257,7 +2277,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); @@ -2283,16 +2304,17 @@ rmw_take_response( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( client, client->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 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; @@ -2323,20 +2345,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; } @@ -2361,11 +2386,12 @@ rmw_client_request_publisher_get_actual_qos( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( client, client->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, 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; @@ -2397,7 +2423,7 @@ rmw_create_service( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(service_name, nullptr); @@ -2420,7 +2446,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."); @@ -2461,8 +2488,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", @@ -2472,12 +2499,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 @@ -2508,11 +2537,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]() { @@ -2522,20 +2551,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]() { @@ -2545,16 +2574,16 @@ 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. - rmw_service->implementation_identifier = rmw_zenoh_identifier; + rmw_service->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; rmw_service->service_name = rcutils_strdup(service_name, *allocator); RMW_CHECK_FOR_NULL_WITH_MSG( rmw_service->service_name, @@ -2577,7 +2606,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; @@ -2610,13 +2641,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) { @@ -2672,17 +2705,18 @@ rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( service, service->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 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", @@ -2694,14 +2728,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); @@ -2729,17 +2764,18 @@ rmw_take_request( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( service, service->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); 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; @@ -2772,19 +2808,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; } @@ -2820,7 +2861,7 @@ rmw_send_response( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( service, service->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_FOR_NULL_WITH_MSG( @@ -2828,10 +2869,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 @@ -2908,11 +2950,12 @@ rmw_service_request_subscription_get_actual_qos( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( service, service->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, 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; @@ -2949,10 +2992,12 @@ rmw_create_guard_condition(rmw_context_t * context) allocator->deallocate(guard_condition, allocator->state); }); - guard_condition->implementation_identifier = rmw_zenoh_identifier; + 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", @@ -2962,11 +3007,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(); @@ -2985,7 +3031,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); } @@ -3002,10 +3048,10 @@ rmw_trigger_guard_condition(const rmw_guard_condition_t * guard_condition) RMW_CHECK_TYPE_IDENTIFIERS_MATCH( guard_condition, guard_condition->implementation_identifier, - rmw_zenoh_identifier, + 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; } @@ -3021,7 +3067,7 @@ rmw_create_wait_set(rmw_context_t * context, size_t max_conditions) RMW_CHECK_TYPE_IDENTIFIERS_MATCH( context, context->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return nullptr); rcutils_allocator_t * allocator = &context->options.allocator; @@ -3037,9 +3083,11 @@ rmw_create_wait_set(rmw_context_t * context, size_t max_conditions) allocator->deallocate(wait_set, allocator->state); }); - wait_set->implementation_identifier = rmw_zenoh_identifier; + 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", @@ -3050,15 +3098,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(); @@ -3077,10 +3125,10 @@ rmw_destroy_wait_set(rmw_wait_set_t * wait_set) RMW_CHECK_TYPE_IDENTIFIERS_MATCH( wait_set, wait_set->implementation_identifier, - rmw_zenoh_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); rcutils_allocator_t * allocator = &wait_set_data->context->options.allocator; @@ -3103,7 +3151,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; @@ -3117,9 +3166,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; @@ -3134,7 +3183,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; @@ -3145,7 +3195,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; @@ -3156,7 +3206,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; @@ -3183,10 +3234,10 @@ rmw_wait( RMW_CHECK_ARGUMENT_FOR_NULL(wait_set, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( wait set handle, - wait_set->implementation_identifier, rmw_zenoh_identifier, + 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", @@ -3220,7 +3271,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); } @@ -3231,7 +3283,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); } @@ -3242,7 +3295,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); } @@ -3253,7 +3306,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); } @@ -3263,10 +3317,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); @@ -3294,7 +3348,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 @@ -3310,10 +3365,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 @@ -3329,7 +3384,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 @@ -3345,7 +3401,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 @@ -3361,7 +3417,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 @@ -3433,7 +3490,7 @@ rmw_count_publishers( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, RMW_RET_INVALID_ARGUMENT); int validation_result = RMW_TOPIC_VALID; @@ -3463,7 +3520,7 @@ rmw_count_subscribers( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, RMW_RET_INVALID_ARGUMENT); int validation_result = RMW_TOPIC_VALID; @@ -3493,7 +3550,7 @@ rmw_count_clients( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(service_name, RMW_RET_INVALID_ARGUMENT); int validation_result = RMW_TOPIC_VALID; @@ -3523,7 +3580,7 @@ rmw_count_services( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(service_name, RMW_RET_INVALID_ARGUMENT); int validation_result = RMW_TOPIC_VALID; @@ -3549,9 +3606,10 @@ 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_identifier; + gid->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; memcpy(gid->data, pub_data->pub_gid, RMW_GID_STORAGE_SIZE); return RMW_RET_OK; @@ -3565,9 +3623,10 @@ 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_identifier; + gid->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; memcpy(gid->data, client_data->client_gid, RMW_GID_STORAGE_SIZE); return RMW_RET_OK; @@ -3582,13 +3641,13 @@ rmw_compare_gids_equal(const rmw_gid_t * gid1, const rmw_gid_t * gid2, bool * re RMW_CHECK_TYPE_IDENTIFIERS_MATCH( gid1, gid1->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(gid2, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( gid2, gid2->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(result, RMW_RET_INVALID_ARGUMENT); @@ -3609,13 +3668,14 @@ rmw_service_server_is_available( RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node, node->implementation_identifier, - rmw_zenoh_identifier, + rmw_zenoh_cpp::rmw_zenoh_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); 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); @@ -3656,8 +3716,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); @@ -3673,8 +3733,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); @@ -3690,8 +3750,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 e6b969ab..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 ((get_z_config(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; }