~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;
}
