niklas/rmw_cyclonedds
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

fix extern "C" use upsetting gcc (and accepted by clang)

Browse source
This commit is contained in:
Erik Boasson 2018-07-10 08:14:20 +02:00
parent 01ef31359a
commit 9c71a0bed2
1 changed files with 1328 additions and 1331 deletions
Download patch file Download diff file Expand all files Collapse all files

405
rmw_cyclonedds_cpp/src/rmw_node.cpp
View file

@ -40,7 +40,7 @@
unsigned integers unsigned integers
- ... and many more things ... - ... and many more things ...
*/ */
#include <algorithm> #include <algorithm>
#include <array> #include <array>
@ -327,28 +327,26 @@ static bool desermsg(eprosima::fastcdr::Cdr& deser, void *ros_message, std::func
return false; return false;
} }
extern "C"
{
#pragma GCC visibility push(default) #pragma GCC visibility push(default)
const char *rmw_get_implementation_identifier() extern "C" const char *rmw_get_implementation_identifier()
{ {
return adlink_cyclonedds_identifier; return adlink_cyclonedds_identifier;
} }
rmw_ret_t rmw_init() extern "C" rmw_ret_t rmw_init()
{ {
return RMW_RET_OK; return RMW_RET_OK;
} }
///////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////
/////////// /////////// /////////// ///////////
/////////// NODES /////////// /////////// NODES ///////////
/////////// /////////// /////////// ///////////
///////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////
rmw_node_t *rmw_create_node(const char *name, const char *namespace_, size_t domain_id, const rmw_node_security_options_t *security_options) extern "C" rmw_node_t *rmw_create_node(const char *name, const char *namespace_, size_t domain_id, const rmw_node_security_options_t *security_options)
{ {
RET_NULL_X(name, return nullptr); RET_NULL_X(name, return nullptr);
RET_NULL_X(namespace_, return nullptr); RET_NULL_X(namespace_, return nullptr);
(void)domain_id; (void)domain_id;
@ -402,10 +400,10 @@ extern "C"
delete node_impl; delete node_impl;
fail_node_impl: fail_node_impl:
return nullptr; return nullptr;
} }
rmw_ret_t rmw_destroy_node(rmw_node_t *node) extern "C" rmw_ret_t rmw_destroy_node(rmw_node_t *node)
{ {
rmw_ret_t result_ret = RMW_RET_OK; rmw_ret_t result_ret = RMW_RET_OK;
RET_WRONG_IMPLID(node); RET_WRONG_IMPLID(node);
auto node_impl = static_cast<CddsNode *>(node->data); auto node_impl = static_cast<CddsNode *>(node->data);
@ -423,24 +421,24 @@ extern "C"
} }
delete node_impl; delete node_impl;
return result_ret; return result_ret;
} }
const rmw_guard_condition_t *rmw_node_get_graph_guard_condition(const rmw_node_t *node) extern "C" const rmw_guard_condition_t *rmw_node_get_graph_guard_condition(const rmw_node_t *node)
{ {
RET_WRONG_IMPLID_X(node, return nullptr); RET_WRONG_IMPLID_X(node, return nullptr);
auto node_impl = static_cast<CddsNode *>(node->data); auto node_impl = static_cast<CddsNode *>(node->data);
RET_NULL_X(node_impl, return nullptr); RET_NULL_X(node_impl, return nullptr);
return node_impl->graph_guard_condition; return node_impl->graph_guard_condition;
} }
///////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////
/////////// /////////// /////////// ///////////
/////////// PUBLICATIONS /////////// /////////// PUBLICATIONS ///////////
/////////// /////////// /////////// ///////////
///////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////
static rmw_ret_t rmw_write_ser(dds_entity_t pubh, eprosima::fastcdr::Cdr& ser) static rmw_ret_t rmw_write_ser(dds_entity_t pubh, eprosima::fastcdr::Cdr& ser)
{ {
const size_t sz = ser.getSerializedDataLength(); const size_t sz = ser.getSerializedDataLength();
const void *raw = static_cast<void *>(ser.getBufferPointer()); const void *raw = static_cast<void *>(ser.getBufferPointer());
/* shifting by 4 bytes skips the CDR header -- it should be identical and the entire /* shifting by 4 bytes skips the CDR header -- it should be identical and the entire
@ -453,10 +451,10 @@ extern "C"
//return RMW_RET_ERROR; //return RMW_RET_ERROR;
return RMW_RET_OK; return RMW_RET_OK;
} }
} }
rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message) extern "C" rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message)
{ {
RET_WRONG_IMPLID(publisher); RET_WRONG_IMPLID(publisher);
RET_NULL(ros_message); RET_NULL(ros_message);
auto pub = static_cast<CddsPublisher *>(publisher->data); auto pub = static_cast<CddsPublisher *>(publisher->data);
@ -469,10 +467,10 @@ extern "C"
RMW_SET_ERROR_MSG("cannot serialize data"); RMW_SET_ERROR_MSG("cannot serialize data");
return RMW_RET_ERROR; return RMW_RET_ERROR;
} }
} }
static const rosidl_message_type_support_t *get_typesupport(const rosidl_message_type_support_t *type_supports) static const rosidl_message_type_support_t *get_typesupport(const rosidl_message_type_support_t *type_supports)
{ {
const rosidl_message_type_support_t *ts; const rosidl_message_type_support_t *ts;
if ((ts = get_message_typesupport_handle(type_supports, rosidl_typesupport_introspection_c__identifier)) != nullptr) { if ((ts = get_message_typesupport_handle(type_supports, rosidl_typesupport_introspection_c__identifier)) != nullptr) {
return ts; return ts;
@ -482,24 +480,24 @@ extern "C"
RMW_SET_ERROR_MSG("type support not from this implementation"); RMW_SET_ERROR_MSG("type support not from this implementation");
return nullptr; return nullptr;
} }
} }
static std::string make_fqtopic(const char *prefix, const char *topic_name, const char *suffix, bool avoid_ros_namespace_conventions) static std::string make_fqtopic(const char *prefix, const char *topic_name, const char *suffix, bool avoid_ros_namespace_conventions)
{ {
if (avoid_ros_namespace_conventions) { if (avoid_ros_namespace_conventions) {
return std::string(topic_name) + "__" + std::string(suffix); return std::string(topic_name) + "__" + std::string(suffix);
} else { } else {
return std::string(prefix) + "/" + make_fqtopic(prefix, topic_name, suffix, true); return std::string(prefix) + "/" + make_fqtopic(prefix, topic_name, suffix, true);
} }
} }
static std::string make_fqtopic(const char *prefix, const char *topic_name, const char *suffix, const rmw_qos_profile_t *qos_policies) static std::string make_fqtopic(const char *prefix, const char *topic_name, const char *suffix, const rmw_qos_profile_t *qos_policies)
{ {
return make_fqtopic(prefix, topic_name, suffix, qos_policies->avoid_ros_namespace_conventions); return make_fqtopic(prefix, topic_name, suffix, qos_policies->avoid_ros_namespace_conventions);
} }
static dds_qos_t *create_readwrite_qos(const rmw_qos_profile_t *qos_policies) static dds_qos_t *create_readwrite_qos(const rmw_qos_profile_t *qos_policies)
{ {
dds_qos_t *qos = dds_qos_create(); dds_qos_t *qos = dds_qos_create();
switch (qos_policies->history) { switch (qos_policies->history) {
case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT:
@ -534,10 +532,10 @@ extern "C"
break; break;
} }
return qos; return qos;
} }
static CddsPublisher *create_cdds_publisher(const rmw_node_t *node, const rosidl_message_type_support_t *type_supports, const char *topic_name, const rmw_qos_profile_t *qos_policies) static CddsPublisher *create_cdds_publisher(const rmw_node_t *node, const rosidl_message_type_support_t *type_supports, const char *topic_name, const rmw_qos_profile_t *qos_policies)
{ {
RET_WRONG_IMPLID_X(node, return nullptr); RET_WRONG_IMPLID_X(node, return nullptr);
RET_NULL_OR_EMPTYSTR_X(topic_name, return nullptr); RET_NULL_OR_EMPTYSTR_X(topic_name, return nullptr);
RET_NULL_X(qos_policies, return nullptr); RET_NULL_X(qos_policies, return nullptr);
@ -588,10 +586,10 @@ extern "C"
fail_topic: fail_topic:
delete pub; delete pub;
return nullptr; return nullptr;
} }
rmw_publisher_t *rmw_create_publisher(const rmw_node_t *node, const rosidl_message_type_support_t *type_supports, const char *topic_name, const rmw_qos_profile_t *qos_policies) extern "C" rmw_publisher_t *rmw_create_publisher(const rmw_node_t *node, const rosidl_message_type_support_t *type_supports, const char *topic_name, const rmw_qos_profile_t *qos_policies)
{ {
CddsPublisher *pub; CddsPublisher *pub;
rmw_publisher_t *rmw_publisher; rmw_publisher_t *rmw_publisher;
auto node_impl = static_cast<CddsNode *>(node->data); auto node_impl = static_cast<CddsNode *>(node->data);
@ -616,10 +614,10 @@ extern "C"
delete pub; delete pub;
fail_common_init: fail_common_init:
return nullptr; return nullptr;
} }
rmw_ret_t rmw_get_gid_for_publisher(const rmw_publisher_t *publisher, rmw_gid_t *gid) extern "C" rmw_ret_t rmw_get_gid_for_publisher(const rmw_publisher_t *publisher, rmw_gid_t *gid)
{ {
RET_WRONG_IMPLID(publisher); RET_WRONG_IMPLID(publisher);
RET_NULL(gid); RET_NULL(gid);
auto pub = static_cast<const CddsPublisher *>(publisher->data); auto pub = static_cast<const CddsPublisher *>(publisher->data);
@ -629,10 +627,10 @@ extern "C"
assert(sizeof(pub->pubiid) <= sizeof(gid->data)); assert(sizeof(pub->pubiid) <= sizeof(gid->data));
memcpy(gid->data, &pub->pubiid, sizeof(pub->pubiid)); memcpy(gid->data, &pub->pubiid, sizeof(pub->pubiid));
return RMW_RET_OK; return RMW_RET_OK;
} }
rmw_ret_t rmw_compare_gids_equal(const rmw_gid_t *gid1, const rmw_gid_t *gid2, bool *result) extern "C" rmw_ret_t rmw_compare_gids_equal(const rmw_gid_t *gid1, const rmw_gid_t *gid2, bool *result)
{ {
RET_WRONG_IMPLID(gid1); RET_WRONG_IMPLID(gid1);
RET_WRONG_IMPLID(gid2); RET_WRONG_IMPLID(gid2);
RET_NULL(result); RET_NULL(result);
@ -640,10 +638,10 @@ extern "C"
memcmp instead of a simple integer comparison */ memcmp instead of a simple integer comparison */
*result = memcmp(gid1->data, gid2->data, sizeof(gid1->data)) == 0; *result = memcmp(gid1->data, gid2->data, sizeof(gid1->data)) == 0;
return RMW_RET_OK; return RMW_RET_OK;
} }
rmw_ret_t rmw_destroy_publisher(rmw_node_t *node, rmw_publisher_t *publisher) extern "C" rmw_ret_t rmw_destroy_publisher(rmw_node_t *node, rmw_publisher_t *publisher)
{ {
RET_WRONG_IMPLID(node); RET_WRONG_IMPLID(node);
RET_WRONG_IMPLID(publisher); RET_WRONG_IMPLID(publisher);
auto node_impl = static_cast<CddsNode *>(node->data); auto node_impl = static_cast<CddsNode *>(node->data);
@ -659,23 +657,23 @@ extern "C"
publisher->topic_name = nullptr; publisher->topic_name = nullptr;
rmw_publisher_free(publisher); rmw_publisher_free(publisher);
return RMW_RET_OK; return RMW_RET_OK;
} }
///////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////
/////////// /////////// /////////// ///////////
/////////// SUBSCRIPTIONS /////////// /////////// SUBSCRIPTIONS ///////////
/////////// /////////// /////////// ///////////
///////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////
static void subhandler(dds_entity_t rd, void *vsub) static void subhandler(dds_entity_t rd, void *vsub)
{ {
CddsSubscription *sub = static_cast<CddsSubscription *>(vsub); CddsSubscription *sub = static_cast<CddsSubscription *>(vsub);
(void)rd; (void)rd;
condition_add_trigger(&sub->cond, 1); condition_add_trigger(&sub->cond, 1);
} }
static CddsSubscription *create_cdds_subscription(const rmw_node_t *node, const rosidl_message_type_support_t *type_supports, const char *topic_name, const rmw_qos_profile_t *qos_policies, bool ignore_local_publications) static CddsSubscription *create_cdds_subscription(const rmw_node_t *node, const rosidl_message_type_support_t *type_supports, const char *topic_name, const rmw_qos_profile_t *qos_policies, bool ignore_local_publications)
{ {
RET_WRONG_IMPLID_X(node, return nullptr); RET_WRONG_IMPLID_X(node, return nullptr);
RET_NULL_OR_EMPTYSTR_X(topic_name, return nullptr); RET_NULL_OR_EMPTYSTR_X(topic_name, return nullptr);
RET_NULL_X(qos_policies, return nullptr); RET_NULL_X(qos_policies, return nullptr);
@ -726,10 +724,10 @@ extern "C"
fail_topic: fail_topic:
delete sub; delete sub;
return nullptr; return nullptr;
} }
rmw_subscription_t *rmw_create_subscription(const rmw_node_t *node, const rosidl_message_type_support_t *type_supports, const char *topic_name, const rmw_qos_profile_t *qos_policies, bool ignore_local_publications) extern "C" rmw_subscription_t *rmw_create_subscription(const rmw_node_t *node, const rosidl_message_type_support_t *type_supports, const char *topic_name, const rmw_qos_profile_t *qos_policies, bool ignore_local_publications)
{ {
CddsSubscription *sub; CddsSubscription *sub;
rmw_subscription_t *rmw_subscription; rmw_subscription_t *rmw_subscription;
if ((sub = create_cdds_subscription(node, type_supports, topic_name, qos_policies, ignore_local_publications)) == nullptr) { if ((sub = create_cdds_subscription(node, type_supports, topic_name, qos_policies, ignore_local_publications)) == nullptr) {
@ -752,10 +750,10 @@ extern "C"
delete sub; delete sub;
fail_common_init: fail_common_init:
return nullptr; return nullptr;
} }
rmw_ret_t rmw_destroy_subscription(rmw_node_t *node, rmw_subscription_t *subscription) extern "C" rmw_ret_t rmw_destroy_subscription(rmw_node_t *node, rmw_subscription_t *subscription)
{ {
RET_WRONG_IMPLID(node); RET_WRONG_IMPLID(node);
RET_WRONG_IMPLID(subscription); RET_WRONG_IMPLID(subscription);
auto sub = static_cast<CddsSubscription *>(subscription->data); auto sub = static_cast<CddsSubscription *>(subscription->data);
@ -769,10 +767,10 @@ extern "C"
subscription->topic_name = nullptr; subscription->topic_name = nullptr;
rmw_subscription_free(subscription); rmw_subscription_free(subscription);
return RMW_RET_OK; return RMW_RET_OK;
} }
static rmw_ret_t rmw_take_int(const rmw_subscription_t *subscription, void *ros_message, bool *taken, rmw_message_info_t *message_info) static rmw_ret_t rmw_take_int(const rmw_subscription_t *subscription, void *ros_message, bool *taken, rmw_message_info_t *message_info)
{ {
RET_NULL(taken); RET_NULL(taken);
RET_NULL(ros_message); RET_NULL(ros_message);
RET_WRONG_IMPLID(subscription); RET_WRONG_IMPLID(subscription);
@ -804,31 +802,31 @@ extern "C"
} }
*taken = false; *taken = false;
return RMW_RET_OK; return RMW_RET_OK;
} }
rmw_ret_t rmw_take(const rmw_subscription_t *subscription, void *ros_message, bool *taken) extern "C" rmw_ret_t rmw_take(const rmw_subscription_t *subscription, void *ros_message, bool *taken)
{ {
return rmw_take_int(subscription, ros_message, taken, nullptr); return rmw_take_int(subscription, ros_message, taken, nullptr);
} }
rmw_ret_t rmw_take_with_info(const rmw_subscription_t *subscription, void *ros_message, bool *taken, rmw_message_info_t *message_info) extern "C" rmw_ret_t rmw_take_with_info(const rmw_subscription_t *subscription, void *ros_message, bool *taken, rmw_message_info_t *message_info)
{ {
return rmw_take_int(subscription, ros_message, taken, message_info); return rmw_take_int(subscription, ros_message, taken, message_info);
} }
///////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////
/////////// /////////// /////////// ///////////
/////////// GUARDS AND WAITSETS /////////// /////////// GUARDS AND WAITSETS ///////////
/////////// /////////// /////////// ///////////
///////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////
struct CddsWaitset { struct CddsWaitset {
std::condition_variable condition; std::condition_variable condition;
std::mutex condition_mutex; std::mutex condition_mutex;
}; };
static void condition_set_trigger(struct condition *cond, unsigned value) static void condition_set_trigger(struct condition *cond, unsigned value)
{ {
std::lock_guard<std::mutex> lock(cond->internalMutex_); std::lock_guard<std::mutex> lock(cond->internalMutex_);
if (cond->conditionMutex_ != nullptr) { if (cond->conditionMutex_ != nullptr) {
std::unique_lock<std::mutex> clock(*cond->conditionMutex_); std::unique_lock<std::mutex> clock(*cond->conditionMutex_);
@ -844,10 +842,10 @@ extern "C"
} else { } else {
cond->triggerValue_ = value; cond->triggerValue_ = value;
} }
} }
static void condition_add_trigger(struct condition *cond, int delta) static void condition_add_trigger(struct condition *cond, int delta)
{ {
std::lock_guard<std::mutex> lock(cond->internalMutex_); std::lock_guard<std::mutex> lock(cond->internalMutex_);
if (cond->conditionMutex_ != nullptr) { if (cond->conditionMutex_ != nullptr) {
std::unique_lock<std::mutex> clock(*cond->conditionMutex_); std::unique_lock<std::mutex> clock(*cond->conditionMutex_);
@ -865,65 +863,65 @@ extern "C"
assert(delta >= 0 || cond->triggerValue_ >= (unsigned)-delta); assert(delta >= 0 || cond->triggerValue_ >= (unsigned)-delta);
cond->triggerValue_ += delta; cond->triggerValue_ += delta;
} }
} }
static void condition_attach(struct condition *cond, std::mutex *conditionMutex, std::condition_variable *conditionVariable) static void condition_attach(struct condition *cond, std::mutex *conditionMutex, std::condition_variable *conditionVariable)
{ {
std::lock_guard<std::mutex> lock(cond->internalMutex_); std::lock_guard<std::mutex> lock(cond->internalMutex_);
cond->conditionMutex_ = conditionMutex; cond->conditionMutex_ = conditionMutex;
cond->conditionVariable_ = conditionVariable; cond->conditionVariable_ = conditionVariable;
} }
static void condition_detach(struct condition *cond) static void condition_detach(struct condition *cond)
{ {
std::lock_guard<std::mutex> lock(cond->internalMutex_); std::lock_guard<std::mutex> lock(cond->internalMutex_);
cond->conditionMutex_ = nullptr; cond->conditionMutex_ = nullptr;
cond->conditionVariable_ = nullptr; cond->conditionVariable_ = nullptr;
} }
static bool condition_read(const struct condition *cond) static bool condition_read(const struct condition *cond)
{ {
return cond->triggerValue_ > 0; return cond->triggerValue_ > 0;
} }
static bool condition_read(struct condition *cond) static bool condition_read(struct condition *cond)
{ {
return condition_read(const_cast<const struct condition *>(cond)); return condition_read(const_cast<const struct condition *>(cond));
} }
static bool condition_take(struct condition *cond) static bool condition_take(struct condition *cond)
{ {
std::lock_guard<std::mutex> lock(cond->internalMutex_); std::lock_guard<std::mutex> lock(cond->internalMutex_);
bool ret = cond->triggerValue_ > 0; bool ret = cond->triggerValue_ > 0;
cond->triggerValue_ = 0; cond->triggerValue_ = 0;
return ret; return ret;
} }
rmw_guard_condition_t *rmw_create_guard_condition() extern "C" rmw_guard_condition_t *rmw_create_guard_condition()
{ {
rmw_guard_condition_t *guard_condition_handle = new rmw_guard_condition_t; rmw_guard_condition_t *guard_condition_handle = new rmw_guard_condition_t;
guard_condition_handle->implementation_identifier = adlink_cyclonedds_identifier; guard_condition_handle->implementation_identifier = adlink_cyclonedds_identifier;
guard_condition_handle->data = new CddsGuardCondition(); guard_condition_handle->data = new CddsGuardCondition();
return guard_condition_handle; return guard_condition_handle;
} }
rmw_ret_t rmw_destroy_guard_condition(rmw_guard_condition_t *guard_condition) extern "C" rmw_ret_t rmw_destroy_guard_condition(rmw_guard_condition_t *guard_condition)
{ {
RET_NULL(guard_condition); RET_NULL(guard_condition);
delete static_cast<CddsGuardCondition *>(guard_condition->data); delete static_cast<CddsGuardCondition *>(guard_condition->data);
delete guard_condition; delete guard_condition;
return RMW_RET_OK; return RMW_RET_OK;
} }
rmw_ret_t rmw_trigger_guard_condition(const rmw_guard_condition_t *guard_condition_handle) extern "C" rmw_ret_t rmw_trigger_guard_condition(const rmw_guard_condition_t *guard_condition_handle)
{ {
RET_WRONG_IMPLID(guard_condition_handle); RET_WRONG_IMPLID(guard_condition_handle);
condition_set_trigger(static_cast<CddsGuardCondition *>(guard_condition_handle->data), 1); condition_set_trigger(static_cast<CddsGuardCondition *>(guard_condition_handle->data), 1);
return RMW_RET_OK; return RMW_RET_OK;
} }
rmw_wait_set_t *rmw_create_wait_set(size_t max_conditions) extern "C" rmw_wait_set_t *rmw_create_wait_set(size_t max_conditions)
{ {
(void)max_conditions; (void)max_conditions;
rmw_wait_set_t * wait_set = rmw_wait_set_allocate(); rmw_wait_set_t * wait_set = rmw_wait_set_allocate();
CddsWaitset *ws = nullptr; CddsWaitset *ws = nullptr;
@ -948,10 +946,10 @@ extern "C"
rmw_wait_set_free(wait_set); rmw_wait_set_free(wait_set);
fail_alloc_wait_set: fail_alloc_wait_set:
return nullptr; return nullptr;
} }
rmw_ret_t rmw_destroy_wait_set(rmw_wait_set_t * wait_set) extern "C" rmw_ret_t rmw_destroy_wait_set(rmw_wait_set_t * wait_set)
{ {
RET_WRONG_IMPLID(wait_set); RET_WRONG_IMPLID(wait_set);
auto result = RMW_RET_OK; auto result = RMW_RET_OK;
auto ws = static_cast<CddsWaitset *>(wait_set->data); auto ws = static_cast<CddsWaitset *>(wait_set->data);
@ -962,19 +960,19 @@ extern "C"
rmw_free(wait_set->data); rmw_free(wait_set->data);
rmw_wait_set_free(wait_set); rmw_wait_set_free(wait_set);
return result; return result;
} }
static bool check_wait_set_for_data(const rmw_subscriptions_t *subs, const rmw_guard_conditions_t *gcs, const rmw_services_t *srvs, const rmw_clients_t *cls) static bool check_wait_set_for_data(const rmw_subscriptions_t *subs, const rmw_guard_conditions_t *gcs, const rmw_services_t *srvs, const rmw_clients_t *cls)
{ {
if (subs) { for (auto&& x : *subs) { if (condition_read(x)) return true; } } if (subs) { for (auto&& x : *subs) { if (condition_read(x)) return true; } }
if (cls) { for (auto&& x : *cls) { if (condition_read(x)) return true; } } if (cls) { for (auto&& x : *cls) { if (condition_read(x)) return true; } }
if (srvs) { for (auto&& x : *srvs) { if (condition_read(x)) return true; } } if (srvs) { for (auto&& x : *srvs) { if (condition_read(x)) return true; } }
if (gcs) { for (auto&& x : *gcs) { if (condition_read(x)) return true; } } if (gcs) { for (auto&& x : *gcs) { if (condition_read(x)) return true; } }
return false; return false;
} }
rmw_ret_t rmw_wait(rmw_subscriptions_t *subs, rmw_guard_conditions_t *gcs, rmw_services_t *srvs, rmw_clients_t *cls, rmw_wait_set_t *wait_set, const rmw_time_t *wait_timeout) extern "C" rmw_ret_t rmw_wait(rmw_subscriptions_t *subs, rmw_guard_conditions_t *gcs, rmw_services_t *srvs, rmw_clients_t *cls, rmw_wait_set_t *wait_set, const rmw_time_t *wait_timeout)
{ {
RET_NULL(wait_set); RET_NULL(wait_set);
CddsWaitset *ws = static_cast<CddsWaitset *>(wait_set->data); CddsWaitset *ws = static_cast<CddsWaitset *>(wait_set->data);
RET_NULL(ws); RET_NULL(ws);
@ -1026,16 +1024,16 @@ extern "C"
if (gcs) { for (auto&& x : *gcs) { condition_detach(x); if (!condition_take(x)) x = nullptr; } } if (gcs) { for (auto&& x : *gcs) { condition_detach(x); if (!condition_take(x)) x = nullptr; } }
return timeout ? RMW_RET_TIMEOUT : RMW_RET_OK; return timeout ? RMW_RET_TIMEOUT : RMW_RET_OK;
} }
///////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////
/////////// /////////// /////////// ///////////
/////////// CLIENTS AND SERVERS /////////// /////////// CLIENTS AND SERVERS ///////////
/////////// /////////// /////////// ///////////
///////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////
static rmw_ret_t rmw_take_response_request(CddsCS *cs, rmw_request_id_t *request_header, void *ros_data, bool *taken, dds_instance_handle_t srcfilter) static rmw_ret_t rmw_take_response_request(CddsCS *cs, rmw_request_id_t *request_header, void *ros_data, bool *taken, dds_instance_handle_t srcfilter)
{ {
RET_NULL(taken); RET_NULL(taken);
RET_NULL(ros_data); RET_NULL(ros_data);
RET_NULL(request_header); RET_NULL(request_header);
@ -1066,24 +1064,24 @@ extern "C"
} }
*taken = false; *taken = false;
return RMW_RET_OK; return RMW_RET_OK;
} }
rmw_ret_t rmw_take_response(const rmw_client_t *client, rmw_request_id_t *request_header, void *ros_response, bool *taken) extern "C" rmw_ret_t rmw_take_response(const rmw_client_t *client, rmw_request_id_t *request_header, void *ros_response, bool *taken)
{ {
RET_WRONG_IMPLID(client); RET_WRONG_IMPLID(client);
auto info = static_cast<CddsClient *>(client->data); auto info = static_cast<CddsClient *>(client->data);
return rmw_take_response_request(&info->client, request_header, ros_response, taken, info->client.pub->pubiid); return rmw_take_response_request(&info->client, request_header, ros_response, taken, info->client.pub->pubiid);
} }
rmw_ret_t rmw_take_request(const rmw_service_t *service, rmw_request_id_t *request_header, void *ros_request, bool *taken) extern "C" rmw_ret_t rmw_take_request(const rmw_service_t *service, rmw_request_id_t *request_header, void *ros_request, bool *taken)
{ {
RET_WRONG_IMPLID(service); RET_WRONG_IMPLID(service);
auto info = static_cast<CddsService *>(service->data); auto info = static_cast<CddsService *>(service->data);
return rmw_take_response_request(&info->service, request_header, ros_request, taken, 0); return rmw_take_response_request(&info->service, request_header, ros_request, taken, 0);
} }
static rmw_ret_t rmw_send_response_request(CddsCS *cs, cdds_request_header_t *header, const void *ros_data) static rmw_ret_t rmw_send_response_request(CddsCS *cs, cdds_request_header_t *header, const void *ros_data)
{ {
eprosima::fastcdr::FastBuffer buffer; eprosima::fastcdr::FastBuffer buffer;
eprosima::fastcdr::Cdr ser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); eprosima::fastcdr::Cdr ser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
if (sermsg(ros_data, ser, [&header](eprosima::fastcdr::Cdr& ser) { ser << header->guid; ser << header->seq; }, cs->pub->ts)) { if (sermsg(ros_data, ser, [&header](eprosima::fastcdr::Cdr& ser) { ser << header->guid; ser << header->seq; }, cs->pub->ts)) {
@ -1092,10 +1090,10 @@ extern "C"
RMW_SET_ERROR_MSG("cannot serialize data"); RMW_SET_ERROR_MSG("cannot serialize data");
return RMW_RET_ERROR; return RMW_RET_ERROR;
} }
} }
rmw_ret_t rmw_send_response(const rmw_service_t *service, rmw_request_id_t *request_header, void *ros_response) extern "C" rmw_ret_t rmw_send_response(const rmw_service_t *service, rmw_request_id_t *request_header, void *ros_response)
{ {
RET_WRONG_IMPLID(service); RET_WRONG_IMPLID(service);
RET_NULL(request_header); RET_NULL(request_header);
RET_NULL(ros_response); RET_NULL(ros_response);
@ -1104,10 +1102,10 @@ extern "C"
memcpy(&header.guid, request_header->writer_guid, sizeof(header.guid)); memcpy(&header.guid, request_header->writer_guid, sizeof(header.guid));
header.seq = request_header->sequence_number; header.seq = request_header->sequence_number;
return rmw_send_response_request(&info->service, &header, ros_response); return rmw_send_response_request(&info->service, &header, ros_response);
} }
rmw_ret_t rmw_send_request(const rmw_client_t *client, const void *ros_request, int64_t *sequence_id) extern "C" rmw_ret_t rmw_send_request(const rmw_client_t *client, const void *ros_request, int64_t *sequence_id)
{ {
static std::atomic_uint next_request_id; static std::atomic_uint next_request_id;
RET_WRONG_IMPLID(client); RET_WRONG_IMPLID(client);
RET_NULL(ros_request); RET_NULL(ros_request);
@ -1117,10 +1115,10 @@ extern "C"
header.guid = info->client.pub->pubiid; header.guid = info->client.pub->pubiid;
header.seq = *sequence_id = next_request_id++; header.seq = *sequence_id = next_request_id++;
return rmw_send_response_request(&info->client, &header, ros_request); return rmw_send_response_request(&info->client, &header, ros_request);
} }
static const rosidl_service_type_support_t *get_service_typesupport(const rosidl_service_type_support_t *type_supports) static const rosidl_service_type_support_t *get_service_typesupport(const rosidl_service_type_support_t *type_supports)
{ {
const rosidl_service_type_support_t *ts; const rosidl_service_type_support_t *ts;
if ((ts = get_service_typesupport_handle(type_supports, rosidl_typesupport_introspection_c__identifier)) != nullptr) { if ((ts = get_service_typesupport_handle(type_supports, rosidl_typesupport_introspection_c__identifier)) != nullptr) {
return ts; return ts;
@ -1130,10 +1128,10 @@ extern "C"
RMW_SET_ERROR_MSG("service type support not from this implementation"); RMW_SET_ERROR_MSG("service type support not from this implementation");
return nullptr; return nullptr;
} }
} }
static rmw_ret_t rmw_init_cs(CddsCS *cs, const rmw_node_t *node, const rosidl_service_type_support_t *type_supports, const char *service_name, const rmw_qos_profile_t *qos_policies, bool is_service) static rmw_ret_t rmw_init_cs(CddsCS *cs, const rmw_node_t *node, const rosidl_service_type_support_t *type_supports, const char *service_name, const rmw_qos_profile_t *qos_policies, bool is_service)
{ {
RET_WRONG_IMPLID(node); RET_WRONG_IMPLID(node);
RET_NULL_OR_EMPTYSTR(service_name); RET_NULL_OR_EMPTYSTR(service_name);
RET_NULL(qos_policies); RET_NULL(qos_policies);
@ -1227,17 +1225,17 @@ extern "C"
/* leak pubtopic */ /* leak pubtopic */
fail_pubtopic: fail_pubtopic:
return RMW_RET_ERROR; return RMW_RET_ERROR;
} }
static void rmw_fini_cs(CddsCS *cs) static void rmw_fini_cs(CddsCS *cs)
{ {
dds_delete(cs->sub->subh); dds_delete(cs->sub->subh);
dds_delete(cs->pub->pubh); dds_delete(cs->pub->pubh);
cs->sub->node->own_writers.erase(cs->pub->pubiid); cs->sub->node->own_writers.erase(cs->pub->pubiid);
} }
rmw_client_t *rmw_create_client(const rmw_node_t *node, const rosidl_service_type_support_t *type_supports, const char *service_name, const rmw_qos_profile_t *qos_policies) extern "C" rmw_client_t *rmw_create_client(const rmw_node_t *node, const rosidl_service_type_support_t *type_supports, const char *service_name, const rmw_qos_profile_t *qos_policies)
{ {
CddsClient *info = new CddsClient(); CddsClient *info = new CddsClient();
if (rmw_init_cs(&info->client, node, type_supports, service_name, qos_policies, false) != RMW_RET_OK) { if (rmw_init_cs(&info->client, node, type_supports, service_name, qos_policies, false) != RMW_RET_OK) {
delete(info); delete(info);
@ -1256,10 +1254,10 @@ extern "C"
fail_client: fail_client:
rmw_fini_cs(&info->client); rmw_fini_cs(&info->client);
return nullptr; return nullptr;
} }
rmw_ret_t rmw_destroy_client(rmw_node_t *node, rmw_client_t *client) extern "C" rmw_ret_t rmw_destroy_client(rmw_node_t *node, rmw_client_t *client)
{ {
RET_WRONG_IMPLID(node); RET_WRONG_IMPLID(node);
RET_WRONG_IMPLID(client); RET_WRONG_IMPLID(client);
auto info = static_cast<CddsClient *>(client->data); auto info = static_cast<CddsClient *>(client->data);
@ -1267,10 +1265,10 @@ extern "C"
rmw_free(const_cast<char *>(client->service_name)); rmw_free(const_cast<char *>(client->service_name));
rmw_client_free(client); rmw_client_free(client);
return RMW_RET_OK; return RMW_RET_OK;
} }
rmw_service_t *rmw_create_service(const rmw_node_t *node, const rosidl_service_type_support_t *type_supports, const char *service_name, const rmw_qos_profile_t *qos_policies) extern "C" rmw_service_t *rmw_create_service(const rmw_node_t *node, const rosidl_service_type_support_t *type_supports, const char *service_name, const rmw_qos_profile_t *qos_policies)
{ {
CddsService *info = new CddsService(); CddsService *info = new CddsService();
if (rmw_init_cs(&info->service, node, type_supports, service_name, qos_policies, true) != RMW_RET_OK) { if (rmw_init_cs(&info->service, node, type_supports, service_name, qos_policies, true) != RMW_RET_OK) {
delete(info); delete(info);
@ -1289,10 +1287,10 @@ extern "C"
fail_service: fail_service:
rmw_fini_cs(&info->service); rmw_fini_cs(&info->service);
return nullptr; return nullptr;
} }
rmw_ret_t rmw_destroy_service(rmw_node_t *node, rmw_service_t *service) extern "C" rmw_ret_t rmw_destroy_service(rmw_node_t *node, rmw_service_t *service)
{ {
RET_WRONG_IMPLID(node); RET_WRONG_IMPLID(node);
RET_WRONG_IMPLID(service); RET_WRONG_IMPLID(service);
auto info = static_cast<CddsService *>(service->data); auto info = static_cast<CddsService *>(service->data);
@ -1300,16 +1298,16 @@ extern "C"
rmw_free(const_cast<char *>(service->service_name)); rmw_free(const_cast<char *>(service->service_name));
rmw_service_free(service); rmw_service_free(service);
return RMW_RET_OK; return RMW_RET_OK;
} }
///////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////
/////////// /////////// /////////// ///////////
/////////// INTROSPECTION /////////// /////////// INTROSPECTION ///////////
/////////// /////////// /////////// ///////////
///////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////
rmw_ret_t rmw_get_node_names(const rmw_node_t *node, rcutils_string_array_t *node_names) extern "C" rmw_ret_t rmw_get_node_names(const rmw_node_t *node, rcutils_string_array_t *node_names)
{ {
#if 0 // NIY #if 0 // NIY
RET_WRONG_IMPLID(node); RET_WRONG_IMPLID(node);
if (rmw_check_zero_rmw_string_array(node_names) != RMW_RET_OK) { if (rmw_check_zero_rmw_string_array(node_names) != RMW_RET_OK) {
@ -1345,10 +1343,10 @@ extern "C"
(void)node; (void)node_names; (void)node; (void)node_names;
return RMW_RET_TIMEOUT; return RMW_RET_TIMEOUT;
#endif #endif
} }
rmw_ret_t rmw_get_topic_names_and_types(const rmw_node_t *node, rcutils_allocator_t *allocator, bool no_demangle, rmw_names_and_types_t *topic_names_and_types) extern "C" rmw_ret_t rmw_get_topic_names_and_types(const rmw_node_t *node, rcutils_allocator_t *allocator, bool no_demangle, rmw_names_and_types_t *topic_names_and_types)
{ {
#if 0 // NIY #if 0 // NIY
RET_NULL(allocator); RET_NULL(allocator);
RET_WRONG_IMPLID(node); RET_WRONG_IMPLID(node);
@ -1461,10 +1459,10 @@ extern "C"
(void)node; (void)allocator; (void)no_demangle; (void)topic_names_and_types; (void)node; (void)allocator; (void)no_demangle; (void)topic_names_and_types;
return RMW_RET_TIMEOUT; return RMW_RET_TIMEOUT;
#endif #endif
} }
rmw_ret_t rmw_get_service_names_and_types(const rmw_node_t *node, rcutils_allocator_t *allocator, rmw_names_and_types_t *service_names_and_types) extern "C" rmw_ret_t rmw_get_service_names_and_types(const rmw_node_t *node, rcutils_allocator_t *allocator, rmw_names_and_types_t *service_names_and_types)
{ {
#if 0 // NIY #if 0 // NIY
if (!allocator) { if (!allocator) {
RMW_SET_ERROR_MSG("allocator is null") RMW_SET_ERROR_MSG("allocator is null")
@ -1588,10 +1586,10 @@ extern "C"
(void)node; (void)allocator; (void)service_names_and_types; (void)node; (void)allocator; (void)service_names_and_types;
return RMW_RET_TIMEOUT; return RMW_RET_TIMEOUT;
#endif #endif
} }
rmw_ret_t rmw_service_server_is_available(const rmw_node_t * node, const rmw_client_t * client, bool * is_available) extern "C" rmw_ret_t rmw_service_server_is_available(const rmw_node_t * node, const rmw_client_t * client, bool * is_available)
{ {
#if 0 // NIY #if 0 // NIY
if (!node) { if (!node) {
RMW_SET_ERROR_MSG("node handle is null"); RMW_SET_ERROR_MSG("node handle is null");
@ -1685,10 +1683,10 @@ extern "C"
(void)node; (void)client; (void)is_available; (void)node; (void)client; (void)is_available;
return RMW_RET_TIMEOUT; return RMW_RET_TIMEOUT;
#endif #endif
} }
rmw_ret_t rmw_count_publishers(const rmw_node_t *node, const char *topic_name, size_t *count) extern "C" rmw_ret_t rmw_count_publishers(const rmw_node_t *node, const char *topic_name, size_t *count)
{ {
#if 0 #if 0
// safechecks // safechecks
@ -1725,10 +1723,10 @@ extern "C"
(void)node; (void)topic_name; (void)count; (void)node; (void)topic_name; (void)count;
return RMW_RET_TIMEOUT; return RMW_RET_TIMEOUT;
#endif #endif
} }
rmw_ret_t rmw_count_subscribers(const rmw_node_t *node, const char *topic_name, size_t *count) extern "C" rmw_ret_t rmw_count_subscribers(const rmw_node_t *node, const char *topic_name, size_t *count)
{ {
#if 0 #if 0
// safechecks // safechecks
@ -1765,5 +1763,4 @@ extern "C"
(void)node; (void)topic_name; (void)count; (void)node; (void)topic_name; (void)count;
return RMW_RET_TIMEOUT; return RMW_RET_TIMEOUT;
#endif #endif
} }
} // extern "C"