00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef CParameterizedTrajectoryGenerator_H
00029 #define CParameterizedTrajectoryGenerator_H
00030
00031 #include <mrpt/utils/CDynamicGrid.h>
00032 #include <mrpt/utils/CStream.h>
00033 #include <mrpt/reactivenav/link_pragmas.h>
00034
00035 namespace mrpt
00036 {
00037 namespace reactivenav
00038 {
00039 using namespace mrpt::utils;
00040
00048 class RNAVDLLIMPEXP CParameterizedTrajectoryGenerator
00049 {
00050 public:
00053 CParameterizedTrajectoryGenerator(
00054 float refDistance,
00055 float xResolution,
00056 float yResolution,
00057 float V_MAX,
00058 float W_MAX,
00059 float system_TAU,
00060 float system_DELAY,
00061 vector_float securityDistances);
00062
00063
00066 virtual std::string getDescription() = 0;
00067
00071 void initializeCollisionsGrid(float refDistance, float xResolution, float yResolution, vector_float securityDistances);
00072
00075 virtual ~CParameterizedTrajectoryGenerator();
00076
00079 void simulateTrajectories(
00080 unsigned short alfaValuesCount,
00081 float max_time,
00082 float max_dist,
00083 unsigned int max_n,
00084 float diferencial_t,
00085 float min_dist,
00086 float *out_max_acc_v = NULL,
00087 float *out_max_acc_w = NULL);
00088
00091 virtual void lambdaFunction( float x, float y, int &out_k, float &out_d );
00092
00095 void directionToMotionCommand( unsigned short k, float &out_v, float &out_w );
00096
00097 size_t getAlfaValuesCount() { return alfaValuesCount; };
00098 size_t getPointsCountInCPath_k(unsigned short k) { return CPoints[k].size(); };
00099
00100 void getCPointWhen_d_Is ( float d, unsigned short k, float &x, float &y, float &phi, float &t, float *v = NULL, float *w = NULL );
00101
00102 float GetCPathPoint_x( unsigned short k, int n ){ return CPoints[k][n].x; }
00103 float GetCPathPoint_y( unsigned short k, int n ){ return CPoints[k][n].y; }
00104 float GetCPathPoint_phi(unsigned short k, int n ){ return CPoints[k][n].phi; }
00105 float GetCPathPoint_t( unsigned short k, int n ){ return CPoints[k][n].t; }
00106 float GetCPathPoint_d( unsigned short k, int n ){ return CPoints[k][n].dist; }
00107 float GetCPathPoint_v( unsigned short k, int n ){ return CPoints[k][n].v; }
00108 float GetCPathPoint_w( unsigned short k, int n ){ return CPoints[k][n].w; }
00109
00110 void allocMemForVerticesData( int nVertices );
00111
00112 void setVertex_xy( unsigned short k, int n, int m, float x, float y )
00113 {
00114 vertexPoints_x[k][ n*nVertices + m ] = x;
00115 vertexPoints_y[k][ n*nVertices + m ] = y;
00116 }
00117
00118 float getVertex_x( unsigned short k, int n, int m )
00119 {
00120 int idx = n*nVertices + m;
00121
00122 return vertexPoints_x[k][idx];
00123 }
00124
00125 float getVertex_y( unsigned short k, int n, int m )
00126 {
00127 int idx = n*nVertices + m;
00128
00129 return vertexPoints_y[k][idx];
00130 }
00131
00132 float* getVertixesArray_x( unsigned short k, int n )
00133 {
00134 int idx = n*nVertices;
00135 return &vertexPoints_x[k][idx];
00136 }
00137
00138 float* getVertixesArray_y( unsigned short k, int n )
00139 {
00140 int idx = n*nVertices;
00141 return &vertexPoints_y[k][idx];
00142 }
00143
00144 unsigned int getVertixesCount() { return nVertices; }
00145
00146 float getMax_V() { return V_MAX; }
00147 float getMax_W() { return W_MAX; }
00148 float getMax_V_inTPSpace() { return maxV_inTPSpace; }
00149
00153 float index2alfa( unsigned short k )
00154 {
00155 return (float)(M_PI * (-1 + 2 * (k+0.5f) / ((float)alfaValuesCount) ));
00156 }
00157
00161 unsigned short alfa2index( float alfa )
00162 {
00163 if (alfa>M_PI) alfa-=(float)M_2PI;
00164 if (alfa<-M_PI) alfa+=(float)M_2PI;
00165 return (unsigned short)(0.5f*(alfaValuesCount*(1+alfa/M_PI) - 1));
00166 }
00167
00170 void debugDumpInFiles(int nPT);
00171
00174 class CColisionGrid
00175 {
00176 public:
00177 CColisionGrid( float rangoXmin, float rangoXmax, float rangoYmin, float rangoYmax, float resolucionX, float resolucionY, float securityDistance );
00178 ~CColisionGrid();
00179
00180
00181
00182 bool saveToFile( mrpt::utils::CStream* fil );
00183 bool loadFromFile( mrpt::utils::CStream* fil );
00184
00185
00186
00187 float getXmin() { return rangoXmin; };
00188 float getXmax() { return rangoXmax; };
00189 float getYmin() { return rangoYmin; };
00190 float getYmax() { return rangoYmax; };
00191 int getCellsX() { return nCeldasX; }
00192 int getCellsY() { return nCeldasY; }
00193 int getCellsTotal() { return nCeldasX*nCeldasY; };
00194 int getCellIndex( int idx_x, int idx_y ) { return idx_y * nCeldasX + idx_x; }
00195 float getSecurityDistance() { return securityDistance; }
00196
00197
00198
00199
00200
00201
00202
00203 void getTPObstacle( float obsX, float obsY,size_t &nRegistros, unsigned short** distancias, unsigned short **ks );
00204
00205 void getCellCoords( int cellIndex, float &x1, float &y1, float &x2, float &y2 );
00206
00213 void updateCellInfo( int cellIndex, unsigned short k, float dist );
00214
00217 int Coord_a_indice( float x, float y);
00218
00221 void Coord_a_indice( float x, float y, int &idx_x, int &idx_y);
00222
00225 struct TCell
00226 {
00227 int nRegistros;
00228 int nRegistrosMax;
00229
00230
00231 unsigned short *distanciaCol;
00232 unsigned short *kColision;
00233 };
00234
00235 private:
00236 int nCeldasX, nCeldasY;
00237 float rangoXmin, rangoXmax, rangoYmin, rangoYmax;
00238 float resolucionX, resolucionY;
00239 float securityDistance;
00240
00241 TCell *celdas;
00242
00243 };
00244
00247 class CListColisionGrids : public std::vector<CColisionGrid*>
00248 {
00249 public:
00250
00251
00252 bool SaveToFile( const std::string &filename );
00253 bool LoadFromFile( const std::string &filename );
00254 };
00255
00256
00257 CListColisionGrids collisionGrids;
00258
00259 float refDistance;
00260
00263 virtual void PTG_Generator( float alfa, float t, float x, float y, float phi, float &v, float &w) = 0;
00264
00267 virtual bool PTG_IsIntoDomain( float x, float y ) = 0;
00268
00269 protected:
00270
00271 float V_MAX, W_MAX;
00272 float TAU, DELAY;
00273
00274 float turningRadiusReference;
00275
00279 struct TCellForLambdaFunction
00280 {
00281 TCellForLambdaFunction()
00282 {
00283 k_min=k_max=n_min=n_max=-1;
00284 }
00285
00286 int k_min,k_max,n_min,n_max;
00287 };
00288
00291 CDynamicGrid<TCellForLambdaFunction> m_lambdaFunctionOptimizer;
00292
00293
00294 float maxV_inTPSpace;
00295
00296 bool flag1, flag2;
00297
00300 unsigned int alfaValuesCount;
00301
00304 struct TCPoint
00305 {
00306 TCPoint( float x,
00307 float y,
00308 float phi,
00309 float t,
00310 float dist,
00311 float v,
00312 float w)
00313 {
00314 this->x = x;
00315 this->y = y;
00316 this->phi = phi;
00317 this->t = t;
00318 this->dist = dist;
00319 this->v = v;
00320 this->w = w;
00321 };
00322
00323 float x, y, phi,t, dist,v,w;
00324 };
00325 typedef std::vector<TCPoint> TCPointVector;
00326 std::vector<TCPointVector> CPoints;
00327
00330 std::vector<vector_float> vertexPoints_x,vertexPoints_y;
00331 int nVertices;
00332
00335 void FreeMemory();
00336
00337 };
00338 }
00339 }
00340
00341
00342 #endif
00343