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