Exemplo n.º 1
0
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();
		}
	}


	}
Exemplo n.º 2
0
// ------------------------------------------------------
//				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;
	}
}