Main MRPT website > C++ reference for MRPT 1.5.7
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-2016, 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::utils;
18 using namespace mrpt::math;
19 using namespace mrpt::poses;
20 using namespace std;
21 
23  robot_shape_circular_radius(0.30),
24  ptg_cache_files_directory("."),
25  goalBias(0.05),
26  maxLength(1.0),
27  minDistanceBetweenNewNodes(0.10),
28  minAngBetweenNewNodes(mrpt::utils::DEG2RAD(15)),
29  ptg_verbose(true),
30  save_3d_log_freq(0)
31 {
32  robot_shape.push_back(mrpt::math::TPoint2D(-0.5, -0.5));
33  robot_shape.push_back(mrpt::math::TPoint2D(0.8, -0.4));
34  robot_shape.push_back(mrpt::math::TPoint2D(0.8, 0.4));
35  robot_shape.push_back(mrpt::math::TPoint2D(-0.5, 0.5));
36 }
37 
38 
40  m_initialized_PTG(false)
41 {
42 }
43 
45 {
46  ASSERTMSG_(!m_PTGs.empty(), "No PTG was defined! At least one must be especified.");
47 
48  // Convert to CPolygon for API requisites:
49  mrpt::math::CPolygon poly_robot_shape;
50  poly_robot_shape.clear();
51  if (!params.robot_shape.empty())
52  {
53  vector<double> xm, ym;
54  params.robot_shape.getPlotData(xm, ym);
55  poly_robot_shape.setAllVertices(xm, ym);
56  }
57 
58  for (size_t i = 0; i<m_PTGs.size(); i++)
59  {
60  mrpt::utils::CTimeLoggerEntry tle(m_timelogger, "PTG_initialization");
61 
62  // Polygonal robot shape?
63  {
65  if (diff_ptg) {
66  ASSERTMSG_(!poly_robot_shape.empty(), "No polygonal robot shape specified, and PTG requires one!");
67  diff_ptg->setRobotShape(poly_robot_shape);
68  }
69  }
70  // Circular robot shape?
71  {
73  if (ptg) {
74  ASSERTMSG_(params.robot_shape_circular_radius>0, "No circular robot shape specified, and PTG requires one!");
75  ptg->setRobotShapeRadius(params.robot_shape_circular_radius);
76  }
77  }
78 
79  m_PTGs[i]->initialize(
80  mrpt::format("%s/TPRRT_PTG_%03u.dat.gz", params.ptg_cache_files_directory.c_str(), static_cast<unsigned int>(i)),
81  params.ptg_verbose
82  );
83  }
84 
85  m_initialized_PTG = true;
86 }
87 
89 {
90  // Robot shape:
91  // ==========================
92  // polygonal shape
93  {
94  // Robot shape is a bit special to load:
95  params.robot_shape.clear();
96  const std::string sShape = ini.read_string(sSect, "robot_shape", "");
97  if (!sShape.empty())
98  {
99  CMatrixDouble mShape;
100  if (!mShape.fromMatlabStringFormat(sShape))
101  THROW_EXCEPTION_FMT("Error parsing robot_shape matrix: '%s'", sShape.c_str());
102  ASSERT_(size(mShape, 1) == 2);
103  ASSERT_(size(mShape, 2) >= 3);
104 
105  for (size_t i = 0; i < size(mShape, 2); i++)
106  params.robot_shape.push_back(TPoint2D(mShape(0, i), mShape(1, i)));
107  }
108  }
109  // circular shape
110  params.robot_shape_circular_radius = ini.read_double(sSect, "robot_shape_circular_radius", 0.0);
111 
112  // Load PTG tables:
113  // ==========================
114  m_PTGs.clear();
115 
116  const size_t PTG_COUNT = ini.read_int(sSect, "PTG_COUNT", 0, true); //load the number of PTGs
117  for (unsigned int n = 0; n<PTG_COUNT; n++)
118  {
119  // Generate it:
120  const std::string sPTGName = ini.read_string(sSect, format("PTG%u_Type", n), "", true);
121  m_PTGs.push_back(CParameterizedTrajectoryGeneratorPtr(CParameterizedTrajectoryGenerator::CreatePTG(sPTGName, ini, sSect, format("PTG%u_", n))));
122  }
123 }
124 
125 // Auxiliary function:
127  const mrpt::maps::CPointsMap & in_map,
128  mrpt::maps::CPointsMap & out_map,
129  const mrpt::poses::CPose2D & asSeenFrom,
130  const double MAX_DIST_XY
131 )
132 {
133  size_t nObs;
134  const float *obs_xs, *obs_ys, *obs_zs;
135  in_map.getPointsBuffer(nObs, obs_xs, obs_ys, obs_zs);
136 
137  out_map.clear();
138  out_map.reserve(nObs); // Prealloc mem for speed-up
139 
140  const CPose2D invPose = -asSeenFrom;
141  // We can safely discard the rest of obstacles, since they cannot be converted into TP-Obstacles anyway!
142 
143  for (size_t obs = 0; obs<nObs; obs++)
144  {
145  const double gx = obs_xs[obs], gy = obs_ys[obs];
146 
147  if (std::abs(gx - asSeenFrom.x())>MAX_DIST_XY || std::abs(gy - asSeenFrom.y())>MAX_DIST_XY)
148  continue; // ignore this obstacle: anyway, I don't know how to map it to TP-Obs!
149 
150  double ox, oy;
151  invPose.composePoint(gx, gy, ox, oy);
152 
153  out_map.insertPointFast(ox, oy, 0);
154  }
155 }
156 
157 /*---------------------------------------------------------------
158 SpaceTransformer
159 ---------------------------------------------------------------*/
161  const mrpt::maps::CSimplePointsMap &in_obstacles,
163  const double MAX_DIST,
164  std::vector<double> &out_TPObstacles
165 )
166 {
167  using namespace mrpt::nav;
168  try
169  {
170  // Take "k_rand"s and "distances" such that the collision hits the obstacles
171  // in the "grid" of the given PT
172  // --------------------------------------------------------------------
173  size_t nObs;
174  const float *obs_xs, *obs_ys, *obs_zs;
175  // = in_obstacles.getPointsCount();
176  in_obstacles.getPointsBuffer(nObs, obs_xs, obs_ys, obs_zs);
177 
178  // Init obs ranges:
179  in_PTG->initTPObstacles(out_TPObstacles);
180 
181  for (size_t obs = 0; obs<nObs; obs++)
182  {
183  const float ox = obs_xs[obs];
184  const float oy = obs_ys[obs];
185 
186  if (std::abs(ox)>MAX_DIST || std::abs(oy)>MAX_DIST)
187  continue; // ignore this obstacle: anyway, I don't know how to map it to TP-Obs!
188 
189  in_PTG->updateTPObstacle(ox, oy, out_TPObstacles);
190  }
191 
192  // Leave distances in out_TPObstacles un-normalized ([0,1]), so they just represent real distances in meters.
193  }
194  catch (std::exception &e)
195  {
196  cerr << "[PT_RRT::SpaceTransformer] Exception:" << endl;
197  cerr << e.what() << endl;
198  }
199  catch (...)
200  {
201  cerr << "\n[PT_RRT::SpaceTransformer] Unexpected exception!:\n";
202  cerr << format("*in_PTG = %p\n", (void*)in_PTG);
203  if (in_PTG)
204  cerr << format("PTG = %s\n", in_PTG->getDescription().c_str());
205  cerr << endl;
206  }
207 }
208 
210  const int tp_space_k_direction,
211  const mrpt::maps::CSimplePointsMap &in_obstacles,
213  const double MAX_DIST,
214  double &out_TPObstacle_k
215 )
216 {
217  using namespace mrpt::nav;
218  try
219  {
220  // Take "k_rand"s and "distances" such that the collision hits the obstacles
221  // in the "grid" of the given PT
222  // --------------------------------------------------------------------
223  size_t nObs;
224  const float *obs_xs, *obs_ys, *obs_zs;
225  // = in_obstacles.getPointsCount();
226  in_obstacles.getPointsBuffer(nObs, obs_xs, obs_ys, obs_zs);
227 
228  // Init obs ranges:
229  in_PTG->initTPObstacleSingle(tp_space_k_direction, out_TPObstacle_k);
230 
231  for (size_t obs = 0; obs<nObs; obs++)
232  {
233  const float ox = obs_xs[obs];
234  const float oy = obs_ys[obs];
235 
236  if (std::abs(ox)>MAX_DIST || std::abs(oy)>MAX_DIST)
237  continue; // ignore this obstacle: anyway, I don't know how to map it to TP-Obs!
238 
239  in_PTG->updateTPObstacleSingle(ox, oy, tp_space_k_direction, out_TPObstacle_k);
240  }
241 
242  // Leave distances in out_TPObstacles un-normalized ([0,1]), so they just represent real distances in meters.
243  }
244  catch (std::exception &e)
245  {
246  cerr << "[PT_RRT::SpaceTransformer] Exception:" << endl;
247  cerr << e.what() << endl;
248  }
249  catch (...)
250  {
251  cerr << "\n[PT_RRT::SpaceTransformer] Unexpected exception!:\n";
252  cerr << format("*in_PTG = %p\n", (void*)in_PTG);
253  if (in_PTG)
254  cerr << format("PTG = %s\n", in_PTG->getDescription().c_str());
255  cerr << endl;
256  }
257 }
258 
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:34
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
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.
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.
void internal_loadConfig_PTG(const mrpt::utils::CConfigFileBase &cfgSource, const std::string &sSectionName=std::string("PTG_CONFIG"))
Load all PTG params from a config file source.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
GLenum GLsizei n
Definition: glext.h:4618
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)
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:25
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
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:178
This class allows loading and storing values and vectors of different types from a configuration text...
void initTPObstacleSingle(uint16_t k, double &TP_Obstacle_k) const
#define MAX_DIST(s)
Definition: deflate.h:280
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
This is the base class for any user-defined PTG.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
void setRobotShapeRadius(const double robot_radius)
Robot shape must be set before initialization, either from ctor params or via this method...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
#define DEG2RAD
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
GLsizei const GLchar ** string
Definition: glext.h:3919
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
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.
A safe way to call enter() and leave() of a mrpt::utils::CTimeLogger upon construction and destructio...
Definition: CTimeLogger.h:127
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 NULL if there is no points in the map.
Definition: CPointsMap.cpp:252
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
#define ASSERT_(f)
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:4504
GLsizeiptr size
Definition: glext.h:3779
virtual void insertPointFast(float x, float y, float z=0)=0
The virtual method for insertPoint() without calling mark_as_modified()
Lightweight 2D point.
#define ASSERTMSG_(f, __ERROR_MSG)
GLenum const GLfloat * params
Definition: glext.h:3514
static CParameterizedTrajectoryGenerator * CreatePTG(const std::string &ptgClassName, const mrpt::utils::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...
mrpt::utils::CTimeLogger m_timelogger



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