void
DdsRosBridge::poll(void)
{
struct DDS_Duration_t wait_timeout = {2,0};
DDSConditionSeq activeConditionsSeq;
DDS_ReturnCode_t retcode = m_waitset->wait(activeConditionsSeq, wait_timeout);
if (retcode == DDS_RETCODE_TIMEOUT)
{
return;
}
if (retcode != DDS_RETCODE_OK)
{
ROS_ERROR("wait error: %d", retcode);
return;
}
for (size_t i = 0; i < activeConditionsSeq.length(); ++i)
{
int cameraId = m_ddsConditionMap[activeConditionsSeq[i]];
px_comm::DDSImageDataReader* reader = m_ddsImageReader.at(cameraId);
px_comm::DDSImageSeq dataSeq;
DDS_SampleInfoSeq infoSeq;
retcode = reader->take(dataSeq, infoSeq,
DDS_LENGTH_UNLIMITED, DDS_ANY_SAMPLE_STATE,
DDS_ANY_VIEW_STATE, DDS_ANY_INSTANCE_STATE);
if (retcode == DDS_RETCODE_NO_DATA)
{
continue;
}
else if (retcode != DDS_RETCODE_OK)
{
ROS_WARN("take error %d\n", retcode);
continue;
}
for (int j = 0; j < dataSeq.length(); ++j)
{
if (!infoSeq[j].valid_data)
{
continue;
}
ddsImageCallback(&dataSeq[j], &m_rosImagePublisher.at(cameraId),
m_rosImage.at(cameraId));
}
retcode = reader->return_loan(dataSeq, infoSeq);
if (retcode != DDS_RETCODE_OK)
{
ROS_WARN("return_loan error %d\n", retcode);
continue;
}
}
}
void
DDS_WaitSet_i::convert_conditions (const DDSConditionSeq & dds_conditions,
::DDS::ConditionSeq & conditions)
{
DDS4CCM_TRACE ("DDS_WaitSet_i::convert_conditions");
conditions.length (dds_conditions.length ());
for (DDS_Long i = 0;
i < dds_conditions.length();
++i)
{
DDSQueryCondition * dds_qc =
dynamic_cast <DDSQueryCondition *> (dds_conditions[i]);
if (dds_qc)
{
::DDS::QueryCondition_var cond;
ACE_NEW_THROW_EX (cond,
DDS_QueryCondition_i (dds_qc,
::DDS::DataReader::_nil ()),
::CORBA::NO_MEMORY ());
conditions[i] = ::DDS::QueryCondition::_duplicate (cond.in ());
}
else
{
DDSReadCondition * dds_rc =
dynamic_cast <DDSReadCondition *> (dds_conditions[i]);
if (dds_rc)
{
::DDS::ReadCondition_var cond;
ACE_NEW_THROW_EX (cond,
DDS_ReadCondition_i (dds_rc,
::DDS::DataReader::_nil ()),
::CORBA::NO_MEMORY ());
conditions[i] = ::DDS::ReadCondition::_duplicate (cond.in ());
}
}
}
}
void read (DDSDataReader * dr,
DDSQueryCondition * qc,
DDSReadCondition * rc,
const int & run)
{
QueryConditionTestDataReader* typed_dr = QueryConditionTestDataReader::narrow (dr);
DDSConditionSeq cond;
if (ws_->wait (cond, dur_) == DDS_RETCODE_TIMEOUT)
{
cerr << "ERROR: wait timed out" << endl;
}
else
{
//wait a moment to allow DDS to write all samples we need here...
QueryConditionTestSeq data;
DDS_SampleInfoSeq info_seq;
sleep_now (3);
for (DDS_Long i = 0; i < cond.length (); ++i)
{
if (run == 3)
{
sleep_now (10);
if (cond[i] == rc)
{
// *************** read one by one : max_number of samples = 1 ************
int loop = 0;
cout << "CHECKING..... SAMPLES with read_w_condition with readcondition:" << endl;
received_samples = 0;
while (loop < 30)
{
DDS_ReturnCode_t retcode = typed_dr->read_w_condition (data,
info_seq,
1,
rc);
loop ++;
if (retcode == 0)
{
for (DDS_Long y = 0; y < data.length (); ++y)
{
if (info_seq[y].valid_data)
{
check_iter (data[y], info_seq[y], run);
received_samples ++;
}
}
}
typed_dr->return_loan (data, info_seq);
}
// *************** end read one by one
if (received_samples != expected_samples_run3)
{
cerr << "ERROR: RUN 3 UNEXPECTED NUMBER OF SAMPLES RECEIVED : "
<< "with read_w_condition and read condition, expected < "
<< expected_samples_run3 << "> - received <"
<< received_samples << ">" << endl;
}
// check readcondition
//DDS_SampleStateMask sample = rc->get_sample_state_mask ();
//DDS_ViewStateMask view = rc->get_view_state_mask ();
//DDS_InstanceStateMask instance = rc->get_instance_state_mask ();
//::printf("************sample_state %ld, view_state %ld instance_state %ld\n",
// (long)sample, (long)view, (long)instance);
}
else
{
cerr << "ERROR: Should be woken up on ReadCondition" << endl;
}
}
else // run 1 and 2
{
if (cond[i] == qc)
{
cout << "CHECKING..... SAMPLES with read_w_condition with querycondition:" << endl;
received_samples = 0;
typed_dr->read_w_condition (data,
info_seq,
DDS_LENGTH_UNLIMITED,
qc);
for (DDS_Long i = 0; i < data.length (); ++i)
{
if (info_seq[i].valid_data)
{
check_iter (data[i], info_seq[i], run);
received_samples ++;
}
}
typed_dr->return_loan (data, info_seq);
if (run == 1)
{
if (received_samples != expected_samples_run1)
{
cerr << "ERROR: run 1 unexpected number of samples received : "
<< "expected < "
<< expected_samples_run1 << "> - received <"
<< received_samples << ">" << endl;
}
}
else
{
if (received_samples != expected_samples_run2)
{
cerr << "ERROR: run 2 unexpected number of samples received : "
<< "expected < "
<< expected_samples_run2 << "> - received <"
<< received_samples << ">" << endl;
}
}
}
else
{
cerr << "ERROR: Should be woken up on QueryCondition" << endl;
}
}
}
}
//check after run 3 if all samples are still available.
if (run == 3)
{
QueryConditionTestSeq data;
DDS_SampleInfoSeq info_seq;
sleep_now (3);
typed_dr->read (data,
info_seq,
DDS_LENGTH_UNLIMITED);
cout << "\nCHECKING..... TOTAL NUMBER OF SAMPLES IN DDS with a read : <"
<< data.length ()
<< ">"
<< endl;
for (DDS_Long i = 0; i < data.length (); ++i)
{
cout << "Received: key <"
<< data[i].symbol
<< "> - iteration <"
<< data[i].iteration
<< "> - instance status <"
<< info_seq[i].instance_state
<< "> - view status <"
<< info_seq[i].view_state
<< "> - sample status <"
<< info_seq[i].sample_state
<< ">"
<< endl;
}
if (data.length () != 18)
{
cerr << "ERROR: unexpected number of samples received : "
<< "expected <18> - received <"
<< data.length () << ">" << endl;
}
typed_dr->return_loan (data, info_seq);
}
sleep_now (5);
}