void CFormMotionModel::applyToLoadedRawlog() { WX_START_TRY // Apply changes: size_t changes = 0; { wxBusyCursor cursor; size_t first = 0; size_t last = rawlog.size()-1; if (!cbAll->GetValue()) { first = atoi( string(edRangeFrom->GetValue().mb_str()).c_str() ); last = atoi( string(edRangeTo->GetValue().mb_str()).c_str() ); } for (size_t i=first;i<=last;i++) { // Check type: if ( rawlog.getType(i)==CRawlog::etActionCollection ) { // This is an action: CActionCollectionPtr acts = rawlog.getAsAction(i); CActionRobotMovement2DPtr firstActionMov = acts->getActionByClass<CActionRobotMovement2D>( ); if (firstActionMov) { if (firstActionMov->estimationMethod==CActionRobotMovement2D::emOdometry) { // Use the kinematics motion model to estimate a pose change gaussian approximation: firstActionMov->computeFromOdometry( firstActionMov->rawOdometryIncrementReading, options); } else { // Take the mean value firstActionMov->computeFromOdometry( firstActionMov->poseChange->getMeanVal(), options); } changes++; } } // end is action } // end for i } wxString str; str.sprintf(_("Objects changed: %d"),changes); wxMessageBox( str, _("Result:"), wxOK,this); WX_END_TRY }
// ------------------------------------------------------ // MAIN THREAD // ------------------------------------------------------ int main(int argc, char **argv) { try { printf(" rawlog-grabber - Part of the MRPT\n"); printf(" MRPT C++ Library: %s - BUILD DATE %s\n", MRPT_getVersion().c_str(), MRPT_getCompilationDate().c_str()); printf("-------------------------------------------------------------------\n"); // Process arguments: if (argc<2) { printf("Usage: %s <config_file.ini>\n\n",argv[0]); mrpt::system::pause(); return -1; } string INI_FILENAME( argv[1] ); ASSERT_FILE_EXISTS_(INI_FILENAME) CConfigFile iniFile( INI_FILENAME ); // ------------------------------------------ // Load config from file: // ------------------------------------------ string rawlog_prefix = "dataset"; int time_between_launches = 300; double SF_max_time_span = 0.25; // Seconds bool use_sensoryframes = false; int GRABBER_PERIOD_MS = 1000; int rawlog_GZ_compress_level = 1; // 0: No compress, 1-9: compress level MRPT_LOAD_CONFIG_VAR( rawlog_prefix, string, iniFile, GLOBAL_SECTION_NAME ); MRPT_LOAD_CONFIG_VAR( time_between_launches, int, iniFile, GLOBAL_SECTION_NAME ); MRPT_LOAD_CONFIG_VAR( SF_max_time_span, float, iniFile, GLOBAL_SECTION_NAME ); MRPT_LOAD_CONFIG_VAR( use_sensoryframes, bool, iniFile, GLOBAL_SECTION_NAME ); MRPT_LOAD_CONFIG_VAR( GRABBER_PERIOD_MS, int, iniFile, GLOBAL_SECTION_NAME ); MRPT_LOAD_CONFIG_VAR( rawlog_GZ_compress_level, int, iniFile, GLOBAL_SECTION_NAME ); // Build full rawlog file name: string rawlog_postfix = "_"; //rawlog_postfix += dateTimeToString( now() ); mrpt::system::TTimeParts parts; mrpt::system::timestampToParts(now(), parts, true); rawlog_postfix += format("%04u-%02u-%02u_%02uh%02um%02us", (unsigned int)parts.year, (unsigned int)parts.month, (unsigned int)parts.day, (unsigned int)parts.hour, (unsigned int)parts.minute, (unsigned int)parts.second ); rawlog_postfix = mrpt::system::fileNameStripInvalidChars( rawlog_postfix ); // Only set this if we want externally stored images: rawlog_ext_imgs_dir = rawlog_prefix+fileNameStripInvalidChars( rawlog_postfix+string("_Images") ); // Also, set the path in CImage to enable online visualization in a GUI window: CImage::IMAGES_PATH_BASE = rawlog_ext_imgs_dir; rawlog_postfix += string(".rawlog"); rawlog_postfix = fileNameStripInvalidChars( rawlog_postfix ); string rawlog_filename = rawlog_prefix + rawlog_postfix; cout << endl ; cout << "Output rawlog filename: " << rawlog_filename << endl; cout << "External image storage: " << rawlog_ext_imgs_dir << endl << endl; // ---------------------------------------------- // Launch threads: // ---------------------------------------------- vector_string sections; iniFile.getAllSections( sections ); vector<TThreadHandle> lstThreads; for (vector_string::iterator it=sections.begin();it!=sections.end();++it) { if (*it==GLOBAL_SECTION_NAME || it->empty() || iniFile.read_bool(*it,"rawlog-grabber-ignore",false,false) ) continue; // This is not a sensor: //cout << "Launching thread for sensor '" << *it << "'" << endl; TThreadParams threParms; threParms.cfgFile = &iniFile; threParms.sensor_label = *it; TThreadHandle thre = createThread(SensorThread, threParms); lstThreads.push_back(thre); sleep(time_between_launches); } // ---------------------------------------------- // Run: // ---------------------------------------------- CFileGZOutputStream out_file; out_file.open( rawlog_filename, rawlog_GZ_compress_level ); CSensoryFrame curSF; CGenericSensor::TListObservations copy_of_global_list_obs; cout << endl << "Press any key to exit program" << endl; while (!os::kbhit() && !allThreadsMustExit) { // See if we have observations and process them: { synch::CCriticalSectionLocker lock (&cs_global_list_obs); copy_of_global_list_obs.clear(); if (!global_list_obs.empty()) { CGenericSensor::TListObservations::iterator itEnd = global_list_obs.begin(); std::advance( itEnd, global_list_obs.size() / 2 ); copy_of_global_list_obs.insert(global_list_obs.begin(),itEnd ); global_list_obs.erase(global_list_obs.begin(), itEnd); } } // End of cs lock if (use_sensoryframes) { // ----------------------- // USE SENSORY-FRAMES // ----------------------- for (CGenericSensor::TListObservations::iterator it=copy_of_global_list_obs.begin();it!=copy_of_global_list_obs.end();++it) { // If we have an action, save the SF and start a new one: if (IS_DERIVED(it->second, CAction)) { CActionPtr act = CActionPtr( it->second); out_file << curSF; cout << "[" << dateTimeToString(now()) << "] Saved SF with " << curSF.size() << " objects." << endl; curSF.clear(); CActionCollection acts; acts.insert(*act); act.clear_unique(); out_file << acts; } else if (IS_CLASS(it->second,CObservationOdometry) ) { CObservationOdometryPtr odom = CObservationOdometryPtr( it->second ); CActionRobotMovement2DPtr act = CActionRobotMovement2D::Create(); act->timestamp = odom->timestamp; // Compute the increment since the last reading: static CActionRobotMovement2D::TMotionModelOptions odomOpts; static CObservationOdometry last_odo; static bool last_odo_first = true; CPose2D odo_incr; int64_t lticks_incr, rticks_incr; if (last_odo_first) { last_odo_first = false; odo_incr = CPose2D(0,0,0); lticks_incr = rticks_incr = 0; } else { odo_incr = odom->odometry - last_odo.odometry; lticks_incr = odom->encoderLeftTicks - last_odo.encoderLeftTicks; rticks_incr = odom->encoderRightTicks - last_odo.encoderRightTicks; last_odo = *odom; } // Save as action & dump to file: act->computeFromOdometry( odo_incr, odomOpts ); act->hasEncodersInfo = true; act->encoderLeftTicks = lticks_incr; act->encoderRightTicks = rticks_incr; act->hasVelocities = true; act->velocityLin = odom->velocityLin; act->velocityAng = odom->velocityAng; out_file << curSF; cout << "[" << dateTimeToString(now()) << "] Saved SF with " << curSF.size() << " objects." << endl; curSF.clear(); CActionCollection acts; acts.insert(*act); act.clear_unique(); out_file << acts; } else if (IS_DERIVED(it->second, CObservation) ) { CObservationPtr obs = CObservationPtr(it->second); // First, check if inserting this OBS into the SF would overflow "SF_max_time_span": if (curSF.size()!=0 && timeDifference( curSF.getObservationByIndex(0)->timestamp, obs->timestamp ) > SF_max_time_span ) { // Show GPS mode: CObservationGPSPtr gps; size_t idx=0; do { gps = curSF.getObservationByClass<CObservationGPS>(idx++ ); if (gps) { cout << " GPS mode: " << (int)gps->GGA_datum.fix_quality << " label: " << gps->sensorLabel << endl; } } while (gps); // Show IMU angles: CObservationIMUPtr imu = curSF.getObservationByClass<CObservationIMU>(); if (imu) { cout << format(" IMU angles (degrees): (yaw,pitch,roll)=(%.06f, %.06f, %.06f)", RAD2DEG( imu->rawMeasurements[IMU_YAW] ), RAD2DEG( imu->rawMeasurements[IMU_PITCH] ), RAD2DEG( imu->rawMeasurements[IMU_ROLL] ) ) << endl; } // Save and start a new one: out_file << curSF; cout << "[" << dateTimeToString(now()) << "] Saved SF with " << curSF.size() << " objects." << endl; curSF.clear(); } // Now, insert the observation in the SF: curSF.insert( obs ); } else THROW_EXCEPTION("*** ERROR *** Class is not an action or an observation"); } } else { // --------------------------- // DO NOT USE SENSORY-FRAMES // --------------------------- CObservationIMUPtr imu; // Default:NULL for (CGenericSensor::TListObservations::iterator it=copy_of_global_list_obs.begin();it!=copy_of_global_list_obs.end();++it) { out_file << *(it->second); // Show GPS mode: if ( (it->second)->GetRuntimeClass() == CLASS_ID(CObservationGPS) ) { CObservationGPSPtr gps = CObservationGPSPtr( it->second ); cout << " GPS mode: " << (int)gps->GGA_datum.fix_quality << " label: " << gps->sensorLabel << endl; } else if ( (it->second)->GetRuntimeClass() == CLASS_ID(CObservationIMU) ) { imu = CObservationIMUPtr( it->second ); } } // Show IMU angles: if (imu) { cout << format(" IMU angles (degrees): (yaw,pitch,roll)=(%.06f, %.06f, %.06f)", RAD2DEG( imu->rawMeasurements[IMU_YAW] ), RAD2DEG( imu->rawMeasurements[IMU_PITCH] ), RAD2DEG( imu->rawMeasurements[IMU_ROLL] ) ) << endl; } if (!copy_of_global_list_obs.empty()) { cout << "[" << dateTimeToString(now()) << "] Saved " << copy_of_global_list_obs.size() << " objects." << endl; } } sleep(GRABBER_PERIOD_MS); } if (allThreadsMustExit) { cerr << "[main thread] Ended due to other thread signal to exit application." << endl; } // Flush file to disk: out_file.close(); // Wait all threads: // ---------------------------- allThreadsMustExit = true; mrpt::system::sleep(300); cout << endl << "Waiting for all threads to close..." << endl; for (vector<TThreadHandle>::iterator th=lstThreads.begin();th!=lstThreads.end();++th) joinThread( *th ); return 0; } catch (std::exception &e) { std::cerr << e.what() << std::endl << "Program finished for an exception!!" << std::endl; mrpt::system::pause(); return -1; } catch (...) { std::cerr << "Untyped exception!!" << std::endl; mrpt::system::pause(); return -1; } }
void xRawLogViewerFrame::OnMenuCompactRawlog(wxCommandEvent& event) { WX_START_TRY bool onlyOnePerLabel = (wxYES==wxMessageBox( _("Keep only one observation of each label within each sensoryframe?"), _("Compact rawlog"),wxYES_NO, this )); int progress_N = static_cast<int>(rawlog.size()); int progress_i=progress_N; wxProgressDialog progDia( wxT("Compacting rawlog"), wxT("Processing..."), progress_N, // range this, // parent wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE | wxPD_ELAPSED_TIME | wxPD_ESTIMATED_TIME | wxPD_REMAINING_TIME); wxTheApp->Yield(); // Let the app. process messages CActionRobotMovement2DPtr lastAct; CSensoryFramePtr lastSF; // = NULL; unsigned counter_loops = 0; unsigned nActionsDel = 0; unsigned nEmptySFDel = 0; CRawlog::iterator it=rawlog.begin(); for (progress_i=0 ;it!=rawlog.end();progress_i--) { if (counter_loops++ % 50 == 0) { if (!progDia.Update( progress_N-progress_i )) break; wxTheApp->Yield(); // Let the app. process messages } bool deleteThis = false; if (it.getType()==CRawlog::etActionCollection) { // Is this a part of multiple actions? if (lastAct) { // Remove this one and add it to the first in the series: CActionRobotMovement2DPtr act = CActionCollectionPtr(*it)->getMovementEstimationByType( CActionRobotMovement2D::emOdometry ); ASSERT_(act); lastAct->computeFromOdometry( lastAct->rawOdometryIncrementReading + act->rawOdometryIncrementReading, lastAct->motionModelConfiguration); deleteThis=true; nActionsDel++; } else { // This is the first one: lastAct = CActionCollectionPtr(*it)->getMovementEstimationByType( CActionRobotMovement2D::emOdometry ); ASSERT_(lastAct); // Before leaving the last SF, leave only one observation for each sensorLabel: if (onlyOnePerLabel && lastSF) { CSensoryFramePtr newSF = CSensoryFrame::Create(); set<string> knownLabels; for (CSensoryFrame::const_iterator o=lastSF->begin();o!=lastSF->end();++o) { if (knownLabels.find((*o)->sensorLabel)==knownLabels.end()) newSF->insert(*o); knownLabels.insert((*o)->sensorLabel); } *lastSF = *newSF; } // Ok, done: lastSF.clear_unique(); } } else if (it.getType()==CRawlog::etSensoryFrame) { // Is this a part of a series? if (lastSF) { // remove this one and accumulate in the first in the serie: lastSF->moveFrom( * CSensoryFramePtr(*it) ); deleteThis = true; nEmptySFDel++; } else { // This is the first SF: CSensoryFramePtr sf = CSensoryFramePtr(*it); // Only take into account if not empty! if (sf->size()) { lastSF = sf; ASSERT_(lastSF); lastAct.clear_unique(); } else { deleteThis = true; nEmptySFDel++; } } } else THROW_EXCEPTION("Unexpected class found!") if (deleteThis) { it = rawlog.erase(it); progress_i--; // Extra count } else it++; } progDia.Update( progress_N ); string str= format( "%u actions deleted\n%u sensory frames deleted", nActionsDel, nEmptySFDel ); ::wxMessageBox( _U( str.c_str() ) ); rebuildTreeView(); WX_END_TRY }