MRPT  1.9.9
PlannerRRT_common.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 "nav-precomp.h" // Precomp header
11 
12 #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  : ptg_cache_files_directory("."),
23 
24  minAngBetweenNewNodes(mrpt::DEG2RAD(15))
25 
26 {
27  robot_shape.push_back(mrpt::math::TPoint2D(-0.5, -0.5));
28  robot_shape.push_back(mrpt::math::TPoint2D(0.8, -0.4));
29  robot_shape.push_back(mrpt::math::TPoint2D(0.8, 0.4));
30  robot_shape.push_back(mrpt::math::TPoint2D(-0.5, 0.5));
31 }
32 
35 {
36  ASSERTMSG_(
37  !m_PTGs.empty(),
38  "No PTG was defined! At least one must be especified.");
39 
40  // Convert to CPolygon for API requisites:
41  mrpt::math::CPolygon poly_robot_shape;
42  poly_robot_shape.clear();
43  if (!params.robot_shape.empty())
44  {
45  vector<double> xm, ym;
46  params.robot_shape.getPlotData(xm, ym);
47  poly_robot_shape.setAllVertices(xm, ym);
48  }
49 
50  for (size_t i = 0; i < m_PTGs.size(); i++)
51  {
52  mrpt::system::CTimeLoggerEntry tle(m_timelogger, "PTG_initialization");
53 
54  // Polygonal robot shape?
55  {
56  auto* diff_ptg =
58  m_PTGs[i].get());
59  if (diff_ptg)
60  {
61  ASSERTMSG_(
62  !poly_robot_shape.empty(),
63  "No polygonal robot shape specified, and PTG requires "
64  "one!");
65  diff_ptg->setRobotShape(poly_robot_shape);
66  }
67  }
68  // Circular robot shape?
69  {
70  auto* ptg = dynamic_cast<mrpt::nav::CPTG_RobotShape_Circular*>(
71  m_PTGs[i].get());
72  if (ptg)
73  {
74  ASSERTMSG_(
75  params.robot_shape_circular_radius > 0,
76  "No circular robot shape specified, and PTG requires one!");
77  ptg->setRobotShapeRadius(params.robot_shape_circular_radius);
78  }
79  }
80 
81  m_PTGs[i]->initialize(
83  "%s/TPRRT_PTG_%03u.dat.gz",
84  params.ptg_cache_files_directory.c_str(),
85  static_cast<unsigned int>(i)),
86  params.ptg_verbose);
87  }
88 
89  m_initialized_PTG = true;
90 }
91 
93  const mrpt::config::CConfigFileBase& ini, const std::string& sSect)
94 {
95  // Robot shape:
96  // ==========================
97  // polygonal shape
98  {
99  // Robot shape is a bit special to load:
100  params.robot_shape.clear();
101  const std::string sShape = ini.read_string(sSect, "robot_shape", "");
102  if (!sShape.empty())
103  {
104  CMatrixDouble mShape;
105  if (!mShape.fromMatlabStringFormat(sShape))
107  "Error parsing robot_shape matrix: '%s'", sShape.c_str());
108  ASSERT_(mShape.rows() == 2);
109  ASSERT_(mShape.cols() >= 3);
110 
111  for (int i = 0; i < mShape.cols(); i++)
112  params.robot_shape.push_back(
113  TPoint2D(mShape(0, i), mShape(1, i)));
114  }
115  }
116  // circular shape
117  params.robot_shape_circular_radius =
118  ini.read_double(sSect, "robot_shape_circular_radius", 0.0);
119 
120  // Load PTG tables:
121  // ==========================
122  m_PTGs.clear();
123 
124  const size_t PTG_COUNT =
125  ini.read_int(sSect, "PTG_COUNT", 0, true); // load the number of PTGs
126  for (unsigned int n = 0; n < PTG_COUNT; n++)
127  {
128  // Generate it:
129  const std::string sPTGName =
130  ini.read_string(sSect, format("PTG%u_Type", n), "", true);
133  sPTGName, ini, sSect, format("PTG%u_", n))));
134  }
135 }
136 
137 // Auxiliary function:
139  const mrpt::maps::CPointsMap& in_map, mrpt::maps::CPointsMap& out_map,
140  const mrpt::poses::CPose2D& asSeenFrom, const double MAX_DIST_XY)
141 {
142  size_t nObs;
143  const float *obs_xs, *obs_ys, *obs_zs;
144  in_map.getPointsBuffer(nObs, obs_xs, obs_ys, obs_zs);
145 
146  out_map.clear();
147  out_map.reserve(nObs); // Prealloc mem for speed-up
148 
149  const CPose2D invPose = -asSeenFrom;
150  // We can safely discard the rest of obstacles, since they cannot be
151  // converted into TP-Obstacles anyway!
152 
153  for (size_t obs = 0; obs < nObs; obs++)
154  {
155  const double gx = obs_xs[obs], gy = obs_ys[obs];
156 
157  if (std::abs(gx - asSeenFrom.x()) > MAX_DIST_XY ||
158  std::abs(gy - asSeenFrom.y()) > MAX_DIST_XY)
159  continue; // ignore this obstacle: anyway, I don't know how to map
160  // it to TP-Obs!
161 
162  double ox, oy;
163  invPose.composePoint(gx, gy, ox, oy);
164 
165  out_map.insertPointFast(ox, oy, 0);
166  }
167 }
168 
169 /*---------------------------------------------------------------
170 SpaceTransformer
171 ---------------------------------------------------------------*/
173  const mrpt::maps::CSimplePointsMap& in_obstacles,
175  const double MAX_DIST, std::vector<double>& out_TPObstacles)
176 {
177  using namespace mrpt::nav;
178  try
179  {
180  // Take "k_rand"s and "distances" such that the collision hits the
181  // obstacles
182  // in the "grid" of the given PT
183  // --------------------------------------------------------------------
184  size_t nObs;
185  const float *obs_xs, *obs_ys, *obs_zs;
186  // = in_obstacles.getPointsCount();
187  in_obstacles.getPointsBuffer(nObs, obs_xs, obs_ys, obs_zs);
188 
189  // Init obs ranges:
190  in_PTG->initTPObstacles(out_TPObstacles);
191 
192  for (size_t obs = 0; obs < nObs; obs++)
193  {
194  const float ox = obs_xs[obs];
195  const float oy = obs_ys[obs];
196 
197  if (std::abs(ox) > MAX_DIST || std::abs(oy) > MAX_DIST)
198  continue; // ignore this obstacle: anyway, I don't know how to
199  // map it to TP-Obs!
200 
201  in_PTG->updateTPObstacle(ox, oy, out_TPObstacles);
202  }
203 
204  // Leave distances in out_TPObstacles un-normalized ([0,1]), so they
205  // just represent real distances in meters.
206  }
207  catch (const std::exception& e)
208  {
209  cerr << "[PT_RRT::SpaceTransformer] Exception:" << endl;
210  cerr << e.what() << endl;
211  }
212  catch (...)
213  {
214  cerr << "\n[PT_RRT::SpaceTransformer] Unexpected exception!:\n";
215  cerr << format("*in_PTG = %p\n", (void*)in_PTG);
216  if (in_PTG)
217  cerr << format("PTG = %s\n", in_PTG->getDescription().c_str());
218  cerr << endl;
219  }
220 }
221 
223  const int tp_space_k_direction,
224  const mrpt::maps::CSimplePointsMap& in_obstacles,
226  const double MAX_DIST, double& out_TPObstacle_k)
227 {
228  using namespace mrpt::nav;
229  try
230  {
231  // Take "k_rand"s and "distances" such that the collision hits the
232  // obstacles
233  // in the "grid" of the given PT
234  // --------------------------------------------------------------------
235  size_t nObs;
236  const float *obs_xs, *obs_ys, *obs_zs;
237  // = in_obstacles.getPointsCount();
238  in_obstacles.getPointsBuffer(nObs, obs_xs, obs_ys, obs_zs);
239 
240  // Init obs ranges:
241  in_PTG->initTPObstacleSingle(tp_space_k_direction, out_TPObstacle_k);
242 
243  for (size_t obs = 0; obs < nObs; obs++)
244  {
245  const float ox = obs_xs[obs];
246  const float oy = obs_ys[obs];
247 
248  if (std::abs(ox) > MAX_DIST || std::abs(oy) > MAX_DIST)
249  continue; // ignore this obstacle: anyway, I don't know how to
250  // map it to TP-Obs!
251 
252  in_PTG->updateTPObstacleSingle(
253  ox, oy, tp_space_k_direction, out_TPObstacle_k);
254  }
255 
256  // Leave distances in out_TPObstacles un-normalized ([0,1]), so they
257  // just represent real distances in meters.
258  }
259  catch (const std::exception& e)
260  {
261  cerr << "[PT_RRT::SpaceTransformer] Exception:" << endl;
262  cerr << e.what() << endl;
263  }
264  catch (...)
265  {
266  cerr << "\n[PT_RRT::SpaceTransformer] Unexpected exception!:\n";
267  cerr << format("*in_PTG = %p\n", (void*)in_PTG);
268  if (in_PTG)
269  cerr << format("PTG = %s\n", in_PTG->getDescription().c_str());
270  cerr << endl;
271  }
272 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:30
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...
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
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
GLenum GLsizei n
Definition: glext.h:5136
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:19
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.
static CParameterizedTrajectoryGenerator::Ptr 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 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:199
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:284
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:65
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.
constexpr double DEG2RAD(const double x)
Degrees to radians.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
bool fromMatlabStringFormat(const std::string &s, mrpt::optional_ref< std::ostream > dump_errors_here=std::nullopt)
Reads a matrix from a string in Matlab-like format, for example: "[1 0 2; 0 4 -1]" The string must st...
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
GLsizei const GLchar ** string
Definition: glext.h:4116
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
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() ...
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:218
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
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...
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:69
Lightweight 2D point.
Definition: TPoint2D.h:31
GLenum const GLfloat * params
Definition: glext.h:3538



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: cb560b230 Wed Nov 13 08:06:48 2019 +0100 at miƩ nov 13 08:15:10 CET 2019