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; }
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; }
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; }