Go to the documentation of this file.
22 : ptg_cache_files_directory(
"."),
38 "No PTG was defined! At least one must be especified.");
42 poly_robot_shape.clear();
45 vector<double> xm, ym;
50 for (
size_t i = 0; i <
m_PTGs.size(); i++)
62 !poly_robot_shape.empty(),
63 "No polygonal robot shape specified, and PTG requires "
65 diff_ptg->setRobotShape(poly_robot_shape);
76 "No circular robot shape specified, and PTG requires one!");
83 "%s/TPRRT_PTG_%03u.dat.gz",
85 static_cast<unsigned int>(i)),
101 const std::string sShape = ini.
read_string(sSect,
"robot_shape",
"");
107 "Error parsing robot_shape matrix: '%s'", sShape.c_str());
111 for (
int i = 0; i < mShape.
cols(); i++)
113 TPoint2D(mShape(0, i), mShape(1, i)));
118 ini.
read_double(sSect,
"robot_shape_circular_radius", 0.0);
124 const size_t PTG_COUNT =
125 ini.
read_int(sSect,
"PTG_COUNT", 0,
true);
126 for (
unsigned int n = 0; n < PTG_COUNT; n++)
129 const std::string sPTGName =
132 sPTGName, ini, sSect,
format(
"PTG%u_", n)));
142 const float *obs_xs, *obs_ys, *obs_zs;
148 const CPose2D invPose = -asSeenFrom;
152 for (
size_t obs = 0; obs < nObs; obs++)
154 const double gx = obs_xs[obs], gy = obs_ys[obs];
156 if (std::abs(gx - asSeenFrom.
x()) > MAX_DIST_XY ||
157 std::abs(gy - asSeenFrom.
y()) > MAX_DIST_XY)
174 const double MAX_DIST, std::vector<double>& out_TPObstacles)
184 const float *obs_xs, *obs_ys, *obs_zs;
191 for (
size_t obs = 0; obs < nObs; obs++)
193 const float ox = obs_xs[obs];
194 const float oy = obs_ys[obs];
196 if (std::abs(ox) > MAX_DIST || std::abs(oy) > MAX_DIST)
206 catch (
const std::exception& e)
208 cerr <<
"[PT_RRT::SpaceTransformer] Exception:" << endl;
209 cerr << e.what() << endl;
213 cerr <<
"\n[PT_RRT::SpaceTransformer] Unexpected exception!:\n";
214 cerr <<
format(
"*in_PTG = %p\n", (
void*)in_PTG);
216 cerr <<
format(
"PTG = %s\n", in_PTG->getDescription().c_str());
222 const int tp_space_k_direction,
225 const double MAX_DIST,
double& out_TPObstacle_k)
235 const float *obs_xs, *obs_ys, *obs_zs;
242 for (
size_t obs = 0; obs < nObs; obs++)
244 const float ox = obs_xs[obs];
245 const float oy = obs_ys[obs];
247 if (std::abs(ox) > MAX_DIST || std::abs(oy) > MAX_DIST)
252 ox, oy, tp_space_k_direction, out_TPObstacle_k);
258 catch (
const std::exception& e)
260 cerr <<
"[PT_RRT::SpaceTransformer] Exception:" << endl;
261 cerr << e.what() << endl;
265 cerr <<
"\n[PT_RRT::SpaceTransformer] Unexpected exception!:\n";
266 cerr <<
format(
"*in_PTG = %p\n", (
void*)in_PTG);
268 cerr <<
format(
"PTG = %s\n", in_PTG->getDescription().c_str());
Base class for all PTGs using a 2D circular robot shape model.
virtual void reserve(size_t newLength)=0
Reserves memory for a given number of points: the size of the map does not change,...
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)
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 x() const
Common members of all points & poses classes.
double read_double(const std::string §ion, const std::string &name, double defaultValue, bool failIfNotFound=false) const
bool ptg_verbose
Display PTG construction info (default=true)
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
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.
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,...
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::system::CTimeLogger m_timelogger
#define ASSERT_(f)
Defines an assertion mechanism.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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...
RRTAlgorithmParams params
Parameters specific to this path solver algorithm.
PlannerTPS_VirtualBase()
ctor
std::string read_string(const std::string §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
void spaceTransformer(const mrpt::maps::CSimplePointsMap &in_obstacles, const mrpt::nav::CParameterizedTrajectoryGenerator *in_PTG, const double MAX_DIST, std::vector< double > &out_TPObstacles)
static void transformPointcloudWithSquareClipping(const mrpt::maps::CPointsMap &in_map, mrpt::maps::CPointsMap &out_map, const mrpt::poses::CPose2D &asSeenFrom, const double MAX_DIST_XY)
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
void initTPObstacles(std::vector< double > &TP_Obstacles) const
Resizes and populates the initial appropriate contents in a vector of tp-obstacles (collision-free ra...
This class allows loading and storing values and vectors of different types from a configuration text...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
void initTPObstacleSingle(uint16_t k, double &TP_Obstacle_k) const
Base class for all PTGs suitable to non-holonomic, differentially-driven (or Ackermann) vehicles base...
double robot_shape_circular_radius
The radius of a circular-shape-model of the robot shape.
constexpr double DEG2RAD(const double x)
Degrees to radians
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...
std::string ptg_cache_files_directory
(Default: ".")
void getPlotData(std::vector< double > &x, std::vector< double > &y) const
Gets plot data, ready to use on a 2D plot.
void setAllVertices(const std::vector< double > &x, const std::vector< double > &y)
Set all vertices at once.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
A safe way to call enter() and leave() of a mrpt::system::CTimeLogger upon construction and destructi...
A wrapper of a TPolygon2D class, implementing CSerializable.
void internal_initialize_PTG()
Must be called after setting all params (see internal_loadConfig_PTG()) and before calling solve()
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.
virtual void insertPointFast(float x, float y, float z=0)=0
The virtual method for insertPoint() without calling mark_as_modified()
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.
This base provides a set of functions for maths stuff.
size_type cols() const
Number of columns in the matrix.
void clear()
Erase all the contents of the map.
mrpt::math::TPolygon2D robot_shape
The robot shape used when computing collisions; it's loaded from the config file/text as a single 2xN...
mrpt::nav::TListPTGPtr m_PTGs
size_type rows() const
Number of rows in the matrix.
This is the base class for any user-defined PTG.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sat Jun 27 14:00:59 UTC 2020 | |