MRPT  2.0.3
test.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
14 #include <mrpt/poses/CPose2D.h>
16 #include <mrpt/system/CTicTac.h>
17 #include <mrpt/system/filesystem.h>
18 #include <iostream>
19 
20 using namespace mrpt;
21 using namespace mrpt::maps;
22 using namespace mrpt::nav;
23 using namespace mrpt::serialization;
24 using namespace mrpt::img;
25 using namespace mrpt::math;
26 using namespace mrpt::poses;
27 using namespace mrpt::io;
28 using namespace mrpt::system;
29 using namespace std;
30 
31 #include <mrpt/examples_config.h>
32 
33 string myGridMap(
34  MRPT_EXAMPLES_BASE_DIRECTORY +
35  string("../share/mrpt/datasets/2006-MalagaCampus.gridmap.gz"));
36 
37 // ------------------------------------------------------
38 // TestPathPlanning
39 // ------------------------------------------------------
40 void TestPathPlanning()
41 {
42  // Load the gridmap:
43  COccupancyGridMap2D gridmap;
44 
46  THROW_EXCEPTION_FMT("Map file '%s' not found", myGridMap.c_str());
47 
48  printf("Loading gridmap...");
49  {
51  auto arch = archiveFrom(f);
52  arch >> gridmap;
53  }
54  printf(
55  "Done! %f x %f m\n", gridmap.getXMax() - gridmap.getXMin(),
56  gridmap.getYMax() - gridmap.getYMin());
57 
58  // Find path:
59  PlannerSimple2D pathPlanning;
60  pathPlanning.robotRadius = 0.30f;
61 
62  std::deque<TPoint2D> thePath;
63  bool notFound;
64  CTicTac tictac;
65 
66  CPose2D origin(20, -110, 0);
67  CPose2D target(90, 40, 0);
68 
69  cout << "Origin: " << origin << endl;
70  cout << "Target: " << target << endl;
71 
72  cout << "Searching path...";
73  cout.flush();
74  tictac.Tic();
75 
76  pathPlanning.computePath(
77  gridmap, origin, target, thePath, notFound, 100.0f /* Max. distance */);
78 
79  double t = tictac.Tac();
80  cout << "Done in " << t * 1000 << " ms" << endl;
81 
82  printf("Path found: %s\n", notFound ? "NO" : "YES");
83  printf("Path has %u steps\n", (unsigned)thePath.size());
84 
85  // Save result:
86  CImage img;
87  gridmap.getAsImage(img, false, true); // Force a RGB image
88 
89  // Draw the path:
90  // ---------------------
91  int R = round(pathPlanning.robotRadius / gridmap.getResolution());
92 
93  for (std::deque<TPoint2D>::const_iterator it = thePath.begin();
94  it != thePath.end(); ++it)
95  img.drawCircle(
96  gridmap.x2idx(it->x), gridmap.getSizeY() - 1 - gridmap.y2idx(it->y),
97  R, TColor(0, 0, 255));
98 
99  img.drawMark(
100  gridmap.x2idx(origin.x()),
101  gridmap.getSizeY() - 1 - gridmap.y2idx(origin.y()),
102  TColor(0x20, 0x20, 0x20), '+', 10);
103  img.drawMark(
104  gridmap.x2idx(target.x()),
105  gridmap.getSizeY() - 1 - gridmap.y2idx(target.y()),
106  TColor(0x50, 0x50, 0x50), 'x', 10);
107 
108  const std::string dest = "path_planning.png";
109  cout << "Saving output to: " << dest << endl;
110  img.saveToFile(dest);
111  printf("Done\n");
112 
113 #if MRPT_HAS_WXWIDGETS
114  mrpt::gui::CDisplayWindow3D win("Computed path");
115  win.setImageView(img);
116  win.repaint();
117 
118  win.waitForKey();
119 #endif
120 }
121 
122 int main(int argc, char** argv)
123 {
124  try
125  {
127  return 0;
128  }
129  catch (exception& e)
130  {
131  cout << "MRPT exception caught: " << e.what() << endl;
132  return -1;
133  }
134  catch (...)
135  {
136  printf("Another exception!!");
137  return -1;
138  }
139 }
filesystem.h
TestPathPlanning
void TestPathPlanning()
Definition: vision_stereo_rectify/test.cpp:40
COccupancyGridMap2D.h
mrpt::nav::PlannerSimple2D
Searches for collision-free path in 2D occupancy grids for holonomic circular robots.
Definition: PlannerSimple2D.h:30
mrpt::io
Definition: img/CImage.h:24
mrpt::system::CTicTac
A high-performance stopwatch, with typical resolution of nanoseconds.
Definition: system/CTicTac.h:17
mrpt::nav
Definition: CAbstractHolonomicReactiveMethod.h:20
mrpt::maps::COccupancyGridMap2D::getAsImage
void getAsImage(mrpt::img::CImage &img, bool verticalFlip=false, bool forceRGB=false, bool tricolor=false) const
Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if ...
Definition: COccupancyGridMap2D_getAs.cpp:29
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
R
const float R
Definition: CKinematicChain.cpp:137
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
CPose2D.h
mrpt::system::fileExists
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
CDisplayWindow3D.h
mrpt::maps::COccupancyGridMap2D::getXMax
float getXMax() const
Returns the "x" coordinate of right side of grid map.
Definition: COccupancyGridMap2D.h:280
mrpt::system::CTicTac::Tac
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
mrpt::maps::COccupancyGridMap2D::getSizeY
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: COccupancyGridMap2D.h:276
main
int main()
Definition: vision_stereo_rectify/test.cpp:78
mrpt::maps::COccupancyGridMap2D::getYMax
float getYMax() const
Returns the "y" coordinate of bottom side of grid map.
Definition: COccupancyGridMap2D.h:284
mrpt::img
Definition: CCanvas.h:16
mrpt::io::CFileGZInputStream
Transparently opens a compressed "gz" file and reads uncompressed data from it.
Definition: io/CFileGZInputStream.h:26
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24
win
mrpt::gui::CDisplayWindow3D::Ptr win
Definition: vision_stereo_rectify/test.cpp:31
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::maps::COccupancyGridMap2D::y2idx
int y2idx(float y) const
Definition: COccupancyGridMap2D.h:292
mrpt::img::CCanvas::drawMark
void drawMark(int x0, int y0, const mrpt::img::TColor color, char type, int size=5, unsigned int width=1)
Draw a mark.
Definition: CCanvas.cpp:304
mrpt::maps::COccupancyGridMap2D::x2idx
int x2idx(float x) const
Transform a coordinate value into a cell index.
Definition: COccupancyGridMap2D.h:288
mrpt::img::TColor
A RGB color - 8bit.
Definition: TColor.h:25
mrpt::maps::COccupancyGridMap2D::getYMin
float getYMin() const
Returns the "y" coordinate of top side of grid map.
Definition: COccupancyGridMap2D.h:282
argv
const char * argv[]
Definition: RawlogGrabberApp_unittest.cpp:40
mrpt::img::CImage::drawCircle
void drawCircle(int x, int y, int radius, const mrpt::img::TColor &color=mrpt::img::TColor(255, 255, 255), unsigned int width=1) override
Draws a circle of a given radius.
Definition: CImage.cpp:1130
mrpt::system::CTicTac::Tic
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
mrpt::serialization
Definition: aligned_serialization.h:13
mrpt::img::CImage
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
mrpt::img::CImage::saveToFile
bool saveToFile(const std::string &fileName, int jpeg_quality=95) const
Save the image to a file, whose format is determined from the extension (internally uses OpenCV).
Definition: CImage.cpp:330
myGridMap
string myGridMap(MRPT_EXAMPLES_BASE_DIRECTORY+string("../share/mrpt/datasets/2006-MalagaCampus.gridmap.gz"))
mrpt::serialization::archiveFrom
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT's CStream,...
Definition: CArchive.h:592
mrpt::maps::COccupancyGridMap2D::getResolution
float getResolution() const
Returns the resolution of the grid map.
Definition: COccupancyGridMap2D.h:286
CTicTac.h
argc
const int argc
Definition: RawlogGrabberApp_unittest.cpp:41
mrpt::maps::COccupancyGridMap2D::getXMin
float getXMin() const
Returns the "x" coordinate of left side of grid map.
Definition: COccupancyGridMap2D.h:278
CFileGZInputStream.h
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
PlannerSimple2D.h
mrpt::maps::COccupancyGridMap2D
A class for storing an occupancy grid map.
Definition: COccupancyGridMap2D.h:53
mrpt::nav::PlannerSimple2D::robotRadius
float robotRadius
The aproximate robot radius used in the planification.
Definition: PlannerSimple2D.h:51
mrpt::maps
Definition: CBeacon.h:21
CArchive.h
mrpt::nav::PlannerSimple2D::computePath
void computePath(const mrpt::maps::COccupancyGridMap2D &theMap, const mrpt::poses::CPose2D &origin, const mrpt::poses::CPose2D &target, std::deque< mrpt::math::TPoint2D > &path, bool &notFound, float maxSearchPathLength=-1) const
This method compute the optimal path for a circular robot, in the given occupancy grid map,...
Definition: PlannerSimple2D.cpp:32
mrpt::gui::CDisplayWindow3D
A graphical user interface (GUI) for efficiently rendering 3D scenes in real-time.
Definition: CDisplayWindow3D.h:117
mrpt::system
Definition: backtrace.h:14



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020