MRPT  1.9.9
PTGs_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
12 #include <mrpt/system/filesystem.h>
13 #include <gtest/gtest.h>
14 
15 // Defined in tests/test_main.cpp
16 namespace mrpt
17 {
19 }
20 
21 TEST(NavTests, PTGs_tests)
22 {
23  using namespace std;
24  using namespace mrpt;
25  using namespace mrpt::nav;
26 
27  const string sFil = mrpt::MRPT_GLOBAL_UNITTEST_SRC_DIR +
28  string("/tests/PTGs_for_tests.ini");
29  if (!mrpt::system::fileExists(sFil))
30  {
31  cerr << "**WARNING* Skipping tests since file cannot be found: '"
32  << sFil << "'\n";
33  return;
34  }
35 
36  mrpt::config::CConfigFile cfg(sFil);
37 
38  const unsigned int PTG_COUNT =
39  cfg.read_int("PTG_UNIT_TESTS", "PTG_COUNT", 0, true);
40  EXPECT_TRUE(PTG_COUNT > 0);
41  vector<CParameterizedTrajectoryGenerator*> PTGs(PTG_COUNT);
42 
43  for (unsigned int n = 0; n < PTG_COUNT; n++)
44  {
45  // Factory:
46  const string sPTGName = cfg.read_string(
47  "PTG_UNIT_TESTS", format("PTG%u_Type", n), "", true);
48  PTGs[n] = CParameterizedTrajectoryGenerator::CreatePTG(
49  sPTGName, cfg, "PTG_UNIT_TESTS", format("PTG%u_", n));
50  EXPECT_TRUE(PTGs[n] != nullptr) << "Failed creating PTG #" << n << endl;
51 
52  try
53  {
54  PTGs[n]->initialize(string(), false /*verbose */);
55  }
56  catch (std::exception& e)
57  {
58  GTEST_FAIL() << "Failed initializing PTG #" << n << endl
59  << e.what() << endl;
60  }
61  }
62 
63  // Run tests:
64  // ---------------------------------------------------------
65  for (unsigned int n = 0; n < PTG_COUNT; n++)
66  {
68 
69  const std::string sPTGDesc = ptg->getDescription();
70  const double refDist = ptg->getRefDistance();
71  const size_t num_paths = ptg->getPathCount();
72  size_t num_tests_run = 0;
73 
74  // TEST: step <-> dist match
75  {
76  for (double dist = 0.1; dist < refDist * 0.5; dist += 0.2)
77  {
78  bool any_good = false;
79  for (size_t k = 0; k < num_paths; k++)
80  {
81  uint32_t step;
82 
83  if (ptg->getPathStepForDist(k, dist, step))
84  {
85  any_good = true;
86  double d = ptg->getPathDist(k, step);
87  EXPECT_NEAR(d, dist, 0.05)
88  << "Test: step <-> dist match\n PTG: " << sPTGDesc
89  << endl
90  << "dist:" << dist << endl;
91  num_tests_run++;
92  }
93  }
94  EXPECT_TRUE(any_good)
95  << "Test: step <-> dist match\n PTG: " << sPTGDesc << endl
96  << "dist:" << dist << endl;
97  }
98  }
99 
100  // TEST: inverseMap_WS2TP
101  {
102  bool any_ok = false;
103  bool skip_this_ptg = false;
104  for (double tx = -refDist * 0.5;
105  !skip_this_ptg && tx < refDist * 0.5; tx += 0.1)
106  {
107  for (double ty = -refDist * 0.5;
108  !skip_this_ptg && ty < refDist * 0.5; ty += 0.1)
109  {
110  if (std::abs(tx) < 1e-2 && std::abs(ty) < 1e-2)
111  continue; // TP-Space does not include the WS point
112  // (0,0) in its domain
113 
114  const double tolerance_dist = std::max(
115  0.10, 10.0 * std::sqrt(tx * tx + ty * ty) * M_PI * 2 /
116  ptg->getPathCount());
117 
118  int k;
119  double normalized_d;
120  bool valid = ptg->inverseMap_WS2TP(tx, ty, k, normalized_d);
121  if (valid && normalized_d < 1.0)
122  {
123  any_ok = true;
124  // Now, do the inverse operation:
125  uint32_t step;
126  bool step_ok = ptg->getPathStepForDist(
127  k, normalized_d * refDist, step);
128  EXPECT_TRUE(step_ok)
129  << "PTG: " << sPTGDesc << endl
130  << "(tx,ty): " << tx << " " << ty << " k= " << k
131  << " normalized_d=" << normalized_d << endl;
132  if (step_ok)
133  {
134  mrpt::math::TPose2D pose;
135  ptg->getPathPose(k, step, pose);
136  EXPECT_NEAR(pose.x, tx, 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  EXPECT_NEAR(pose.y, ty, tolerance_dist)
142  << "Test: inverseMap_WS2TP\n PTG#" << n << ": "
143  << sPTGDesc << endl
144  << "(tx,ty): " << tx << " " << ty << " k= " << k
145  << " normalized_d=" << normalized_d << endl;
146 
147  if (std::abs(pose.x - tx) >= tolerance_dist ||
148  std::abs(pose.y - ty) >= tolerance_dist)
149  skip_this_ptg = true;
150  else
151  num_tests_run++;
152  }
153  }
154  }
155  }
156  EXPECT_TRUE(any_ok) << "PTG: " << sPTGDesc << endl;
157  }
158 
159  // TEST: TP_obstacles
160  {
161  bool skip_this_ptg = false;
162  bool any_change_all = false;
163  for (double ox = -refDist * 0.5;
164  !skip_this_ptg && ox < refDist * 0.5; ox += 0.1)
165  {
166  for (double oy = -refDist * 0.5;
167  !skip_this_ptg && oy < refDist * 0.5; oy += 0.1)
168  {
169  if (std::abs(ox) < 1e-2 && std::abs(oy) < 1e-2)
170  continue; // TP-Space does not include the WS point
171  // (0,0) in its domain
172 
173  std::vector<double> TP_obstacles;
174  ptg->initTPObstacles(TP_obstacles);
175 
176  const std::vector<double> TP_obstacles_org = TP_obstacles;
177  ptg->updateTPObstacle(ox, oy, TP_obstacles);
178 
179  const bool any_change = (TP_obstacles_org != TP_obstacles);
180  if (any_change) any_change_all = true;
181  num_tests_run++;
182  }
183  }
184  EXPECT_TRUE(any_change_all);
185  }
186 
187  printf(
188  "PTG `%50s` run %6u tests.\n", sPTGDesc.c_str(),
189  (unsigned int)num_tests_run);
190 
191  } // for each ptg
192 
193  // Clean up:
194  for (unsigned int n = 0; n < PTG_COUNT; n++) delete PTGs[n];
195 }
virtual bool getPathStepForDist(uint16_t k, double dist, uint32_t &out_step) const =0
Access path k ([0,N-1]=>[-pi,pi] in alpha): largest step count for which the traversed distance is < ...
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.
This class allows loading and storing values and vectors of different types from ".ini" files easily.
GLenum GLsizei n
Definition: glext.h:5074
void initTPObstacles(std::vector< double > &TP_Obstacles) const
Resizes and populates the initial appropriate contents in a vector of tp-obstacles (collision-free ra...
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:127
STL namespace.
uint16_t getPathCount() const
Get the number of different, discrete paths in this family.
virtual double getPathDist(uint16_t k, uint32_t step) const =0
Access path k ([0,N-1]=>[-pi,pi] in alpha): traversed distance at discrete step step.
virtual void getPathPose(uint16_t k, uint32_t step, mrpt::math::TPose2D &p) const =0
Access path k ([0,N-1]=>[-pi,pi] in alpha): pose of the vehicle at discrete step step.
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
This is the base class for any user-defined PTG.
GLbyte ty
Definition: glext.h:6092
GLsizei const GLchar ** string
Definition: glext.h:4101
std::string MRPT_GLOBAL_UNITTEST_SRC_DIR
virtual void updateTPObstacle(double ox, double oy, std::vector< double > &tp_obstacles) const =0
Updates the radial map of closest TP-Obstacles given a single obstacle point at (ox,oy)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Lightweight 2D pose.
GLenum GLsizei GLenum format
Definition: glext.h:3531
TEST(NavTests, PTGs_tests)
unsigned __int32 uint32_t
Definition: rptypes.h:47
virtual bool inverseMap_WS2TP(double x, double y, int &out_k, double &out_normalized_d, double tolerance_dist=0.10) const =0
Computes the closest (alpha,d) TP coordinates of the trajectory point closest to the Workspace (WS) C...
virtual std::string getDescription() const =0
Gets a short textual description of the PTG and its parameters.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020