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
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050 class RNAVDLLIMPEXP CParameterizedTrajectoryGenerator
00051 {
00052 protected:
00053
00054
00055
00056
00057
00058
00059 CParameterizedTrajectoryGenerator(const TParameters<double> ¶ms);
00060
00061
00062 void initializeCollisionsGrid(float refDistance,float resolution);
00063
00064 public:
00065
00066
00067
00068
00069
00070
00071
00072
00073 static CParameterizedTrajectoryGenerator * CreatePTG(const TParameters<double> ¶ms);
00074
00075
00076
00077 virtual std::string getDescription() = 0;
00078
00079
00080 virtual ~CParameterizedTrajectoryGenerator() { }
00081
00082
00083
00084 void simulateTrajectories(
00085 uint16_t alfaValuesCount,
00086 float max_time,
00087 float max_dist,
00088 unsigned int max_n,
00089 float diferencial_t,
00090 float min_dist,
00091 float *out_max_acc_v = NULL,
00092 float *out_max_acc_w = NULL);
00093
00094
00095
00096 virtual void lambdaFunction( float x, float y, int &out_k, float &out_d );
00097
00098
00099
00100 void directionToMotionCommand( uint16_t k, float &out_v, float &out_w );
00101
00102 size_t getAlfaValuesCount() { return alfaValuesCount; };
00103 size_t getPointsCountInCPath_k(uint16_t k) { return CPoints[k].size(); };
00104
00105 void getCPointWhen_d_Is ( float d, uint16_t k, float &x, float &y, float &phi, float &t, float *v = NULL, float *w = NULL );
00106
00107 float GetCPathPoint_x( uint16_t k, int n ){ return CPoints[k][n].x; }
00108 float GetCPathPoint_y( uint16_t k, int n ){ return CPoints[k][n].y; }
00109 float GetCPathPoint_phi(uint16_t k, int n ){ return CPoints[k][n].phi; }
00110 float GetCPathPoint_t( uint16_t k, int n ){ return CPoints[k][n].t; }
00111 float GetCPathPoint_d( uint16_t k, int n ){ return CPoints[k][n].dist; }
00112 float GetCPathPoint_v( uint16_t k, int n ){ return CPoints[k][n].v; }
00113 float GetCPathPoint_w( uint16_t k, int n ){ return CPoints[k][n].w; }
00114
00115 void allocMemForVerticesData( int nVertices );
00116
00117 void setVertex_xy( uint16_t k, int n, int m, float x, float y )
00118 {
00119 vertexPoints_x[k][ n*nVertices + m ] = x;
00120 vertexPoints_y[k][ n*nVertices + m ] = y;
00121 }
00122
00123 float getVertex_x( uint16_t k, int n, int m )
00124 {
00125 int idx = n*nVertices + m;
00126
00127 return vertexPoints_x[k][idx];
00128 }
00129
00130 float getVertex_y( uint16_t k, int n, int m )
00131 {
00132 int idx = n*nVertices + m;
00133
00134 return vertexPoints_y[k][idx];
00135 }
00136
00137 float* getVertixesArray_x( uint16_t k, int n )
00138 {
00139 int idx = n*nVertices;
00140 return &vertexPoints_x[k][idx];
00141 }
00142
00143 float* getVertixesArray_y( uint16_t k, int n )
00144 {
00145 int idx = n*nVertices;
00146 return &vertexPoints_y[k][idx];
00147 }
00148
00149 unsigned int getVertixesCount() { return nVertices; }
00150
00151 float getMax_V() { return V_MAX; }
00152 float getMax_W() { return W_MAX; }
00153 float getMax_V_inTPSpace() { return maxV_inTPSpace; }
00154
00155
00156
00157
00158 float index2alfa( uint16_t k )
00159 {
00160 return (float)(M_PI * (-1 + 2 * (k+0.5f) / ((float)alfaValuesCount) ));
00161 }
00162
00163
00164
00165
00166 uint16_t alfa2index( float alfa )
00167 {
00168 if (alfa>M_PI) alfa-=(float)M_2PI;
00169 if (alfa<-M_PI) alfa+=(float)M_2PI;
00170 return (uint16_t)(0.5f*(alfaValuesCount*(1+alfa/M_PI) - 1));
00171 }
00172
00173
00174
00175 void debugDumpInFiles(int nPT);
00176
00177
00178
00179
00180
00181 typedef std::map<uint16_t,float> TCollisionCell;
00182
00183
00184 class RNAVDLLIMPEXP CColisionGrid : public mrpt::utils::CDynamicGrid<TCollisionCell>
00185 {
00186 public:
00187 CColisionGrid(float x_min, float x_max,float y_min, float y_max, float resolution )
00188 : mrpt::utils::CDynamicGrid<TCollisionCell>(x_min,x_max,y_min,y_max,resolution)
00189 {
00190 }
00191 virtual ~CColisionGrid() { }
00192
00193 bool saveToFile( mrpt::utils::CStream* fil );
00194 bool loadFromFile( mrpt::utils::CStream* fil );
00195
00196
00197
00198 const TCollisionCell & getTPObstacle( const float obsX, const float obsY) const;
00199
00200
00201
00202
00203
00204
00205 void updateCellInfo( const unsigned int icx, const unsigned int icy, const uint16_t k, const float dist );
00206 };
00207
00208
00209 CColisionGrid m_collisionGrid;
00210
00211
00212 bool SaveColGridsToFile( const std::string &filename );
00213 bool LoadColGridsFromFile( const std::string &filename );
00214
00215
00216 float refDistance;
00217
00218
00219
00220 virtual void PTG_Generator( float alfa, float t, float x, float y, float phi, float &v, float &w) = 0;
00221
00222
00223
00224 virtual bool PTG_IsIntoDomain( float x, float y ) = 0;
00225
00226 protected:
00227
00228 float V_MAX, W_MAX;
00229 float TAU, DELAY;
00230
00231 float turningRadiusReference;
00232
00233
00234
00235
00236 struct TCellForLambdaFunction
00237 {
00238 TCellForLambdaFunction()
00239 {
00240 k_min=k_max=n_min=n_max=-1;
00241 }
00242
00243 int k_min,k_max,n_min,n_max;
00244 };
00245
00246
00247
00248 CDynamicGrid<TCellForLambdaFunction> m_lambdaFunctionOptimizer;
00249
00250
00251 float maxV_inTPSpace;
00252
00253 bool flag1, flag2;
00254
00255
00256
00257 unsigned int alfaValuesCount;
00258
00259
00260
00261 struct TCPoint
00262 {
00263 TCPoint( float x,
00264 float y,
00265 float phi,
00266 float t,
00267 float dist,
00268 float v,
00269 float w)
00270 {
00271 this->x = x;
00272 this->y = y;
00273 this->phi = phi;
00274 this->t = t;
00275 this->dist = dist;
00276 this->v = v;
00277 this->w = w;
00278 };
00279
00280 float x, y, phi,t, dist,v,w;
00281 };
00282 typedef std::vector<TCPoint> TCPointVector;
00283 std::vector<TCPointVector> CPoints;
00284
00285
00286
00287 std::vector<vector_float> vertexPoints_x,vertexPoints_y;
00288 int nVertices;
00289
00290
00291
00292 void FreeMemory();
00293
00294 };
00295
00296
00297 typedef std::vector<mrpt::reactivenav::CParameterizedTrajectoryGenerator*> TListPTGs;
00298
00299 }
00300 }
00301
00302
00303 #endif
00304