Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 
11 #include <mrpt/utils/CConfigFile.h>
12 #include <mrpt/system/filesystem.h>
13 #include <gtest/gtest.h>
14 
15 // Defined in tests/test_main.cpp
16 namespace mrpt { namespace utils {
18  }
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::utils::MRPT_GLOBAL_UNITTEST_SRC_DIR + string("/tests/PTGs_for_tests.ini");
28  if (!mrpt::system::fileExists(sFil))
29  {
30  cerr << "**WARNING* Skipping tests since file cannot be found: '" << sFil << "'\n";
31  return;
32  }
33 
34  mrpt::utils::CConfigFile cfg(sFil);
35 
36  const unsigned int PTG_COUNT = cfg.read_int("PTG_UNIT_TESTS","PTG_COUNT",0, true );
37  EXPECT_TRUE(PTG_COUNT>0);
38  vector<CParameterizedTrajectoryGenerator *> PTGs(PTG_COUNT);
39 
40  for ( unsigned int n=0;n<PTG_COUNT;n++)
41  {
42  // Factory:
43  const string sPTGName = cfg.read_string("PTG_UNIT_TESTS",format("PTG%u_Type", n ),"", true );
44  PTGs[n] = CParameterizedTrajectoryGenerator::CreatePTG(sPTGName,cfg,"PTG_UNIT_TESTS", format("PTG%u_",n) );
45  EXPECT_TRUE(PTGs[n]!=NULL) << "Failed creating PTG #" << n << endl;
46 
47  try
48  {
49  PTGs[n]->initialize( string(), false /*verbose */ );
50  } catch (std::exception &e)
51  {
52  GTEST_FAIL() << "Failed initializing PTG #" << n << endl << e.what() << endl;
53  }
54  }
55 
56  // Run tests:
57  // ---------------------------------------------------------
58  for ( unsigned int n=0;n<PTG_COUNT;n++)
59  {
61 
62  const std::string sPTGDesc = ptg->getDescription();
63  const double refDist = ptg->getRefDistance();
64  const size_t num_paths = ptg->getPathCount();
65  size_t num_tests_run = 0;
66 
67  // TEST: step <-> dist match
68  {
69  for (double dist=0.1;dist<refDist*0.5;dist+=0.2)
70  {
71  bool any_good = false;
72  for (size_t k=0;k<num_paths;k++)
73  {
74  uint32_t step;
75 
76  if (ptg->getPathStepForDist(k,dist,step))
77  {
78  any_good = true;
79  double d = ptg->getPathDist(k,step);
80  EXPECT_NEAR(d,dist, 0.05) << "Test: step <-> dist match\n PTG: " << sPTGDesc << endl << "dist:" << dist << endl;
81  num_tests_run++;
82  }
83  }
84  EXPECT_TRUE(any_good) << "Test: step <-> dist match\n PTG: " << sPTGDesc << endl << "dist:" << dist << endl;
85  }
86  }
87 
88  // TEST: inverseMap_WS2TP
89  {
90  bool any_ok = false;
91  bool skip_this_ptg = false;
92  for (double tx=-refDist*0.5;!skip_this_ptg && tx<refDist*0.5;tx+=0.1)
93  {
94  for (double ty=-refDist*0.5;!skip_this_ptg && ty<refDist*0.5;ty+=0.1)
95  {
96  if (std::abs(tx)<1e-2 && std::abs(ty)<1e-2)
97  continue; // TP-Space does not include the WS point (0,0) in its domain
98 
99  const double tolerance_dist = std::max(0.10, 10.0 * std::sqrt( tx*tx+ty*ty ) * M_PI *2 / ptg->getPathCount() );
100 
101  int k;
102  double normalized_d;
103  bool valid= ptg->inverseMap_WS2TP(tx,ty,k,normalized_d);
104  if (valid && normalized_d<1.0)
105  {
106  any_ok = true;
107  // Now, do the inverse operation:
108  uint32_t step;
109  bool step_ok = ptg->getPathStepForDist(k,normalized_d*refDist,step);
110  EXPECT_TRUE(step_ok) << "PTG: " << sPTGDesc << endl << "(tx,ty): " << tx << " " << ty << " k= " << k << " normalized_d=" << normalized_d << endl;
111  if (step_ok)
112  {
113  mrpt::math::TPose2D pose;
114  ptg->getPathPose(k,step,pose);
115  EXPECT_NEAR(pose.x, tx, tolerance_dist) << "Test: inverseMap_WS2TP\n PTG#"<<n<< ": " << sPTGDesc << endl << "(tx,ty): " << tx << " " << ty << " k= " << k << " normalized_d=" << normalized_d << endl;
116  EXPECT_NEAR(pose.y, ty, tolerance_dist) << "Test: inverseMap_WS2TP\n PTG#"<<n<< ": " << sPTGDesc << endl << "(tx,ty): " << tx << " " << ty << " k= " << k << " normalized_d=" << normalized_d << endl;
117 
118  if (std::abs(pose.x-tx)>=tolerance_dist ||std::abs(pose.y-ty)>=tolerance_dist)
119  skip_this_ptg = true;
120  else num_tests_run++;
121  }
122  }
123  }
124  }
125  EXPECT_TRUE(any_ok) << "PTG: " << sPTGDesc << endl;
126  }
127 
128  // TEST: TP_obstacles
129  {
130  bool skip_this_ptg = false;
131  bool any_change_all = false;
132  for (double ox=-refDist*0.5;!skip_this_ptg && ox<refDist*0.5;ox+=0.1)
133  {
134  for (double oy=-refDist*0.5;!skip_this_ptg && oy<refDist*0.5;oy+=0.1)
135  {
136  if (std::abs(ox)<1e-2 && std::abs(oy)<1e-2)
137  continue; // TP-Space does not include the WS point (0,0) in its domain
138 
139  std::vector<double> TP_obstacles;
140  ptg->initTPObstacles(TP_obstacles);
141 
142  const std::vector<double> TP_obstacles_org = TP_obstacles;
143  ptg->updateTPObstacle(ox,oy, TP_obstacles);
144 
145  const bool any_change = (TP_obstacles_org!=TP_obstacles);
146  if (any_change) any_change_all=true;
147  num_tests_run++;
148  }
149  }
150  EXPECT_TRUE(any_change_all);
151  }
152 
153 
154  printf("PTG `%50s` run %6u tests.\n", sPTGDesc.c_str(), (unsigned int)num_tests_run );
155 
156  } // for each ptg
157 
158  // Clean up:
159  for ( unsigned int n=0;n<PTG_COUNT;n++)
160  delete PTGs[n];
161 
162 
163 }
164 
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 MRPT_GLOBAL_UNITTEST_SRC_DIR
This class allows loading and storing values and vectors of different types from ".ini" files easily.
Definition: CConfigFile.h:30
GLenum GLsizei n
Definition: glext.h:4618
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 BASE_IMPEXP fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:124
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
STL namespace.
#define M_PI
Definition: bits.h:78
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:5441
GLsizei const GLchar ** string
Definition: glext.h:3919
double y
X,Y coordinates.
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:3513
TEST(NavTests, PTGs_tests)
unsigned __int32 uint32_t
Definition: rptypes.h:49
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.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019