9 #ifndef CParameterizedTrajectoryGenerator_H 10 #define CParameterizedTrajectoryGenerator_H 22 namespace opengl {
class CSetOfLines; }
34 TCPoint(
const float x_,
const float y_,
const float phi_,
35 const float t_,
const float dist_,
36 const float v_,
const float w_) :
37 x(x_), y(y_), phi(phi_),
t(t_), dist(dist_), v(v_), w(w_)
39 float x,
y, phi,
t, dist,v,w;
71 void initializeCollisionsGrid(
float refDistance,
float resolution);
85 virtual std::string getDescription()
const = 0 ;
92 void simulateTrajectories(
93 uint16_t alphaValuesCount,
99 float *out_max_acc_v = NULL,
100 float *out_max_acc_w = NULL);
116 virtual bool inverseMap_WS2TP(
float x,
float y,
int &out_k,
float &out_d,
float tolerance_dist = 0.10f)
const;
120 "Use inverseMap_WS2TP() instead", \
121 void lambdaFunction(
float x,
float y,
int &out_k,
float &out_d ) \
126 void directionToMotionCommand( uint16_t k,
float &out_v,
float &out_w );
132 void getCPointWhen_d_Is (
float d, uint16_t k,
float &x,
float &y,
float &phi,
float &
t,
float *v = NULL,
float *w = NULL );
151 return (
float)(
M_PI * (-1 + 2 * (k+0.5f) / ((
float)m_alphaValuesCount) ));
161 return (uint16_t)(0.5f*(m_alphaValuesCount*(1+alpha/
M_PI) - 1));
173 bool debugDumpInFiles(
const int nPT);
181 void renderPathAsSimpleLine(
184 const float decimate_distance = 0.1f,
185 const float max_path_distance = 0.0f)
const;
203 :
mrpt::utils::CDynamicGrid<TCollisionCell>(x_min,x_max,y_min,y_max,resolution),
214 const TCollisionCell & getTPObstacle(
const float obsX,
const float obsY)
const;
221 void updateCellInfo(
const unsigned int icx,
const unsigned int icy,
const uint16_t k,
const float dist );
229 bool SaveColGridsToFile(
const std::string &filename,
const mrpt::math::CPolygon & computed_robotShape );
230 bool LoadColGridsFromFile(
const std::string &filename,
const mrpt::math::CPolygon & current_robotShape );
236 virtual void PTG_Generator(
float alpha,
float t,
float x,
float y,
float phi,
float &v,
float &w) = 0;
239 virtual bool PTG_IsIntoDomain(
float x,
float y ) = 0;
255 k_min(
std::numeric_limits<uint16_t>::max() ),
256 k_max(
std::numeric_limits<uint16_t>::min() ),
257 n_min(
std::numeric_limits<uint32_t>::max() ),
258 n_max(
std::numeric_limits<uint32_t>::min() )
264 bool isEmpty()
const {
return k_min==std::numeric_limits<uint16_t>::max(); }
284 typedef std::vector<mrpt::nav::CParameterizedTrajectoryGenerator*>
TListPTGs;
285 typedef std::vector<mrpt::nav::CParameterizedTrajectoryGeneratorPtr>
TListPTGPtr;
302 m_text_description = CParameterizedTrajectoryGenerator::loadTrajectories(in);
303 return m_text_description;
305 virtual void PTG_Generator(
float alpha,
float t,
float x,
float y,
float phi,
float &v,
float &w) {
throw std::runtime_error(
"Should not call this method in a dummy PTG!"); }
306 virtual bool PTG_IsIntoDomain(
float x,
float y ) {
throw std::runtime_error(
"Should not call this method in a dummy PTG!"); }
std::vector< std::pair< uint16_t, float > > TCollisionCell
A list of all the pairs (alpha,distance) such as the robot collides at that cell. ...
float GetCPathPoint_w(uint16_t k, int n) const
mrpt::utils::CDynamicGrid< TCellForLambdaFunction > m_lambdaFunctionOptimizer
This grid will contain indexes data for speeding-up the default, brute-force lambda function...
#define MRPT_DECLARE_TTYPENAME_NAMESPACE(_TYPE, __NS)
OPENGL_IMPEXP mrpt::utils::CStream & operator<<(mrpt::utils::CStream &out, const mrpt::opengl::CLight &o)
For usage when passing a dynamic number of (numeric) arguments to a function, by name.
float GetCPathPoint_x(uint16_t k, int n) const
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
::mrpt::utils::CStream & operator>>(mrpt::utils::CStream &in, CAngularObservationMeshPtr &pObj)
Trajectory points in C-Space.
A dummy PTG, used mainly to call loadTrajectories() without knowing the exact derived PTG class and s...
std::vector< TCPointVector > CPoints
A wrapper of a TPolygon2D class, implementing CSerializable.
EIGEN_STRONG_INLINE const AdjointReturnType t() const
Transpose.
float turningRadiusReference
float GetCPathPoint_d(uint16_t k, int n) const
TCPoint(const float x_, const float y_, const float phi_, const float t_, const float dist_, const float v_, const float w_)
float GetCPathPoint_v(uint16_t k, int n) const
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
This is the base class for any user-defined PTG.
A 2D grid of dynamic size which stores any kind of data at each cell.
CParameterizedTrajectoryGenerator()
Protected constructor for CPTG_Dummy; does not init collision grid.
stlplus::smart_ptr< CParameterizedTrajectoryGenerator > CParameterizedTrajectoryGeneratorPtr
Smart pointer to a PTG.
#define MRPT_DECLARE_DEPRECATED_FUNCTION(__MSG, __FUNC)
Usage: MRPT_DECLARE_DEPRECATED_FUNCTION("Use XX instead", void myFunc(double));.
CColisionGrid m_collisionGrid
The collision grid.
float GetCPathPoint_t(uint16_t k, int n) const
virtual std::string loadTrajectories(mrpt::utils::CStream &in)
Loads the simulated trajectories and other parameters from a target stream.
virtual bool PTG_IsIntoDomain(float x, float y)
To be implemented in derived classes.
float index2alpha(uint16_t k) const
Alfa value for the discrete corresponding value.
uint16_t alpha2index(float alpha) const
Discrete index value for the corresponding alpha value.
An internal class for storing the collision grid.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
float GetCPathPoint_phi(uint16_t k, int n) const
size_t getPointsCountInCPath_k(uint16_t k) const
std::vector< TCPoint > TCPointVector
float getMax_V_inTPSpace() const
CColisionGrid(float x_min, float x_max, float y_min, float y_max, float resolution, CParameterizedTrajectoryGenerator *parent)
Specifies the min/max values for "k" and "n", respectively.
float GetCPathPoint_y(uint16_t k, int n) const
std::vector< mrpt::nav::CParameterizedTrajectoryGeneratorPtr > TListPTGPtr
A list of PTGs (smart pointers)
virtual std::string getDescription() const
Gets a short textual description of the PTG and its parameters.
A set of independent lines (or segments), one line with its own start and end positions (X...
virtual ~CParameterizedTrajectoryGenerator()
Destructor.
std::vector< mrpt::nav::CParameterizedTrajectoryGenerator * > TListPTGs
A list of PTGs (bare pointers)
uint16_t getAlfaValuesCount() const
uint16_t m_alphaValuesCount
The number of discrete values for "alpha" between -PI and +PI.
CParameterizedTrajectoryGenerator const * m_parent
std::string m_text_description
virtual void PTG_Generator(float alpha, float t, float x, float y, float phi, float &v, float &w)
The main method to be implemented in derived classes.