Main MRPT website > C++ reference for MRPT 1.5.7
rnav_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 
15 #include <mrpt/utils/CConfigFile.h>
16 #include <mrpt/system/filesystem.h>
17 #include <gtest/gtest.h>
18 
20 
21 template <typename RNAVCLASS>
23  const std::string &sFilename,
24  const std::string &sHoloMethod,
25  const TPoint2D &nav_target,
26  const TPoint2D &world_topleft,
27  const TPoint2D &world_rightbottom,
28  const TPoint2D &block_obstacle_topleft = TPoint2D(0,0),
29  const TPoint2D &block_obstacle_rightbottom = TPoint2D(0,0)
30  )
31 {
32  using namespace std;
33  using namespace mrpt;
34  using namespace mrpt::nav;
35 
36  const std::string sFil = mrpt::system::find_mrpt_shared_dir() + std::string("config_files/navigation-ptgs/") + sFilename;
37 
38  if (!mrpt::system::fileExists(sFil))
39  {
40  cerr << "**WARNING* Skipping tests since file cannot be found: '" << sFil << "'\n";
41  return;
42  }
43 
44  mrpt::utils::CConfigFile cfg(sFil);
45  cfg.write("CAbstractPTGBasedReactive", "holonomic_method", sHoloMethod);
47 
48  // Create a grid map with a synthetic test environment with a simple obstacle:
50  grid.setSize(world_topleft.x, world_rightbottom.x, world_rightbottom.y, world_topleft.y, 0.10f /*resolution*/);
51  grid.fill(0.9f);
52 
53  // Create obstacle:
54  {
55  const int xi0 = grid.x2idx(block_obstacle_topleft.x), xi1 = grid.x2idx(block_obstacle_rightbottom.x);
56  const int yi0 = grid.y2idx(block_obstacle_rightbottom.y), yi1 = grid.y2idx(block_obstacle_topleft.y);
57 
58  for (int xi = xi0; xi < xi1; xi++) {
59  for (int yi = yi0; yi < yi1; yi++) {
60  grid.setCell(xi, yi, 0);
61  }
62  }
63  }
64 
65  struct MyDummyRobotIF : public CRobot2NavInterfaceForSimulator_DiffDriven
66  {
68 
71  m_grid(grid)
72  {
73  this->setMinLoggingLevel(mrpt::utils::LVL_ERROR); // less verbose output for tests
74  }
75 
76  void sendNavigationStartEvent () MRPT_OVERRIDE { }
77  void sendNavigationEndEvent() MRPT_OVERRIDE { }
78 
79  bool senseObstacles(mrpt::maps::CSimplePointsMap &obstacles, mrpt::system::TTimeStamp &timestamp) MRPT_OVERRIDE
80  {
81  obstacles.clear();
82  timestamp = mrpt::system::now();
83 
84  mrpt::math::TPose2D curPose, odomPose;
85  std::string pose_frame_id;
86  mrpt::math::TTwist2D curVel;
87  mrpt::system::TTimeStamp pose_tim;
88  getCurrentPoseAndSpeeds(curPose, curVel, pose_tim, odomPose, pose_frame_id);
89 
91  scan.aperture = mrpt::utils::DEG2RAD(270.0);
92  scan.maxRange = 20.0;
93  scan.sensorPose.z(0.4); // height of the lidar (important! it must intersect with the robot height)
94 
95  m_grid.laserScanSimulator(scan, mrpt::poses::CPose2D(curPose), 0.4f, 180);
96 
98  obstacles.loadFromRangeScan(scan);
99 
100  return true;
101  }
102  };
103 
105  MyDummyRobotIF robot2nav_if(robot_simul, grid);
106 
107  RNAVCLASS rnav(robot2nav_if, false /*no console output*/);
108  // Logging:
109  {
110  rnav.enableTimeLog(false);
111 #ifdef _DEBUG
112  rnav.setMinLoggingLevel(mrpt::utils::LVL_DEBUG);
113 #else
114  rnav.setMinLoggingLevel(mrpt::utils::LVL_ERROR); // quiet
115 #endif
116  const std::string sTmpFil = mrpt::system::getTempFileName();
117  const std::string sTmpDir = mrpt::system::extractFileDirectory(sTmpFil);
118  //printf("[run_rnav_test] navlog dir: `%s`\n", sTmpDir.c_str());
119  rnav.setLogFileDirectory(sTmpDir);
120  rnav.enableLogFile(true);
121  }
122 
123  // Load options:
124  rnav.loadConfigFile(cfg);
125 
126  // And initialize:
127  rnav.initialize();
128 
129  // Nav:
131  np.target.target_coords = mrpt::math::TPose2D(nav_target);
132  np.target.targetAllowedDistance = 0.35f;
133 
134  rnav.navigate(&np);
135 
136  unsigned int MAX_ITERS = 200;
137  for (unsigned int i = 0; i < MAX_ITERS; i++)
138  {
139  //printf("[run_rnav_test] iter: %i robot_pose: %s\n",i, robot_simul.getCurrentGTPose().asString().c_str());
140  // Run nav:
141  rnav.navigationStep();
142 
143  EXPECT_TRUE(rnav.getCurrentState() != CAbstractNavigator::NAV_ERROR);
144  if (rnav.getCurrentState() == CAbstractNavigator::IDLE)
145  break;
146 
147  robot_simul.simulateOneTimeStep(0.2 /*sec*/);
148  }
149 
150  EXPECT_LT((TPoint2D(robot_simul.getCurrentGTPose()) - nav_target).norm(), 0.4);
151  EXPECT_TRUE(rnav.getCurrentState() == CAbstractNavigator::IDLE);
152 
153  const_cast<mrpt::utils::CTimeLogger&>(rnav.getTimeLogger()).clear(true); // do not show timelog table to console
154  const_cast<mrpt::utils::CTimeLogger&>(rnav.getDelaysTimeLogger()).clear(true);
155 }
156 
157 const TPoint2D no_obs_trg(2.0, 0.4), no_obs_topleft(-10, 10), no_obs_bottomright(10, -10);
158 const TPoint2D with_obs_trg(9.0, 4.0), with_obs_topleft(-10, 10), with_obs_bottomright(30, -10), obs_tl(4.0,2.0), obs_br(5.0,-2.0);
159 
160 TEST(CReactiveNavigationSystem, no_obstacle_nav_VFF) {
161  run_rnav_test<mrpt::nav::CReactiveNavigationSystem>("reactive2d_config.ini", "CHolonomicVFF", no_obs_trg, no_obs_topleft, no_obs_bottomright );
162 }
163 TEST(CReactiveNavigationSystem, no_obstacle_nav_ND) {
164  run_rnav_test<mrpt::nav::CReactiveNavigationSystem>("reactive2d_config.ini", "CHolonomicND", no_obs_trg, no_obs_topleft, no_obs_bottomright);
165 }
166 TEST(CReactiveNavigationSystem, no_obstacle_nav_FullEval) {
167  run_rnav_test<mrpt::nav::CReactiveNavigationSystem>("reactive2d_config.ini", "CHolonomicFullEval", no_obs_trg, no_obs_topleft, no_obs_bottomright);
168 }
169 
170 TEST(CReactiveNavigationSystem3D, no_obstacle_nav_VFF) {
171  run_rnav_test<mrpt::nav::CReactiveNavigationSystem3D>("reactive3d_config.ini", "CHolonomicVFF", no_obs_trg, no_obs_topleft, no_obs_bottomright);
172 }
173 TEST(CReactiveNavigationSystem3D, no_obstacle_nav_ND) {
174  run_rnav_test<mrpt::nav::CReactiveNavigationSystem3D>("reactive3d_config.ini", "CHolonomicND", no_obs_trg, no_obs_topleft, no_obs_bottomright);
175 }
176 TEST(CReactiveNavigationSystem3D, no_obstacle_nav_FullEval) {
177  run_rnav_test<mrpt::nav::CReactiveNavigationSystem3D>("reactive3d_config.ini", "CHolonomicFullEval", no_obs_trg, no_obs_topleft, no_obs_bottomright);
178 }
179 
180 TEST(CReactiveNavigationSystem, with_obstacle_nav_VFF) {
181  run_rnav_test<mrpt::nav::CReactiveNavigationSystem>("reactive2d_config.ini", "CHolonomicVFF", with_obs_trg, with_obs_topleft, with_obs_bottomright, obs_tl, obs_br);
182 }
183 TEST(CReactiveNavigationSystem, with_obstacle_nav_ND) {
184  run_rnav_test<mrpt::nav::CReactiveNavigationSystem>("reactive2d_config.ini", "CHolonomicND", with_obs_trg, with_obs_topleft, with_obs_bottomright, obs_tl, obs_br);
185 }
186 TEST(CReactiveNavigationSystem, with_obstacle_nav_FullEval) {
187  run_rnav_test<mrpt::nav::CReactiveNavigationSystem>("reactive2d_config.ini", "CHolonomicFullEval", with_obs_trg, with_obs_topleft, with_obs_bottomright, obs_tl, obs_br);
188 }
189 
190 TEST(CReactiveNavigationSystem3D, with_obstacle_nav_VFF) {
191  run_rnav_test<mrpt::nav::CReactiveNavigationSystem3D>("reactive3d_config.ini", "CHolonomicVFF", with_obs_trg, with_obs_topleft, with_obs_bottomright, obs_tl, obs_br);
192 }
193 TEST(CReactiveNavigationSystem3D, with_obstacle_nav_ND) {
194  run_rnav_test<mrpt::nav::CReactiveNavigationSystem3D>("reactive3d_config.ini", "CHolonomicND", with_obs_trg, with_obs_topleft, with_obs_bottomright, obs_tl, obs_br);
195 }
196 TEST(CReactiveNavigationSystem3D, with_obstacle_nav_FullEval) {
197  run_rnav_test<mrpt::nav::CReactiveNavigationSystem3D>("reactive3d_config.ini", "CHolonomicFullEval", with_obs_trg, with_obs_topleft, with_obs_bottomright, obs_tl, obs_br);
198 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:34
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
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 y
X,Y coordinates.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
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.
Definition: CConfigFile.h:30
void discardSavingChanges()
Discard saving (current) changes to physical file upon destruction.
Definition: CConfigFile.cpp:79
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 BASE_IMPEXP fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:124
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
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::utils::DEG2RAD(0)) const
Simulates a laser range scan into the current grid map.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
STL namespace.
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
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 BASE_IMPEXP find_mrpt_shared_dir()
Finds the "[MRPT]/share/mrpt/" directory, if available in the system.
Definition: os.cpp:557
float maxRange
The maximum range allowed by the device, in meters (e.g. 80m, 50m,...)
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
The struct for configuring navigation requests.
std::string BASE_IMPEXP getTempFileName()
Returns the name of a proposed temporary file name.
Definition: filesystem.cpp:273
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...
Simulates the kinematics of a differential-driven planar mobile robot/vehicle, including odometry err...
const TPoint2D with_obs_trg(9.0, 4.0)
double DEG2RAD(const double x)
Degrees to radians.
Definition: bits.h:82
GLsizei const GLchar ** string
Definition: glext.h:3919
mrpt::math::TPose2D target_coords
Coordinates of desired target location. Heading may be ignored by some reactive implementations.
const TPoint2D obs_br(5.0,-2.0)
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...
virtual void loadFromRangeScan(const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose=NULL) MRPT_OVERRIDE
See CPointsMap::loadFromRangeScan()
const TPoint2D no_obs_bottomright(10, -10)
void write(const std::string &section, const std::string &name, const data_t &value, const int name_padding_width=-1, const int value_padding_width=-1, const std::string &comment=std::string())
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
Lightweight 2D pose.
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
Definition: CTimeLogger.h:41
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.
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.
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 BASE_IMPEXP 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:77
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...



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