Esempio n. 1
0
bool
RosDdsBridge::start(void)
{
    // set up DDS
    m_ddsParticipant = createDDSParticipant(0);
    if (m_ddsParticipant == 0)
    {
        return false;
    }

    DDSPublisher* ddsPublisher =
        m_ddsParticipant->create_publisher(DDS_PUBLISHER_QOS_DEFAULT, 0, DDS_STATUS_MASK_NONE);
    if (ddsPublisher == 0)
    {
        ROS_ERROR("create_publisher error");
        ddsShutdown(m_ddsParticipant);
        return false;
    }

    DDS_DataWriterQos writer_qos;
    DDS_ReturnCode_t retcode = ddsPublisher->get_default_datawriter_qos(writer_qos);

    if (retcode != DDS_RETCODE_OK)
    {
        ROS_ERROR("get_default_datawriter_qos error %d", retcode);
        ddsShutdown(m_ddsParticipant);
        return false;
    }

    // use non-strict reliability model
    writer_qos.publish_mode.kind = DDS_ASYNCHRONOUS_PUBLISH_MODE_QOS;
    writer_qos.liveliness.lease_duration.sec = 1;
    writer_qos.liveliness.lease_duration.nanosec = 0;

    writer_qos.reliability.kind = DDS_RELIABLE_RELIABILITY_QOS;
    writer_qos.history.kind = DDS_KEEP_LAST_HISTORY_QOS;
    writer_qos.history.depth = 2;

    writer_qos.resource_limits.max_samples =
            writer_qos.resource_limits.initial_samples = 2;
    writer_qos.resource_limits.max_samples_per_instance =
            writer_qos.resource_limits.max_samples;

    writer_qos.protocol.rtps_reliable_writer.heartbeats_per_max_samples = 1;

    writer_qos.protocol.rtps_reliable_writer.high_watermark = 1;
    writer_qos.protocol.rtps_reliable_writer.low_watermark = 0;

    retcode = ddsPublisher->set_default_datawriter_qos(writer_qos);

    if (retcode != DDS_RETCODE_OK)
    {
        ROS_ERROR("set_default_datawriter_qos error %d", retcode);
        ddsShutdown(m_ddsParticipant);
        return false;
    }

    // register type before creating topic
    const char* type_name = px_comm::DDSImageTypeSupport::get_type_name();
    retcode = px_comm::DDSImageTypeSupport::register_type(m_ddsParticipant, type_name);

    if (retcode != DDS_RETCODE_OK)
    {
        ROS_ERROR("register_type error %d", retcode);
        ddsShutdown(m_ddsParticipant);
        return false;
    }

    m_ddsImageWriter.resize(k_nCameras);
    m_ddsImage.resize(k_nCameras);
    for (int i = 0; i < k_nCameras; ++i)
    {
        std::ostringstream oss;
        oss << "cam" << i;

        DDSTopic* topic =
            m_ddsParticipant->create_topic(oss.str().c_str(), type_name,
                                           DDS_TOPIC_QOS_DEFAULT, 0, DDS_STATUS_MASK_NONE);

        if (topic == 0)
        {
            ROS_ERROR("create_topic error");
            ddsShutdown(m_ddsParticipant);
            return false;
        }

        DDSDataWriter* writer =
            ddsPublisher->create_datawriter(topic, DDS_DATAWRITER_QOS_DEFAULT, 0,
                                            DDS_STATUS_MASK_NONE);
        if (writer == 0)
        {
            ROS_ERROR("create_datawriter error");
            ddsShutdown(m_ddsParticipant);
            return false;
        }

        m_ddsImageWriter.at(i) = px_comm::DDSImageDataWriter::narrow(writer);
        if (m_ddsImageWriter.at(i) == 0)
        {
            ROS_ERROR("DataWriter narrow error");
            ddsShutdown(m_ddsParticipant);
            return false;
        }

        m_ddsImage.at(i) = px_comm::DDSImageTypeSupport::create_data();
        if (m_ddsImage.at(i) == 0)
        {
            ROS_ERROR("create_data error");
            ddsShutdown(m_ddsParticipant);
            return false;
        }

        ROS_INFO("Publishing to DDS topic: %s", oss.str().c_str());
    }

    // set up ROS
    m_rosImageSubscriber.resize(k_nCameras);
    for (int i = 0; i < k_nCameras; ++i)
    {
        std::ostringstream oss;
        oss << "cam" << i << "/image_raw";

        m_rosImageSubscriber.at(i) = m_imageTransport.subscribe(oss.str(), 1, boost::bind(&RosDdsBridge::rosImageCallback, this, _1, m_ddsImageWriter.at(i), m_ddsImage.at(i)));

        ROS_INFO("Subscribed to ROS topic: %s", m_rosImageSubscriber.at(i).getTopic().c_str());
    }

    return true;
}
Esempio n. 2
0
int ACE_TMAIN (int argc, ACE_TCHAR *argv[])
{
    DDS_ReturnCode_t    retcode;
    DDSTopic*           topic = 0;
    DDSDataWriter*      data_writer = 0;
    DDSPublisher*       publisher = 0;
    int                 main_result = 1; /* error by default */

//     NDDS_Config_LogVerbosity n_verbosity =
//       static_cast <NDDS_Config_LogVerbosity> (3);
//     NDDSConfigLogger::get_instance()->set_verbosity (n_verbosity);
    const ACE_TCHAR * env_domain_id = 0;
    if (argc > 1)
    {
        env_domain_id = argv[1];
    }
    if (!env_domain_id)
    {
        printf ("Environment variable DEFAULT_DOMAIN_ID not set "
                "=> setting it to 2\n");
        env_domain_id = "2";
    }
    else
        printf ("Domain ID set to %s\n", env_domain_id);

    const int domain_id = ACE_OS::atoi (env_domain_id);

    DDSDomainParticipant *participant = DDSDomainParticipantFactory::get_instance()->
                                        create_participant_with_profile (domain_id,    /* Domain ID */
                                                LIBRARY_NAME, /* QoS */
                                                PROFILE_NAME,
                                                0,            /* Listener */
                                                DDS_STATUS_MASK_NONE);
    if (!participant) {
        cerr << "SENDER: Error creating participant" << endl;
        goto clean_exit;
    }

    retcode = QueryConditionTestTypeSupport::register_type(
                  participant, QueryConditionTestTypeSupport::get_type_name ());

    if (retcode != DDS_RETCODE_OK) {
        cerr << "SENDER: Error registering type" << endl;
        goto clean_exit;
    }

    topic = participant->create_topic_with_profile (
                "QC",                   /* Topic name*/
                QueryConditionTestTypeSupport::get_type_name (), /* Type name */
                LIBRARY_NAME,       /* QoS */
                PROFILE_NAME,
                0,                  /* Listener  */
                DDS_STATUS_MASK_NONE);
    if (!topic) {
        cerr << "SENDER: Error creating topic" << endl;
        goto clean_exit;
    }

    publisher = participant->create_publisher_with_profile (LIBRARY_NAME,
                PROFILE_NAME,
                0,
                0);
    if (!publisher) {
        cerr << "SENDER: Error creating publisher" << endl;
        goto clean_exit;
    }

    data_writer = publisher->create_datawriter_with_profile (
                      topic,
                      LIBRARY_NAME, /* QoS */
                      PROFILE_NAME,
                      0,            /* Listener */
                      DDS_STATUS_MASK_NONE);
    if (!data_writer) {
        cerr << "SENDER: Error creating data writer" << endl;
        goto clean_exit;
    }

    write (data_writer);
    sleep_now (8);
    write (data_writer);
    sleep_now (8);
    write (data_writer);
    sleep_now (30);

    main_result = 0;
clean_exit:
    cout << "SENDER: Exiting ..." << endl;
    if (participant) {
        retcode = participant->delete_contained_entities();
        if (retcode != DDS_RETCODE_OK) {
            main_result = 1;
        }
        retcode = DDSDomainParticipantFactory::get_instance()->
                  delete_participant(participant);
        if (retcode != DDS_RETCODE_OK) {
            main_result = 1;
        }
    }
    DDSDomainParticipantFactory::finalize_instance ();
    return main_result;
}
Esempio n. 3
0
	void MyPublisher::publish(unsigned short *image, const std::string& qosProfileName, 
		uint32_t domainId, const std::string& topic)
	{
		DDSDomainParticipant *participant = DDSDomainParticipantFactory::get_instance()->
			create_participant_with_profile(
											domainId,
											"DataExchangeProfile",
											qosProfileName.c_str(),
											NULL,
											DDS_STATUS_MASK_NONE);

		if(participant == NULL) {
			std::cout << "Error while creating participant" << std::endl;
		}

		DataExchangeTypeSupport::register_type(participant, DataExchangeTypeSupport::get_type_name());

		DDSTopic *ddstopic = participant->create_topic(
			topic.c_str(),
			DataExchangeTypeSupport::get_type_name(),
			DDS_TOPIC_QOS_DEFAULT,
			NULL,           /* listener */
			DDS_STATUS_MASK_NONE);

		if(ddstopic == NULL) {
			std::cout << "Publisher: Error while creating topic: " << topic << std::endl;
			std::cout << "With typename: " << DataExchangeTypeSupport::get_type_name() << std::endl;
		}

		DDSPublisher *publisher = participant->create_publisher(
			DDS_PUBLISHER_QOS_DEFAULT,
			NULL,           /* listener */
			DDS_STATUS_MASK_NONE);

		if(publisher == NULL) {
			std::cout << "Error while creating publisher" << std::endl;
		}

		DDSDataWriter *dataWriter = publisher->create_datawriter(
			ddstopic,
			DDS_DATAWRITER_QOS_DEFAULT,
			NULL,           /* listener */
			DDS_STATUS_MASK_NONE);

		if(dataWriter == NULL) {
			std::cout << "Error while creating dataWriter" << std::endl;
		}

		DataExchangeDataWriter *dataExchangeWriter    = NULL;
		dataExchangeWriter = DataExchangeDataWriter::narrow(dataWriter);

		DDS_Duration_t disc_period = {1,0};
		NDDSUtility::sleep(disc_period);

		DataExchange *instance                 = NULL;

		instance = DataExchangeTypeSupport::create_data_ex(DDS_BOOLEAN_FALSE);
		int rows = 1024;
		int cols = 1024;
		DDS_UnsignedShortSeq_ensure_length(&instance->pixels, rows*cols, MAX_PIXELS_SEQUENCE_LENGTH);

		for (int i = 0; i < rows*cols; ++i) {
			instance->pixels[i] = i;
		}

		std::cout << "Sending dataaa...";
		instance->rows = rows;
		instance->cols = cols;
		dataExchangeWriter->write(*instance, DDS_HANDLE_NIL);
		std::cout << " DONE!" << std::endl;
	}