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