//* Flood path planning test *

#include "../00_nav_library/nav_map.h" #include "../00_nav_library/nav_field_potential.h" #include <fstream> #include <iostream> #include <iomanip> using namespace std; int main(){ NAV_Map MAP_GLOBAL(100); NAV_Map MAP_LOCAL(100); cout << "Reading the global map. . ." << endl; // read the map with no markers on it except "all robots" trails URAMMMMMpppppppp if (! MAP_GLOBAL.read("map_global.ppm", 0x00)) { // -^^^^^^^-------- can be masked cout << "Cannot read the global map. Exiting. . ." << endl; exit(-1); } cout << "Map loaded. . ." << endl; ArPose dest(0, 0); NAV_FIELD_Potential ROUTE( &MAP_GLOBAL, // pointer to the global map, use map locking if map updated simultanously dest, // the destination 300, /*, */ // grid size make larger for faster routing but it may close narrow passages 200 /* 150 */ // robot radius, enlarge all obstacles by this length as routed robot is a point ); ROUTE.setUseGoal(true); cout << "Computing and saving results. . ." << endl; { double dx = MAP_GLOBAL.getResolution(); double xmax = MAP_GLOBAL.getMaxX()-dx; double dy = MAP_GLOBAL.getResolution(); double ymax = MAP_GLOBAL.getMaxY()-dy; ofstream results; results.open("map_gnuplot.txt"); ROUTE.lock(); // GNUPLOT> // GNUPLOT> // GNUPLOT> splot "D:/PROJECTS/EENAV/programs/42_path_potential/map_gnuplot.txt" results<<"#X\tY\tZ"<< std::endl; for (double x=MAP_GLOBAL.getMinX()+dx; x<xmax; x=x+dx) { for (double y=MAP_GLOBAL.getMinY()+dy; y<ymax; y=y+dy) { double val = ROUTE.getValue(x, y); if (val==MAXSHORT) val = -100; else if (val==0) val = -200; results<<x<<"\t"<<y<<"\t"<<val<< std::endl; } } ROUTE.unlock(); results.close(); results.open("map_values.csv"); ROUTE.lock(); for (double y=MAP_GLOBAL.getMinY()+dy; y<ymax; y=y+dy) results<<","<<y; results << std::endl; for (double x=MAP_GLOBAL.getMinX()+dx; x<xmax; x=x+dx) { results<<x; for (double y=MAP_GLOBAL.getMinY()+dy; y<ymax; y=y+dy) { double val = ROUTE.getValue(x, y); if (val==MAXSHORT) val = -100; else if (val==0) val = -200; results<<","<<val; } results << std::endl; } ROUTE.unlock(); results.close(); results.open("map_headings.csv"); ROUTE.lock(); for (double y=MAP_GLOBAL.getMinY()+dy; y<ymax; y=y+dy) results<<","<<y; results << std::endl; for (double x=MAP_GLOBAL.getMinX()+dx; x<xmax; x=x+dx) { results<<x; for (double y=MAP_GLOBAL.getMinY()+dy; y<ymax; y=y+dy) { results<<","<<ROUTE.getHeading(x, y); } results << std::endl; } ROUTE.unlock(); results.close(); } cout << "Done. . ." << endl; return(0); }