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