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  // Use dynamic memory instead of stack-allocated object to prevent stack
121  // overflow on MSVC (!).
122  auto rnav_ptr =
123  std::make_unique<RNAVCLASS>(robot2nav_if, false /*no console output*/);
124  auto& rnav = *rnav_ptr;
125  // Logging:
126  {
127  rnav.enableTimeLog(false);
128 #ifdef _DEBUG
129  rnav.setMinLoggingLevel(mrpt::system::LVL_DEBUG);
130 #else
131  rnav.setMinLoggingLevel(mrpt::system::LVL_ERROR); // quiet
132 #endif
133  const std::string sTmpFil = mrpt::system::getTempFileName();
134  const std::string sTmpDir = mrpt::system::extractFileDirectory(sTmpFil);
135  // printf("[run_rnav_test] navlog dir: `%s`\n", sTmpDir.c_str());
136  rnav.setLogFileDirectory(sTmpDir);
137  rnav.enableLogFile(true);
138  }
139 
140  // Load options:
141  rnav.loadConfigFile(cfg);
142 
143  // And initialize:
144  rnav.initialize();
145 
146  // Nav:
148  np.target.target_coords = mrpt::math::TPose2D(nav_target);
149  np.target.targetAllowedDistance = 0.35f;
150 
151  rnav.navigate(&np);
152 
153  unsigned int MAX_ITERS = 200;
154  for (unsigned int i = 0; i < MAX_ITERS; i++)
155  {
156  // printf("[run_rnav_test] iter: %i robot_pose: %s\n",i,
157  // robot_simul.getCurrentGTPose().asString().c_str());
158  // Run nav:
159  rnav.navigationStep();
160 
161  EXPECT_TRUE(rnav.getCurrentState() != CAbstractNavigator::NAV_ERROR);
162  if (rnav.getCurrentState() == CAbstractNavigator::IDLE) break;
163 
164  robot_simul.simulateOneTimeStep(0.2 /*sec*/);
165  }
166 
167  EXPECT_LT(
168  (TPoint2D(robot_simul.getCurrentGTPose()) - nav_target).norm(), 0.4);
169  EXPECT_TRUE(rnav.getCurrentState() == CAbstractNavigator::IDLE);
170 
171  const_cast<mrpt::system::CTimeLogger&>(rnav.getTimeLogger())
172  .clear(true); // do not show timelog table to console
173  const_cast<mrpt::system::CTimeLogger&>(rnav.getDelaysTimeLogger())
174  .clear(true);
175 }
176 
177 const TPoint2D no_obs_trg(2.0, 0.4), no_obs_topleft(-10, 10),
178  no_obs_bottomright(10, -10);
179 const TPoint2D with_obs_trg(9.0, 4.0), with_obs_topleft(-10, 10),
180  with_obs_bottomright(30, -10), obs_tl(4.0, 2.0), obs_br(5.0, -2.0);
181 
182 TEST(CReactiveNavigationSystem, no_obstacle_nav_VFF)
183 {
184  run_rnav_test<mrpt::nav::CReactiveNavigationSystem>(
185  "reactive2d_config.ini", "CHolonomicVFF", no_obs_trg, no_obs_topleft,
187 }
188 TEST(CReactiveNavigationSystem, no_obstacle_nav_ND)
189 {
190  run_rnav_test<mrpt::nav::CReactiveNavigationSystem>(
191  "reactive2d_config.ini", "CHolonomicND", no_obs_trg, no_obs_topleft,
193 }
194 TEST(CReactiveNavigationSystem, no_obstacle_nav_FullEval)
195 {
196  run_rnav_test<mrpt::nav::CReactiveNavigationSystem>(
197  "reactive2d_config.ini", "CHolonomicFullEval", no_obs_trg,
199 }
200 
201 TEST(CReactiveNavigationSystem3D, no_obstacle_nav_VFF)
202 {
203  run_rnav_test<mrpt::nav::CReactiveNavigationSystem3D>(
204  "reactive3d_config.ini", "CHolonomicVFF", no_obs_trg, no_obs_topleft,
206 }
207 TEST(CReactiveNavigationSystem3D, no_obstacle_nav_ND)
208 {
209  run_rnav_test<mrpt::nav::CReactiveNavigationSystem3D>(
210  "reactive3d_config.ini", "CHolonomicND", no_obs_trg, no_obs_topleft,
212 }
213 TEST(CReactiveNavigationSystem3D, no_obstacle_nav_FullEval)
214 {
215  run_rnav_test<mrpt::nav::CReactiveNavigationSystem3D>(
216  "reactive3d_config.ini", "CHolonomicFullEval", no_obs_trg,
218 }
219 
220 TEST(CReactiveNavigationSystem, with_obstacle_nav_VFF)
221 {
222  run_rnav_test<mrpt::nav::CReactiveNavigationSystem>(
223  "reactive2d_config.ini", "CHolonomicVFF", with_obs_trg,
225 }
226 TEST(CReactiveNavigationSystem, with_obstacle_nav_ND)
227 {
228  run_rnav_test<mrpt::nav::CReactiveNavigationSystem>(
229  "reactive2d_config.ini", "CHolonomicND", with_obs_trg, with_obs_topleft,
231 }
232 TEST(CReactiveNavigationSystem, with_obstacle_nav_FullEval)
233 {
234  run_rnav_test<mrpt::nav::CReactiveNavigationSystem>(
235  "reactive2d_config.ini", "CHolonomicFullEval", with_obs_trg,
237 }
238 
239 TEST(CReactiveNavigationSystem3D, with_obstacle_nav_VFF)
240 {
241  run_rnav_test<mrpt::nav::CReactiveNavigationSystem3D>(
242  "reactive3d_config.ini", "CHolonomicVFF", with_obs_trg,
244 }
245 TEST(CReactiveNavigationSystem3D, with_obstacle_nav_ND)
246 {
247  run_rnav_test<mrpt::nav::CReactiveNavigationSystem3D>(
248  "reactive3d_config.ini", "CHolonomicND", with_obs_trg, with_obs_topleft,
250 }
251 TEST(CReactiveNavigationSystem3D, with_obstacle_nav_FullEval)
252 {
253  run_rnav_test<mrpt::nav::CReactiveNavigationSystem3D>(
254  "reactive3d_config.ini", "CHolonomicFullEval", with_obs_trg,
256 }
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.
EXPECT_LT(out.final_rmse, 3.0)
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
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
const TPoint2D obs_br(5.0, -2.0)
app setMinLoggingLevel(mrpt::system::LVL_ERROR)
void simulateOneTimeStep(const double dt)
Runs the simulator during "dt" seconds.
const TPoint2D with_obs_topleft(-10, 10)
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...
constexpr double DEG2RAD(const double x)
Degrees to radians.
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:273
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:183
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:234



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019