/*---------------------------------------------------------------
addActions
---------------------------------------------------------------*/
void CRawlog::addActions(
CActionCollection &actions )
{
m_seqOfActObs.push_back( CSerializablePtr( actions.duplicateGetSmartPtr() ) );
}
int main(int argc, char ** argv)
{
try
{
bool showHelp = argc>1 && !os::_strcmp(argv[1],"--help");
bool showVersion = argc>1 && !os::_strcmp(argv[1],"--version");
printf(" simul-landmarks - Part of the MRPT\n");
printf(" MRPT C++ Library: %s - BUILD DATE %s\n", MRPT_getVersion().c_str(), MRPT_getCompilationDate().c_str());
if (showVersion)
return 0; // Program end
// Process arguments:
if (argc<2 || showHelp )
{
printf("Usage: %s <config_file.ini>\n\n",argv[0]);
if (!showHelp)
{
mrpt::system::pause();
return -1;
}
else return 0;
}
string INI_FILENAME = std::string( argv[1] );
ASSERT_FILE_EXISTS_(INI_FILENAME)
CConfigFile ini( INI_FILENAME );
const int random_seed = ini.read_int("Params","random_seed",0);
if (random_seed!=0)
randomGenerator.randomize(random_seed);
else randomGenerator.randomize();
// Set default values:
unsigned int nLandmarks = 3;
unsigned int nSteps = 100;
std::string outFile("out.rawlog");
std::string outDir("OUT");
float min_x =-5;
float max_x =5;
float min_y =-5;
float max_y =5;
float min_z =0;
float max_z =0;
float odometryNoiseXY_std = 0.001f;
float odometryNoisePhi_std_deg = 0.01f;
float odometryNoisePitch_std_deg = 0.01f;
float odometryNoiseRoll_std_deg = 0.01f;
float minSensorDistance = 0;
float maxSensorDistance = 10;
float fieldOfView_deg= 180.0f;
float sensorPose_x = 0;
float sensorPose_y = 0;
float sensorPose_z = 0;
float sensorPose_yaw_deg = 0;
float sensorPose_pitch_deg = 0;
float sensorPose_roll_deg = 0;
float stdRange = 0.01f;
float stdYaw_deg = 0.1f;
float stdPitch_deg = 0.1f;
bool sensorDetectsIDs=true;
bool circularPath = true;
bool random6DPath = false;
size_t squarePathLength=40;
// Load params from INI:
MRPT_LOAD_CONFIG_VAR(outFile,string, ini,"Params");
MRPT_LOAD_CONFIG_VAR(outDir,string, ini,"Params");
MRPT_LOAD_CONFIG_VAR(nSteps,int, ini,"Params");
MRPT_LOAD_CONFIG_VAR(circularPath,bool, ini,"Params");
MRPT_LOAD_CONFIG_VAR(random6DPath,bool, ini,"Params");
MRPT_LOAD_CONFIG_VAR(squarePathLength,int,ini,"Params");
MRPT_LOAD_CONFIG_VAR(sensorDetectsIDs,bool, ini,"Params");
MRPT_LOAD_CONFIG_VAR(odometryNoiseXY_std,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(odometryNoisePhi_std_deg,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(odometryNoisePitch_std_deg,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(odometryNoiseRoll_std_deg,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(sensorPose_x,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(sensorPose_y,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(sensorPose_z,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(sensorPose_yaw_deg,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(sensorPose_pitch_deg,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(sensorPose_roll_deg,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(minSensorDistance,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(maxSensorDistance,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(fieldOfView_deg,float, ini,"Params");
bool show_in_3d = false;
MRPT_LOAD_CONFIG_VAR(show_in_3d,bool, ini,"Params");
MRPT_LOAD_CONFIG_VAR(stdRange,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(stdYaw_deg,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(stdPitch_deg,float, ini,"Params");
float stdYaw = DEG2RAD(stdYaw_deg);
float stdPitch = DEG2RAD(stdPitch_deg);
float odometryNoisePhi_std = DEG2RAD(odometryNoisePhi_std_deg);
float odometryNoisePitch_std = DEG2RAD(odometryNoisePitch_std_deg);
float odometryNoiseRoll_std = DEG2RAD(odometryNoiseRoll_std_deg);
float fieldOfView = DEG2RAD(fieldOfView_deg);
CPose3D sensorPoseOnRobot(
sensorPose_x,
sensorPose_y,
sensorPose_z,
DEG2RAD( sensorPose_yaw_deg ),
DEG2RAD( sensorPose_pitch_deg ),
DEG2RAD( sensorPose_roll_deg ) );
// Create out dir:
mrpt::system::createDirectory(outDir);
// ---------------------------------------------
// Create the point-beacons:
// ---------------------------------------------
printf("Creating landmark map...");
mrpt::maps::CLandmarksMap landmarkMap;
int randomSetCount = 0;
int uniqueIds = 1;
// Process each of the "RANDOMSET_%i" found:
do
{
string sectName = format("RANDOMSET_%i",++randomSetCount);
nLandmarks = 0;
MRPT_LOAD_CONFIG_VAR(nLandmarks,uint64_t, ini,sectName);
MRPT_LOAD_CONFIG_VAR(min_x,float, ini,sectName);
MRPT_LOAD_CONFIG_VAR(max_x,float, ini,sectName);
MRPT_LOAD_CONFIG_VAR(min_y,float, ini,sectName);
MRPT_LOAD_CONFIG_VAR(max_y,float, ini,sectName);
MRPT_LOAD_CONFIG_VAR(min_z,float, ini,sectName);
MRPT_LOAD_CONFIG_VAR(max_z,float, ini,sectName);
for (size_t i=0; i<nLandmarks; i++)
{
CLandmark LM;
CPointPDFGaussian pt3D;
// Random coordinates:
pt3D.mean = CPoint3D(
randomGenerator.drawUniform(min_x,max_x),
randomGenerator.drawUniform(min_y,max_y),
randomGenerator.drawUniform(min_z,max_z) );
// Add:
LM.createOneFeature();
LM.features[0]->type = featBeacon;
LM.ID = uniqueIds++;
LM.setPose( pt3D );
landmarkMap.landmarks.push_back(LM);
}
if (nLandmarks) cout << nLandmarks << " generated for the 'randomset' " << randomSetCount << endl;
}
while (nLandmarks);
landmarkMap.saveToTextFile( format("%s/%s_ground_truth.txt",outDir.c_str(),outFile.c_str()) );
printf("Done!\n");
// ---------------------------------------------
// Simulate:
// ---------------------------------------------
size_t nWarningsNoSight=0;
CActionRobotMovement2D::TMotionModelOptions opts;
opts.modelSelection = CActionRobotMovement2D::mmGaussian;
opts.gausianModel.a1=0;
opts.gausianModel.a2=0;
opts.gausianModel.a3=0;
opts.gausianModel.a4=0;
opts.gausianModel.minStdXY = odometryNoiseXY_std;
opts.gausianModel.minStdPHI = odometryNoisePhi_std;
// Output rawlog, gz-compressed.
CFileGZOutputStream fil( format("%s/%s",outDir.c_str(),outFile.c_str()));
CPose3D realPose;
const size_t N_STEPS_STOP_AT_THE_BEGINNING = 4;
CMatrixDouble GT_path;
for (size_t i=0; i<nSteps; i++)
{
cout << "Generating step " << i << "...\n";
CSensoryFrame SF;
CActionCollection acts;
CPose3D incPose3D;
bool incPose_is_3D = random6DPath;
if (i>=N_STEPS_STOP_AT_THE_BEGINNING)
{
if (random6DPath)
{ // 3D path
const double Ar = DEG2RAD(3);
TPose3D Ap = TPose3D(0.20*cos(Ar),0.20*sin(Ar),0,Ar,0,0);
//Ap.z += randomGenerator.drawGaussian1D(0,0.05);
Ap.yaw += randomGenerator.drawGaussian1D(0,DEG2RAD(0.2));
Ap.pitch += randomGenerator.drawGaussian1D(0,DEG2RAD(2));
Ap.roll += randomGenerator.drawGaussian1D(0,DEG2RAD(4));
incPose3D = CPose3D(Ap);
}
else
{ // 2D path:
if (circularPath)
{
// Circular path:
float Ar = DEG2RAD(5);
incPose3D = CPose3D(CPose2D(0.20f*cos(Ar),0.20f*sin(Ar),Ar));
}
else
{
// Square path:
if ( (i % squarePathLength) > (squarePathLength-5) )
incPose3D = CPose3D(CPose2D(0,0,DEG2RAD(90.0f/4)));
else incPose3D = CPose3D(CPose2D(0.20f,0,0));
}
}
}
else
{
// Robot is still at the beginning:
incPose3D = CPose3D(0,0,0,0,0,0);
}
// Simulate observations:
CObservationBearingRangePtr obs=CObservationBearingRange::Create();
obs->minSensorDistance=minSensorDistance;
obs->maxSensorDistance=maxSensorDistance;
obs->fieldOfView_yaw = fieldOfView;
obs->fieldOfView_pitch = fieldOfView;
obs->sensorLocationOnRobot = sensorPoseOnRobot;
landmarkMap.simulateRangeBearingReadings(
realPose,
sensorPoseOnRobot,
*obs,
sensorDetectsIDs, // wheter to identy landmarks
stdRange,
stdYaw,
stdPitch );
// Keep the GT of the robot pose:
GT_path.setSize(i+1,6);
for (size_t k=0; k<6; k++)
GT_path(i,k)=realPose[k];
cout << obs->sensedData.size() << " landmarks in sight";
if (obs->sensedData.empty()) nWarningsNoSight++;
SF.push_back( obs );
// Simulate odometry, from "incPose3D" with noise:
if (!incPose_is_3D)
{ // 2D odometry:
CActionRobotMovement2D act;
CPose2D incOdo( incPose3D );
if (incPose3D.x()!=0 || incPose3D.y()!=0 || incPose3D.yaw()!=0)
{
incOdo.x_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std );
incOdo.y_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std );
incOdo.phi_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoisePhi_std );
}
act.computeFromOdometry(incOdo, opts);
acts.insert( act );
}
else
{ // 3D odometry:
CActionRobotMovement3D act;
act.estimationMethod = CActionRobotMovement3D::emOdometry;
CPose3D noisyIncPose = incPose3D;
if (incPose3D.x()!=0 || incPose3D.y()!=0 || incPose3D.yaw()!=0)
{
noisyIncPose.x_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std );
noisyIncPose.y_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std );
noisyIncPose.z_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std );
noisyIncPose.setYawPitchRoll(
noisyIncPose.yaw()+ randomGenerator.drawGaussian1D_normalized() * odometryNoisePhi_std,
noisyIncPose.pitch()+ randomGenerator.drawGaussian1D_normalized() * odometryNoisePitch_std,
noisyIncPose.roll()+ randomGenerator.drawGaussian1D_normalized() * odometryNoiseRoll_std );
}
act.poseChange.mean = noisyIncPose;
act.poseChange.cov.eye();
act.poseChange.cov(0,0) =
act.poseChange.cov(1,1) =
act.poseChange.cov(2,2) = square(odometryNoiseXY_std);
act.poseChange.cov(3,3) = square(odometryNoisePhi_std);
act.poseChange.cov(4,4) = square(odometryNoisePitch_std);
act.poseChange.cov(5,5) = square(odometryNoiseRoll_std);
acts.insert( act );
}
// Save:
fil << SF << acts;
// Next pose:
realPose = realPose + incPose3D;
cout << endl;
}
// Save the ground truth for the robot poses as well:
GT_path.saveToTextFile(
format("%s/%s_ground_truth_robot_path.txt",outDir.c_str(),outFile.c_str()),
MATRIX_FORMAT_FIXED,
true,
"% Columns are: x(m) y(m) z(m) yaw(rad) pitch(rad) roll(rad)\n");
cout << "Data saved to directory: " << outDir << endl;
if (nWarningsNoSight)
cout << "WARNING: " << nWarningsNoSight << " observations contained zero landmarks in the sensor range." << endl;
// Optionally, display in 3D:
if (show_in_3d && size(GT_path,1)>1)
{
#if MRPT_HAS_OPENGL_GLUT && MRPT_HAS_WXWIDGETS
mrpt::gui::CDisplayWindow3D win("Final simulation",400,300);
mrpt::opengl::COpenGLScenePtr &scene = win.get3DSceneAndLock();
scene->insert( mrpt::opengl::CGridPlaneXY::Create( min_x-10,max_x+10,min_y-10,max_y+10,0 ));
scene->insert( mrpt::opengl::stock_objects::CornerXYZ() );
// Insert all landmarks:
for (CLandmarksMap::TCustomSequenceLandmarks::const_iterator it=landmarkMap.landmarks.begin(); it!=landmarkMap.landmarks.end(); ++it)
{
mrpt::opengl::CSpherePtr lm = mrpt::opengl::CSphere::Create();
lm->setColor(1,0,0);
lm->setRadius(0.1);
lm->setLocation( it->pose_mean );
lm->setName( format("LM#%u",(unsigned) it->ID ) );
//lm->enableShowName(true);
scene->insert(lm);
}
// Insert all robot poses:
const size_t N = size(GT_path,1);
mrpt::opengl::CSetOfLinesPtr pathLines = mrpt::opengl::CSetOfLines::Create();
pathLines->setColor(0,0,1,0.5);
pathLines->setLineWidth(3.0);
pathLines->resize(N-1);
for (size_t i=0; i<N-1; i++)
pathLines->setLineByIndex(i, GT_path(i,0),GT_path(i,1),GT_path(i,2), GT_path(i+1,0),GT_path(i+1,1),GT_path(i+1,2) );
scene->insert(pathLines);
for (size_t i=0; i<N; i++)
{
mrpt::opengl::CSetOfObjectsPtr corner = mrpt::opengl::stock_objects::CornerXYZ();
corner->setScale(0.2);
corner->setPose(TPose3D(GT_path(i,0),GT_path(i,1),GT_path(i,2),GT_path(i,3),GT_path(i,4),GT_path(i,5)));
scene->insert(corner);
}
win.unlockAccess3DScene();
win.forceRepaint();
cout << "Press any key or close the 3D window to exit." << endl;
win.waitForKey();
#endif // MRPT_HAS_OPENGL_GLUT && MRPT_HAS_WXWIDGETS
}
}
catch (std::exception &e)
{
std::cout << e.what();
}
catch (...)
{
std::cout << "Untyped exception!";
}
}
/*---------------------------------------------------------------
processActionObservation
---------------------------------------------------------------*/
void CMetricMapBuilderRBPF::processActionObservation(
CActionCollection &action,
CSensoryFrame &observations )
{
MRPT_START
// Enter critical section (updating map)
enterCriticalSection();
// Update the traveled distance estimations:
{
CActionRobotMovement3DPtr act3D = action.getActionByClass<CActionRobotMovement3D>();
CActionRobotMovement2DPtr act2D = action.getActionByClass<CActionRobotMovement2D>();
if (act3D)
{
odoIncrementSinceLastMapUpdate += act3D->poseChange.getMeanVal();
odoIncrementSinceLastLocalization += act3D->poseChange;
}
else if (act2D)
{
odoIncrementSinceLastMapUpdate += act2D->poseChange->getMeanVal();
odoIncrementSinceLastLocalization.mean += act2D->poseChange->getMeanVal();
}
else
{
std::cerr << "[CMetricMapBuilderRBPF] WARNING: action contains no odometry." << std::endl;
}
}
// Execute particle filter:
// But only if the traveled distance since the last update is long enough,
// or this is the first observation, etc...
// ----------------------------------------------------------------------------
bool do_localization = (
!mapPDF.SFs.size() || // This is the first observation!
options.debugForceInsertion ||
odoIncrementSinceLastLocalization.mean.norm()>localizeLinDistance ||
std::abs(odoIncrementSinceLastLocalization.mean.yaw())>localizeAngDistance);
bool do_map_update = (
!mapPDF.SFs.size() || // This is the first observation!
options.debugForceInsertion ||
odoIncrementSinceLastMapUpdate.norm()>insertionLinDistance ||
std::abs(odoIncrementSinceLastMapUpdate.yaw())>insertionAngDistance);
// Used any "options.alwaysInsertByClass" ??
for (CListOfClasses::const_iterator itCl=options.alwaysInsertByClass.begin();!do_map_update && itCl!=options.alwaysInsertByClass.end();++itCl)
for ( CSensoryFrame::iterator it=observations.begin();it!=observations.end();++it)
if ((*it)->GetRuntimeClass()==*itCl)
{
do_map_update = true;
do_localization = true;
break;
}
if (do_map_update)
do_localization = true;
if (do_localization)
{
// Create an artificial action object, since
// we've been collecting them until a threshold:
// ------------------------------------------------
CActionCollection fakeActs;
{
CActionRobotMovement3DPtr act3D = action.getActionByClass<CActionRobotMovement3D>();
if (act3D)
{
CActionRobotMovement3D newAct;
newAct.estimationMethod = act3D->estimationMethod;
newAct.poseChange = odoIncrementSinceLastLocalization;
newAct.timestamp = act3D->timestamp;
fakeActs.insert(newAct);
}
else
{
// It must be 2D odometry:
CActionRobotMovement2DPtr act2D = action.getActionByClass<CActionRobotMovement2D>();
ASSERT_(act2D)
CActionRobotMovement2D newAct;
newAct.computeFromOdometry( CPose2D(odoIncrementSinceLastLocalization.mean), act2D->motionModelConfiguration );
newAct.timestamp = act2D->timestamp;
fakeActs.insert(newAct);
}
}
// Reset distance counters:
odoIncrementSinceLastLocalization.mean.setFromValues(0,0,0,0,0,0);
odoIncrementSinceLastLocalization.cov.zeros();
CParticleFilter pf;
pf.m_options = m_PF_options;
pf.executeOn( mapPDF, &fakeActs, &observations );
if (options.verbose)
{
// Get current pose estimation:
CPose3DPDFParticles poseEstimation;
CPose3D meanPose;
mapPDF.getEstimatedPosePDF(poseEstimation);
poseEstimation.getMean(meanPose);
CPose3D estPos;
CMatrixDouble66 cov;
poseEstimation.getCovarianceAndMean(cov,estPos);
std::cout << " New pose=" << estPos << std::endl << "New ESS:"<< mapPDF.ESS() << std::endl;
std::cout << format(" STDs: x=%2.3f y=%2.3f z=%.03f yaw=%2.3fdeg\n", sqrt(cov(0,0)),sqrt(cov(1,1)),sqrt(cov(2,2)),RAD2DEG(sqrt(cov(3,3))));
}
}
if (do_map_update)
{
odoIncrementSinceLastMapUpdate.setFromValues(0,0,0,0,0,0);
// Update the particles' maps:
// -------------------------------------------------
if (options.verbose)
printf(" 3) New observation inserted into the map!\n");
// Add current observation to the map:
mapPDF.insertObservation(observations);
m_statsLastIteration.observationsInserted = true;
}
else
{
m_statsLastIteration.observationsInserted = false;
}
// Added 29/JUN/2007 JLBC: Tell all maps that they can now free aux. variables
// (if any) since one PF cycle is over:
for (CMultiMetricMapPDF::CParticleList::iterator it = mapPDF.m_particles.begin(); it!=mapPDF.m_particles.end();++it)
it->d->mapTillNow.auxParticleFilterCleanUp();
leaveCriticalSection(); /* Leaving critical section (updating map) */
MRPT_END_WITH_CLEAN_UP( leaveCriticalSection(); /* Leaving critical section (updating map) */ );
/*---------------------------------------------------------------
---------------------------------------------------------------*/
void CRawlog::addAction( CAction &action )
{
CActionCollection *temp = new CActionCollection();
temp->insert( action );
m_seqOfActObs.push_back( CSerializablePtr(temp) );
}
void xRawLogViewerFrame::OnMenuLossLessDecimate(wxCommandEvent& event)
{
WX_START_TRY
CRawlog newRawLog;
newRawLog.setCommentText(rawlog.getCommentText());
wxString strDecimation = wxGetTextFromUser(
_("The number of observations will be decimated (only 1 out of M will "
"be kept). Enter the decimation ratio M:"),
_("Decimation"), _("1"));
long DECIMATE_RATIO;
strDecimation.ToLong(&DECIMATE_RATIO);
ASSERT_(DECIMATE_RATIO >= 1)
wxBusyCursor busyCursor;
wxTheApp->Yield(); // Let the app. process messages
size_t i, N = rawlog.size();
// ------------------------------------------------------------------------------
// METHOD TO BE MEMORY EFFICIENT:
// To free the memory of the current rawlog entries as we create the new
// one,
// then call "clearWithoutDelete" at the end.
// ------------------------------------------------------------------------------
CSensoryFrame::Ptr accum_sf;
CActionRobotMovement2D::TMotionModelOptions odometryOptions;
bool cummMovementInit = false;
long SF_counter = 0;
// Reset cummulative pose change:
CPose2D accumMovement(0, 0, 0);
// For each entry:
for (i = 0; i < N; i++)
{
CSerializable::Ptr obj = rawlog.getAsGeneric(i);
if (rawlog.getType(i) == CRawlog::etActionCollection)
{
// Accumulate Actions
// ----------------------
CActionCollection::Ptr curActs =
std::dynamic_pointer_cast<CActionCollection>(obj);
CActionRobotMovement2D::Ptr mov =
curActs->getBestMovementEstimation();
if (mov)
{
CPose2D incrPose = mov->poseChange->getMeanVal();
// If we have previous observations, shift them according to
// this new increment:
if (accum_sf)
{
CPose3D inv_incrPose3D(
CPose3D(0, 0, 0) - CPose3D(incrPose));
for (CSensoryFrame::iterator it = accum_sf->begin();
it != accum_sf->end(); ++it)
{
CPose3D tmpPose;
(*it)->getSensorPose(tmpPose);
tmpPose = inv_incrPose3D + tmpPose;
(*it)->setSensorPose(tmpPose);
}
}
// Accumulate from odometry:
accumMovement = accumMovement + incrPose;
// Copy the probabilistic options from the first entry we find:
if (!cummMovementInit)
{
odometryOptions = mov->motionModelConfiguration;
cummMovementInit = true;
}
}
}
else if (rawlog.getType(i) == CRawlog::etSensoryFrame)
{
// Decimate Observations
// ---------------------------
// Add observations to the accum. SF:
if (!accum_sf)
accum_sf = mrpt::make_aligned_shared<CSensoryFrame>();
// Copy pointers to observations only (fast):
accum_sf->moveFrom(*std::dynamic_pointer_cast<CSensoryFrame>(obj));
if (++SF_counter >= DECIMATE_RATIO)
{
SF_counter = 0;
// INSERT OBSERVATIONS:
newRawLog.addObservationsMemoryReference(accum_sf);
accum_sf.reset();
// INSERT ACTIONS:
CActionCollection actsCol;
if (cummMovementInit)
{
CActionRobotMovement2D cummMovement;
cummMovement.computeFromOdometry(
accumMovement, odometryOptions);
actsCol.insert(cummMovement);
// Reset odometry accumulation:
accumMovement = CPose2D(0, 0, 0);
}
newRawLog.addActions(actsCol);
}
}
else
THROW_EXCEPTION(
"This operation implemented for SF-based rawlogs only.");
// Delete object
obj.reset();
} // end for i each entry
// Clear the list only (objects already deleted)
rawlog.clear();
// Copy as new log:
rawlog = newRawLog;
rebuildTreeView();
WX_END_TRY
}
// Convert from observations only to actions-SF format:
void xRawLogViewerFrame::OnMenuConvertSF(wxCommandEvent& event)
{
WX_START_TRY
bool onlyOnePerLabel =
(wxYES == wxMessageBox(
_("Keep only one observation of each label within each "
"sensoryframe?"),
_("Convert to sensoryframe's"), wxYES_NO, this));
wxString strMaxL = wxGetTextFromUser(
_("Maximum length of each sensoryframe (seconds):"),
_("Convert to sensoryframe's"), _("1.0"));
double maxLengthSF;
strMaxL.ToDouble(&maxLengthSF);
// Process:
CRawlog new_rawlog;
new_rawlog.setCommentText(rawlog.getCommentText());
wxBusyCursor waitCursor;
unsigned int nEntries = (unsigned int)rawlog.size();
wxProgressDialog progDia(
wxT("Progress"), wxT("Parsing rawlog..."),
nEntries, // range
this, // parent
wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE |
wxPD_ELAPSED_TIME | wxPD_ESTIMATED_TIME | wxPD_REMAINING_TIME);
wxTheApp->Yield(); // Let the app. process messages
CSensoryFrame SF_new;
set<string> SF_new_labels;
TTimeStamp SF_new_first_t = INVALID_TIMESTAMP;
CObservationOdometry::Ptr last_sf_odo, cur_sf_odo;
for (unsigned int countLoop = 0; countLoop < nEntries; countLoop++)
{
if (countLoop % 20 == 0)
{
if (!progDia.Update(
countLoop,
wxString::Format(
wxT("Parsing rawlog... %u objects"), countLoop)))
{
return;
}
wxTheApp->Yield(); // Let the app. process messages
}
switch (rawlog.getType(countLoop))
{
case CRawlog::etSensoryFrame:
case CRawlog::etActionCollection:
{
THROW_EXCEPTION(
"The rawlog already has sensory frames and/or actions!");
}
break;
case CRawlog::etObservation:
{
CObservation::Ptr o = rawlog.getAsObservation(countLoop);
// Update stats:
bool label_existed =
SF_new_labels.find(o->sensorLabel) != SF_new_labels.end();
double SF_len =
SF_new_first_t == INVALID_TIMESTAMP
? 0
: timeDifference(SF_new_first_t, o->timestamp);
// Decide:
// End SF and start a new one?
if (SF_len > maxLengthSF && SF_new.size() != 0)
{
new_rawlog.addObservations(SF_new);
// Odometry increments:
CActionCollection acts;
if (last_sf_odo && cur_sf_odo)
{
CActionRobotMovement2D act;
act.timestamp = cur_sf_odo->timestamp;
CActionRobotMovement2D::TMotionModelOptions opts;
act.computeFromOdometry(
cur_sf_odo->odometry - last_sf_odo->odometry, opts);
acts.insert(act);
}
new_rawlog.addActions(acts);
last_sf_odo = cur_sf_odo;
cur_sf_odo.reset();
SF_new.clear();
SF_new_labels.clear();
SF_new_first_t = INVALID_TIMESTAMP;
}
// Insert into SF:
if (!onlyOnePerLabel || !label_existed)
{
SF_new.insert(o);
SF_new_labels.insert(o->sensorLabel);
}
if (SF_new_first_t == INVALID_TIMESTAMP)
SF_new_first_t = o->timestamp;
if (o->GetRuntimeClass() == CLASS_ID(CObservationOdometry))
{
cur_sf_odo =
std::dynamic_pointer_cast<CObservationOdometry>(o);
}
}
break;
default:
break;
} // end for each entry
} // end while keep loading
// Remaining obs:
if (SF_new.size())
{
new_rawlog.addObservations(SF_new);
SF_new.clear();
}
progDia.Update(nEntries);
// Update:
rawlog.moveFrom(new_rawlog);
rebuildTreeView();
WX_END_TRY
}
int main(int argc, char ** argv)
{
try
{
bool showHelp = argc>1 && !os::_strcmp(argv[1],"--help");
bool showVersion = argc>1 && !os::_strcmp(argv[1],"--version");
printf(" simul-beacons - Part of the MRPT\n");
printf(" MRPT C++ Library: %s - BUILD DATE %s\n", MRPT_getVersion().c_str(), MRPT_getCompilationDate().c_str());
if (showVersion)
return 0; // Program end
// Process arguments:
if (argc<2 || showHelp )
{
printf("Usage: %s <config_file.ini>\n\n",argv[0]);
if (!showHelp)
{
mrpt::system::pause();
return -1;
}
else return 0;
}
string INI_FILENAME = std::string( argv[1] );
ASSERT_FILE_EXISTS_(INI_FILENAME)
CConfigFile ini( INI_FILENAME );
randomGenerator.randomize();
int i;
char auxStr[2000];
// Set default values:
int nBeacons = 3;
int nSteps = 100;
std::string outFile("out.rawlog");
std::string outDir("OUT");
float min_x =-5;
float max_x =5;
float min_y =-5;
float max_y =5;
float min_z =0;
float max_z =0;
float odometryNoiseXY_std = 0.001f;
float odometryBias_Y = 0;
float minSensorDistance = 0;
float maxSensorDistance = 10;
float stdError = 0.04f;
bool circularPath = true;
int squarePathLength=40;
float ratio_outliers = 0;
float ratio_outliers_first_step = 0;
float outlier_uniform_min=0;
float outlier_uniform_max=5.0;
// Load params from INI:
MRPT_LOAD_CONFIG_VAR(nBeacons,int, ini,"Params");
MRPT_LOAD_CONFIG_VAR(outFile,string, ini,"Params");
MRPT_LOAD_CONFIG_VAR(outDir,string, ini,"Params");
MRPT_LOAD_CONFIG_VAR(nSteps,int, ini,"Params");
MRPT_LOAD_CONFIG_VAR(circularPath,bool, ini,"Params");
MRPT_LOAD_CONFIG_VAR(squarePathLength,int,ini,"Params");
MRPT_LOAD_CONFIG_VAR(ratio_outliers,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(ratio_outliers_first_step,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(outlier_uniform_min,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(outlier_uniform_max,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(min_x,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(max_x,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(min_y,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(max_y,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(min_z,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(odometryNoiseXY_std,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(odometryBias_Y,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(minSensorDistance,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(maxSensorDistance,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(stdError,float, ini,"Params");
// Create out dir:
mrpt::system::createDirectory(outDir);
// ---------------------------------------------
// Create the point-beacons:
// ---------------------------------------------
printf("Creating beacon map...");
mrpt::slam::CBeaconMap beaconMap;
for (i=0;i<nBeacons;i++)
{
CBeacon b;
CPoint3D pt3D;
// Random coordinates:
pt3D.x( randomGenerator.drawUniform(min_x,max_x) );
pt3D.y( randomGenerator.drawUniform(min_y,max_y) );
pt3D.z( randomGenerator.drawUniform(min_z,max_z) );
// Add:
b.m_typePDF=CBeacon::pdfMonteCarlo;
b.m_locationMC.setSize(1,pt3D);
b.m_ID = i;
beaconMap.push_back(b);
}
os::sprintf(auxStr,sizeof(auxStr),"%s/outSimMap.txt",outDir.c_str());
beaconMap.saveToTextFile(auxStr);
printf("Done!\n");
//beaconMap.simulateBeaconReadings( );
// ---------------------------------------------
// Simulate:
// ---------------------------------------------
CActionRobotMovement2D::TMotionModelOptions opts;
CPoint3D null3D(0,0,0);
opts.modelSelection = CActionRobotMovement2D::mmGaussian;
opts.gausianModel.a1=0;
opts.gausianModel.a2=0;
opts.gausianModel.a3=0;
opts.gausianModel.a4=0;
opts.gausianModel.minStdXY = odometryNoiseXY_std;
opts.gausianModel.minStdPHI = DEG2RAD( 0.002f );
os::sprintf(auxStr,sizeof(auxStr),"%s/%s",outDir.c_str(),outFile.c_str());
CFileOutputStream fil(auxStr);
CPose2D realPose;
CPose2D incPose;
int stopSteps = 4;
for (i=0;i<nSteps;i++)
{
printf("Generating step %i...",i);
CSensoryFrame SF;
CActionCollection acts;
CActionRobotMovement2D act;
CPose3D pose3D( realPose );
if (i>=stopSteps)
{
if (circularPath)
{
// Circular path:
float Ar = DEG2RAD(5);
incPose = CPose2D(0.20f*cos(Ar),0.20f*sin(Ar),Ar);
}
else
{
// Square path:
if ( (i % squarePathLength) > (squarePathLength-5) )
incPose = CPose2D(0,0,DEG2RAD(90.0f/4));
else incPose = CPose2D(0.20f,0,0);
}
}
else incPose = CPose2D(0,0,0);
// Simulate observations:
CObservationBeaconRangesPtr obs=CObservationBeaconRanges::Create();
obs->minSensorDistance=minSensorDistance;
obs->maxSensorDistance=maxSensorDistance;
obs->stdError=stdError;
beaconMap.simulateBeaconReadings( pose3D,null3D,*obs );
// Corrupt with ourliers:
float probability_corrupt = i==0 ? ratio_outliers_first_step : ratio_outliers;
for (size_t q=0;q<obs->sensedData.size();q++)
{
if ( randomGenerator.drawUniform(0.0f,1.0f) < probability_corrupt )
{
obs->sensedData[q].sensedDistance += randomGenerator.drawUniform( outlier_uniform_min,outlier_uniform_max );
if (obs->sensedData[q].sensedDistance<0)
obs->sensedData[q].sensedDistance = 0;
}
}
std::cout << obs->sensedData.size() << " beacons at range";
SF.push_back( obs );
// Simulate actions:
CPose2D incOdo( incPose );
if (incPose.x()!=0 || incPose.y()!=0 || incPose.phi()!=0)
{
incOdo.x_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std );
incOdo.y_incr( odometryBias_Y + randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std );
}
act.computeFromOdometry( incOdo, opts);
acts.insert( act );
// Save:
fil << SF << acts;
// Next pose:
realPose = realPose + incPose;
printf("\n");
}
cout << "Data saved to directory: " << outDir << endl;
}
catch(std::exception &e)
{
std::cout << e.what();
}
catch(...)
{
std::cout << "Untyped exception!";
}
}
/*---------------------------------------------------------------
processActionObservation
---------------------------------------------------------------*/
void CMetricMapBuilderRBPF::processActionObservation(
CActionCollection& action, CSensoryFrame& observations)
{
MRPT_START
std::lock_guard<std::mutex> csl(
critZoneChangingMap); // Enter critical section (updating map)
// Update the traveled distance estimations:
{
CActionRobotMovement3D::Ptr act3D =
action.getActionByClass<CActionRobotMovement3D>();
CActionRobotMovement2D::Ptr act2D =
action.getActionByClass<CActionRobotMovement2D>();
if (act3D)
{
MRPT_LOG_DEBUG_STREAM(
"processActionObservation(): Input action is "
"CActionRobotMovement3D="
<< act3D->poseChange.getMeanVal().asString());
odoIncrementSinceLastMapUpdate += act3D->poseChange.getMeanVal();
odoIncrementSinceLastLocalization += act3D->poseChange;
}
else if (act2D)
{
MRPT_LOG_DEBUG_STREAM(
"processActionObservation(): Input action is "
"CActionRobotMovement2D="
<< act2D->poseChange->getMeanVal().asString());
odoIncrementSinceLastMapUpdate +=
mrpt::poses::CPose3D(act2D->poseChange->getMeanVal());
odoIncrementSinceLastLocalization.mean +=
mrpt::poses::CPose3D(act2D->poseChange->getMeanVal());
}
else
{
MRPT_LOG_WARN("Action contains no odometry.\n");
}
}
// Execute particle filter:
// But only if the traveled distance since the last update is long enough,
// or this is the first observation, etc...
// ----------------------------------------------------------------------------
bool do_localization =
(!mapPDF.SFs.size() || // This is the first observation!
options.debugForceInsertion ||
odoIncrementSinceLastLocalization.mean.norm() > localizeLinDistance ||
std::abs(odoIncrementSinceLastLocalization.mean.yaw()) >
localizeAngDistance);
bool do_map_update =
(!mapPDF.SFs.size() || // This is the first observation!
options.debugForceInsertion ||
odoIncrementSinceLastMapUpdate.norm() > insertionLinDistance ||
std::abs(odoIncrementSinceLastMapUpdate.yaw()) > insertionAngDistance);
// Used any "options.alwaysInsertByClass" ??
for (auto itCl = options.alwaysInsertByClass.data.begin();
!do_map_update && itCl != options.alwaysInsertByClass.data.end();
++itCl)
for (auto& o : observations)
if (o->GetRuntimeClass() == *itCl)
{
do_map_update = true;
do_localization = true;
break;
}
if (do_map_update) do_localization = true;
MRPT_LOG_DEBUG(mrpt::format(
"do_map_update=%s do_localization=%s", do_map_update ? "YES" : "NO",
do_localization ? "YES" : "NO"));
if (do_localization)
{
// Create an artificial action object, since
// we've been collecting them until a threshold:
// ------------------------------------------------
CActionCollection fakeActs;
{
CActionRobotMovement3D::Ptr act3D =
action.getActionByClass<CActionRobotMovement3D>();
if (act3D)
{
CActionRobotMovement3D newAct;
newAct.estimationMethod = act3D->estimationMethod;
newAct.poseChange = odoIncrementSinceLastLocalization;
newAct.timestamp = act3D->timestamp;
fakeActs.insert(newAct);
}
else
{
// It must be 2D odometry:
CActionRobotMovement2D::Ptr act2D =
action.getActionByClass<CActionRobotMovement2D>();
ASSERT_(act2D);
CActionRobotMovement2D newAct;
newAct.computeFromOdometry(
CPose2D(odoIncrementSinceLastLocalization.mean),
act2D->motionModelConfiguration);
newAct.timestamp = act2D->timestamp;
fakeActs.insert(newAct);
}
}
MRPT_LOG_DEBUG_STREAM(
"odoIncrementSinceLastLocalization before resetting = "
<< odoIncrementSinceLastLocalization.mean);
// Reset distance counters:
odoIncrementSinceLastLocalization.mean.setFromValues(0, 0, 0, 0, 0, 0);
odoIncrementSinceLastLocalization.cov.zeros();
CParticleFilter pf;
pf.m_options = m_PF_options;
pf.setVerbosityLevel(this->getMinLoggingLevel());
pf.executeOn(mapPDF, &fakeActs, &observations);
if (isLoggingLevelVisible(mrpt::system::LVL_INFO))
{
// Get current pose estimation:
CPose3DPDFParticles poseEstimation;
CPose3D meanPose;
mapPDF.getEstimatedPosePDF(poseEstimation);
poseEstimation.getMean(meanPose);
CPose3D estPos;
CMatrixDouble66 cov;
poseEstimation.getCovarianceAndMean(cov, estPos);
MRPT_LOG_INFO_STREAM(
"New pose=" << estPos << std::endl
<< "New ESS:" << mapPDF.ESS() << std::endl);
MRPT_LOG_INFO(format(
" STDs: x=%2.3f y=%2.3f z=%.03f yaw=%2.3fdeg\n",
sqrt(cov(0, 0)), sqrt(cov(1, 1)), sqrt(cov(2, 2)),
RAD2DEG(sqrt(cov(3, 3)))));
}
}
if (do_map_update)
{
odoIncrementSinceLastMapUpdate.setFromValues(0, 0, 0, 0, 0, 0);
// Update the particles' maps:
// -------------------------------------------------
MRPT_LOG_INFO("New observation inserted into the map.");
// Add current observation to the map:
const bool anymap_update = mapPDF.insertObservation(observations);
if (!anymap_update)
MRPT_LOG_WARN_STREAM(
"**No map was updated** after inserting a CSensoryFrame with "
<< observations.size());
m_statsLastIteration.observationsInserted = true;
}
else
{
m_statsLastIteration.observationsInserted = false;
}
// Added 29/JUN/2007 JLBC: Tell all maps that they can now free aux.
// variables
// (if any) since one PF cycle is over:
for (auto& m_particle : mapPDF.m_particles)
m_particle.d->mapTillNow.auxParticleFilterCleanUp();
MRPT_END;
}
void CRawlog::addActions(CActionCollection& actions)
{
m_seqOfActObs.push_back(
std::dynamic_pointer_cast<CSerializable>(
actions.duplicateGetSmartPtr()));
}
// ------------------------------------------------------
// 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;
}
}
void CActionCollection_insert2(CActionCollection &self, CActionRobotMovement3D &action)
{
self.insert(action);
}
