22 : ptg_cache_files_directory(
"."),
38 "No PTG was defined! At least one must be especified.");
42 poly_robot_shape.clear();
43 if (!
params.robot_shape.empty())
45 vector<double> xm, ym;
47 poly_robot_shape.setAllVertices(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);
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);
83 "%s/TPRRT_PTG_%03u.dat.gz",
84 params.ptg_cache_files_directory.c_str(),
85 static_cast<unsigned int>(i)),
100 params.robot_shape.clear();
107 "Error parsing robot_shape matrix: '%s'", sShape.c_str());
111 for (
int i = 0; i < mShape.
cols(); i++)
112 params.robot_shape.push_back(
113 TPoint2D(mShape(0, i), mShape(1, i)));
117 params.robot_shape_circular_radius =
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++)
133 sPTGName, ini, sSect,
format(
"PTG%u_",
n))));
143 const float *obs_xs, *obs_ys, *obs_zs;
149 const CPose2D invPose = -asSeenFrom;
153 for (
size_t obs = 0; obs < nObs; obs++)
155 const double gx = obs_xs[obs], gy = obs_ys[obs];
157 if (std::abs(gx - asSeenFrom.
x()) > MAX_DIST_XY ||
158 std::abs(gy - asSeenFrom.
y()) > MAX_DIST_XY)
175 const double MAX_DIST, std::vector<double>& out_TPObstacles)
185 const float *obs_xs, *obs_ys, *obs_zs;
192 for (
size_t obs = 0; obs < nObs; obs++)
194 const float ox = obs_xs[obs];
195 const float oy = obs_ys[obs];
207 catch (
const std::exception& e)
209 cerr <<
"[PT_RRT::SpaceTransformer] Exception:" << endl;
210 cerr << e.what() << endl;
214 cerr <<
"\n[PT_RRT::SpaceTransformer] Unexpected exception!:\n";
215 cerr <<
format(
"*in_PTG = %p\n", (
void*)in_PTG);
217 cerr <<
format(
"PTG = %s\n", in_PTG->getDescription().c_str());
223 const int tp_space_k_direction,
226 const double MAX_DIST,
double& out_TPObstacle_k)
236 const float *obs_xs, *obs_ys, *obs_zs;
243 for (
size_t obs = 0; obs < nObs; obs++)
245 const float ox = obs_xs[obs];
246 const float oy = obs_ys[obs];
253 ox, oy, tp_space_k_direction, out_TPObstacle_k);
259 catch (
const std::exception& e)
261 cerr <<
"[PT_RRT::SpaceTransformer] Exception:" << endl;
262 cerr << e.what() << endl;
266 cerr <<
"\n[PT_RRT::SpaceTransformer] Unexpected exception!:\n";
267 cerr <<
format(
"*in_PTG = %p\n", (
void*)in_PTG);
269 cerr <<
format(
"PTG = %s\n", in_PTG->getDescription().c_str());
void clear()
Erase all the contents of the map.
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 §ion, 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...
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
double DEG2RAD(const double x)
Degrees to radians.
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.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
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...
PlannerTPS_VirtualBase()
ctor
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...
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
void initTPObstacleSingle(uint16_t k, double &TP_Obstacle_k) const
#define ASSERT_(f)
Defines an assertion mechanism.
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 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.
mrpt::nav::TListPTGPtr m_PTGs
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
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 §ion, const std::string &name, double defaultValue, bool failIfNotFound=false) const
double x() const
Common members of all points & poses classes.
GLsizei const GLchar ** string
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...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
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'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,...)
GLenum const GLfloat * params