bool senseObstacles( mrpt::slam::CSimplePointsMap &obstacles )
{
CPose2D robotPose;
robotSim.getRealPose(robotPose);
CObservation2DRangeScan laserScan;
laserScan.aperture = M_2PIf;
laserScan.rightToLeft = true;
laserScan.maxRange = 7.0f;
laserScan.stdError = 0.003f;
gridMap.laserScanSimulator(
laserScan,
robotPose,
0.5f, // grid cell threshold
361, // Number of rays
0 // Noise std
);
// Build the points map:
obstacles.insertionOptions.minDistBetweenLaserPoints = 0.005f;
obstacles.insertionOptions.also_interpolate = false;
obstacles.clear();
obstacles.insertObservation( &laserScan );
return true;
}
bool getCurrentPoseAndSpeeds( mrpt::poses::CPose2D &curPose, float &curV, float &curW)
{
robotSim.getRealPose( curPose );
double phi = M_PI/2 - curPose.phi();
if(phi<0)
{
cout << "changing" <<phi << endl;
phi+= 2*M_PI;
cout << "to" << phi << endl;
}
//curPose.phi(phi);
/*float temp = curPose.x();
curPose.x(curPose.y());
curPose.y(temp);*/
curV = robotSim.getV();
curW = robotSim.getW();
return true;
}
bool changeSpeeds( float v, float w )
{
robotSim.movementCommand(v,w);
return true;
}
int main(int argc, char ** argv)
{
try
{
if (argc!=3)
{
cerr << "Usage: " << argv[0] << " <GRIDMAP.png> <PIXEL SIZE (meters)>" << endl;
return -1;
}
COccupancyGridMap2D gridmap;
gridmap.loadFromBitmapFile( argv[1], atof(argv[2]) );
randomGenerator.randomize();
CDisplayWindowPlots win("Gridmap 2D simulator");
CImage bmpImg;
gridmap.getAsImage( bmpImg );
win.image(bmpImg, gridmap.getXMin(), gridmap.getYMin(), gridmap.getSizeX()*gridmap.getResolution(),gridmap.getSizeY()*gridmap.getResolution(), "grid" );
win.axis_equal();
CRobotSimulator robotSim;
std::string outFile("out.rawlog");
std::string outDir("OUT");
// Create out dir:
mrpt::system::createDirectory(outDir);
CFileOutputStream fil(format("%s/%s",outDir.c_str(),outFile.c_str()) );
CTicTac tictac;
tictac.Tic();
CPose2D odoPose, realPose;
double t0 = tictac.Tac();
for (;;)
{
// Real-time simulation:
double t1 = tictac.Tac();
double At = t1 - t0;
t0 = t1;
robotSim.simulateInterval(At);
robotSim.getOdometry(odoPose);
robotSim.getRealPose(realPose);
cout << "[sim] robot: " << realPose << endl;
mrpt::system::sleep(20);
// Process keys:
if (os::kbhit())
{
char c= os::getch();
printf("C:%i\n",c);
if (c==27 || c=='q' || c=='Q') break;
}
}
}
catch(std::exception &e)
{
std::cout << e.what();
}
catch(...)
{
std::cout << "Untyped exception!";
}
}
