示例#1
0
  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);
  }
示例#2
0
  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;
  }
示例#3
0
  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));
      }
    }
  }
示例#4
0
  // 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));
      }
    }
  }
示例#5
0
  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);
  }
示例#6
0
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;
}
示例#7
0
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
示例#8
0
  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.");
        }
      }
    }

  }