23 : robot_shape_circular_radius(0.30),
24 ptg_cache_files_directory(
"."),
27 minDistanceBetweenNewNodes(0.10),
43 "No PTG was defined! At least one must be especified.");
47 poly_robot_shape.clear();
48 if (!
params.robot_shape.empty())
50 vector<double> xm, ym;
52 poly_robot_shape.setAllVertices(xm, ym);
55 for (
size_t i = 0; i <
m_PTGs.size(); i++)
67 !poly_robot_shape.empty(),
68 "No polygonal robot shape specified, and PTG requires " 81 params.robot_shape_circular_radius > 0,
82 "No circular robot shape specified, and PTG requires one!");
89 "%s/TPRRT_PTG_%03u.dat.gz",
90 params.ptg_cache_files_directory.c_str(),
91 static_cast<unsigned int>(i)),
106 params.robot_shape.clear();
111 if (!mShape.fromMatlabStringFormat(sShape))
113 "Error parsing robot_shape matrix: '%s'", sShape.c_str());
117 for (
size_t i = 0; i <
size(mShape, 2); i++)
118 params.robot_shape.push_back(
119 TPoint2D(mShape(0, i), mShape(1, i)));
123 params.robot_shape_circular_radius =
124 ini.
read_double(sSect,
"robot_shape_circular_radius", 0.0);
130 const size_t PTG_COUNT =
131 ini.
read_int(sSect,
"PTG_COUNT", 0,
true);
132 for (
unsigned int n = 0;
n < PTG_COUNT;
n++)
140 sPTGName, ini, sSect,
format(
"PTG%u_",
n))));
150 const float *obs_xs, *obs_ys, *obs_zs;
156 const CPose2D invPose = -asSeenFrom;
160 for (
size_t obs = 0; obs < nObs; obs++)
162 const double gx = obs_xs[obs], gy = obs_ys[obs];
164 if (std::abs(gx - asSeenFrom.
x()) > MAX_DIST_XY ||
165 std::abs(gy - asSeenFrom.
y()) > MAX_DIST_XY)
182 const double MAX_DIST, std::vector<double>& out_TPObstacles)
192 const float *obs_xs, *obs_ys, *obs_zs;
199 for (
size_t obs = 0; obs < nObs; obs++)
201 const float ox = obs_xs[obs];
202 const float oy = obs_ys[obs];
214 catch (std::exception& e)
216 cerr <<
"[PT_RRT::SpaceTransformer] Exception:" << endl;
217 cerr << e.what() << endl;
221 cerr <<
"\n[PT_RRT::SpaceTransformer] Unexpected exception!:\n";
222 cerr <<
format(
"*in_PTG = %p\n", (
void*)in_PTG);
224 cerr <<
format(
"PTG = %s\n", in_PTG->getDescription().c_str());
230 const int tp_space_k_direction,
233 const double MAX_DIST,
double& out_TPObstacle_k)
243 const float *obs_xs, *obs_ys, *obs_zs;
250 for (
size_t obs = 0; obs < nObs; obs++)
252 const float ox = obs_xs[obs];
253 const float oy = obs_ys[obs];
260 ox, oy, tp_space_k_direction, out_TPObstacle_k);
266 catch (std::exception& e)
268 cerr <<
"[PT_RRT::SpaceTransformer] Exception:" << endl;
269 cerr << e.what() << endl;
273 cerr <<
"\n[PT_RRT::SpaceTransformer] Unexpected exception!:\n";
274 cerr <<
format(
"*in_PTG = %p\n", (
void*)in_PTG);
276 cerr <<
format(
"PTG = %s\n", in_PTG->getDescription().c_str());
void clear()
Erase all the contents of the map.
std::shared_ptr< CParameterizedTrajectoryGenerator > Ptr
double x() const
Common members of all points & poses classes.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
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 format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
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.
void internal_loadConfig_PTG(const mrpt::utils::CConfigFileBase &cfgSource, const std::string &sSectionName=std::string("PTG_CONFIG"))
Load all PTG params from a config file source.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
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. ...
std::string read_string(const std::string §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
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.
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...
This class allows loading and storing values and vectors of different types from a configuration text...
void initTPObstacleSingle(uint16_t k, double &TP_Obstacle_k) const
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
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 base provides a set of functions for maths stuff.
mrpt::nav::TListPTGPtr m_PTGs
void setRobotShapeRadius(const double robot_radius)
Robot shape must be set before initialization, either from ctor params or via this method...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
GLsizei const GLchar ** string
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() ...
void setRobotShape(const mrpt::math::CPolygon &robotShape)
Robot shape must be set before initialization, either from ctor params or via this method...
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.
A safe way to call enter() and leave() of a mrpt::utils::CTimeLogger upon construction and destructio...
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...
double read_double(const std::string §ion, const std::string &name, double defaultValue, bool failIfNotFound=false) const
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 ASSERTMSG_(f, __ERROR_MSG)
GLenum const GLfloat * params
static CParameterizedTrajectoryGenerator * CreatePTG(const std::string &ptgClassName, const mrpt::utils::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...
mrpt::utils::CTimeLogger m_timelogger