Example #1
0
//====================================================================
//== main
//====================================================================
int main(int argc, char * argv[])
{
  Miro::Log::init(argc, argv);
  Miro::Robot::init(argc, argv);

  kn::DdsEntitiesFactorySvcParameters * ddsParams = kn::DdsEntitiesFactorySvcParameters::instance();
  ddsParams->defaultLibrary = "RapidQosLibrary";

  kn::DdsSupport::init(argc, argv);

  ddsParams->participants[0].participantName = "RaftFileQueueReceiver";
  ddsParams->participants[0].profile         = "RapidDefaultQos";

  if (parseArgs(argc, argv) != 0) {
    return 1;
  }

  kn::DdsEntitiesFactorySvc ddsEntities;
  ddsEntities.init(ddsParams);

  {
    // make sure our staging and destination directories exist
    FileReceiverParameters* params = FileReceiverParameters::instance();
    QDir dir;
    if( !dir.mkpath(params->dataDestinationDir.c_str()) ) {
      KN_ERROR("Failed to create data destination directory: %s", params->dataDestinationDir.c_str());
      return -1;
    }
    if( !dir.mkpath(params->dataStagingDir.c_str()) ) {
      KN_ERROR("Failed to create staging directory: %s", params->dataStagingDir.c_str());
      return -1;
    }
  }

  {
    // make sure that all dds readers/writers end their scope
    // before we finalize the ddsEntities factory
    FileQueueReceiverSamplePublisher statistics(rapid::FILEQUEUERECEIVER_SAMPLE_TOPIC,
                                                "",
                                                "RapidFileQueueReceiverSampleProfile");
    rapid::RapidHelper::initHeader(statistics.event().hdr);

    DDS::WaitSet waitset;
    DDS::ReturnCode_t retcode = DDS_RETCODE_OK;

    FileAnnounceHandler announceHandler;
    FileAnnounceSubscriber announceSubscriber(rapid::FILEANNOUNCE_TOPIC,
                                              Miro::RobotParameters::instance()->name,
                                              "RapidFileAnnounceProfile");
    FileAnnounce::DataReader& announceReader = announceSubscriber.dataReader();
    DDS::StatusCondition * announceCondition = announceReader.get_statuscondition();

    announceCondition->set_enabled_statuses(DDS_DATA_AVAILABLE_STATUS);
    retcode = waitset.attach_condition(announceCondition);
    if (retcode != DDS_RETCODE_OK) {
      KN_FATAL_OSTR("attach_condition error: " << kn::DdsSupport::getError(retcode));
      return -1;
    }

    FileSampleHandler sampleHandler;
    FileSampleSubscriber sampleSubscriber(rapid::FILEQUEUE_SAMPLE_TOPIC,
                                          "",
                                          "RapidFileQueueSampleProfile");
    FileQueueSample::DataReader& sampleReader = sampleSubscriber.dataReader();
    DDS::StatusCondition * sampleCondition = sampleReader.get_statuscondition();
    sampleCondition->set_enabled_statuses(DDS_DATA_AVAILABLE_STATUS);
    retcode = waitset.attach_condition(sampleCondition);
    if (retcode != DDS_RETCODE_OK) {
      KN_FATAL_OSTR("attach_condition error: " << kn::DdsSupport::getError(retcode));
      return -1;
    }


    DDS::ConditionSeq activeConditions;
    DDS::Duration_t const timeout = {1, 0}; // wait for 1 sec max

    int length;
    FileAnnounceSeq fileAnnounces;
    FileQueueSampleSeq samples;
    DDS::SampleInfoSeq infos;


    ACE_Time_Value lastReceiverSampleSend(0);

    Miro::ShutdownHandler shutdownHandler;
    while (!shutdownHandler.isShutdown()) { // main loop

      // some feedback for now...
      cout << "." << flush;
      // DDS event-loop processing
      // The triggered condition(s) will be placed in activeConditions
      retcode = waitset.wait(activeConditions, timeout);
      if (retcode != DDS_RETCODE_TIMEOUT) {
        if (retcode != DDS_RETCODE_OK) {
          KN_FATAL_OSTR("waitset error" << kn::DdsSupport::getError(retcode));
          return -1;
        }

        for (int i = 0; i < activeConditions.length(); ++i) { // dds subscribers
          if (activeConditions[i] == announceCondition) {
            do {
              announceReader.take(fileAnnounces,
                                  infos,
                                  1024,
                                  DDS::ANY_SAMPLE_STATE,
                                  DDS::ANY_VIEW_STATE,
                                  DDS::ANY_INSTANCE_STATE);

              length = fileAnnounces.length();
              for (DDS::Long j = 0; j < length; ++j) {
                if (infos[j].valid_data) {
                  announceHandler(&fileAnnounces[j]);
                }
                else {
                  KN_INFO("DDS FileSample take !valid_data");
                }
              }
              ACE_OS::sleep(1);
              retcode = announceReader.return_loan(fileAnnounces, infos);
              if (retcode != DDS::RETCODE_OK) {
                KN_ERROR_OSTR("DDS FileAnnounce return loan error: " << kn::DdsSupport::getError(retcode));
              }
            }
            while (length == 1024);
          }

          if (activeConditions[i] == sampleCondition) {
            do {
              sampleReader.take(samples,
                                infos,
                                1024,
                                DDS::ANY_SAMPLE_STATE,
                                DDS::ANY_VIEW_STATE,
                                DDS::ANY_INSTANCE_STATE);

              length = samples.length();
              for (DDS::Long i = 0; i < length; ++i) {
                if (infos[i].valid_data) {
                  KN_INFO("FileQueueSample");
                  sampleHandler(&samples[i]);
                }
                else {
                  KN_INFO("DDS FileSample take !valid_data");
                }
              }

              retcode = sampleReader.return_loan(samples, infos);
              if (retcode != DDS::RETCODE_OK) {
                KN_ERROR_OSTR("DDS FileQueueSample return loan error: " << kn::DdsSupport::getError(retcode));
              }
            }
            while (length == 1024);
          }
        }
      }

      StringQueue& completedFiles = sampleHandler.getCompletedFiles();
      while (!completedFiles.empty()) {
        std::string uuid = completedFiles.pop();

        if(announceHandler.hasUuid(uuid)) {
          moveFile(uuid, announceHandler);
          sampleHandler.removeUuid(uuid);
          announceHandler.removeUuid(uuid);
        }
      }

      StringQueue& announcedFiles = announceHandler.getAnnouncedFiles();
      while (!announcedFiles.empty()) {
        std::string uuid = announcedFiles.pop();

        if(sampleHandler.hasUuid(uuid) && sampleHandler.isFileComplete(uuid)) {
          moveFile(uuid, announceHandler);
          sampleHandler.removeUuid(uuid);
          announceHandler.removeUuid(uuid);
        }
      }

      // send statistics
      ACE_Time_Value now = ACE_OS::gettimeofday();
      if (lastReceiverSampleSend + ACE_Time_Value(1) < now) {
        rapid::FileQueueReceiverSample& event = statistics.event();

        DDS::LivelinessChangedStatus livelinessStatus;
        sampleReader.get_liveliness_changed_status(livelinessStatus);

        // santity checking
        if (livelinessStatus.alive_count > 1) {
          KN_WARN("Subscribed to more than one sample writer.");
        }

        DDS::DataReaderCacheStatus cacheStatus;
        sampleReader.get_datareader_cache_status(cacheStatus);

        rapid::RapidHelper::updateHeader(event.hdr);
        event.connected = livelinessStatus.alive_count > 0;
        event.bufferedChunks = cacheStatus.sample_count;
        event.processedChunks = sampleHandler.getNumTotalSamples();
        event.completedFiles = sampleHandler.getNumCompleted();

        statistics.sendEvent();
        lastReceiverSampleSend = now;
      }
    }

    // print statistics
    // XXX: We are double-counting some announce topics. I'm not sure why...
    std::cout << "Statistics: " << std::endl
              << "  Announced: " << announceHandler.getNumAnnounced() << std::endl
              << "    Started: " << sampleHandler.getNumStarted()     << std::endl
              << "  Completed: " << sampleHandler.getNumCompleted()   << std::endl
              << "      Moved: " << g_numFilesMoved                   << std::endl;
    if (sampleHandler.getNumStarted() != sampleHandler.getNumCompleted()) {
      std::cout << " INCOMPLETE: " << sampleHandler.getNumStarted() - sampleHandler.getNumCompleted() << std::endl;
    }

  }
  ddsEntities.fini();
  return 0;
}
Example #2
0
rmw_ret_t
rmw_wait(
  rmw_subscriptions_t * subscriptions,
  rmw_guard_conditions_t * guard_conditions,
  rmw_services_t * services,
  rmw_clients_t * clients,
  const rmw_time_t * wait_timeout)
{
  // TODO(gerkey)
  // We're making this object static to avoid a race between its destructor
  // being called on exit from this function and an interrupt guard condition
  // that holds a pointer to it being called in parallel from somewhere else.
  // This structure will be changed when we add to rmw the feature of supporting
  // multiple wait sets.
  static DDS::WaitSet waitset;
  // To ensure that we properly clean up the now-static wait set, we declare an
  // object whose destructor will detach what we attached (this was previously
  // being done inside the destructor of the wait set.
  struct atexit_t
  {
    ~atexit_t()
    {
      // Manually detach conditions, to ensure a clean wait set for next time.
      DDS::ConditionSeq attached_conditions;
      waitset.get_conditions(attached_conditions);
      for (uint32_t i = 0; i < attached_conditions.length(); ++i) {
        waitset.detach_condition(attached_conditions[i]);
      }
    }
  } atexit;

  // add a condition for each subscriber
  for (size_t i = 0; i < subscriptions->subscriber_count; ++i) {
    OpenSpliceStaticSubscriberInfo * subscriber_info =
      static_cast<OpenSpliceStaticSubscriberInfo *>(subscriptions->subscribers[i]);
    if (!subscriber_info) {
      RMW_SET_ERROR_MSG("subscriber info handle is null");
      return RMW_RET_ERROR;
    }
    DDS::ReadCondition * read_condition = subscriber_info->read_condition;
    if (!read_condition) {
      RMW_SET_ERROR_MSG("read condition handle is null");
      return RMW_RET_ERROR;
    }
    rmw_ret_t status = check_attach_condition_error(
      waitset.attach_condition(read_condition));
    if (status != RMW_RET_OK) {
      return status;
    }
  }

  // add a condition for each guard condition
  for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) {
    DDS::GuardCondition * guard_condition =
      static_cast<DDS::GuardCondition *>(guard_conditions->guard_conditions[i]);
    if (!guard_condition) {
      RMW_SET_ERROR_MSG("guard condition handle is null");
      return RMW_RET_ERROR;
    }
    rmw_ret_t status = check_attach_condition_error(
      waitset.attach_condition(guard_condition));
    if (status != RMW_RET_OK) {
      return status;
    }
  }

  // add a condition for each service
  for (size_t i = 0; i < services->service_count; ++i) {
    OpenSpliceStaticServiceInfo * service_info =
      static_cast<OpenSpliceStaticServiceInfo *>(services->services[i]);
    if (!service_info) {
      RMW_SET_ERROR_MSG("service info handle is null");
      return RMW_RET_ERROR;
    }
    DDS::ReadCondition * read_condition = service_info->read_condition_;
    if (!read_condition) {
      RMW_SET_ERROR_MSG("read condition handle is null");
      return RMW_RET_ERROR;
    }
    rmw_ret_t status = check_attach_condition_error(
      waitset.attach_condition(read_condition));
    if (status != RMW_RET_OK) {
      return status;
    }
  }

  // add a condition for each client
  for (size_t i = 0; i < clients->client_count; ++i) {
    OpenSpliceStaticClientInfo * client_info =
      static_cast<OpenSpliceStaticClientInfo *>(clients->clients[i]);
    if (!client_info) {
      RMW_SET_ERROR_MSG("client info handle is null");
      return RMW_RET_ERROR;
    }
    DDS::ReadCondition * read_condition = client_info->read_condition_;
    if (!read_condition) {
      RMW_SET_ERROR_MSG("read condition handle is null");
      return RMW_RET_ERROR;
    }
    rmw_ret_t status = check_attach_condition_error(
      waitset.attach_condition(read_condition));
    if (status != RMW_RET_OK) {
      return status;
    }
  }

  // invoke wait until one of the conditions triggers
  DDS::ConditionSeq active_conditions;
  DDS::Duration_t timeout;
  if (!wait_timeout) {
    timeout = DDS::DURATION_INFINITE;
  } else {
    timeout.sec = static_cast<DDS::Long>(wait_timeout->sec);
    timeout.nanosec = static_cast<DDS::Long>(wait_timeout->nsec);
  }
  DDS::ReturnCode_t status = waitset.wait(active_conditions, timeout);

  if (status == DDS::RETCODE_TIMEOUT) {
    return RMW_RET_TIMEOUT;
  }

  if (status != DDS::RETCODE_OK) {
    RMW_SET_ERROR_MSG("failed to wait on waitset");
    return RMW_RET_ERROR;
  }

  // set subscriber handles to zero for all not triggered status conditions
  for (size_t i = 0; i < subscriptions->subscriber_count; ++i) {
    OpenSpliceStaticSubscriberInfo * subscriber_info =
      static_cast<OpenSpliceStaticSubscriberInfo *>(subscriptions->subscribers[i]);
    if (!subscriber_info) {
      RMW_SET_ERROR_MSG("subscriber info handle is null");
      return RMW_RET_ERROR;
    }
    DDS::ReadCondition * read_condition = subscriber_info->read_condition;
    if (!read_condition) {
      RMW_SET_ERROR_MSG("read condition handle is null");
      return RMW_RET_ERROR;
    }
    if (!read_condition->get_trigger_value()) {
      // if the status condition was not triggered
      // reset the subscriber handle
      subscriptions->subscribers[i] = 0;
    }
  }

  // set guard condition handles to zero for all not triggered guard conditions
  for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) {
    DDS::GuardCondition * guard_condition =
      static_cast<DDS::GuardCondition *>(guard_conditions->guard_conditions[i]);
    if (!guard_condition) {
      RMW_SET_ERROR_MSG("guard condition handle is null");
      return RMW_RET_ERROR;
    }

    if (!guard_condition->get_trigger_value()) {
      // if the guard condition was not triggered
      // reset the guard condition handle
      guard_conditions->guard_conditions[i] = 0;
    } else {
      // reset the trigger value
      if (guard_condition->set_trigger_value(false) != DDS::RETCODE_OK) {
        RMW_SET_ERROR_MSG("failed to set trigger value to false");
        return RMW_RET_ERROR;
      }
    }
  }

  // set service handles to zero for all not triggered conditions
  for (size_t i = 0; i < services->service_count; ++i) {
    OpenSpliceStaticServiceInfo * service_info =
      static_cast<OpenSpliceStaticServiceInfo *>(services->services[i]);
    if (!service_info) {
      RMW_SET_ERROR_MSG("service info handle is null");
      return RMW_RET_ERROR;
    }
    DDS::ReadCondition * read_condition = service_info->read_condition_;
    if (!read_condition) {
      RMW_SET_ERROR_MSG("read condition handle is null");
      return RMW_RET_ERROR;
    }

    // search for service condition in active set
    uint32_t j = 0;
    for (; j < active_conditions.length(); ++j) {
      if (active_conditions[j] == read_condition) {
        break;
      }
    }
    // if service condition is not found in the active set
    // reset the service handle
    if (!(j < active_conditions.length())) {
      services->services[i] = 0;
    }
  }

  // set client handles to zero for all not triggered conditions
  for (size_t i = 0; i < clients->client_count; ++i) {
    OpenSpliceStaticClientInfo * client_info =
      static_cast<OpenSpliceStaticClientInfo *>(clients->clients[i]);
    if (!client_info) {
      RMW_SET_ERROR_MSG("client info handle is null");
      return RMW_RET_ERROR;
    }
    DDS::ReadCondition * read_condition = client_info->read_condition_;
    if (!read_condition) {
      RMW_SET_ERROR_MSG("read condition handle is null");
      return RMW_RET_ERROR;
    }

    // search for service condition in active set
    uint32_t j = 0;
    for (; j < active_conditions.length(); ++j) {
      if (active_conditions[j] == read_condition) {
        break;
      }
    }
    // if client condition is not found in the active set
    // reset the client handle
    if (!(j < active_conditions.length())) {
      clients->clients[i] = 0;
    }
  }
  return RMW_RET_OK;
}