void
DdsDomainParticipantRepository::remove(std::string const& name)
throw(ENotRegistered)
{
DDS::DomainParticipantFactory * dpf =
DDS::DomainParticipantFactory::get_instance();
InstanceMap::iterator i = instances_.find(name);
if (i != instances_.end()) {
DDS::DomainParticipant * p = dynamic_cast<DDS::DomainParticipant *>(i->second);
DDS::ReturnCode_t rc = p->delete_contained_entities();
if (rc != DDS::RETCODE_OK) {
MIRO_LOG_OSTR(LL_ERROR, "Error deleting contained entities: " << DdsSupport::getError(rc));
}
rc = dpf->delete_participant(p);
if (rc != DDS_RETCODE_OK) {
MIRO_LOG_OSTR(LL_ERROR, "Error deleting DDS domain particpant: " << DdsSupport::getError(rc));
}
instances_.erase(i);
if (instances_.size() == 0)
SingletonType::ACE_Singleton_Type::close();
return;
}
throw ENotRegistered(name);
}
DdsEntitiesFactory::~DdsEntitiesFactory()
{
MIRO_LOG_DTOR("kn::DdsEntitiesFactory");
// delete all topics we created
{
vector<DDS::Topic *>::const_iterator first, last = m_data->topics.end();
for (first = m_data->topics.begin(); first != last; ++first) {
DDS::DomainParticipant * participant = (*first)->get_participant();
DDS::ReturnCode_t rc = participant->delete_topic(*first);
if (rc != DDS::RETCODE_OK) {
MIRO_LOG_OSTR(LL_ERROR , "Failed to delete topic: " << DdsSupport::getError(rc));
}
}
m_data->topics.clear();
}
MIRO_LOG(LL_NOTICE, "Cleaning up DDS subscribers...");
DdsSubscriberRepository::SingletonType::ACE_Singleton_Type::close();
MIRO_LOG(LL_NOTICE, "Cleaning up DDS publishers...");
DdsPublisherRepository::SingletonType::ACE_Singleton_Type::close();
MIRO_LOG(LL_NOTICE, "Cleaning up DDS flow-controllers...");
{
ParticipantVector::const_iterator first, last = m_params.participants.end();
for (first = m_params.participants.begin(); first != last; ++first) {
DDS::DomainParticipant * dp =
DdsDomainParticipantRepository::instance()->get(first->name);
vector<DdsFlowControllerParameters>::const_iterator f, l = first->flowControllers.end();
for (f = first->flowControllers.begin(); f != l; ++f) {
DDS::FlowController * fc = dp->lookup_flowcontroller(f->name.c_str());
dp->delete_flowcontroller(fc);
}
}
}
#if defined(KNDDS_HAS_RTI_DistLogger)
if(m_params.enableDistLogger) {
MIRO_LOG(LL_NOTICE, "Stop RTI Distributed Logger...");
RTI_DLDistLogger* dl = RTI_DLDistLogger::getInstance();
if(dl) {
dl->finalizeInstance();
}
}
#endif
MIRO_LOG(LL_NOTICE, "Cleaning up DDS participants...");
DdsDomainParticipantRepository::SingletonType::ACE_Singleton_Type::close();
DDS::DomainParticipantFactory * dpf =
DDS::DomainParticipantFactory::get_instance();
dpf->finalize_instance();
delete m_data;
}
DdsPublisherRepository::~DdsPublisherRepository() throw()
{
// just to make sure, the repository should actually be empty at this point.
InstanceMap::iterator first, last = instances_.end();
for (first = instances_.begin(); first != last; ++first) {
// delete first->second;
DDS::Publisher * pub = dynamic_cast<DDS::Publisher *>(first->second);
DDS::DomainParticipant * participant = pub->get_participant();
DDS::ReturnCode_t rc = participant->delete_publisher(pub);
if (rc != DDS_RETCODE_OK) {
MIRO_LOG_OSTR(LL_ERROR, "Error deleting DDS publisher: " << DdsSupport::getError(rc));
}
}
}
// specializations
DdsDomainParticipantRepository::~DdsDomainParticipantRepository() throw()
{
MIRO_LOG_DTOR("DdsDomainParticipantRepository");
DDS::DomainParticipantFactory * dpf =
DDS::DomainParticipantFactory::get_instance();
// just to make sure, the repository should actually be empty at this point.
InstanceMap::iterator first, last = instances_.end();
for (first = instances_.begin(); first != last; ++first) {
DDS::DomainParticipant * p = dynamic_cast<DDS::DomainParticipant *>(first->second);
DDS::ReturnCode_t rc = p->delete_contained_entities();
if (rc != DDS::RETCODE_OK) {
MIRO_LOG_OSTR(LL_ERROR, "Error deleting contained entities: " << DdsSupport::getError(rc));
}
rc = dpf->delete_participant(dynamic_cast<DDS::DomainParticipant *>(p));
if (rc != DDS_RETCODE_OK) {
MIRO_LOG_OSTR(LL_ERROR, "Error deleting DDS domain particpant: " << DdsSupport::getError(rc));
}
}
}
void
DdsSubscriberRepository::remove(std::string const& name)
throw(ENotRegistered)
{
InstanceMap::iterator i = instances_.find(name);
if (i != instances_.end()) {
//delete i->second;
DDS::Subscriber * sub = dynamic_cast<DDS::Subscriber *>(i->second);
DDS::DomainParticipant * participant = sub->get_participant();
DDS::ReturnCode_t rc = participant->delete_subscriber(sub);
if (rc != DDS_RETCODE_OK) {
MIRO_LOG_OSTR(LL_ERROR, "Error deleting DDS subscriber: " << DdsSupport::getError(rc));
}
instances_.erase(i);
if (instances_.size() == 0)
SingletonType::ACE_Singleton_Type::close();
return;
}
throw ENotRegistered(name);
}
rmw_node_t *
rmw_create_node(const char * name, size_t domain_id)
{
if (!name) {
RMW_SET_ERROR_MSG("name is null");
return nullptr;
}
DDS::DomainParticipantFactory_var dp_factory = DDS::DomainParticipantFactory::get_instance();
if (!dp_factory) {
RMW_SET_ERROR_MSG("failed to get domain participant factory");
return nullptr;
}
DDS::DomainId_t domain = static_cast<DDS::DomainId_t>(domain_id);
DDS::DomainParticipant * participant = nullptr;
// Make sure that the OSPL_URI is set, otherwise node creation will fail.
char * ospl_uri = nullptr;
const char * ospl_uri_env = "OSPL_URI";
#ifndef _WIN32
ospl_uri = getenv(ospl_uri_env);
#else
size_t ospl_uri_size;
_dupenv_s(&ospl_uri, &ospl_uri_size, ospl_uri_env);
#endif
if (!ospl_uri) {
RMW_SET_ERROR_MSG("OSPL_URI not set");
return nullptr;
} else {
#ifdef _WIN32
free(ospl_uri);
#endif
}
// Ensure the ROS_DOMAIN_ID env variable is set, otherwise parsing of the config may fail.
// Also make sure the it is set to the domain_id passed in, otherwise it will fail.
// But first backup the current ROS_DOMAIN_ID.
char * ros_domain_id = nullptr;
const char * env_var = "ROS_DOMAIN_ID";
#ifndef _WIN32
ros_domain_id = getenv(env_var);
#else
size_t ros_domain_id_size;
_dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var);
#endif
// On Windows, setting the ROS_DOMAIN_ID does not fix the problem, so error early.
#ifdef _WIN32
if (!ros_domain_id) {
RMW_SET_ERROR_MSG("environment variable ROS_DOMAIN_ID is not set");
fprintf(stderr, "[rmw_opensplice_cpp]: error: %s\n", rmw_get_error_string_safe());
return nullptr;
}
#endif
// Set the ROS_DOMAIN_ID explicitly (if not Windows).
#ifndef _WIN32
auto domain_id_as_string = std::to_string(domain_id);
int ret = 0;
ret = setenv(env_var, domain_id_as_string.c_str(), 1);
if (0 != ret) {
RMW_SET_ERROR_MSG("failed to set the ROS_DOMAIN_ID");
return nullptr;
}
#endif
participant = dp_factory->create_participant(
domain, PARTICIPANT_QOS_DEFAULT, NULL, DDS::STATUS_MASK_NONE);
if (!participant) {
RMW_SET_ERROR_MSG("failed to create domain participant");
return NULL;
}
// Restore the ROS_DOMAIN_ID if necessary (and not Windows).
#ifndef _WIN32
if (ros_domain_id) {
ret = setenv(env_var, ros_domain_id, 1);
if (0 != ret) {
RMW_SET_ERROR_MSG("failed to reset the ROS_DOMAIN_ID");
return nullptr;
}
} else {
// Otherwise unset it again.
ret = unsetenv(env_var);
if (0 != ret) {
RMW_SET_ERROR_MSG("failed to unset the ROS_DOMAIN_ID");
return nullptr;
}
}
#endif
rmw_node_t * node = nullptr;
OpenSpliceStaticNodeInfo * node_info = nullptr;
CustomPublisherListener * publisher_listener = nullptr;
CustomSubscriberListener * subscriber_listener = nullptr;
void * buf = nullptr;
DDS::DataReader * data_reader = nullptr;
DDS::PublicationBuiltinTopicDataDataReader * builtin_publication_datareader = nullptr;
DDS::SubscriptionBuiltinTopicDataDataReader * builtin_subscription_datareader = nullptr;
DDS::Subscriber * 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("DCPSPublication");
builtin_publication_datareader =
DDS::PublicationBuiltinTopicDataDataReader::_narrow(data_reader);
if (!builtin_publication_datareader) {
RMW_SET_ERROR_MSG("builtin publication datareader handle is null");
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)
buf = nullptr;
builtin_publication_datareader->set_listener(publisher_listener, DDS::DATA_AVAILABLE_STATUS);
data_reader = builtin_subscriber->lookup_datareader("DCPSSubscription");
builtin_subscription_datareader =
DDS::SubscriptionBuiltinTopicDataDataReader::_narrow(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)
buf = nullptr;
builtin_subscription_datareader->set_listener(subscriber_listener, DDS::DATA_AVAILABLE_STATUS);
node = rmw_node_allocate();
if (!node) {
RMW_SET_ERROR_MSG("failed to allocate rmw_node_t");
goto fail;
}
node->name = reinterpret_cast<const char *>(rmw_allocate(sizeof(char) * strlen(name) + 1));
if (!node->name) {
RMW_SET_ERROR_MSG("failed to allocate memory for node name");
goto fail;
}
memcpy(const_cast<char *>(node->name), name, strlen(name) + 1);
buf = rmw_allocate(sizeof(OpenSpliceStaticNodeInfo));
if (!buf) {
RMW_SET_ERROR_MSG("failed to allocate memory");
goto fail;
}
RMW_TRY_PLACEMENT_NEW(node_info, buf, goto fail, OpenSpliceStaticNodeInfo)
buf = nullptr;
node_info->participant = participant;
node_info->publisher_listener = publisher_listener;
node_info->subscriber_listener = subscriber_listener;
node->implementation_identifier = opensplice_cpp_identifier;
node->data = node_info;
return node;
fail:
if (participant) {
if (dp_factory->delete_participant(participant) != DDS::RETCODE_OK) {
std::stringstream ss;
ss << "leaking domain participant while handling failure at: " <<
__FILE__ << ":" << __LINE__ << '\n';
(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_info) {
RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(
node_info->~OpenSpliceStaticNodeInfo(), OpenSpliceStaticNodeInfo)
rmw_free(node_info);
}
if (buf) {
rmw_free(buf);
}
if (node) {
if (node->name) {
rmw_free(const_cast<char *>(node->name));
}
rmw_node_free(node);
}
return nullptr;
}
DDS::DomainParticipant* BulkDataNTStream::createDDSParticipant()
{
AUTO_TRACE(__PRETTY_FUNCTION__);
DDS::ReturnCode_t ret;
DDS::DomainParticipantQos participant_qos;
int domainID=0; //TBD: where to get domain ID
DDS::DomainParticipant* domainParticipant;
if (factory_m==NULL)
{
NullPointerExImpl ex(__FILE__, __LINE__, __PRETTY_FUNCTION__);
ex.setVariable("factory_m");
throw ex;
}
// If the profileQoS set on the configuration corresponds to a default stream profile,
// then we use the default library as the library to search for it.
// Otherwise, it means that the user specified it, so it must belong
// to the specified library too
const char *library = 0;
if( configuration_m.libraryQos.compare((const char*)DDSConfiguration::DEFAULT_LIBRARY) == 0 &&
(configuration_m.profileQos.compare((const char*)DDSConfiguration::DEFAULT_RECEIVER_STREAM_PROFILE) == 0 ||
configuration_m.profileQos.compare((const char*)DDSConfiguration::DEFAULT_SENDER_STREAM_PROFILE) == 0)
)
library = DDSConfiguration::DEFAULT_LIBRARY;
else
library = configuration_m.libraryQos.c_str();
ret = factory_m->get_participant_qos_from_profile(participant_qos, library, configuration_m.profileQos.c_str());
if (ret!=DDS::RETCODE_OK)
{
DDSQoSSetProblemExImpl ex(__FILE__, __LINE__, __PRETTY_FUNCTION__);
ex.setDDSTypeCode(ret);
ex.setQoS("get_participant_qos_from_profile");
throw ex;
}//if
// we make sure that what participant creates is created disabled
participant_qos.entity_factory.autoenable_created_entities=DDS_BOOLEAN_FALSE;
domainParticipant =factory_m->create_participant(domainID, participant_qos, NULL, DDS::STATUS_MASK_NONE );
if (domainParticipant==NULL)
{
DDSParticipantCreateProblemExImpl ex(__FILE__, __LINE__, __PRETTY_FUNCTION__);
ex.setDomainID(domainID);
throw ex;
}
ret = domainParticipant->enable();
if (ret!=DDS::RETCODE_OK)
{
DDSParticipantEnableProblemExImpl ex(__FILE__, __LINE__, __PRETTY_FUNCTION__);
ex.setDDSTypeCode(ret);
ex.setDomainID(domainID);
throw ex;
}//if
const char* type_name = ACSBulkData::BulkDataNTFrameTypeSupport::get_type_name();
ret = ACSBulkData::BulkDataNTFrameTypeSupport::register_type(domainParticipant, type_name);
if (ret != DDS::RETCODE_OK)
{
DDSRegisterTypeProblemExImpl ex(__FILE__, __LINE__, __PRETTY_FUNCTION__);
ex.setDDSTypeCode(ret);
ex.setTypeName(type_name);
throw ex;
}
return domainParticipant;
}//createDDSParticipant
DdsEntitiesFactory::DdsEntitiesFactory(DdsEntitiesFactoryParameters const& params) :
m_params(params),
m_data(new Data())
{
MIRO_LOG_CTOR("kn::DdsEntitiesFactory");
// for error reporting
stringstream ostr;
DDS::DomainParticipantFactory * dpf =
DDS::DomainParticipantFactory::get_instance();
//---------------------------------------------------
// disable default locations for Qos policy lookup
DDS::DomainParticipantFactoryQos dpfQos;
DDS_DomainParticipantFactoryQos_initialize(&dpfQos);
DDS::ReturnCode_t rc;
if ((rc = dpf->get_qos(dpfQos)) != DDS_RETCODE_OK) {
ostr << "DdsSupport::init() - Failed to query Qos (" << ((int)rc) << "): " << DdsSupport::getError(rc);
throw Miro::Exception(ostr.str());
}
// start participants disabled, so we can set discovery modules
dpfQos.entity_factory.autoenable_created_entities = DDS_BOOLEAN_FALSE;
// if configuration file is given, disable all other config options
if (!m_params.configFiles.empty()) {
Miro::SearchPaths paths(KNDDS_INSTALL_PREFIX "/etc");
paths.addMiroEtcPaths();
dpfQos.profile.url_profile.maximum(m_params.configFiles.size());
dpfQos.profile.url_profile.length(m_params.configFiles.size());
dpfQos.profile.ignore_user_profile = true;
dpfQos.profile.ignore_environment_profile = true;
dpfQos.profile.ignore_resource_profile = true;
for (unsigned int i = 0; i < m_params.configFiles.size(); ++i) {
string absFilePath = paths.findFile(m_params.configFiles[i]);
if (absFilePath.empty()) {
ostr << "DdsSupport::init() - DDS config file given not found (" << m_params.configFiles[i] << ")";
throw Miro::Exception(ostr.str());
}
dpfQos.profile.url_profile[i] = DDS_String_dup(("file://" + absFilePath).c_str());
}
if ((rc = dpf->set_qos(dpfQos)) != DDS_RETCODE_OK) {
ostr << "DdsSupport::init() - Failed to set Qos: " << DdsSupport::getError(rc);
throw Miro::Exception(ostr.str());
}
}
if (!m_params.defaultLibrary.empty() &&
(rc = dpf->set_default_library(m_params.defaultLibrary.c_str())) != DDS_RETCODE_OK) {
ostr << "DdsSupport::init() - Failed to set default library: " << DdsSupport::getError(rc);
throw Miro::Exception(ostr.str());
}
if (!m_params.defaultProfile.empty() &&
(rc = dpf->set_default_profile(m_params.defaultLibrary.c_str(),
m_params.defaultProfile.c_str())) != DDS_RETCODE_OK) {
ostr << "DdsSupport::init() - Failed to set default library: " << DdsSupport::getError(rc);
throw Miro::Exception(ostr.str());
}
//-------------------------------------------
// create all participants
// (default is 1)
DdsDomainParticipantRepository * partRepo = DdsDomainParticipantRepository::instance();
DdsTypeRegistratorRepository * typeRepo = DdsTypeRegistratorRepository::instance();
{
ParticipantVector::const_iterator first, last = m_params.participants.end();
for (first = m_params.participants.begin(); first != last; ++first) {
// create one participant
char const * library = first->library.empty() ? NULL : first->library.c_str();
char const * profile = first->profile.empty() ? NULL : first->profile.c_str();
DDS::DomainParticipantQos qos;
DDS_DomainParticipantQos_initialize(&qos);
if (profile == NULL &&
(m_params.defaultLibrary.empty() || m_params.defaultProfile.empty())) {
DDS::ReturnCode_t rc = dpf->get_default_participant_qos(qos);
if (rc != DDS_RETCODE_OK) {
ostr << "Get default participant Qos profile: " << DdsSupport::getError(rc);
throw Miro::Exception(ostr.str());
}
}
else {
DDS::ReturnCode_t rc = dpf->get_participant_qos_from_profile(qos, library, profile);
if (rc != DDS_RETCODE_OK) {
ostr << "Load participant Qos profile: " << DdsSupport::getError(rc);
throw Miro::Exception(ostr.str());
}
}
// set participant name from Miro parameters
if (!first->participantName.empty()) {
qos.participant_name.name = DDS_String_dup(first->participantName.c_str());
}
// handle discovery-peers processing
// just print the stuff as a first step...
if (!first->discoveryPeersFiles.empty()) {
DDS_DiscoveryQosPolicy& discovery = qos.discovery;
StringVector peers;
StringVector::const_iterator f, l = first->discoveryPeersFiles.end();
for (f = first->discoveryPeersFiles.begin(); f != l; ++f) {
StringVector const p = parseDiscoveryPeersFile(*f);
peers.insert(peers.end(), p.begin(), p.end());
}
// set peers list in qos
if (discovery.initial_peers.maximum() < (int)peers.size()) {
discovery.initial_peers.maximum(peers.size());
}
discovery.initial_peers.length(peers.size());
for (int i = 0; i < discovery.initial_peers.length(); ++i) {
assert (peers[i].length() < 100);
DDS_String_free(discovery.initial_peers[i]);
discovery.initial_peers[i] = DDS_String_dup(peers[i].c_str());
}
MIRO_LOG_OSTR(LL_NOTICE, "DDS Initial peers layout for " << qos.participant_name.name);
for (int i = 0; i < qos.discovery.initial_peers.length(); ++i) {
MIRO_LOG_OSTR(LL_NOTICE, qos.discovery.initial_peers[i]);
}
}
#ifdef KNDDS_HAS_DDS_Monitor
if (first->enableMonitor) {
int rc;
rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property,
"rti.monitor.library",
"rtimonintoring",
DDS_BOOLEAN_FALSE);
assert (rc == DDS_RETCODE_OK);
char valueBuffer[17];
sprintf(valueBuffer, "%p", RTIDefaultMonitor_create);
rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property,
"rti.monitor.create_function_ptr",
valueBuffer,
DDS_BOOLEAN_FALSE);
assert (rc == DDS_RETCODE_OK);
int monitorId = (first->monitorDomainId >= 0) ? first->monitorDomainId : first->domainId + 1;
sprintf(valueBuffer, "%i", monitorId);
rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property,
"rti.monitor.config.new_participant_domain_id",
valueBuffer,
DDS_BOOLEAN_FALSE);
if (!first->monitorLibrary.empty()) {
rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property,
"rti.monitor.config.qos_library",
first->monitorLibrary.c_str(),
DDS_BOOLEAN_FALSE);
assert (rc == DDS_RETCODE_OK);
rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property,
"rti.monitor.config.qos_profile",
first->monitorProfile.c_str(),
DDS_BOOLEAN_FALSE);
assert (rc == DDS_RETCODE_OK);
}
}
#endif
#if defined(KNDDS_HAS_RTI_DistLogger)
if(m_params.enableDistLogger) {
// Initialize the distributed logger. If distLoggerDomainId is less than
// 0, use the domain of the first participant
int loggerId = (m_params.distLoggerDomainId >= 0) ? m_params.distLoggerDomainId : first->domainId;
RTI_DLDistLogger* dl = NULL;
RTI_DLOptions dlOptions;
// RTI_DLOptions API changed in RTI DDS 5.1.0
#if (RTI_DDS_VERSION_MAJOR >= 5 && RTI_DDS_VERSION_MINOR > 0)
rc = dlOptions.setDomainId(loggerId);
assert (rc == DDS_RETCODE_OK);
rc = dlOptions.setApplicationKind(qos.participant_name.name);
assert (rc == DDS_RETCODE_OK);
#else
dlOptions.setDomainId(loggerId);
dlOptions.setApplicationKind(qos.participant_name.name);
#endif
RTI_DLDistLogger::setOptions(dlOptions);
dl = RTI_DLDistLogger::getInstance();
if(dl) {
dl->info("DdsEntitiesFactory initialized RTI Distributed Logger");
}
else {
KN_ERROR("RTI Distributed Logger - getInstance() returned NULL");
}
}
#endif
#if defined(KNDDS_HAS_DDS_LBPlugin) && defined(RTIDDS_LB_BETA_VERSION)
// disable simple-endpoind discovery if static discovery modules are specified
// we need to double-check if they can coexist peacefully...
if (!first->lbpdFile.empty()) {
qos.discovery_config.builtin_discovery_plugins &= ~DDS_DISCOVERYCONFIG_BUILTIN_SPDP;
}
if (!first->lbedFile.empty()) {
qos.discovery_config.builtin_discovery_plugins &= ~DDS_DISCOVERYCONFIG_BUILTIN_SEDP;
}
#endif
DDS::DomainParticipant * participant =
dpf->create_participant(first->domainId,
qos,
NULL /* listener */,
DDS::STATUS_MASK_NONE);
if (participant == NULL) {
throw Miro::Exception("DdsSupport::init() - Failed to create participant.");
}
// set default library/profile
if (library != NULL &&
(rc = participant->set_default_library(library)) != DDS_RETCODE_OK) {
ostr << "DdsSupport::init() - Failed to set participant default library: " << DdsSupport::getError(rc);
throw Miro::Exception(ostr.str());
}
if (profile != NULL &&
(rc = participant->set_default_profile(library, profile)) != DDS_RETCODE_OK) {
ostr << "DdsSupport::init() - Failed to set participant default profile: " << DdsSupport::getError(rc);
throw Miro::Exception(ostr.str());
}
#if defined(KNDDS_HAS_DDS_LBPlugin)
// add static discovery modules if specified
if (!first->lbpdFile.empty()) {
MIRO_LOG(LL_NOTICE, "DDS Static Participant Discovery");
# if defined(RTIDDS_LB_BETA_VERSION)
DDSLBPDiscoveryPlugin *pPlugin = new DDSLBPDiscoveryPlugin();
pPlugin->parseXmlFile(first->lbpdFile.c_str());
pPlugin->registerPlugin(participant);
# else
rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property,
"dds.discovery.participant.lbpdiscovery.library",
"rtilbpdisc",
DDS_BOOLEAN_FALSE);
assert (rc == DDS_RETCODE_OK);
rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property,
"dds.discovery.participant.lbpdiscovery.create_function",
"DDS_LBPDiscoveryPlugin_create",
DDS_BOOLEAN_FALSE);
assert (rc == DDS_RETCODE_OK);
rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property,
"dds.discovery.participant.lbpdiscovery.config_file",
first->lbpdFile.c_str(),
DDS_BOOLEAN_FALSE);
assert (rc == DDS_RETCODE_OK);
rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property,
"dds.discovery.participant.load_plugin",
"dds.discovery.endpoint.lbpdiscovery",
DDS_BOOLEAN_FALSE);
assert (rc == DDS_RETCODE_OK);
char valueBuffer[18];
sprintf(valueBuffer, "%i", first->lbedLogVerbosity);
rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property,
"dds.discovery.participant.lbpdiscovery.verbosity",
valueBuffer,
DDS_BOOLEAN_FALSE);
assert (rc == DDS_RETCODE_OK);
# endif
}
if (!first->lbedFile.empty()) {
MIRO_LOG(LL_NOTICE, "DDS Static Endpoint Discovery");
# if defined(RTIDDS_LB_BETA_VERSION)
DDSLBEDiscoveryPlugin *ePlugin = new DDSLBEDiscoveryPlugin();
ePlugin->parseXmlFile(first->lbedFile.c_str());
ePlugin->registerPlugin(participant);
# else
rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property,
"dds.discovery.endpoint.lbediscovery.library",
"rtilbedisc",
DDS_BOOLEAN_FALSE);
assert (rc == DDS_RETCODE_OK);
rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property,
"dds.discovery.endpoint.lbediscovery.create_function",
"DDS_LBEDiscoveryPlugin_create",
DDS_BOOLEAN_FALSE);
assert (rc == DDS_RETCODE_OK);
rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property,
"dds.discovery.endpoint.lbediscovery.config_file",
first->lbedFile.c_str(),
DDS_BOOLEAN_FALSE);
assert (rc == DDS_RETCODE_OK);
rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property,
"dds.discovery.endpoint.load_plugin",
"dds.discovery.endpoint.lbediscovery",
DDS_BOOLEAN_FALSE);
assert (rc == DDS_RETCODE_OK);
char valueBuffer[18];
sprintf(valueBuffer, "%i", first->lbedLogVerbosity);
rc = DDS_PropertyQosPolicyHelper_add_property(&qos.property,
"dds.discovery.endpoint.lbediscovery.verbosity",
valueBuffer,
DDS_BOOLEAN_FALSE);
assert (rc == DDS_RETCODE_OK);
# endif
}
#endif
// enable participant
rc = participant->enable();
if (rc != DDS_RETCODE_OK) {
ostr << "DdsSupport::init() - Failed to enable participant: " << DdsSupport::getError(rc);
throw Miro::Exception(ostr.str());
}
try {
partRepo->add(first->name, participant);
}
catch (Miro::RepositoryBase::EAlreadyRegistered const&) {
throw Miro::Exception("DdsSupport::init() - Duplicate name for participant at repo.");
}
//-------------------------------------------------
// register all relevant types with the participant
// by default assume, that all types are for all domains
if (first->types.empty()) {
DdsTypeRegistratorRepository::InstanceMap const& types =
typeRepo->getMap();
DdsTypeRegistratorRepository::InstanceMap::const_iterator f, l = types.end();
for (f = types.begin(); f != l; ++f) {
try {
f->second->registerType(participant);
}
catch (Miro::Exception const& e) {
ostr << "Error registering type" << f->first << " at participant " << first->name << ": " << e.what();
throw Miro::Exception(ostr.str());
}
}
}
else {
StringVector::const_iterator f, l = first->types.end();
for (f = first->types.begin(); f != l; ++f) {
try {
DdsTypeRegistratorBase * tReg = typeRepo->get(*f);
tReg->registerType(participant);
}
catch (Miro::Exception const& e) {
ostr << "Error registering type" << *f << " at participant " << first->name << ": " << e.what();
throw Miro::Exception(ostr.str());
}
}
}
//-------------------------------------------------
// create all flow controllers configured for the participant
vector<DdsFlowControllerParameters>::const_iterator f, l = first->flowControllers.end();
for (f = first->flowControllers.begin(); f != l; ++f) {
DDS::FlowControllerProperty_t property;
property.scheduling_policy =
(f->schedulingPolicy == "DDS_RR_FLOW_CONTROLLER_SCHED_POLICY")?
DDS_RR_FLOW_CONTROLLER_SCHED_POLICY : DDS_EDF_FLOW_CONTROLLER_SCHED_POLICY;
property.token_bucket.max_tokens = (f->tokenBucket.maxTokens < 0)?
DDS_LENGTH_UNLIMITED : f->tokenBucket.maxTokens;
property.token_bucket.tokens_added_per_period = (f->tokenBucket.tokensAddedPerPeriod < 0)?
DDS_LENGTH_UNLIMITED : f->tokenBucket.tokensAddedPerPeriod;
property.token_bucket.tokens_leaked_per_period = (f->tokenBucket.tokensLeakedPerPeriod < 0)?
DDS_LENGTH_UNLIMITED : f->tokenBucket.tokensLeakedPerPeriod;
if (f->tokenBucket.period > ACE_Time_Value::zero) {
property.token_bucket.period.sec = f->tokenBucket.period.sec();
property.token_bucket.period.nanosec = f->tokenBucket.period.usec() * 1000;
}
else {
property.token_bucket.period = DDS_DURATION_INFINITE;
}
property.token_bucket.bytes_per_token = (f->tokenBucket.bytesPerToken < 0)?
DDS_LENGTH_UNLIMITED : f->tokenBucket.bytesPerToken;
participant->create_flowcontroller(DDS::String_dup(f->name.c_str()), property);
}
DDS_DomainParticipantQos_finalize(&qos);
}
}
//-------------------------------------------------
// create all publishers in configuration file
// (default is 1)
DdsPublisherRepository * pubRepo = DdsPublisherRepository::instance();
{
NodeVector::const_iterator first, last = m_params.publishers.end();
for (first = m_params.publishers.begin(); first != last; ++first) {
// per publisher
DDS::DomainParticipant * participant = NULL;
try {
participant = partRepo->get(first->participant);
}
catch (Miro::Exception const&) {
ostr << "Publisher specified unknown participant: " << first->participant;
throw Miro::Exception(ostr.str());
}
char const * profile = first->profile.empty() ? NULL : first->profile.c_str();
char const * library = first->library.empty() ? NULL : first->library.c_str();
DDS::PublisherQos qos;
DDS_PublisherQos_initialize(&qos);
if (profile == NULL &&
(m_params.defaultLibrary.empty() || m_params.defaultProfile.empty())) {
DDS::ReturnCode_t rc = participant->get_default_publisher_qos(qos);
if (rc != DDS_RETCODE_OK) {
ostr << "Get default publisher Qos profile: " << DdsSupport::getError(rc);
throw Miro::Exception(ostr.str());
}
}
else {
DDS::ReturnCode_t rc = dpf->get_publisher_qos_from_profile(qos, library, profile);
if (rc != DDS_RETCODE_OK) {
ostr << "Load publisher Qos profile: " << DdsSupport::getError(rc);
throw Miro::Exception(ostr.str());
}
}
char const * partition = (first->partition.empty() || first->partition == "<NONE>")?
NULL : first->partition.c_str();
if (partition != NULL) {
qos.partition.name.maximum(1);
qos.partition.name.length(1);
qos.partition.name[0] = DDS_String_dup(partition);
}
DDS::Publisher * publisher =
participant->create_publisher(qos,
NULL /* listener */,
DDS::STATUS_MASK_NONE);
if (publisher == NULL) {
throw Miro::Exception("Failed to create publisher.");
}
// set default library/profile
if (library != NULL &&
(rc = publisher->set_default_library(library)) != DDS_RETCODE_OK) {
ostr << "DdsSupport::init() - Failed to set publisher default library: " << DdsSupport::getError(rc);
throw Miro::Exception(ostr.str());
}
if (profile != NULL &&
(rc = publisher->set_default_profile(library, profile)) != DDS_RETCODE_OK) {
ostr << "DdsSupport::init() - Failed to set publisher default profile: " << DdsSupport::getError(rc);
throw Miro::Exception(ostr.str());
}
try {
pubRepo->add(first->name, publisher);
}
catch (Miro::RepositoryBase::EAlreadyRegistered const&) {
throw Miro::Exception("DdsSupport::init() - Duplicate name for publisher at repo.");
}
DDS_PublisherQos_finalize(&qos);
}
}
//-------------------------------------------------
// create all subscribers in configuration file
// (default is 2)
DdsSubscriberRepository * subRepo = DdsSubscriberRepository::instance();
{
NodeVector::const_iterator first, last = m_params.subscribers.end();
for (first = m_params.subscribers.begin(); first != last; ++first) {
// per subscriber
DDS::DomainParticipant * participant = NULL;
try {
participant = partRepo->get(first->participant);
}
catch (Miro::Exception const&) {
ostr << "Subscriber specified unknown participant: " << first->participant;
throw Miro::Exception(ostr.str());
}
char const * profile = first->profile.empty() ? NULL : first->profile.c_str();
char const * library = first->library.empty() ? NULL : first->library.c_str();
DDS::SubscriberQos qos;
DDS_SubscriberQos_initialize(&qos);
if (profile == NULL &&
(m_params.defaultLibrary.empty() || m_params.defaultProfile.empty())) {
DDS::ReturnCode_t rc = participant->get_default_subscriber_qos(qos);
if (rc != DDS_RETCODE_OK) {
ostr << "Get default subscriber Qos profile: " << DdsSupport::getError(rc);
throw Miro::Exception(ostr.str());
}
}
else {
DDS::ReturnCode_t rc = dpf->get_subscriber_qos_from_profile(qos, library, profile);
if (rc != DDS_RETCODE_OK) {
ostr << "Load subscriber Qos profile: " << DdsSupport::getError(rc);
throw Miro::Exception(ostr.str());
}
}
char const * partition = (first->partition.empty() || first->partition == "<NONE>")?
NULL : first->partition.c_str();
if (partition != NULL) {
if (strcmp("<TEAM>", partition) == 0) {
Miro::RobotParameters * rp = Miro::RobotParameters::instance();
if (!rp->teamMembers.empty()) {
qos.partition.name.maximum(rp->teamMembers.size());
qos.partition.name.length(rp->teamMembers.size());
for (unsigned int i = 0; i < rp->teamMembers.size(); ++i) {
qos.partition.name[i] = DDS_String_dup(rp->teamMembers[i].c_str());
}
}
else {
MIRO_LOG(LL_WARNING, "Team partition requested, but RobotParameters::teamMembers is empty, assuming ALL.");
qos.partition.name.maximum(1);
qos.partition.name.length(1);
qos.partition.name[0] = DDS_String_dup("*");
}
}
else {
qos.partition.name.maximum(1);
qos.partition.name.length(1);
qos.partition.name[0] = DDS_String_dup(partition);
}
}
DDS::Subscriber * subscriber =
participant->create_subscriber(qos,
NULL /* listener */,
DDS::STATUS_MASK_NONE);
if (subscriber == NULL) {
throw Miro::Exception("Failed to create subscriber.");
}
// set default library/profile
if (library != NULL &&
(rc = subscriber->set_default_library(library)) != DDS_RETCODE_OK) {
ostr << "DdsSupport::init() - Failed to set subscriber default library: " << DdsSupport::getError(rc);
throw Miro::Exception(ostr.str());
}
if (profile != NULL &&
(rc = subscriber->set_default_profile(library, profile)) != DDS_RETCODE_OK) {
ostr << "DdsSupport::init() - Failed to set subscriber default profile: " << DdsSupport::getError(rc);
throw Miro::Exception(ostr.str());
}
try {
subRepo->add(first->name, subscriber);
}
catch (Miro::RepositoryBase::EAlreadyRegistered const&) {
throw Miro::Exception("DdsSupport::init() - Duplicate name for subscriber at repo.");
}
DDS_SubscriberQos_finalize(&qos);
}
}
//-------------------------------------------------
// create all topics in configuration file
// (default is 0)
{
TopicVector::const_iterator first, last = m_params.topics.end();
for (first = m_params.topics.begin(); first != last; ++first) {
DDS::DomainParticipant * participant = NULL;
try {
participant = partRepo->get(first->participant);
}
catch (Miro::Exception const&) {
ostr << "Topic specified unknown participant: " << first->participant;
throw Miro::Exception(ostr.str());
}
char const * profile = first->profile.empty() ? NULL : first->profile.c_str();
char const * library = first->library.empty() ? NULL : first->library.c_str();
DDS::Topic * topic =
(profile ||
(!m_params.defaultLibrary.empty() && !m_params.defaultProfile.empty())) ?
participant->create_topic_with_profile(first->name.c_str(),
first->typeName.c_str(),
library,
profile,
NULL /* listener */,
DDS::STATUS_MASK_NONE) :
participant->create_topic(first->name.c_str(),
first->typeName.c_str(),
DDS_TOPIC_QOS_DEFAULT,
NULL /* listener */,
DDS::STATUS_MASK_NONE);
if (topic == NULL) {
throw Miro::Exception("Failed to create topic.");
}
}
}
}
