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