~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]); } }
//==================================================================== //== 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; }
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; }