Skip to content

Commit

Permalink
More namespacing
Browse files Browse the repository at this point in the history
Signed-off-by: Yadunund <[email protected]>
  • Loading branch information
Yadunund committed May 4, 2024
1 parent d0ed920 commit 369dfad
Show file tree
Hide file tree
Showing 11 changed files with 312 additions and 227 deletions.
6 changes: 4 additions & 2 deletions rmw_zenoh_cpp/src/detail/graph_cache.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -838,7 +838,8 @@ rmw_ret_t GraphCache::publisher_count_matched_subscriptions(
*subscription_count = 0;
GraphNode::TopicMap::const_iterator topic_it = graph_topics_.find(publisher->topic_name);
if (topic_it != graph_topics_.end()) {
rmw_publisher_data_t * pub_data = static_cast<rmw_publisher_data_t *>(publisher->data);
rmw_zenoh_cpp::rmw_publisher_data_t * pub_data =
static_cast<rmw_zenoh_cpp::rmw_publisher_data_t *>(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()) {
Expand Down Expand Up @@ -873,7 +874,8 @@ rmw_ret_t GraphCache::subscription_count_matched_publishers(
*publisher_count = 0;
GraphNode::TopicMap::const_iterator topic_it = graph_topics_.find(subscription->topic_name);
if (topic_it != graph_topics_.end()) {
rmw_subscription_data_t * sub_data = static_cast<rmw_subscription_data_t *>(subscription->data);
rmw_zenoh_cpp::rmw_subscription_data_t * sub_data =
static_cast<rmw_zenoh_cpp::rmw_subscription_data_t *>(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()) {
Expand Down
24 changes: 12 additions & 12 deletions rmw_zenoh_cpp/src/detail/liveliness_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -318,12 +318,12 @@ std::shared_ptr<Entity> Entity::make(

return std::make_shared<Entity>(
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)});
}

///=============================================================================
Expand Down Expand Up @@ -399,12 +399,12 @@ std::shared_ptr<Entity> Entity::make(const std::string & keyexpr)

return std::make_shared<Entity>(
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)});
}

///=============================================================================
Expand Down
21 changes: 11 additions & 10 deletions rmw_zenoh_cpp/src/detail/rmw_data_types.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -31,14 +31,14 @@
#include "attachment_helpers.hpp"
#include "rmw_data_types.hpp"

namespace rmw_zenoh_cpp
{
///=============================================================================
size_t rmw_context_impl_s::get_next_entity_id()
{
return next_entity_id_++;
}

namespace rmw_zenoh_cpp
{
///=============================================================================
saved_msg_data::saved_msg_data(
zc_owned_payload_t p,
Expand Down Expand Up @@ -104,7 +104,7 @@ std::unique_ptr<saved_msg_data> rmw_subscription_data_t::pop_next_message()
return nullptr;
}

std::unique_ptr<saved_msg_data> msg_data = std::move(message_queue_.front());
std::unique_ptr<rmw_zenoh_cpp::saved_msg_data> msg_data = std::move(message_queue_.front());
message_queue_.pop_front();

return msg_data;
Expand Down Expand Up @@ -291,7 +291,7 @@ void rmw_client_data_t::notify()
}

///=============================================================================
void rmw_client_data_t::add_new_reply(std::unique_ptr<ZenohReply> reply)
void rmw_client_data_t::add_new_reply(std::unique_ptr<rmw_zenoh_cpp::ZenohReply> reply)
{
std::lock_guard<std::mutex> lock(reply_queue_mutex_);
if (reply_queue_.size() >= adapted_qos_profile.depth) {
Expand Down Expand Up @@ -336,15 +336,15 @@ void rmw_client_data_t::detach_condition()
}

///=============================================================================
std::unique_ptr<ZenohReply> rmw_client_data_t::pop_next_reply()
std::unique_ptr<rmw_zenoh_cpp::ZenohReply> rmw_client_data_t::pop_next_reply()
{
std::lock_guard<std::mutex> lock(reply_queue_mutex_);

if (reply_queue_.empty()) {
return nullptr;
}

std::unique_ptr<ZenohReply> latest_reply = std::move(reply_queue_.front());
std::unique_ptr<rmw_zenoh_cpp::ZenohReply> latest_reply = std::move(reply_queue_.front());
reply_queue_.pop_front();

return latest_reply;
Expand All @@ -361,7 +361,7 @@ void sub_data_handler(
z_drop(z_move(keystr));
});

auto sub_data = static_cast<rmw_subscription_data_t *>(data);
auto sub_data = static_cast<rmw_zenoh_cpp::rmw_subscription_data_t *>(data);
if (sub_data == nullptr) {
RCUTILS_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
Expand Down Expand Up @@ -431,7 +431,8 @@ void service_data_handler(const z_query_t * query, void * data)
z_drop(z_move(keystr));
});

rmw_service_data_t * service_data = static_cast<rmw_service_data_t *>(data);
rmw_zenoh_cpp::rmw_service_data_t * service_data =
static_cast<rmw_zenoh_cpp::rmw_service_data_t *>(data);
if (service_data == nullptr) {
RCUTILS_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
Expand Down Expand Up @@ -477,7 +478,7 @@ size_t rmw_client_data_t::get_next_sequence_number()
//==============================================================================
void client_data_handler(z_owned_reply_t * reply, void * data)
{
auto client_data = static_cast<rmw_client_data_t *>(data);
auto client_data = static_cast<rmw_zenoh_cpp::rmw_client_data_t *>(data);
if (client_data == nullptr) {
RCUTILS_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
Expand Down Expand Up @@ -506,7 +507,7 @@ void client_data_handler(z_owned_reply_t * reply, void * data)
return;
}

client_data->add_new_reply(std::make_unique<ZenohReply>(reply));
client_data->add_new_reply(std::make_unique<rmw_zenoh_cpp::ZenohReply>(reply));
// Since we took ownership of the reply, null it out here
*reply = z_reply_null();
}
Expand Down
13 changes: 7 additions & 6 deletions rmw_zenoh_cpp/src/detail/rmw_data_types.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,8 +38,7 @@
#include "service_type_support.hpp"

/// Structs for various type erased data fields.
namespace rmw_zenoh_cpp
{

///=============================================================================
class rmw_context_impl_s final
{
Expand All @@ -60,7 +59,7 @@ class rmw_context_impl_s final
/// Guard condition that should be triggered when the graph changes.
rmw_guard_condition_t * graph_guard_condition;

std::unique_ptr<GraphCache> graph_cache;
std::unique_ptr<rmw_zenoh_cpp::GraphCache> graph_cache;

size_t get_next_entity_id();

Expand All @@ -69,6 +68,8 @@ class rmw_context_impl_s final
size_t next_entity_id_{0};
};

namespace rmw_zenoh_cpp
{
///=============================================================================
struct rmw_node_data_t
{
Expand Down Expand Up @@ -317,15 +318,15 @@ class rmw_client_data_t final

size_t get_next_sequence_number();

void add_new_reply(std::unique_ptr<ZenohReply> reply);
void add_new_reply(std::unique_ptr<rmw_zenoh_cpp::ZenohReply> reply);

bool reply_queue_is_empty() const;

void attach_condition(std::condition_variable * condition_variable);

void detach_condition();

std::unique_ptr<ZenohReply> pop_next_reply();
std::unique_ptr<rmw_zenoh_cpp::ZenohReply> pop_next_reply();

DataCallbackManager data_callback_mgr;

Expand All @@ -338,7 +339,7 @@ class rmw_client_data_t final
std::condition_variable * condition_{nullptr};
std::mutex condition_mutex_;

std::deque<std::unique_ptr<ZenohReply>> reply_queue_;
std::deque<std::unique_ptr<rmw_zenoh_cpp::ZenohReply>> reply_queue_;
mutable std::mutex reply_queue_mutex_;
};
} // namespace rmw_zenoh_cpp
Expand Down
48 changes: 27 additions & 21 deletions rmw_zenoh_cpp/src/rmw_event.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,8 @@ rmw_publisher_event_init(
RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT);
RMW_CHECK_ARGUMENT_FOR_NULL(publisher->implementation_identifier, RMW_RET_INVALID_ARGUMENT);
RMW_CHECK_ARGUMENT_FOR_NULL(publisher->data, RMW_RET_INVALID_ARGUMENT);
rmw_publisher_data_t * pub_data = static_cast<rmw_publisher_data_t *>(publisher->data);
rmw_zenoh_cpp::rmw_publisher_data_t * pub_data =
static_cast<rmw_zenoh_cpp::rmw_publisher_data_t *>(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);
Expand All @@ -47,8 +48,8 @@ rmw_publisher_event_init(
return RMW_RET_INCORRECT_RMW_IMPLEMENTATION;
}

auto rmw_event_it = event_map.find(event_type);
if (rmw_event_it == event_map.end()) {
auto rmw_event_it = rmw_zenoh_cpp::event_map.find(event_type);
if (rmw_event_it == rmw_zenoh_cpp::event_map.end()) {
RMW_SET_ERROR_MSG_WITH_FORMAT_STRING(
"provided event_type %d is not supported by rmw_zenoh_cpp", event_type);
return RMW_RET_UNSUPPORTED;
Expand All @@ -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<rmw_zenoh_event_status_t> zenoh_event)
event_id =
rmw_event_it->second](std::unique_ptr<rmw_zenoh_cpp::rmw_zenoh_event_status_t> zenoh_event)
{
if (pub_data == nullptr) {
return;
Expand All @@ -89,7 +91,8 @@ rmw_subscription_event_init(
RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT);
RMW_CHECK_ARGUMENT_FOR_NULL(subscription->implementation_identifier, RMW_RET_INVALID_ARGUMENT);
RMW_CHECK_ARGUMENT_FOR_NULL(subscription->data, RMW_RET_INVALID_ARGUMENT);
rmw_subscription_data_t * sub_data = static_cast<rmw_subscription_data_t *>(subscription->data);
rmw_zenoh_cpp::rmw_subscription_data_t * sub_data =
static_cast<rmw_zenoh_cpp::rmw_subscription_data_t *>(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);
Expand All @@ -100,8 +103,8 @@ rmw_subscription_event_init(
return RMW_RET_INCORRECT_RMW_IMPLEMENTATION;
}

auto rmw_event_it = event_map.find(event_type);
if (rmw_event_it == event_map.end()) {
auto rmw_event_it = rmw_zenoh_cpp::event_map.find(event_type);
if (rmw_event_it == rmw_zenoh_cpp::event_map.end()) {
RMW_SET_ERROR_MSG_WITH_FORMAT_STRING(
"provided event_type %d is not supported by rmw_zenoh_cpp", event_type);
return RMW_RET_UNSUPPORTED;
Expand All @@ -113,15 +116,16 @@ 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;
}

sub_data->context->impl->graph_cache->set_qos_event_callback(
sub_data->entity,
rmw_event_it->second,
[sub_data,
event_id = rmw_event_it->second](std::unique_ptr<rmw_zenoh_event_status_t> zenoh_event)
event_id =
rmw_event_it->second](std::unique_ptr<rmw_zenoh_cpp::rmw_zenoh_event_status_t> zenoh_event)
{
if (sub_data == nullptr) {
return;
Expand All @@ -146,16 +150,17 @@ 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);
return RMW_RET_ERROR;
}

// Both rmw_subscription_data_t and rmw_publisher_data_t inherit EventsBase.
EventsManager * event_data = static_cast<EventsManager *>(rmw_event->data);
rmw_zenoh_cpp::EventsManager * event_data =
static_cast<rmw_zenoh_cpp::EventsManager *>(rmw_event->data);
RMW_CHECK_ARGUMENT_FOR_NULL(event_data, RMW_RET_INVALID_ARGUMENT);
event_data->event_set_callback(
zenoh_event_it->second,
Expand Down Expand Up @@ -184,17 +189,18 @@ rmw_take_event(
return RMW_RET_INCORRECT_RMW_IMPLEMENTATION;
}

auto zenoh_event_it = event_map.find(event_handle->event_type);
if (zenoh_event_it == event_map.end()) {
auto zenoh_event_it = rmw_zenoh_cpp::event_map.find(event_handle->event_type);
if (zenoh_event_it == rmw_zenoh_cpp::event_map.end()) {
RMW_SET_ERROR_MSG_WITH_FORMAT_STRING(
"RMW Zenoh does not support event [%d]",
event_handle->event_type);
return RMW_RET_ERROR;
}

EventsManager * event_data = static_cast<EventsManager *>(event_handle->data);
rmw_zenoh_cpp::EventsManager * event_data =
static_cast<rmw_zenoh_cpp::EventsManager *>(event_handle->data);
RMW_CHECK_ARGUMENT_FOR_NULL(event_data, RMW_RET_INVALID_ARGUMENT);
std::unique_ptr<rmw_zenoh_event_status_t> st = event_data->pop_next_event(
std::unique_ptr<rmw_zenoh_cpp::rmw_zenoh_event_status_t> st = event_data->pop_next_event(
zenoh_event_it->second);
if (st == nullptr) {
RMW_SET_ERROR_MSG_WITH_FORMAT_STRING(
Expand All @@ -205,24 +211,24 @@ 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<rmw_requested_qos_incompatible_event_status_t *>(event_info);
RMW_CHECK_ARGUMENT_FOR_NULL(ei, RMW_RET_INVALID_ARGUMENT);
ei->total_count = st->total_count;
ei->total_count_change = st->total_count_change;
*taken = true;
return RMW_RET_OK;
}
case ZENOH_EVENT_MESSAGE_LOST: {
case rmw_zenoh_cpp::ZENOH_EVENT_MESSAGE_LOST: {
auto ei = static_cast<rmw_message_lost_status_t *>(event_info);
RMW_CHECK_ARGUMENT_FOR_NULL(ei, RMW_RET_INVALID_ARGUMENT);
ei->total_count = st->total_count;
ei->total_count_change = st->total_count_change;
*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<rmw_matched_status_t *>(event_info);
RMW_CHECK_ARGUMENT_FOR_NULL(ei, RMW_RET_INVALID_ARGUMENT);
ei->total_count = st->total_count;
Expand All @@ -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<rmw_offered_qos_incompatible_event_status_t *>(event_info);
RMW_CHECK_ARGUMENT_FOR_NULL(ei, RMW_RET_INVALID_ARGUMENT);
ei->total_count = st->total_count;
Expand Down
8 changes: 4 additions & 4 deletions rmw_zenoh_cpp/src/rmw_get_node_info_and_types.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ rmw_get_subscriber_names_and_types_by_node(
RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT);
RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT);
return node->context->impl->graph_cache->get_entity_names_and_types_by_node(
liveliness::EntityType::Subscription,
rmw_zenoh_cpp::liveliness::EntityType::Subscription,
allocator,
node_name,
node_namespace,
Expand Down Expand Up @@ -72,7 +72,7 @@ rmw_get_publisher_names_and_types_by_node(
RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT);
RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT);
return node->context->impl->graph_cache->get_entity_names_and_types_by_node(
liveliness::EntityType::Publisher,
rmw_zenoh_cpp::liveliness::EntityType::Publisher,
allocator,
node_name,
node_namespace,
Expand All @@ -99,7 +99,7 @@ rmw_get_service_names_and_types_by_node(
RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT);
RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT);
return node->context->impl->graph_cache->get_entity_names_and_types_by_node(
liveliness::EntityType::Service,
rmw_zenoh_cpp::liveliness::EntityType::Service,
allocator,
node_name,
node_namespace,
Expand All @@ -126,7 +126,7 @@ rmw_get_client_names_and_types_by_node(
RMW_CHECK_ARGUMENT_FOR_NULL(node->context, RMW_RET_INVALID_ARGUMENT);
RMW_CHECK_ARGUMENT_FOR_NULL(node->context->impl, RMW_RET_INVALID_ARGUMENT);
return node->context->impl->graph_cache->get_entity_names_and_types_by_node(
liveliness::EntityType::Client,
rmw_zenoh_cpp::liveliness::EntityType::Client,
allocator,
node_name,
node_namespace,
Expand Down
Loading

0 comments on commit 369dfad

Please sign in to comment.