void testGPS(CFileOutputStream& rawlogFile) { CGPSInterface mygps; string SERIAL_NAME = "COM6"; // Name of the serial port to open mygps.setSerialPortName(SERIAL_NAME); CGenericSensor::TListObservations lstObs; CGenericSensor::TListObservations::iterator itObs; while (! mrpt::system::os::kbhit()) { mygps.doProcess(); mrpt::system::sleep( 200 ); CSensoryFrame SF; mygps.getObservations( lstObs ); if (lstObs.empty()) { printf("[Test_GPS] Waiting for data...\n"); } else { for (itObs=lstObs.begin();itObs!=lstObs.end();itObs++) { CObservationGPSPtr gpsData=CObservationGPSPtr(itObs->second); gpsData->dumpToConsole(); SF.insert(gpsData); rawlogFile << SF; //coordinatesTransformation_WGS84( } lstObs.clear(); } } }
// ------------------------------------------------------ // 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; } }
// ------------------------------------------------------ // SensorThread // ------------------------------------------------------ void SensorThread(TThreadParams params) { try { string driver_name = params.cfgFile->read_string(params.sensor_label,"driver","",true); CGenericSensorPtr sensor = CGenericSensor::createSensorPtr(driver_name ); if (!sensor) { cerr << endl << "***ERROR***: Class name not recognized: " << driver_name << endl; allThreadsMustExit = true; } // Load common & sensor specific parameters: sensor->loadConfig( *params.cfgFile, params.sensor_label ); cout << format("[thread_%s] Starting...",params.sensor_label.c_str()) << " at " << sensor->getProcessRate() << " Hz" << endl; ASSERTMSG_(sensor->getProcessRate()>0,"process_rate must be set to a valid value (>0 Hz)."); int process_period_ms = round( 1000.0 / sensor->getProcessRate() ); // For imaging sensors, set external storage directory: sensor->setPathForExternalImages( rawlog_ext_imgs_dir ); // Init device: sensor->initialize(); while (! allThreadsMustExit ) { TTimeStamp t0= now(); // Process sensor->doProcess(); // Get new observations CGenericSensor::TListObservations lstObjs; sensor->getObservations( lstObjs ); { synch::CCriticalSectionLocker lock (&cs_global_list_obs); global_list_obs.insert( lstObjs.begin(), lstObjs.end() ); } lstObjs.clear(); // wait until the process period: TTimeStamp t1= now(); double At = timeDifference(t0,t1); int At_rem_ms = process_period_ms - At*1000; if (At_rem_ms>0) sleep(At_rem_ms); } sensor.clear(); cout << format("[thread_%s] Closing...",params.sensor_label.c_str()) << endl; } catch (std::exception &e) { cerr << e.what() << endl; allThreadsMustExit = true; } catch (...) { cerr << "Untyped exception!!" << endl; allThreadsMustExit = true; } }