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_ret_t destroy_node(const char * implementation_identifier, rmw_node_t * node) { if (!node) { RMW_SET_ERROR_MSG("node handle is null"); return RMW_RET_ERROR; } RMW_CHECK_TYPE_IDENTIFIERS_MATCH( node handle, node->implementation_identifier, implementation_identifier, return RMW_RET_ERROR) DDSDomainParticipantFactory * dpf_ = DDSDomainParticipantFactory::get_instance(); if (!dpf_) { RMW_SET_ERROR_MSG("failed to get participant factory"); return RMW_RET_ERROR; } auto node_info = static_cast<ConnextNodeInfo *>(node->data); if (!node_info) { RMW_SET_ERROR_MSG("node info handle is null"); return RMW_RET_ERROR; } auto participant = static_cast<DDSDomainParticipant *>(node_info->participant); if (!participant) { RMW_SET_ERROR_MSG("participant handle is null"); } // This unregisters types and destroys topics which were shared between // publishers and subscribers and could not be cleaned up in the delete functions. if (participant->delete_contained_entities() != DDS::RETCODE_OK) { RMW_SET_ERROR_MSG("failed to delete contained entities of participant"); return RMW_RET_ERROR; } DDS_ReturnCode_t ret = dpf_->delete_participant(participant); if (ret != DDS_RETCODE_OK) { RMW_SET_ERROR_MSG("failed to delete participant"); return RMW_RET_ERROR; } if (node_info->publisher_listener) { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( node_info->publisher_listener->~CustomPublisherListener(), CustomPublisherListener) rmw_free(node_info->publisher_listener); node_info->publisher_listener = nullptr; } if (node_info->subscriber_listener) { RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( node_info->subscriber_listener->~CustomSubscriberListener(), CustomSubscriberListener) rmw_free(node_info->subscriber_listener); node_info->subscriber_listener = nullptr; } if (node_info->graph_guard_condition) { rmw_ret_t ret = destroy_guard_condition(implementation_identifier, node_info->graph_guard_condition); if (ret != RMW_RET_OK) { RMW_SET_ERROR_MSG("failed to delete graph guard condition"); return RMW_RET_ERROR; } node_info->graph_guard_condition = nullptr; } rmw_free(node_info); node->data = nullptr; rmw_free(const_cast<char *>(node->name)); node->name = nullptr; rmw_node_free(node); return RMW_RET_OK; }
rmw_ret_t rmw_destroy_guard_condition(rmw_guard_condition_t * guard_condition) { return destroy_guard_condition(rti_connext_identifier, guard_condition); }