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