// PlannerSimple2D
tuple PlannerSimple2D_computePath(PlannerSimple2D &self, COccupancyGridMap2D &theMap, CPose2D &origin, CPose2D &target)
{
// function args
std::deque<mrpt::math::TPoint2D> path;
bool notFound;
// invoke function
self.computePath(theMap, origin, target, path, notFound);
// convert to python compatible
boost::python::list ret_val;
boost::python::list py_path;
for (uint32_t i = 0; i < path.size(); ++i) {
py_path.append(path[i]);
}
// setup return value
ret_val.append(py_path);
ret_val.append(notFound);
return tuple(ret_val);
}
// ------------------------------------------------------
// TestPathPlanning
// ------------------------------------------------------
void TestPathPlanning()
{
// Load the gridmap:
COccupancyGridMap2D gridmap;
if (!mrpt::system::fileExists(myGridMap))
THROW_EXCEPTION_FMT("Map file '%s' not found", myGridMap.c_str());
printf("Loading gridmap...");
{
CFileGZInputStream f(myGridMap);
auto arch = archiveFrom(f);
arch >> gridmap;
}
printf(
"Done! %f x %f m\n", gridmap.getXMax() - gridmap.getXMin(),
gridmap.getYMax() - gridmap.getYMin());
// Find path:
PlannerSimple2D pathPlanning;
pathPlanning.robotRadius = 0.30f;
std::deque<TPoint2D> thePath;
bool notFound;
CTicTac tictac;
CPose2D origin(20, -110, 0);
CPose2D target(90, 40, 0);
cout << "Origin: " << origin << endl;
cout << "Target: " << target << endl;
cout << "Searching path...";
cout.flush();
tictac.Tic();
pathPlanning.computePath(
gridmap, origin, target, thePath, notFound, 100.0f /* Max. distance */);
double t = tictac.Tac();
cout << "Done in " << t * 1000 << " ms" << endl;
printf("Path found: %s\n", notFound ? "NO" : "YES");
printf("Path has %u steps\n", (unsigned)thePath.size());
// Save result:
CImage img;
gridmap.getAsImage(img, false, true); // Force a RGB image
// Draw the path:
// ---------------------
int R = round(pathPlanning.robotRadius / gridmap.getResolution());
for (std::deque<TPoint2D>::const_iterator it = thePath.begin();
it != thePath.end(); ++it)
img.drawCircle(
gridmap.x2idx(it->x), gridmap.getSizeY() - 1 - gridmap.y2idx(it->y),
R, TColor(0, 0, 255));
img.drawMark(
gridmap.x2idx(origin.x()),
gridmap.getSizeY() - 1 - gridmap.y2idx(origin.y()),
TColor(0x20, 0x20, 0x20), '+', 10);
img.drawMark(
gridmap.x2idx(target.x()),
gridmap.getSizeY() - 1 - gridmap.y2idx(target.y()),
TColor(0x50, 0x50, 0x50), 'x', 10);
const std::string dest = "path_planning.png";
cout << "Saving output to: " << dest << endl;
img.saveToFile(dest);
printf("Done\n");
#if MRPT_HAS_WXWIDGETS
mrpt::gui::CDisplayWindow3D win("Computed path");
win.setImageView(img);
win.repaint();
win.waitForKey();
#endif
}
