MRPT  2.0.4
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-2020, 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;
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_(
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",
85  static_cast<unsigned int>(i)),
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
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);
132  sPTGName, ini, sSect, format("PTG%u_", n)));
133  }
134 }
135 
136 // Auxiliary function:
138  const mrpt::maps::CPointsMap& in_map, mrpt::maps::CPointsMap& out_map,
139  const mrpt::poses::CPose2D& asSeenFrom, const double MAX_DIST_XY)
140 {
141  size_t nObs;
142  const float *obs_xs, *obs_ys, *obs_zs;
143  in_map.getPointsBuffer(nObs, obs_xs, obs_ys, obs_zs);
144 
145  out_map.clear();
146  out_map.reserve(nObs); // Prealloc mem for speed-up
147 
148  const CPose2D invPose = -asSeenFrom;
149  // We can safely discard the rest of obstacles, since they cannot be
150  // converted into TP-Obstacles anyway!
151 
152  for (size_t obs = 0; obs < nObs; obs++)
153  {
154  const double gx = obs_xs[obs], gy = obs_ys[obs];
155 
156  if (std::abs(gx - asSeenFrom.x()) > MAX_DIST_XY ||
157  std::abs(gy - asSeenFrom.y()) > MAX_DIST_XY)
158  continue; // ignore this obstacle: anyway, I don't know how to map
159  // it to TP-Obs!
160 
161  double ox, oy;
162  invPose.composePoint(gx, gy, ox, oy);
163 
164  out_map.insertPointFast(ox, oy, 0);
165  }
166 }
167 
168 /*---------------------------------------------------------------
169 SpaceTransformer
170 ---------------------------------------------------------------*/
172  const mrpt::maps::CSimplePointsMap& in_obstacles,
174  const double MAX_DIST, std::vector<double>& out_TPObstacles)
175 {
176  using namespace mrpt::nav;
177  try
178  {
179  // Take "k_rand"s and "distances" such that the collision hits the
180  // obstacles
181  // in the "grid" of the given PT
182  // --------------------------------------------------------------------
183  size_t nObs;
184  const float *obs_xs, *obs_ys, *obs_zs;
185  // = in_obstacles.getPointsCount();
186  in_obstacles.getPointsBuffer(nObs, obs_xs, obs_ys, obs_zs);
187 
188  // Init obs ranges:
189  in_PTG->initTPObstacles(out_TPObstacles);
190 
191  for (size_t obs = 0; obs < nObs; obs++)
192  {
193  const float ox = obs_xs[obs];
194  const float oy = obs_ys[obs];
195 
196  if (std::abs(ox) > MAX_DIST || std::abs(oy) > MAX_DIST)
197  continue; // ignore this obstacle: anyway, I don't know how to
198  // map it to TP-Obs!
199 
200  in_PTG->updateTPObstacle(ox, oy, out_TPObstacles);
201  }
202 
203  // Leave distances in out_TPObstacles un-normalized ([0,1]), so they
204  // just represent real distances in meters.
205  }
206  catch (const std::exception& e)
207  {
208  cerr << "[PT_RRT::SpaceTransformer] Exception:" << endl;
209  cerr << e.what() << endl;
210  }
211  catch (...)
212  {
213  cerr << "\n[PT_RRT::SpaceTransformer] Unexpected exception!:\n";
214  cerr << format("*in_PTG = %p\n", (void*)in_PTG);
215  if (in_PTG)
216  cerr << format("PTG = %s\n", in_PTG->getDescription().c_str());
217  cerr << endl;
218  }
219 }
220 
222  const int tp_space_k_direction,
223  const mrpt::maps::CSimplePointsMap& in_obstacles,
225  const double MAX_DIST, double& out_TPObstacle_k)
226 {
227  using namespace mrpt::nav;
228  try
229  {
230  // Take "k_rand"s and "distances" such that the collision hits the
231  // obstacles
232  // in the "grid" of the given PT
233  // --------------------------------------------------------------------
234  size_t nObs;
235  const float *obs_xs, *obs_ys, *obs_zs;
236  // = in_obstacles.getPointsCount();
237  in_obstacles.getPointsBuffer(nObs, obs_xs, obs_ys, obs_zs);
238 
239  // Init obs ranges:
240  in_PTG->initTPObstacleSingle(tp_space_k_direction, out_TPObstacle_k);
241 
242  for (size_t obs = 0; obs < nObs; obs++)
243  {
244  const float ox = obs_xs[obs];
245  const float oy = obs_ys[obs];
246 
247  if (std::abs(ox) > MAX_DIST || std::abs(oy) > MAX_DIST)
248  continue; // ignore this obstacle: anyway, I don't know how to
249  // map it to TP-Obs!
250 
251  in_PTG->updateTPObstacleSingle(
252  ox, oy, tp_space_k_direction, out_TPObstacle_k);
253  }
254 
255  // Leave distances in out_TPObstacles un-normalized ([0,1]), so they
256  // just represent real distances in meters.
257  }
258  catch (const std::exception& e)
259  {
260  cerr << "[PT_RRT::SpaceTransformer] Exception:" << endl;
261  cerr << e.what() << endl;
262  }
263  catch (...)
264  {
265  cerr << "\n[PT_RRT::SpaceTransformer] Unexpected exception!:\n";
266  cerr << format("*in_PTG = %p\n", (void*)in_PTG);
267  if (in_PTG)
268  cerr << format("PTG = %s\n", in_PTG->getDescription().c_str());
269  cerr << endl;
270  }
271 }
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
Base class for all PTGs using a 2D circular robot shape model.
mrpt::system::CTimeLogger m_timelogger
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
double robot_shape_circular_radius
The radius of a circular-shape-model of the robot shape.
#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:67
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.
void getPlotData(std::vector< double > &x, std::vector< double > &y) const
Gets plot data, ready to use on a 2D plot.
Definition: TPolygon2D.cpp:146
#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
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.
RRTAlgorithmParams params
Parameters specific to this path solver algorithm.
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:222
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
bool ptg_verbose
Display PTG construction info (default=true)
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
std::string ptg_cache_files_directory
(Default: ".")



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020