CORBA::ULong
Reader_T<READER_TYPE, TYPED_DDS_READER, VALUE_TYPE, SEQ_VALUE_TYPE>::get_nr_valid_samples (
const ::DDS::SampleInfoSeq& sample_infos,
const bool determine_last)
{
DDS4CCM_TRACE ("Reader_T::get_nr_valid_samples");
CORBA::ULong nr_of_samples = 0;
if (determine_last)
{
for (::CORBA::ULong i = 0 ; i < sample_infos.length(); ++i)
{
if (sample_infos[i].valid_data &&
sample_infos[i].sample_rank == 0)
{
++nr_of_samples;
}
}
}
else
{
for (::CORBA::ULong i = 0 ; i < sample_infos.length(); ++i)
{
if (sample_infos[i].valid_data)
{
++nr_of_samples;
}
}
}
return nr_of_samples;
}
void
Reader_T<READER_TYPE, TYPED_DDS_READER, VALUE_TYPE, SEQ_VALUE_TYPE>::convert_data (
const SEQ_VALUE_TYPE & all_data,
SEQ_VALUE_TYPE & data_to_return,
::CCM_DDS::ReadInfoSeq& infos,
const ::DDS::SampleInfoSeq & sample_info)
{
DDS4CCM_TRACE ("Reader_T::convert_data");
CORBA::ULong samples_to_return = this->get_nr_valid_samples (sample_info,
false);
infos.length (samples_to_return);
data_to_return.length (samples_to_return);
CORBA::ULong ix = 0;
for (::CORBA::ULong i = 0 ; i < sample_info.length(); ++i)
{
if (sample_info[i].valid_data)
{
infos[ix] <<= sample_info[i];
data_to_return[ix] = all_data[i];
++ix;
}
}
}
::DDS::ReturnCode_t
DataReader_T <TYPED_DDS_READER, TYPED_READER_TYPE, VALUE_TYPE, SEQ_TYPE, RTI_SEQ_TYPE>::complete_read (
RTI_SEQ_TYPE & dds_data_values,
SEQ_TYPE & data_values,
DDS_SampleInfoSeq & dds_sample_infos,
::DDS::SampleInfoSeq & sample_infos,
const ::DDS::ReturnCode_t & retcode,
const char * method_name)
{
if (retcode == ::DDS::RETCODE_OK)
{
data_values.length (dds_data_values.length ());
sample_infos.length (dds_sample_infos.length ());
for (::DDS_Long i = 0 ; i < dds_sample_infos.length(); ++i)
{
sample_infos[i] <<= dds_sample_infos[i];
data_values[i] = dds_data_values[i];
}
}
else if (retcode != ::DDS::RETCODE_NO_DATA)
{
DDS4CCM_ERROR (DDS4CCM_LOG_LEVEL_ERROR, (LM_ERROR, DDS4CCM_INFO
ACE_TEXT ("DataReader_T::%C - ")
ACE_TEXT ("Error while reading samples from DDS - <%C>\n"),
method_name,
::CIAO::DDS4CCM::translate_retcode (retcode)));
}
::DDS::ReturnCode_t const retcode_return_loan =
this->rti_entity ()->return_loan (dds_data_values, dds_sample_infos);
if (retcode_return_loan != ::DDS::RETCODE_OK)
{
DDS4CCM_ERROR (DDS4CCM_LOG_LEVEL_ERROR, (LM_ERROR, DDS4CCM_INFO
ACE_TEXT ("DataReader_T::%C - ")
ACE_TEXT ("Error returning loan to DDS - <%C>\n"),
method_name,
::CIAO::DDS4CCM::translate_retcode (retcode_return_loan)));
// In case a read action from DDS causes errors, the users wants to see
// this error (and not the return loan error).
if (retcode == ::DDS::RETCODE_OK)
return retcode_return_loan;
}
return retcode;
}
void EBRouter::run() {
DDSEventChannel::EventContainerSeq eventContainerSeq;
DDSEventChannel::EventContainer rcvEventContainer;
DDS::SampleInfoSeq infoSeq;
init();
while (!shutdownRequested) {
eventContainerDataReader->take_w_condition(eventContainerSeq, infoSeq,
DDS::LENGTH_UNLIMITED, readCondition);
if (infoSeq.length() > 0) {
for (unsigned int i = 0; i < eventContainerSeq.length(); i++) {
if (infoSeq[i].valid_data) {
EBEvent event;
rcvEventContainer = eventContainerSeq[i];
event.publisherID = rcvEventContainer.publisherID;
event.eventID = rcvEventContainer.eventID;
event.eventCatType = rcvEventContainer.eventCatType;
event.eventDefType = rcvEventContainer.eventDefType;
event.eventData = rcvEventContainer.eventData;
notifySubscribers(event);
}
}
}
eventContainerDataReader->return_loan(eventContainerSeq, infoSeq);
publishPending();
boost::this_thread::sleep(boost::posix_time::milliseconds(30));
}
threadHasComplete = true;
}
::DDS::ReturnCode_t
ROSDDS::MsgDataReader_impl::check_preconditions(
ROSDDS::MsgSeq & received_data,
::DDS::SampleInfoSeq & info_seq,
::DDS::Long max_samples)
{
::DDS::ReturnCode_t status = ::DDS::RETCODE_PRECONDITION_NOT_MET;
if ( received_data.length() == info_seq.length() &&
received_data.maximum() == info_seq.maximum() &&
received_data.release() == info_seq.release() ) {
if ( received_data.maximum() == 0 || received_data.release() ) {
if (received_data.maximum() == 0 ||
max_samples <= static_cast<DDS::Long>(received_data.maximum()) ||
max_samples == ::DDS::LENGTH_UNLIMITED ) {
status = ::DDS::RETCODE_OK;
}
}
}
return status;
}
::DDS::ReturnCode_t
rcl_interfaces::srv::dds_::Sample_SetParameters_Request_DataReader_impl::check_preconditions(
rcl_interfaces::srv::dds_::Sample_SetParameters_Request_Seq & received_data,
::DDS::SampleInfoSeq & info_seq,
::DDS::Long max_samples)
{
::DDS::ReturnCode_t status = ::DDS::RETCODE_PRECONDITION_NOT_MET;
if ( received_data.length() == info_seq.length() &&
received_data.maximum() == info_seq.maximum() &&
received_data.release() == info_seq.release() ) {
if ( received_data.maximum() == 0 || received_data.release() ) {
if (received_data.maximum() == 0 ||
max_samples <= static_cast<DDS::Long>(received_data.maximum()) ||
max_samples == ::DDS::LENGTH_UNLIMITED ) {
status = ::DDS::RETCODE_OK;
}
}
}
return status;
}
::DDS::ReturnCode_t
geometry_msgs::msg::dds_::PoseWithCovariance_DataReader_impl::check_preconditions(
geometry_msgs::msg::dds_::PoseWithCovariance_Seq & received_data,
::DDS::SampleInfoSeq & info_seq,
::DDS::Long max_samples)
{
::DDS::ReturnCode_t status = ::DDS::RETCODE_PRECONDITION_NOT_MET;
if ( received_data.length() == info_seq.length() &&
received_data.maximum() == info_seq.maximum() &&
received_data.release() == info_seq.release() ) {
if ( received_data.maximum() == 0 || received_data.release() ) {
if (received_data.maximum() == 0 ||
max_samples <= static_cast<DDS::Long>(received_data.maximum()) ||
max_samples == ::DDS::LENGTH_UNLIMITED ) {
status = ::DDS::RETCODE_OK;
}
}
}
return status;
}
DDS::ReturnCode_t
PoseWithCovarianceDataReader_impl::check_preconditions(
PoseWithCovarianceSeq & received_data,
DDS::SampleInfoSeq & info_seq,
CORBA::Long max_samples)
{
DDS::ReturnCode_t status = DDS::RETCODE_PRECONDITION_NOT_MET;
if ( received_data.length() == info_seq.length() &&
received_data.maximum() == info_seq.maximum() &&
received_data.release() == info_seq.release() ) {
if ( received_data.maximum() == 0 || received_data.release() ) {
if (received_data.maximum() == 0 ||
max_samples <= static_cast<CORBA::Long>(received_data.maximum()) ||
max_samples == DDS::LENGTH_UNLIMITED ) {
status = DDS::RETCODE_OK;
}
}
}
return status;
}
void KVStore::receiveUpdates() {
DDSKVStore::TransactionSeq transactionSeq;
DDSKVStore::Transaction rcvTransaction;
DDS::SampleInfoSeq infoSeq;
transactionDataReader->take_w_condition(transactionSeq, infoSeq,
DDS::LENGTH_UNLIMITED, readCondition);
if (infoSeq.length() > 0) {
for (unsigned int i = 0; i < transactionSeq.length(); i++) {
if (infoSeq[i].valid_data) {
rcvTransaction = transactionSeq[i];
// Only update the dataStore from transactions originating
// from other kvstore publishers
if ((rcvTransaction.action == DDSKVStore::DBASE_SET)
&& rcvTransaction.publisherID != publisherID) {
for (unsigned int j = 0; j < rcvTransaction.keyValueSet.length();
j++) {
int bufferLength =
rcvTransaction.keyValueSet[j].byteBuffer.length();
KVSObject kvsObject;
kvsObject.key = rcvTransaction.keyValueSet[j].key;
kvsObject.typeInfo = rcvTransaction.keyValueSet[j].typeInfo;
kvsObject.numberElements =
rcvTransaction.keyValueSet[j].numberElements;
kvsObject.byteBuffer.resize(bufferLength);
memcpy(&kvsObject.byteBuffer[0],
&rcvTransaction.keyValueSet[j].byteBuffer[0],
bufferLength);
// Perform byte swapping (if required)
if (rcvTransaction.systemByteOrder != systemByteOrder) {
int8_t* value_ptr = (int8_t *) &kvsObject.byteBuffer[0];
switch (kvsObject.typeInfo) {
case DDSKVStore::TYPE_INT16:
if (kvsObject.numberElements == 1)
ByteSwapper::swapTwoBytes(value_ptr);
else
ByteSwapper::swapTwoByteArray(value_ptr,
kvsObject.numberElements);
break;
case DDSKVStore::TYPE_INT32:
case DDSKVStore::TYPE_FLOAT:
if (kvsObject.numberElements == 1)
ByteSwapper::swapFourBytes(value_ptr);
else
ByteSwapper::swapFourByteArray(value_ptr,
kvsObject.numberElements);
break;
case DDSKVStore::TYPE_INT64:
case DDSKVStore::TYPE_DOUBLE:
if (kvsObject.numberElements == 1)
ByteSwapper::swapEightBytes(value_ptr);
else
ByteSwapper::swapEightByteArray(value_ptr,
kvsObject.numberElements);
break;
default:
break;
}
}
dataStore[kvsObject.key] = kvsObject;
}
}
}
}
}
transactionDataReader->return_loan(transactionSeq, infoSeq);
}