MRPT  2.0.4
PTGs_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>
13 #include <mrpt/system/filesystem.h>
14 #include <test_mrpt_common.h>
15 
16 TEST(NavTests, PTGs_tests)
17 {
18  using namespace std;
19  using namespace mrpt;
20  using namespace mrpt::nav;
21 
22  const string sFil =
23  mrpt::UNITTEST_BASEDIR + string("/tests/PTGs_for_tests.ini");
24  if (!mrpt::system::fileExists(sFil))
25  {
26  cerr << "**WARNING* Skipping tests since file cannot be found: '"
27  << sFil << "'\n";
28  return;
29  }
30 
31  mrpt::config::CConfigFile cfg(sFil);
32 
33  const unsigned int PTG_COUNT =
34  cfg.read_int("PTG_UNIT_TESTS", "PTG_COUNT", 0, true);
35  EXPECT_TRUE(PTG_COUNT > 0);
36  vector<CParameterizedTrajectoryGenerator::Ptr> PTGs(PTG_COUNT);
37 
38  for (unsigned int n = 0; n < PTG_COUNT; n++)
39  {
40  // Factory:
41  const string sPTGName = cfg.read_string(
42  "PTG_UNIT_TESTS", format("PTG%u_Type", n), "", true);
43  PTGs[n] = CParameterizedTrajectoryGenerator::CreatePTG(
44  sPTGName, cfg, "PTG_UNIT_TESTS", format("PTG%u_", n));
45  EXPECT_TRUE(PTGs[n] != nullptr) << "Failed creating PTG #" << n << endl;
46 
47  try
48  {
49  PTGs[n]->initialize(string(), false /*verbose */);
50  }
51  catch (const std::exception& e)
52  {
53  GTEST_FAIL() << "Failed initializing PTG #" << n << endl
54  << e.what() << endl;
55  }
56  }
57 
58  // Run tests:
59  // ---------------------------------------------------------
60  for (unsigned int n = 0; n < PTG_COUNT; n++)
61  {
63 
64  const std::string sPTGDesc = ptg->getDescription();
65  const double refDist = ptg->getRefDistance();
66  const size_t num_paths = ptg->getPathCount();
67  size_t num_tests_run = 0;
68 
69  // TEST: step <-> dist match
70  {
71  for (double dist = 0.1; dist < refDist * 0.5; dist += 0.2)
72  {
73  bool any_good = false;
74  for (size_t k = 0; k < num_paths; k++)
75  {
76  uint32_t step;
77 
78  if (ptg->getPathStepForDist(k, dist, step))
79  {
80  any_good = true;
81  double d = ptg->getPathDist(k, step);
82  EXPECT_NEAR(d, dist, 0.05)
83  << "Test: step <-> dist match\n PTG: " << sPTGDesc
84  << endl
85  << "dist:" << dist << endl;
86  num_tests_run++;
87  }
88  }
89  EXPECT_TRUE(any_good)
90  << "Test: step <-> dist match\n PTG: " << sPTGDesc << endl
91  << "dist:" << dist << endl;
92  }
93  }
94 
95  // TEST: inverseMap_WS2TP
96  {
97  bool any_ok = false;
98  bool skip_this_ptg = false;
99  for (double tx = -refDist * 0.5;
100  !skip_this_ptg && tx < refDist * 0.5; tx += 0.1)
101  {
102  for (double ty = -refDist * 0.5;
103  !skip_this_ptg && ty < refDist * 0.5; ty += 0.1)
104  {
105  if (std::abs(tx) < 1e-2 && std::abs(ty) < 1e-2)
106  continue; // TP-Space does not include the WS point
107  // (0,0) in its domain
108 
109  const double tolerance_dist = std::max(
110  0.10, 10.0 * std::sqrt(tx * tx + ty * ty) * M_PI * 2 /
111  ptg->getPathCount());
112 
113  int k;
114  double normalized_d;
115  bool valid = ptg->inverseMap_WS2TP(tx, ty, k, normalized_d);
116  if (valid && normalized_d < 1.0)
117  {
118  any_ok = true;
119  // Now, do the inverse operation:
120  uint32_t step;
121  bool step_ok = ptg->getPathStepForDist(
122  k, normalized_d * refDist, step);
123  EXPECT_TRUE(step_ok)
124  << "PTG: " << sPTGDesc << endl
125  << "(tx,ty): " << tx << " " << ty << " k= " << k
126  << " normalized_d=" << normalized_d << endl;
127  if (step_ok)
128  {
129  mrpt::math::TPose2D pose;
130  ptg->getPathPose(k, step, pose);
131  EXPECT_NEAR(pose.x, tx, tolerance_dist)
132  << "Test: inverseMap_WS2TP\n PTG#" << n << ": "
133  << sPTGDesc << endl
134  << "(tx,ty): " << tx << " " << ty << " k= " << k
135  << " normalized_d=" << normalized_d << endl;
136  EXPECT_NEAR(pose.y, ty, tolerance_dist)
137  << "Test: inverseMap_WS2TP\n PTG#" << n << ": "
138  << sPTGDesc << endl
139  << "(tx,ty): " << tx << " " << ty << " k= " << k
140  << " normalized_d=" << normalized_d << endl;
141 
142  if (std::abs(pose.x - tx) >= tolerance_dist ||
143  std::abs(pose.y - ty) >= tolerance_dist)
144  skip_this_ptg = true;
145  else
146  num_tests_run++;
147  }
148  }
149  }
150  }
151  EXPECT_TRUE(any_ok) << "PTG: " << sPTGDesc << endl;
152  }
153 
154  // TEST: TP_obstacles
155  {
156  bool skip_this_ptg = false;
157  bool any_change_all = false;
158  for (double ox = -refDist * 0.5;
159  !skip_this_ptg && ox < refDist * 0.5; ox += 0.1)
160  {
161  for (double oy = -refDist * 0.5;
162  !skip_this_ptg && oy < refDist * 0.5; oy += 0.1)
163  {
164  if (std::abs(ox) < 1e-2 && std::abs(oy) < 1e-2)
165  continue; // TP-Space does not include the WS point
166  // (0,0) in its domain
167 
168  std::vector<double> TP_obstacles;
169  ptg->initTPObstacles(TP_obstacles);
170 
171  const std::vector<double> TP_obstacles_org = TP_obstacles;
172  ptg->updateTPObstacle(ox, oy, TP_obstacles);
173 
174  const bool any_change = (TP_obstacles_org != TP_obstacles);
175  if (any_change) any_change_all = true;
176  num_tests_run++;
177  }
178  }
179  EXPECT_TRUE(any_change_all);
180  }
181 
182  printf(
183  "PTG `%50s` run %6u tests.\n", sPTGDesc.c_str(),
184  (unsigned int)num_tests_run);
185 
186  } // for each ptg
187 }
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
double x
X,Y coordinates.
Definition: TPose2D.h:30
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
This class allows loading and storing values and vectors of different types from ".ini" files easily.
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
STL namespace.
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Lightweight 2D pose.
Definition: TPose2D.h:22
TEST(NavTests, PTGs_tests)
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020