//* Flood path planning test *

#include "../00_nav_library/nav_map.h" #include "../00_nav_library/nav_path_flood.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); cout << "Routing. . ." << endl; NAV_Path_Flood 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 ); cout << "Global Map Routed. . ." << endl; ROUTE.register_map(&MAP_LOCAL); // Local map is empty for now but this illustrates how to use multiple maps cout << "Rerouting. . ." << endl; ROUTE.recalculate(); cout << "Rerouted. . ." << endl; cout << "Rerouting. . ." << endl; ROUTE.setGoal(ArPose(-1001, 5000)); cout << "Rerouted. . ." << endl; cout << "Saving results. . ." << endl; { double dx = ROUTE.getResolution(); double xmax = ROUTE.getMaxX()-dx; double dy = ROUTE.getResolution(); double ymax = ROUTE.getMaxY()-dy; ofstream results; results.open("map_gnuplot.txt"); ROUTE.lock(); // GNUPLOT> // GNUPLOT> // GNUPLOT> splot "D:/PROJECTS/EENAV/programs/41_path_flood/map_gnuplot.txt" results<<"#X\tY\tZ"<< std::endl; for (double x=ROUTE.getMinX()+dx; x<xmax; x=x+dx) { for (double y=ROUTE.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=ROUTE.getMinY()+dy; y<ymax; y=y+dy) results<<","<<y; results << std::endl; for (double x=ROUTE.getMinX()+dx; x<xmax; x=x+dx) { results<<x; for (double y=ROUTE.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=ROUTE.getMinY()+dy; y<ymax; y=y+dy) results<<","<<y; results << std::endl; for (double x=ROUTE.getMinX()+dx; x<xmax; x=x+dx) { results<<x; for (double y=ROUTE.getMinY()+dy; y<ymax; y=y+dy) { results<<","<<ROUTE.getHeading(x, y); } results << std::endl; } ROUTE.unlock(); results.close(); } cout << "Done. . ." << endl; return(0); }