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_)
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 ) \
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));
184 const float decimate_distance = 0.1f,
185 const float max_path_distance = 0.0f)
const;
221 void updateCellInfo(
const unsigned int icx,
const unsigned int icy,
const uint16_t k,
const float dist );
236 virtual void PTG_Generator(
float alpha,
float t,
float x,
float y,
float phi,
float &v,
float &w) = 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;
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!"); }
#define MRPT_DECLARE_TTYPENAME_NAMESPACE(_TYPE, __NS)
A wrapper of a TPolygon2D class, implementing CSerializable.
A dummy PTG, used mainly to call loadTrajectories() without knowing the exact derived PTG class and s...
std::string m_text_description
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.
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.
virtual std::string getDescription() const
Gets a short textual description of the PTG and its parameters.
An internal class for storing the collision grid
bool saveToFile(mrpt::utils::CStream *fil, const mrpt::math::CPolygon &computed_robotShape)
Save to file, true = OK.
const TCollisionCell & getTPObstacle(const float obsX, const float obsY) const
For an obstacle (x,y), returns a vector with all the pairs (a,d) such as the robot collides.
void updateCellInfo(const unsigned int icx, const unsigned int icy, const uint16_t k, const float dist)
Updates the info into a cell: It updates the cell only if the distance d for the path k is lower than...
bool loadFromFile(mrpt::utils::CStream *fil, const mrpt::math::CPolygon ¤t_robotShape)
Load from file, true = OK.
CParameterizedTrajectoryGenerator const * m_parent
CColisionGrid(float x_min, float x_max, float y_min, float y_max, float resolution, CParameterizedTrajectoryGenerator *parent)
This is the base class for any user-defined PTG.
void saveTrajectories(mrpt::utils::CStream &out) const
Saves the simulated trajectories and other parameters to a target stream.
float GetCPathPoint_v(uint16_t k, int n) const
void simulateTrajectories(uint16_t alphaValuesCount, float max_time, float max_dist, unsigned int max_n, float diferencial_t, float min_dist, float *out_max_acc_v=NULL, float *out_max_acc_w=NULL)
The main method: solves the diferential equation to generate a family of parametrical trajectories.
bool SaveColGridsToFile(const std::string &filename, const mrpt::math::CPolygon &computed_robotShape)
uint16_t m_alphaValuesCount
The number of discrete values for "alpha" between -PI and +PI.
float GetCPathPoint_w(uint16_t k, int n) const
void FreeMemory()
Free all the memory buffers.
float GetCPathPoint_d(uint16_t k, int n) const
size_t getPointsCountInCPath_k(uint16_t k) const
void renderPathAsSimpleLine(const uint16_t k, mrpt::opengl::CSetOfLines &gl_obj, const float decimate_distance=0.1f, const float max_path_distance=0.0f) const
Returns the representation of one trajectory of this PTG as a 3D OpenGL object (a simple curved line)...
virtual std::string getDescription() const =0
Gets a short textual description of the PTG and its parameters.
float GetCPathPoint_t(uint16_t k, int n) const
float GetCPathPoint_x(uint16_t k, int n) const
float GetCPathPoint_y(uint16_t k, int n) const
float turningRadiusReference
virtual void PTG_Generator(float alpha, float t, float x, float y, float phi, float &v, float &w)=0
The main method to be implemented in derived classes.
std::vector< TCPointVector > CPoints
virtual bool PTG_IsIntoDomain(float x, float y)=0
To be implemented in derived classes.
bool LoadColGridsFromFile(const std::string &filename, const mrpt::math::CPolygon ¤t_robotShape)
void getCPointWhen_d_Is(float d, uint16_t k, float &x, float &y, float &phi, float &t, float *v=NULL, float *w=NULL)
Returns the C-Space coordinates (pose) when the robot has transversed a distance d along trajectory i...
void initializeCollisionsGrid(float refDistance, float resolution)
Initialized the collision grid with the given size and resolution.
CParameterizedTrajectoryGenerator(const mrpt::utils::TParameters< double > ¶ms)
Constructor: possible values in "params":
void directionToMotionCommand(uint16_t k, float &out_v, float &out_w)
Converts an "alpha" value (into the discrete set) into a feasible motion command.
CColisionGrid m_collisionGrid
The collision grid.
virtual bool inverseMap_WS2TP(float x, float y, int &out_k, float &out_d, float tolerance_dist=0.10f) const
Computes the closest (alpha,d) TP coordinates of the trajectory point closest to the Workspace (WS) C...
float GetCPathPoint_phi(uint16_t k, int n) const
CParameterizedTrajectoryGenerator()
Protected constructor for CPTG_Dummy; does not init collision grid.
float getMax_V_inTPSpace() const
uint16_t alpha2index(float alpha) const
Discrete index value for the corresponding alpha value.
mrpt::utils::CDynamicGrid< TCellForLambdaFunction > m_lambdaFunctionOptimizer
This grid will contain indexes data for speeding-up the default, brute-force lambda function.
virtual std::string loadTrajectories(mrpt::utils::CStream &in)
Loads the simulated trajectories and other parameters from a target stream.
bool debugDumpInFiles(const int nPT)
Dump PTG trajectories in a binary file "./reactivenav.logs/PTGs/PTG%i.dat", with "%i" being the user-...
std::vector< std::pair< uint16_t, float > > TCollisionCell
A list of all the pairs (alpha,distance) such as the robot collides at that cell.
uint16_t getAlfaValuesCount() const
static CParameterizedTrajectoryGenerator * CreatePTG(const mrpt::utils::TParameters< double > ¶ms)
The class factory for creating a PTG from a list of parameters "params".
virtual ~CParameterizedTrajectoryGenerator()
Destructor.
float index2alpha(uint16_t k) const
Alfa value for the discrete corresponding value.
A set of independent lines (or segments), one line with its own start and end positions (X,...
A 2D grid of dynamic size which stores any kind of data at each cell.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
EIGEN_STRONG_INLINE const AdjointReturnType t() const
Transpose.
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
#define MRPT_DECLARE_DEPRECATED_FUNCTION(__MSG, __FUNC)
Usage: MRPT_DECLARE_DEPRECATED_FUNCTION("Use XX instead", void myFunc(double));.
std::vector< mrpt::nav::CParameterizedTrajectoryGenerator * > TListPTGs
A list of PTGs (bare pointers)
std::vector< mrpt::nav::CParameterizedTrajectoryGeneratorPtr > TListPTGPtr
A list of PTGs (smart pointers)
mrpt::utils::CStream NAV_IMPEXP & operator<<(mrpt::utils::CStream &o, const mrpt::nav::TCPoint &p)
stlplus::smart_ptr< CParameterizedTrajectoryGenerator > CParameterizedTrajectoryGeneratorPtr
Smart pointer to a PTG.
::mrpt::utils::CStream & operator>>(mrpt::utils::CStream &in, CHolonomicLogFileRecordPtr &pObj)
std::vector< TCPoint > TCPointVector
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Specifies the min/max values for "k" and "n", respectively.
Trajectory points in C-Space.
TCPoint(const float x_, const float y_, const float phi_, const float t_, const float dist_, const float v_, const float w_)
For usage when passing a dynamic number of (numeric) arguments to a function, by name.