MRPT  2.0.4
PlannerSimple2D_unittest.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 
10 #include <gtest/gtest.h>
14 #include <test_mrpt_common.h>
15 
16 TEST(PlannerSimple2D, findPath)
17 {
18  using namespace std::string_literals;
19 
20  const auto fil = mrpt::UNITTEST_BASEDIR +
21  "/share/mrpt/datasets/2006-MalagaCampus.gridmap.gz"s;
22 
23  // Load the gridmap:
25 
26  {
28  auto arch = mrpt::serialization::archiveFrom(f);
29  arch >> gridmap;
30  }
31 
32  // Find path:
33  mrpt::nav::PlannerSimple2D pathPlanning;
34  pathPlanning.robotRadius = 0.30f;
35 
36  {
37  std::deque<mrpt::math::TPoint2D> thePath;
38  bool notFound;
39  const mrpt::poses::CPose2D origin(20, -110, 0), target(90, 40, 0);
40  pathPlanning.computePath(gridmap, origin, target, thePath, notFound);
41 
42  EXPECT_FALSE(notFound);
43  EXPECT_EQ(thePath.size(), 416U);
44  EXPECT_NEAR(thePath.at(0).x, origin.x(), 1.0);
45  EXPECT_NEAR(thePath.at(0).y, origin.y(), 1.0);
46  EXPECT_NEAR(thePath.back().x, target.x(), 1.0);
47  EXPECT_NEAR(thePath.back().y, target.y(), 1.0);
48  }
49  {
50  std::deque<mrpt::math::TPoint2D> thePath;
51  bool notFound;
52  const mrpt::poses::CPose2D origin(90, 40, 0), target(20, -110, 0);
53  pathPlanning.computePath(
54  gridmap, origin, target, thePath, notFound,
55  300.0f /* Max. distance */);
56 
57  EXPECT_FALSE(notFound);
58  EXPECT_EQ(thePath.size(), 416U);
59  EXPECT_NEAR(thePath.at(0).x, origin.x(), 1.0);
60  EXPECT_NEAR(thePath.at(0).y, origin.y(), 1.0);
61  EXPECT_NEAR(thePath.back().x, target.x(), 1.0);
62  EXPECT_NEAR(thePath.back().y, target.y(), 1.0);
63  }
64 
65  {
66  std::deque<mrpt::math::TPoint2D> thePath;
67  bool notFound;
68  const mrpt::poses::CPose2D origin(20, -110, 0), target(90, 40, 0);
69  pathPlanning.computePath(
70  gridmap, origin, target, thePath, notFound,
71  10.0f /* Max. distance */);
72 
73  EXPECT_TRUE(notFound);
74  }
75 
76  {
77  std::deque<mrpt::math::TPoint2D> thePath;
78  bool notFound;
79  const mrpt::poses::CPose2D origin(20, -110, 0), target(900, 40, 0);
80  pathPlanning.computePath(
81  gridmap, origin, target, thePath, notFound,
82  100.0f /* Max. distance */);
83 
84  EXPECT_TRUE(notFound);
85  EXPECT_EQ(thePath.size(), 0U);
86  }
87 }
mrpt::nav::PlannerSimple2D
Searches for collision-free path in 2D occupancy grids for holonomic circular robots.
Definition: PlannerSimple2D.h:31
EXPECT_TRUE
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
EXPECT_NEAR
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
mrpt::io::CFileGZInputStream
Transparently opens a compressed "gz" file and reads uncompressed data from it.
Definition: io/CFileGZInputStream.h:26
TEST
TEST(PlannerSimple2D, findPath)
Definition: PlannerSimple2D_unittest.cpp:16
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::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:147
EXPECT_EQ
EXPECT_EQ(out.image_pair_was_used.size(), NUM_IMGS)
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
CFileGZInputStream.h
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
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:26



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 15:15:43 UTC 2020