Main MRPT website > C++ reference for MRPT 1.9.9
PlannerRRT_common.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 
10 #include "nav-precomp.h" // Precomp header
11 
14 #include <mrpt/math/CPolygon.h>
15 
16 using namespace mrpt::nav;
17 using namespace mrpt::math;
18 using namespace mrpt::poses;
19 using namespace std;
20 
22  : robot_shape_circular_radius(0.30),
23  ptg_cache_files_directory("."),
24  goalBias(0.05),
25  maxLength(1.0),
26  minDistanceBetweenNewNodes(0.10),
27  minAngBetweenNewNodes(mrpt::DEG2RAD(15)),
28  ptg_verbose(true),
29  save_3d_log_freq(0)
30 {
31  robot_shape.push_back(mrpt::math::TPoint2D(-0.5, -0.5));
32  robot_shape.push_back(mrpt::math::TPoint2D(0.8, -0.4));
33  robot_shape.push_back(mrpt::math::TPoint2D(0.8, 0.4));
34  robot_shape.push_back(mrpt::math::TPoint2D(-0.5, 0.5));
35 }
36 
37 PlannerTPS_VirtualBase::PlannerTPS_VirtualBase() : m_initialized_PTG(false) {}
39 {
40  ASSERTMSG_(
41  !m_PTGs.empty(),
42  "No PTG was defined! At least one must be especified.");
43 
44  // Convert to CPolygon for API requisites:
45  mrpt::math::CPolygon poly_robot_shape;
46  poly_robot_shape.clear();
47  if (!params.robot_shape.empty())
48  {
49  vector<double> xm, ym;
50  params.robot_shape.getPlotData(xm, ym);
51  poly_robot_shape.setAllVertices(xm, ym);
52  }
53 
54  for (size_t i = 0; i < m_PTGs.size(); i++)
55  {
56  mrpt::system::CTimeLoggerEntry tle(m_timelogger, "PTG_initialization");
57 
58  // Polygonal robot shape?
59  {
62  m_PTGs[i].get());
63  if (diff_ptg)
64  {
65  ASSERTMSG_(
66  !poly_robot_shape.empty(),
67  "No polygonal robot shape specified, and PTG requires "
68  "one!");
69  diff_ptg->setRobotShape(poly_robot_shape);
70  }
71  }
72  // Circular robot shape?
73  {
76  m_PTGs[i].get());
77  if (ptg)
78  {
79  ASSERTMSG_(
80  params.robot_shape_circular_radius > 0,
81  "No circular robot shape specified, and PTG requires one!");
82  ptg->setRobotShapeRadius(params.robot_shape_circular_radius);
83  }
84  }
85 
86  m_PTGs[i]->initialize(
88  "%s/TPRRT_PTG_%03u.dat.gz",
89  params.ptg_cache_files_directory.c_str(),
90  static_cast<unsigned int>(i)),
91  params.ptg_verbose);
92  }
93 
94  m_initialized_PTG = true;
95 }
96 
98  const mrpt::config::CConfigFileBase& ini, const std::string& sSect)
99 {
100  // Robot shape:
101  // ==========================
102  // polygonal shape
103  {
104  // Robot shape is a bit special to load:
105  params.robot_shape.clear();
106  const std::string sShape = ini.read_string(sSect, "robot_shape", "");
107  if (!sShape.empty())
108  {
109  CMatrixDouble mShape;
110  if (!mShape.fromMatlabStringFormat(sShape))
112  "Error parsing robot_shape matrix: '%s'", sShape.c_str());
113  ASSERT_(mShape.rows() == 2);
114  ASSERT_(mShape.cols() >= 3);
115 
116  for (int i = 0; i < mShape.cols(); i++)
117  params.robot_shape.push_back(
118  TPoint2D(mShape(0, i), mShape(1, i)));
119  }
120  }
121  // circular shape
122  params.robot_shape_circular_radius =
123  ini.read_double(sSect, "robot_shape_circular_radius", 0.0);
124 
125  // Load PTG tables:
126  // ==========================
127  m_PTGs.clear();
128 
129  const size_t PTG_COUNT =
130  ini.read_int(sSect, "PTG_COUNT", 0, true); // load the number of PTGs
131  for (unsigned int n = 0; n < PTG_COUNT; n++)
132  {
133  // Generate it:
134  const std::string sPTGName =
135  ini.read_string(sSect, format("PTG%u_Type", n), "", true);
136  m_PTGs.push_back(
139  sPTGName, ini, sSect, format("PTG%u_", n))));
140  }
141 }
142 
143 // Auxiliary function:
145  const mrpt::maps::CPointsMap& in_map, mrpt::maps::CPointsMap& out_map,
146  const mrpt::poses::CPose2D& asSeenFrom, const double MAX_DIST_XY)
147 {
148  size_t nObs;
149  const float *obs_xs, *obs_ys, *obs_zs;
150  in_map.getPointsBuffer(nObs, obs_xs, obs_ys, obs_zs);
151 
152  out_map.clear();
153  out_map.reserve(nObs); // Prealloc mem for speed-up
154 
155  const CPose2D invPose = -asSeenFrom;
156  // We can safely discard the rest of obstacles, since they cannot be
157  // converted into TP-Obstacles anyway!
158 
159  for (size_t obs = 0; obs < nObs; obs++)
160  {
161  const double gx = obs_xs[obs], gy = obs_ys[obs];
162 
163  if (std::abs(gx - asSeenFrom.x()) > MAX_DIST_XY ||
164  std::abs(gy - asSeenFrom.y()) > MAX_DIST_XY)
165  continue; // ignore this obstacle: anyway, I don't know how to map
166  // it to TP-Obs!
167 
168  double ox, oy;
169  invPose.composePoint(gx, gy, ox, oy);
170 
171  out_map.insertPointFast(ox, oy, 0);
172  }
173 }
174 
175 /*---------------------------------------------------------------
176 SpaceTransformer
177 ---------------------------------------------------------------*/
179  const mrpt::maps::CSimplePointsMap& in_obstacles,
181  const double MAX_DIST, std::vector<double>& out_TPObstacles)
182 {
183  using namespace mrpt::nav;
184  try
185  {
186  // Take "k_rand"s and "distances" such that the collision hits the
187  // obstacles
188  // in the "grid" of the given PT
189  // --------------------------------------------------------------------
190  size_t nObs;
191  const float *obs_xs, *obs_ys, *obs_zs;
192  // = in_obstacles.getPointsCount();
193  in_obstacles.getPointsBuffer(nObs, obs_xs, obs_ys, obs_zs);
194 
195  // Init obs ranges:
196  in_PTG->initTPObstacles(out_TPObstacles);
197 
198  for (size_t obs = 0; obs < nObs; obs++)
199  {
200  const float ox = obs_xs[obs];
201  const float oy = obs_ys[obs];
202 
203  if (std::abs(ox) > MAX_DIST || std::abs(oy) > MAX_DIST)
204  continue; // ignore this obstacle: anyway, I don't know how to
205  // map it to TP-Obs!
206 
207  in_PTG->updateTPObstacle(ox, oy, out_TPObstacles);
208  }
209 
210  // Leave distances in out_TPObstacles un-normalized ([0,1]), so they
211  // just represent real distances in meters.
212  }
213  catch (std::exception& e)
214  {
215  cerr << "[PT_RRT::SpaceTransformer] Exception:" << endl;
216  cerr << e.what() << endl;
217  }
218  catch (...)
219  {
220  cerr << "\n[PT_RRT::SpaceTransformer] Unexpected exception!:\n";
221  cerr << format("*in_PTG = %p\n", (void*)in_PTG);
222  if (in_PTG)
223  cerr << format("PTG = %s\n", in_PTG->getDescription().c_str());
224  cerr << endl;
225  }
226 }
227 
229  const int tp_space_k_direction,
230  const mrpt::maps::CSimplePointsMap& in_obstacles,
232  const double MAX_DIST, double& out_TPObstacle_k)
233 {
234  using namespace mrpt::nav;
235  try
236  {
237  // Take "k_rand"s and "distances" such that the collision hits the
238  // obstacles
239  // in the "grid" of the given PT
240  // --------------------------------------------------------------------
241  size_t nObs;
242  const float *obs_xs, *obs_ys, *obs_zs;
243  // = in_obstacles.getPointsCount();
244  in_obstacles.getPointsBuffer(nObs, obs_xs, obs_ys, obs_zs);
245 
246  // Init obs ranges:
247  in_PTG->initTPObstacleSingle(tp_space_k_direction, out_TPObstacle_k);
248 
249  for (size_t obs = 0; obs < nObs; obs++)
250  {
251  const float ox = obs_xs[obs];
252  const float oy = obs_ys[obs];
253 
254  if (std::abs(ox) > MAX_DIST || std::abs(oy) > MAX_DIST)
255  continue; // ignore this obstacle: anyway, I don't know how to
256  // map it to TP-Obs!
257 
258  in_PTG->updateTPObstacleSingle(
259  ox, oy, tp_space_k_direction, out_TPObstacle_k);
260  }
261 
262  // Leave distances in out_TPObstacles un-normalized ([0,1]), so they
263  // just represent real distances in meters.
264  }
265  catch (std::exception& e)
266  {
267  cerr << "[PT_RRT::SpaceTransformer] Exception:" << endl;
268  cerr << e.what() << endl;
269  }
270  catch (...)
271  {
272  cerr << "\n[PT_RRT::SpaceTransformer] Unexpected exception!:\n";
273  cerr << format("*in_PTG = %p\n", (void*)in_PTG);
274  if (in_PTG)
275  cerr << format("PTG = %s\n", in_PTG->getDescription().c_str());
276  cerr << endl;
277  }
278 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:31
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
virtual void updateTPObstacleSingle(double ox, double oy, uint16_t k, double &tp_obstacle_k) const =0
Like updateTPObstacle() but for one direction only (k) in TP-Space.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
void internal_loadConfig_PTG(const mrpt::config::CConfigFileBase &cfgSource, const std::string &sSectionName=std::string("PTG_CONFIG"))
Load all PTG params from a config file source.
A safe way to call enter() and leave() of a mrpt::system::CTimeLogger upon construction and destructi...
void getPlotData(std::vector< double > &x, std::vector< double > &y) const
Gets plot data, ready to use on a 2D plot.
Base class for all PTGs using a 2D circular robot shape model.
mrpt::system::CTimeLogger m_timelogger
double DEG2RAD(const double x)
Degrees to radians.
GLenum GLsizei n
Definition: glext.h:5074
void spaceTransformerOneDirectionOnly(const int tp_space_k_direction, const mrpt::maps::CSimplePointsMap &in_obstacles, const mrpt::nav::CParameterizedTrajectoryGenerator *in_PTG, const double MAX_DIST, double &out_TPObstacle_k)
static CParameterizedTrajectoryGenerator * CreatePTG(const std::string &ptgClassName, const mrpt::config::CConfigFileBase &cfg, const std::string &sSection, const std::string &sKeyPrefix)
The class factory for creating a PTG from a list of parameters in a section of a given config file (p...
void initTPObstacles(std::vector< double > &TP_Obstacles) const
Resizes and populates the initial appropriate contents in a vector of tp-obstacles (collision-free ra...
void spaceTransformer(const mrpt::maps::CSimplePointsMap &in_obstacles, const mrpt::nav::CParameterizedTrajectoryGenerator *in_PTG, const double MAX_DIST, std::vector< double > &out_TPObstacles)
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:21
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
STL namespace.
virtual void reserve(size_t newLength)=0
Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
void composePoint(double lx, double ly, double &gx, double &gy) const
An alternative, slightly more efficient way of doing with G and L being 2D points and P this 2D pose...
Definition: CPose2D.cpp:175
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
void initTPObstacleSingle(uint16_t k, double &TP_Obstacle_k) const
#define MAX_DIST(s)
Definition: deflate.h:285
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:64
This is the base class for any user-defined PTG.
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
void setRobotShapeRadius(const double robot_radius)
Robot shape must be set before initialization, either from ctor params or via this method...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void internal_initialize_PTG()
Must be called after setting all params (see internal_loadConfig_PTG()) and before calling solve() ...
void setRobotShape(const mrpt::math::CPolygon &robotShape)
Robot shape must be set before initialization, either from ctor params or via this method...
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.
static void transformPointcloudWithSquareClipping(const mrpt::maps::CPointsMap &in_map, mrpt::maps::CPointsMap &out_map, const mrpt::poses::CPose2D &asSeenFrom, const double MAX_DIST_XY)
void getPointsBuffer(size_t &outPointsCount, const float *&xs, const float *&ys, const float *&zs) const
Provides a direct access to points buffer, or nullptr if there is no points in the map...
Definition: CPointsMap.cpp:245
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
Base class for all PTGs suitable to non-holonomic, differentially-driven (or Ackermann) vehicles base...
mrpt::math::TPolygon2D robot_shape
The robot shape used when computing collisions; it&#39;s loaded from the config file/text as a single 2xN...
GLsizei maxLength
Definition: glext.h:4932
virtual void insertPointFast(float x, float y, float z=0)=0
The virtual method for insertPoint() without calling mark_as_modified()
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
Lightweight 2D point.
GLenum const GLfloat * params
Definition: glext.h:3534



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019