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
00029
00030
00031
00032
00033 #ifndef SFGRAD_H
00034 #define SFGRAD_H
00035
00036 #undef SFEXPORT
00037 #if defined(MS_WINDOWS) && !defined(SF_STATIC)
00038 #ifdef GRAD_EXPORTS
00039 #define _declspec( dllexport )
00040 #else
00041 #define _declspec( dllimport )
00042 #endif
00043 #else
00044 #define SFEXPORT
00045 #endif
00046
00122
00123
00125 void sfGradInit(void);
00127
00128
00129
00130
00131
00132
00133
00134
00135 void sfGradInitRes(int res, int turnRadius);
00136
00138 void sfGradSetMap(void *p);
00139
00141 void sfGradUseArtifacts(int useArtifacts);
00143 void sfGradUseMap(int useMap);
00144
00146 void sfGradUseSonar(int useSonar);
00148 void sfGradUseLaser(int useLaser);
00149
00152 void sfGradSetGoal(float x, float y);
00153
00155 void sfGradSetSpeed(int high, int mid, int back);
00156
00158 void sfGradDoGoal(int which);
00159
00161 void sfGradStop(void);
00162
00164 int sfGradStatus(void);
00165
00167 int sfGradIsActive(void);
00168
00170 void sfGradObsParams(int keepout, int decay);
00171
00173 void sfGradSetDone(int close, int done);
00174
00177 int sfGradGetCanBack(void);
00178
00181 void sfGradSetCanBack(int canBack);
00182
00185 void sfGradSetTurnRadius(int turnRadius);
00186
00189 int sfGradGetTurnRadius(void);
00190
00192 void sfGradSetAcc(int acc);
00193
00195 void sfGradSetPnum(int n);
00196
00198 void sfGradSetMax(int width, int height);
00199
00201 void sfGradUseFinalApproach(int which);
00202
00204 void sfGradSetFinalApproachSpeed(int speed);
00205
00207 int sfGradGetFinalApproachSpeed(void);
00208
00211 #define WAVEMAX 20000
00212
00213
00214
00215
00216
00217 #define statusIdle 0
00218 #define statusActive 1
00219 #define statusDone 2
00220 #define statusFailed 3
00221 #define statusSearching 4
00222
00223 class SfGradGrid : public SfCellIndex, public SfDrawable
00224 {
00225 public:
00226 SfGradGrid(double res, SfVector *s);
00227 void SetGradSize(double res, SfVector &s);
00228 bool WaitingToSync(void);
00229 void draw(SfWin *w);
00230 void AddToFrame(SfFr *f);
00231
00232
00233
00234
00235
00236
00237
00238 double fcost, fdecay;
00239 void SetGoal(ArPose &g, bool doit = true);
00240 void ResetGoal(ArPose &g);
00241 void SetGoal2(ArPose &g, bool doit = true);
00242 void SetGoal(SfVector &v, bool doit = true);
00243
00244 double GradValX(double, double);
00245 double GradValY(double, double);
00246
00247 bool CalcPathPoint(double dist, ArPose &p);
00248
00249 bool CalcPathPoint(double dist, ArPose &start, ArPose &p);
00250 double GoalDist();
00251 double GoalDist(ArPose &p);
00252 bool CalcField(int n = -1);
00253 void CalcObs();
00254
00255 void SetMaxBounds(double width, double height);
00256 void SetSearchBounds(SfVector &s);
00257 void UseArtifacts(bool on);
00258 void UseGridMap(SfGrid *g);
00259 bool UseGridMap(bool on);
00260 bool IsMaxSize();
00261 void BuildPath(std::list<ArPose> *path);
00263 void addNewGoalCallback(ArFunctor *newGoalCB,
00264 ArListPos::Pos position);
00266 void remNewGoalCallback(ArFunctor *newGoalCB);
00267
00268 ArPose goal;
00269 ArPose goal2;
00270 ArPose goalCoords;
00271 ArTransform goalTrans;
00272 bool doGoal;
00273 int goalStatus;
00274
00275 void useSonar(bool on);
00276 void useLRF(bool on);
00277 bool lrfFlag, sonarFlag;
00278
00279 bool displayGrad, displayAcc;
00280 bool displayGoal, displayPath;
00281 int gcalcCycleNum;
00282
00283 bool useStraightLine;
00284
00285 bool pathFlag;
00286 double pathDist;
00287 bool CheckCompletePath();
00288
00289
00290 bool useArtifacts;
00291 bool useGridMap;
00292
00293
00294 ArRangeDevice *lr0;
00295
00296 ArSonarDevice *sd0;
00297
00298 int pnum;
00299
00300
00301 void *grad_params;
00302
00303
00304 void DoExpand()
00305 { ArPose p; p.setX(goal.getX()); p.setY(goal.getY()); setGoal(p, false); Expand(fcost, fdecay); };
00306 float *acc, *oacc;
00307
00308 protected:
00309 std::list<ArFunctor *> newGoalCBs;
00310 virtual void AddGoal();
00311 void AddGoalPoint(double x, double y);
00312 void Process();
00313 void Reset();
00314 bool CheckCompletePathStraight(void);
00315
00316 private:
00317
00318 double gradMaxWidth, gradMaxHeight;
00319
00320
00321 SfGrid *costGrid;
00322
00323
00324 int Propagate(int z);
00325 void CalcCost(int i);
00326 void Expand(double, double);
00327 void Gradient();
00328 void Grad(int k);
00329 void SetGoalPose(SfVector &v);
00330 void setGradSize(double res, SfVector &s);
00331 void setGoal(ArPose &g, bool doit = true);
00332 float *cost, *ocost;
00333 float *gradx, *ogradx, *grady, *ogrady;
00334 int *count;
00335 int gridsize;
00336 ArPose pathPoint;
00337 bool firstGoal;
00338 int gcalcCycleN;
00339 SfGrid *gridMap;
00340
00341
00342 bool *done, *odone;
00343 int wf1[WAVEMAX];
00344 int wf2[WAVEMAX];
00345 int wf3[WAVEMAX];
00346 int *wp, *w, *wshunt;
00347 int w_end, wp_end, wshunt_end;
00348 bool obcost;
00349 float thresh;
00350 float inc_thresh;
00351
00352
00353 SfVector syncingSetSizeValue;
00354 double syncingSetSizeGridRes;
00355 bool needToDoSyncingSetSize;
00356
00357 ArPose syncingSetGoalGoal;
00358 bool syncingSetGoalDoit;
00359 bool needToDoSyncingSetGoal;
00360
00361
00362 void update();
00363 };
00364
00365
00366
00367
00368
00369
00370 class SfGradAction : public ArAction
00371 {
00372 public:
00373 SfGradAction(int velTop, int velMid, int velBack, int turnRadius, int canBack);
00374 virtual ~SfGradAction(void);
00375 virtual ArActionDesired *fire(ArActionDesired currentDesired);
00377 virtual void SetSpeed(double high, double mid, double back);
00379 virtual void SetAcc(double);
00381 virtual void SetGrid(SfGradGrid *g);
00383 double nearGoal;
00385 double doneGoal;
00387 virtual int getTurnRadius(void);
00389 virtual void setTurnRadius(int turnRadius);
00391 virtual int getCanBack(void);
00393 virtual void setCanBack(int canBack);
00394
00395
00396 virtual void newGoal(void);
00397 protected:
00398 ArActionDesired myDesired;
00399 ArFunctorC<SfGradAction> newGoalCB;
00400
00401 bool isRadiusClear(int radius);
00402 double velBack;
00403 double turnRadius;
00404 ArPose lastBacked;
00405 bool backing;
00406
00407
00408
00409 int canBack;
00410
00411 double velSlow, velMid, velTop;
00412 double lowAngle, highAngle;
00413 SfGradGrid *grad;
00414
00415 ArPose pathPoint;
00416 double pathDist;
00417 bool pathFlag;
00418 double CurveSpeed(ArPose &begin, ArPose &end);
00419 double MaxSpeed(double dth);
00420 double robotAcc;
00421 int safeDegrees;
00422 double clearDist;
00423 };
00424
00425 class SfGradFinalApproachAction : public ArAction
00426 {
00427 public:
00429 SfGradFinalApproachAction(int doneDist, int speed);
00431 virtual ~SfGradFinalApproachAction(void);
00432 virtual ArActionDesired *fire(ArActionDesired currentDesired);
00433
00435 virtual void SetSpeed(int speed) { mySpeed = speed; }
00437 virtual int GetSpeed(void) { return mySpeed; }
00439 virtual void SetDoneDist(int doneDist) { myDoneDist = doneDist; }
00441 virtual int GetDoneDist(void) { return myDoneDist; }
00443 virtual void SetGrid(SfGradGrid *g);
00444
00445 virtual void newGoal(void);
00446 protected:
00447 ArActionDesired myDesired;
00448 ArFunctorC<SfGradFinalApproachAction> myNewGoalCB;
00449 int myDoneDist;
00450 int mySpeed;
00451 SfGradGrid *grad;
00452 };
00453
00454
00455
00456 extern void (* sfGradFrameFn)(SfGradGrid *x, SfFr *f);
00457 extern void (* sfGradUpdateFn)(SfGradGrid *x, SfFr *f);
00458
00459
00460 extern SfWin *my_grad_win;
00461 extern SfGradGrid *my_grad;
00462 extern SfGradAction *theGradAction;
00463 extern SfGradFinalApproachAction *theGradFinalApproachAction;
00464
00465
00466
00467 #if 0
00468 extern "C" {
00469
00470 void sfGradSetGoal(float x, float y);
00471 }
00472 #endif
00473
00474
00475 #endif // SFGRAD_H