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(); } } }
// ------------------------------------------------------ // 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; } }