rmw_node_t *
create_node(const char * implementation_identifier, const char * name, size_t domain_id)
{
DDSDomainParticipantFactory * dpf_ = DDSDomainParticipantFactory::get_instance();
if (!dpf_) {
RMW_SET_ERROR_MSG("failed to get participant factory");
return NULL;
}
// use loopback interface to enable cross vendor communication
DDS_DomainParticipantQos participant_qos;
DDS_ReturnCode_t status = dpf_->get_default_participant_qos(participant_qos);
if (status != DDS_RETCODE_OK) {
RMW_SET_ERROR_MSG("failed to get default participant qos");
return NULL;
}
// forces local traffic to be sent over loopback,
// even if a more efficient transport (such as shared memory) is installed
// (in which case traffic will be sent over both transports)
status = DDSPropertyQosPolicyHelper::add_property(
participant_qos.property,
"dds.transport.UDPv4.builtin.ignore_loopback_interface",
"0",
DDS_BOOLEAN_FALSE);
if (status != DDS_RETCODE_OK) {
RMW_SET_ERROR_MSG("failed to add qos property");
return NULL;
}
status = DDSPropertyQosPolicyHelper::add_property(
participant_qos.property,
"dds.transport.use_510_compatible_locator_kinds",
"1",
DDS_BOOLEAN_FALSE);
if (status != DDS_RETCODE_OK) {
RMW_SET_ERROR_MSG("failed to add qos property");
return NULL;
}
DDS_DomainId_t domain = static_cast<DDS_DomainId_t>(domain_id);
DDSDomainParticipant * participant = dpf_->create_participant(
domain, participant_qos, NULL,
DDS_STATUS_MASK_NONE);
if (!participant) {
RMW_SET_ERROR_MSG("failed to create participant");
return NULL;
}
rmw_node_t * node_handle = nullptr;
ConnextNodeInfo * node_info = nullptr;
rmw_guard_condition_t * graph_guard_condition = nullptr;
CustomPublisherListener * publisher_listener = nullptr;
CustomSubscriberListener * subscriber_listener = nullptr;
void * buf = nullptr;
DDSDataReader * data_reader = nullptr;
DDSPublicationBuiltinTopicDataDataReader * builtin_publication_datareader = nullptr;
DDSSubscriptionBuiltinTopicDataDataReader * builtin_subscription_datareader = nullptr;
DDSSubscriber * builtin_subscriber = participant->get_builtin_subscriber();
if (!builtin_subscriber) {
RMW_SET_ERROR_MSG("builtin subscriber handle is null");
goto fail;
}
// setup publisher listener
data_reader = builtin_subscriber->lookup_datareader(DDS_PUBLICATION_TOPIC_NAME);
builtin_publication_datareader =
static_cast<DDSPublicationBuiltinTopicDataDataReader *>(data_reader);
if (!builtin_publication_datareader) {
RMW_SET_ERROR_MSG("builtin publication datareader handle is null");
goto fail;
}
graph_guard_condition = create_guard_condition(implementation_identifier);
if (!graph_guard_condition) {
RMW_SET_ERROR_MSG("failed to create graph guard condition");
goto fail;
}
buf = rmw_allocate(sizeof(CustomPublisherListener));
if (!buf) {
RMW_SET_ERROR_MSG("failed to allocate memory");
goto fail;
}
RMW_TRY_PLACEMENT_NEW(
publisher_listener, buf, goto fail, CustomPublisherListener,
implementation_identifier, graph_guard_condition)
buf = nullptr;
builtin_publication_datareader->set_listener(publisher_listener, DDS_DATA_AVAILABLE_STATUS);
data_reader = builtin_subscriber->lookup_datareader(DDS_SUBSCRIPTION_TOPIC_NAME);
builtin_subscription_datareader =
static_cast<DDSSubscriptionBuiltinTopicDataDataReader *>(data_reader);
if (!builtin_subscription_datareader) {
RMW_SET_ERROR_MSG("builtin subscription datareader handle is null");
goto fail;
}
// setup subscriber listener
buf = rmw_allocate(sizeof(CustomSubscriberListener));
if (!buf) {
RMW_SET_ERROR_MSG("failed to allocate memory");
goto fail;
}
RMW_TRY_PLACEMENT_NEW(
subscriber_listener, buf, goto fail, CustomSubscriberListener,
implementation_identifier, graph_guard_condition)
buf = nullptr;
builtin_subscription_datareader->set_listener(subscriber_listener, DDS_DATA_AVAILABLE_STATUS);
node_handle = rmw_node_allocate();
if (!node_handle) {
RMW_SET_ERROR_MSG("failed to allocate memory for node handle");
goto fail;
}
node_handle->implementation_identifier = implementation_identifier;
node_handle->data = participant;
node_handle->name =
reinterpret_cast<const char *>(rmw_allocate(sizeof(char) * strlen(name) + 1));
if (!node_handle->name) {
RMW_SET_ERROR_MSG("failed to allocate memory for node name");
goto fail;
}
memcpy(const_cast<char *>(node_handle->name), name, strlen(name) + 1);
buf = rmw_allocate(sizeof(ConnextNodeInfo));
if (!buf) {
RMW_SET_ERROR_MSG("failed to allocate memory");
goto fail;
}
RMW_TRY_PLACEMENT_NEW(node_info, buf, goto fail, ConnextNodeInfo)
buf = nullptr;
node_info->participant = participant;
node_info->publisher_listener = publisher_listener;
node_info->subscriber_listener = subscriber_listener;
node_info->graph_guard_condition = graph_guard_condition;
node_handle->implementation_identifier = implementation_identifier;
node_handle->data = node_info;
return node_handle;
fail:
status = dpf_->delete_participant(participant);
if (status != DDS_RETCODE_OK) {
std::stringstream ss;
ss << "leaking participant while handling failure at " <<
__FILE__ << ":" << __LINE__;
(std::cerr << ss.str()).flush();
}
if (graph_guard_condition) {
rmw_ret_t ret = destroy_guard_condition(implementation_identifier, graph_guard_condition);
if (ret != RMW_RET_OK) {
std::stringstream ss;
ss << "failed to destroy guard condition while handling failure at " <<
__FILE__ << ":" << __LINE__;
(std::cerr << ss.str()).flush();
}
}
if (publisher_listener) {
RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(
publisher_listener->~CustomPublisherListener(), CustomPublisherListener)
rmw_free(publisher_listener);
}
if (subscriber_listener) {
RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(
subscriber_listener->~CustomSubscriberListener(), CustomSubscriberListener)
rmw_free(subscriber_listener);
}
if (node_handle) {
if (node_handle->name) {
rmw_free(const_cast<char *>(node_handle->name));
}
rmw_free(node_handle);
}
if (node_info) {
RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(
node_info->~ConnextNodeInfo(), ConnextNodeInfo)
rmw_free(node_info);
}
if (buf) {
rmw_free(buf);
}
return NULL;
}
rmw_guard_condition_t *
rmw_create_guard_condition(rmw_context_t * context)
{
return create_guard_condition(rti_connext_identifier, context);
}