MRPT  2.0.3
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 }
mrpt::nav::CPTG_RobotShape_Circular
Base class for all PTGs using a 2D circular robot shape model.
Definition: CParameterizedTrajectoryGenerator.h:565
nav-precomp.h
mrpt::maps::CPointsMap::reserve
virtual void reserve(size_t newLength)=0
Reserves memory for a given number of points: the size of the map does not change,...
mrpt::nav::PlannerTPS_VirtualBase::spaceTransformerOneDirectionOnly
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)
Definition: PlannerRRT_common.cpp:221
mrpt::math::MatrixVectorBase::fromMatlabStringFormat
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...
Definition: MatrixVectorBase_impl.h:24
mrpt::math::TPoint2D_< double >
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
mrpt::config::CConfigFileBase::read_double
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:106
mrpt::nav::RRTAlgorithmParams::ptg_verbose
bool ptg_verbose
Display PTG construction info (default=true)
Definition: PlannerRRT_common.h:121
mrpt::nav
Definition: CAbstractHolonomicReactiveMethod.h:20
mrpt::maps::CPointsMap
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
mrpt::nav::PlannerTPS_VirtualBase::internal_loadConfig_PTG
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.
Definition: PlannerRRT_common.cpp:92
mrpt::nav::CParameterizedTrajectoryGenerator::updateTPObstacle
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,...
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::nav::RRTAlgorithmParams::RRTAlgorithmParams
RRTAlgorithmParams()
Definition: PlannerRRT_common.cpp:21
mrpt::nav::PlannerTPS_VirtualBase::m_timelogger
mrpt::system::CTimeLogger m_timelogger
Definition: PlannerRRT_common.h:228
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::poses::CPose2D::composePoint
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
mrpt::nav::PlannerTPS_VirtualBase::params
RRTAlgorithmParams params
Parameters specific to this path solver algorithm.
Definition: PlannerRRT_common.h:136
mrpt::nav::PlannerTPS_VirtualBase::PlannerTPS_VirtualBase
PlannerTPS_VirtualBase()
ctor
mrpt::config::CConfigFileBase::read_string
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:171
mrpt::nav::PlannerTPS_VirtualBase::spaceTransformer
void spaceTransformer(const mrpt::maps::CSimplePointsMap &in_obstacles, const mrpt::nav::CParameterizedTrajectoryGenerator *in_PTG, const double MAX_DIST, std::vector< double > &out_TPObstacles)
Definition: PlannerRRT_common.cpp:171
mrpt::nav::PlannerTPS_VirtualBase::transformPointcloudWithSquareClipping
static void transformPointcloudWithSquareClipping(const mrpt::maps::CPointsMap &in_map, mrpt::maps::CPointsMap &out_map, const mrpt::poses::CPose2D &asSeenFrom, const double MAX_DIST_XY)
Definition: PlannerRRT_common.cpp:137
mrpt::config::CConfigFileBase::read_int
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:130
PlannerRRT_common.h
mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacles
void initTPObstacles(std::vector< double > &TP_Obstacles) const
Resizes and populates the initial appropriate contents in a vector of tp-obstacles (collision-free ra...
Definition: CParameterizedTrajectoryGenerator.cpp:263
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:147
mrpt::maps::CSimplePointsMap
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
Definition: CSimplePointsMap.h:30
mrpt::nav::PlannerTPS_VirtualBase::m_initialized_PTG
bool m_initialized_PTG
Definition: PlannerRRT_common.h:229
mrpt::nav::CParameterizedTrajectoryGenerator::initTPObstacleSingle
void initTPObstacleSingle(uint16_t k, double &TP_Obstacle_k) const
Definition: CParameterizedTrajectoryGenerator.cpp:270
mrpt::nav::CPTG_DiffDrive_CollisionGridBased
Base class for all PTGs suitable to non-holonomic, differentially-driven (or Ackermann) vehicles base...
Definition: CPTG_DiffDrive_CollisionGridBased.h:52
mrpt::nav::RRTAlgorithmParams::robot_shape_circular_radius
double robot_shape_circular_radius
The radius of a circular-shape-model of the robot shape.
Definition: PlannerRRT_common.h:101
mrpt::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
mrpt::nav::CParameterizedTrajectoryGenerator::CreatePTG
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...
Definition: CParameterizedTrajectoryGenerator_factory.cpp:22
mrpt::nav::RRTAlgorithmParams::ptg_cache_files_directory
std::string ptg_cache_files_directory
(Default: ".")
Definition: PlannerRRT_common.h:104
mrpt::math::TPolygon2D::getPlotData
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
mrpt::math::CPolygon::setAllVertices
void setAllVertices(const std::vector< double > &x, const std::vector< double > &y)
Set all vertices at once.
Definition: CPolygon.cpp:121
CPolygon.h
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
mrpt::system::CTimeLoggerEntry
A safe way to call enter() and leave() of a mrpt::system::CTimeLogger upon construction and destructi...
Definition: system/CTimeLogger.h:189
mrpt::math::CPolygon
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:19
mrpt::nav::PlannerTPS_VirtualBase::internal_initialize_PTG
void internal_initialize_PTG()
Must be called after setting all params (see internal_loadConfig_PTG()) and before calling solve()
Definition: PlannerRRT_common.cpp:34
mrpt::maps::CPointsMap::getPointsBuffer
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
CPTG_DiffDrive_CollisionGridBased.h
mrpt::maps::CPointsMap::insertPointFast
virtual void insertPointFast(float x, float y, float z=0)=0
The virtual method for insertPoint() without calling mark_as_modified()
mrpt::nav::CParameterizedTrajectoryGenerator::updateTPObstacleSingle
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.
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::math::CMatrixDynamic::cols
size_type cols() const
Number of columns in the matrix.
Definition: CMatrixDynamic.h:340
mrpt::maps::CMetricMap::clear
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:30
mrpt::nav::RRTAlgorithmParams::robot_shape
mrpt::math::TPolygon2D robot_shape
The robot shape used when computing collisions; it's loaded from the config file/text as a single 2xN...
Definition: PlannerRRT_common.h:96
mrpt::nav::PlannerTPS_VirtualBase::m_PTGs
mrpt::nav::TListPTGPtr m_PTGs
Definition: PlannerRRT_common.h:230
mrpt::math::CMatrixDynamic< double >
mrpt::math::CMatrixDynamic::rows
size_type rows() const
Number of rows in the matrix.
Definition: CMatrixDynamic.h:337
mrpt::nav::CParameterizedTrajectoryGenerator
This is the base class for any user-defined PTG.
Definition: CParameterizedTrajectoryGenerator.h:78
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Thu May 21 21:53:32 UTC 2020