Exemplo n.º 1
0
    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;
    }
Exemplo n.º 2
0
    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;
            }
        }
    }
Exemplo n.º 3
0
    ::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;
    }
Exemplo n.º 4
0
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;
}
Exemplo n.º 5
0
::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;
}
Exemplo n.º 9
0
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);

}