MRPT  1.9.9
rnav_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-2019, 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>
17 #include <mrpt/system/filesystem.h>
18 
20 
21 template <typename RNAVCLASS>
23  const std::string& sFilename, const std::string& sHoloMethod,
24  const TPoint2D& nav_target, const TPoint2D& world_topleft,
25  const TPoint2D& world_rightbottom,
26  const TPoint2D& block_obstacle_topleft = TPoint2D(0, 0),
27  const TPoint2D& block_obstacle_rightbottom = TPoint2D(0, 0))
28 {
29  using namespace std;
30  using namespace mrpt;
31  using namespace mrpt::nav;
32 
34  std::string("config_files/navigation-ptgs/") +
35  sFilename;
36 
37  if (!mrpt::system::fileExists(sFil))
38  {
39  cerr << "**WARNING* Skipping tests since file cannot be found: '"
40  << sFil << "'\n";
41  return;
42  }
43 
44  mrpt::config::CConfigFile cfg(sFil);
45  cfg.write("CAbstractPTGBasedReactive", "holonomic_method", sHoloMethod);
47 
48  // Create a grid map with a synthetic test environment with a simple
49  // obstacle:
51  grid.setSize(
52  world_topleft.x, world_rightbottom.x, world_rightbottom.y,
53  world_topleft.y, 0.10f /*resolution*/);
54  grid.fill(0.9f);
55 
56  // Create obstacle:
57  {
58  const int xi0 = grid.x2idx(block_obstacle_topleft.x),
59  xi1 = grid.x2idx(block_obstacle_rightbottom.x);
60  const int yi0 = grid.y2idx(block_obstacle_rightbottom.y),
61  yi1 = grid.y2idx(block_obstacle_topleft.y);
62 
63  for (int xi = xi0; xi < xi1; xi++)
64  {
65  for (int yi = yi0; yi < yi1; yi++)
66  {
67  grid.setCell(xi, yi, 0);
68  }
69  }
70  }
71 
72  struct MyDummyRobotIF : public CRobot2NavInterfaceForSimulator_DiffDriven
73  {
75 
76  MyDummyRobotIF(
80  {
81  this->setMinLoggingLevel(
82  mrpt::system::LVL_ERROR); // less verbose output for tests
83  }
84 
85  void sendNavigationStartEvent() override {}
86  void sendNavigationEndEvent() override {}
87  bool senseObstacles(
89  mrpt::system::TTimeStamp& timestamp) override
90  {
91  obstacles.clear();
92  timestamp = mrpt::system::now();
93 
94  mrpt::math::TPose2D curPose, odomPose;
95  std::string pose_frame_id;
96  mrpt::math::TTwist2D curVel;
97  mrpt::system::TTimeStamp pose_tim;
98  getCurrentPoseAndSpeeds(
99  curPose, curVel, pose_tim, odomPose, pose_frame_id);
100 
102  scan.aperture = mrpt::DEG2RAD(270.0);
103  scan.maxRange = 20.0;
104  scan.sensorPose.z(0.4); // height of the lidar (important! it must
105  // intersect with the robot height)
106 
107  m_grid.laserScanSimulator(
108  scan, mrpt::poses::CPose2D(curPose), 0.4f, 180);
109 
111  obstacles.loadFromRangeScan(scan);
112 
113  return true;
114  }
115  };
116 
118  MyDummyRobotIF robot2nav_if(robot_simul, grid);
119 
120  RNAVCLASS rnav(robot2nav_if, false /*no console output*/);
121  // Logging:
122  {
123  rnav.enableTimeLog(false);
124 #ifdef _DEBUG
125  rnav.setMinLoggingLevel(mrpt::system::LVL_DEBUG);
126 #else
127  rnav.setMinLoggingLevel(mrpt::system::LVL_ERROR); // quiet
128 #endif
129  const std::string sTmpFil = mrpt::system::getTempFileName();
130  const std::string sTmpDir = mrpt::system::extractFileDirectory(sTmpFil);
131  // printf("[run_rnav_test] navlog dir: `%s`\n", sTmpDir.c_str());
132  rnav.setLogFileDirectory(sTmpDir);
133  rnav.enableLogFile(true);
134  }
135 
136  // Load options:
137  rnav.loadConfigFile(cfg);
138 
139  // And initialize:
140  rnav.initialize();
141 
142  // Nav:
144  np.target.target_coords = mrpt::math::TPose2D(nav_target);
145  np.target.targetAllowedDistance = 0.35f;
146 
147  rnav.navigate(&np);
148 
149  unsigned int MAX_ITERS = 200;
150  for (unsigned int i = 0; i < MAX_ITERS; i++)
151  {
152  // printf("[run_rnav_test] iter: %i robot_pose: %s\n",i,
153  // robot_simul.getCurrentGTPose().asString().c_str());
154  // Run nav:
155  rnav.navigationStep();
156 
157  EXPECT_TRUE(rnav.getCurrentState() != CAbstractNavigator::NAV_ERROR);
158  if (rnav.getCurrentState() == CAbstractNavigator::IDLE) break;
159 
160  robot_simul.simulateOneTimeStep(0.2 /*sec*/);
161  }
162 
163  EXPECT_LT(
164  (TPoint2D(robot_simul.getCurrentGTPose()) - nav_target).norm(), 0.4);
165  EXPECT_TRUE(rnav.getCurrentState() == CAbstractNavigator::IDLE);
166 
167  const_cast<mrpt::system::CTimeLogger&>(rnav.getTimeLogger())
168  .clear(true); // do not show timelog table to console
169  const_cast<mrpt::system::CTimeLogger&>(rnav.getDelaysTimeLogger())
170  .clear(true);
171 }
172 
173 const TPoint2D no_obs_trg(2.0, 0.4), no_obs_topleft(-10, 10),
174  no_obs_bottomright(10, -10);
175 const TPoint2D with_obs_trg(9.0, 4.0), with_obs_topleft(-10, 10),
176  with_obs_bottomright(30, -10), obs_tl(4.0, 2.0), obs_br(5.0, -2.0);
177 
178 TEST(CReactiveNavigationSystem, no_obstacle_nav_VFF)
179 {
180  run_rnav_test<mrpt::nav::CReactiveNavigationSystem>(
181  "reactive2d_config.ini", "CHolonomicVFF", no_obs_trg, no_obs_topleft,
183 }
184 TEST(CReactiveNavigationSystem, no_obstacle_nav_ND)
185 {
186  run_rnav_test<mrpt::nav::CReactiveNavigationSystem>(
187  "reactive2d_config.ini", "CHolonomicND", no_obs_trg, no_obs_topleft,
189 }
190 TEST(CReactiveNavigationSystem, no_obstacle_nav_FullEval)
191 {
192  run_rnav_test<mrpt::nav::CReactiveNavigationSystem>(
193  "reactive2d_config.ini", "CHolonomicFullEval", no_obs_trg,
195 }
196 
197 TEST(CReactiveNavigationSystem3D, no_obstacle_nav_VFF)
198 {
199  run_rnav_test<mrpt::nav::CReactiveNavigationSystem3D>(
200  "reactive3d_config.ini", "CHolonomicVFF", no_obs_trg, no_obs_topleft,
202 }
203 TEST(CReactiveNavigationSystem3D, no_obstacle_nav_ND)
204 {
205  run_rnav_test<mrpt::nav::CReactiveNavigationSystem3D>(
206  "reactive3d_config.ini", "CHolonomicND", no_obs_trg, no_obs_topleft,
208 }
209 TEST(CReactiveNavigationSystem3D, no_obstacle_nav_FullEval)
210 {
211  run_rnav_test<mrpt::nav::CReactiveNavigationSystem3D>(
212  "reactive3d_config.ini", "CHolonomicFullEval", no_obs_trg,
214 }
215 
216 TEST(CReactiveNavigationSystem, with_obstacle_nav_VFF)
217 {
218  run_rnav_test<mrpt::nav::CReactiveNavigationSystem>(
219  "reactive2d_config.ini", "CHolonomicVFF", with_obs_trg,
221 }
222 TEST(CReactiveNavigationSystem, with_obstacle_nav_ND)
223 {
224  run_rnav_test<mrpt::nav::CReactiveNavigationSystem>(
225  "reactive2d_config.ini", "CHolonomicND", with_obs_trg, with_obs_topleft,
227 }
228 TEST(CReactiveNavigationSystem, with_obstacle_nav_FullEval)
229 {
230  run_rnav_test<mrpt::nav::CReactiveNavigationSystem>(
231  "reactive2d_config.ini", "CHolonomicFullEval", with_obs_trg,
233 }
234 
235 TEST(CReactiveNavigationSystem3D, with_obstacle_nav_VFF)
236 {
237  run_rnav_test<mrpt::nav::CReactiveNavigationSystem3D>(
238  "reactive3d_config.ini", "CHolonomicVFF", with_obs_trg,
240 }
241 TEST(CReactiveNavigationSystem3D, with_obstacle_nav_ND)
242 {
243  run_rnav_test<mrpt::nav::CReactiveNavigationSystem3D>(
244  "reactive3d_config.ini", "CHolonomicND", with_obs_trg, with_obs_topleft,
246 }
247 TEST(CReactiveNavigationSystem3D, with_obstacle_nav_FullEval)
248 {
249  run_rnav_test<mrpt::nav::CReactiveNavigationSystem3D>(
250  "reactive3d_config.ini", "CHolonomicFullEval", with_obs_trg,
252 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:30
void laserScanSimulator(mrpt::obs::CObservation2DRangeScan &inout_Scan, const mrpt::poses::CPose2D &robotPose, float threshold=0.6f, size_t N=361, float noiseStd=0, unsigned int decimation=1, float angleNoiseStd=mrpt::DEG2RAD(.0)) const
Simulates a laser range scan into the current grid map.
const TPoint2D with_obs_bottomright(30, -10)
void setCell(int x, int y, float value)
Change the contents [0,1] of a cell, given its index.
double x
X,Y coordinates.
Definition: TPoint2D.h:23
const TPoint2D obs_br(5.0, -2.0)
void simulateOneTimeStep(const double dt)
Runs the simulator during "dt" seconds.
const TPoint2D with_obs_topleft(-10, 10)
double DEG2RAD(const double x)
Degrees to radians.
This class allows loading and storing values and vectors of different types from ".ini" files easily.
float targetAllowedDistance
(Default=0.5 meters) Allowed distance to target in order to end the navigation.
void run_rnav_test(const std::string &sFilename, const std::string &sHoloMethod, const TPoint2D &nav_target, const TPoint2D &world_topleft, const TPoint2D &world_rightbottom, const TPoint2D &block_obstacle_topleft=TPoint2D(0, 0), const TPoint2D &block_obstacle_rightbottom=TPoint2D(0, 0))
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
STL namespace.
const mrpt::math::TPose2D & getCurrentGTPose() const
Returns the instantaneous, ground truth pose in world coordinates.
const TPoint2D no_obs_trg(2.0, 0.4)
std::string find_mrpt_shared_dir()
Finds the "[MRPT]/share/mrpt/" directory, if available in the system.
Definition: os.cpp:613
float maxRange
The maximum range allowed by the device, in meters (e.g.
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
Definition: TTwist2D.h:19
The struct for configuring navigation requests.
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
std::string getTempFileName()
Returns the name of a proposed temporary file name.
Definition: filesystem.cpp:283
TEST(CReactiveNavigationSystem, no_obstacle_nav_VFF)
const TPoint2D no_obs_topleft(-10, 10)
CRobot2NavInterface implemented for a simulator object based on mrpt::kinematics::CVehicleSimul_DiffD...
void loadFromRangeScan(const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose=nullptr) override
See CPointsMap::loadFromRangeScan()
Simulates the kinematics of a differential-driven planar mobile robot/vehicle, including odometry err...
const TPoint2D with_obs_trg(9.0, 4.0)
GLsizei const GLchar ** string
Definition: glext.h:4116
mrpt::math::TPose2D target_coords
Coordinates of desired target location.
void write(const std::string &section, const std::string &name, enum_t value, const int name_padding_width=-1, const int value_padding_width=-1, const std::string &comment=std::string())
See base class CAbstractPTGBasedReactive for a description and instructions of use.
A class for storing an occupancy grid map.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
const TPoint2D no_obs_bottomright(10, -10)
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
Lightweight 2D pose.
Definition: TPose2D.h:22
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
void fill(float default_value=0.5f)
Fills all the cells with a default value.
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
Definition: CPointsMap.h:271
void discardSavingChanges()
Discard saving (current) changes to physical file upon destruction.
Definition: CConfigFile.cpp:82
const TPoint2D obs_tl(4.0, 2.0)
See base class CAbstractPTGBasedReactive for a description and instructions of use.
void setSize(float x_min, float x_max, float y_min, float y_max, float resolution, float default_value=0.5f)
Change the size of gridmap, erasing all its previous contents.
Lightweight 2D point.
Definition: TPoint2D.h:31
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:182
int x2idx(float x) const
Transform a coordinate value into a cell index.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
std::string extractFileDirectory(const std::string &filePath)
Extract the whole path (the directory) of a filename from a complete path plus name plus extension...
Definition: filesystem.cpp:78
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
Definition: CPointsMap.h:232



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019