Main Page   Modules   Class Hierarchy   Compound List   File List   Compound Members  

SfGrad.h

00001 //
00002 // SfGrad.h
00003 //
00004 // gradient class definitions
00005 
00006 //
00007 // Copyright 1999 by Kurt Konolige
00008 //
00009 // The author hereby grants to SRI permission to use this software.
00010 // The author also grants to SRI permission to distribute this software
00011 // to schools for non-commercial educational use only.
00012 //
00013 // The author hereby grants to other individuals or organizations
00014 // permission to use this software for non-commercial
00015 // educational use only.  This software may not be distributed to others
00016 // except by SRI, under the conditions above.
00017 //
00018 // Other than these cases, no part of this software may be used or
00019 // distributed without written permission of the author.
00020 //
00021 // Neither the author nor SRI make any representations about the 
00022 // suitability of this software for any purpose.  It is provided 
00023 // "as is" without express or implied warranty.
00024 //
00025 // Kurt Konolige
00026 // Senior Computer Scientist
00027 // SRI International
00028 // 333 Ravenswood Avenue
00029 // Menlo Park, CA 94025
00030 // E-mail:  konolige@ai.sri.com
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    uses res in mm for grid cell resolution, default is 100
00129 
00130    if turnRadius is 0 then it will use the normal parameters and
00131    action for movement, buf it turnRadius is not 0 it will make the
00132    grad action that will back up if the robot needs to travel backwards 
00133    and a circle with radius turnRadius is obstructed
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 // the SfGradGrid class is a specialized grid for doing gradient movement
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); // resolution, size and location
00227   void SetGradSize(double res, SfVector &s); // reset size, in Process()
00228   bool WaitingToSync(void); // returns true if the setsize hasn't been done yet
00229   void draw(SfWin *w);  // display it, with outlines
00230   void AddToFrame(SfFr *f); // add GUI stuff
00231 
00232   /*
00233     fcost is the number of grid elements away from an obstacle that
00234     the gradient won't go within, while fdecay is how far away from
00235     the obstacle that the robot will try to avoid if possible
00236   */
00237 
00238   double fcost, fdecay;         // frontier expansion parameters
00239   void SetGoal(ArPose &g, bool doit = true); // sets the goal to a point, in Process()
00240   void ResetGoal(ArPose &g); // resets the goal, no new grad grid
00241   void SetGoal2(ArPose &g, bool doit = true); // sets the goal to two points
00242   void SetGoal(SfVector &v, bool doit = true); // sets the goal to a line
00243   // gradient values
00244   double GradValX(double, double);// interpolated gradient value at this RW pos
00245   double GradValY(double, double);// interpolated gradient value at this RW pos
00246   // returns found point p at dist on path from robot
00247   bool CalcPathPoint(double dist, ArPose &p); 
00248   // returns found point p at dist on path from start
00249   bool CalcPathPoint(double dist, ArPose &start, ArPose &p); 
00250   double GoalDist();            // distance to goal from robot
00251   double GoalDist(ArPose &p); // distance to goal from p
00252   bool CalcField(int n = -1); // reset and recalculate the field
00253   void CalcObs();      // reset and calculate obstacles, for debugging
00254   // boundaries
00255   void SetMaxBounds(double width, double height);
00256   void SetSearchBounds(SfVector &s);
00257   void UseArtifacts(bool on); // sets grad bounds to world by default
00258   void UseGridMap(SfGrid *g); // we want to use our map now
00259   bool UseGridMap(bool on); // turn it on and off
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;          // goal position
00269   ArPose goal2;         // 2nd goal position, for line goal
00270   ArPose goalCoords;    // goal coordinate system 
00271   ArTransform goalTrans;        // transform to goal coord system
00272   bool doGoal;                  // if we're trying to get somewhere
00273   int goalStatus;               // what we're doing
00274 
00275   void useSonar(bool on); // turns on/off sonar obstacles
00276   void useLRF(bool on); // turns on/off LRF obstacles
00277   bool lrfFlag, sonarFlag;      // use flags, for these sensors
00278 
00279   bool displayGrad, displayAcc; // display these guys
00280   bool displayGoal, displayPath;
00281   int gcalcCycleNum;            // how many cycles to wait between gradient calcs
00282 
00283   bool useStraightLine; // if we should use a straight line, for final approach
00284 
00285   bool pathFlag;                // true if shortcut has been found
00286   double pathDist;              // how far ahead we look
00287   bool CheckCompletePath(); // returns true if there is a path from the robot to goal 
00288 
00289   // artifacts and maps as obstacles
00290   bool useArtifacts;
00291   bool useGridMap;
00292 
00293   // device
00294   ArRangeDevice *lr0;           // laser range finder stashed here
00295   // device
00296   ArSonarDevice *sd0;           // sonar device stashed here
00297   // should be private
00298   int pnum;                     // how much to propagate
00299 
00300   // parameter popup window, generic pointer
00301   void *grad_params;
00302 
00303   // testing...
00304   void DoExpand()
00305     { ArPose p; p.setX(goal.getX()); p.setY(goal.getY()); setGoal(p, false); Expand(fcost, fdecay); };
00306   float *acc, *oacc;            // acceleration array (where we don't want to go) 
00307 
00308  protected:
00309   std::list<ArFunctor *> newGoalCBs;
00310   virtual void AddGoal(); // adds goal, can be subclassed
00311   void AddGoalPoint(double x, double y); // adds goal points during AddGoal call
00312   void Process();       // UTask process
00313   void Reset();                 // resets arrays
00314   bool CheckCompletePathStraight(void);
00315 
00316  private:
00317   // boundaries
00318   double gradMaxWidth, gradMaxHeight;   // absolute max gradient grid
00319 
00320   // cost grid, for artifacts and map grid obstacles
00321   SfGrid *costGrid;
00322 
00323   // propagation functions
00324   int Propagate(int z);         // send the wavefront on z times
00325   void CalcCost(int i);         // calculate cost at index i 
00326   void Expand(double, double);  // expand the current cost frontier
00327   void Gradient();              // calculate gradient from cost field
00328   void Grad(int k);             // calculate gradient for a single point
00329   void SetGoalPose(SfVector &v); // sets line goal coord system
00330   void setGradSize(double res, SfVector &s); // reset size, only within Process()
00331   void setGoal(ArPose &g, bool doit = true); // sets the goal to a point, within Process()
00332   float *cost, *ocost;          // cost array
00333   float *gradx, *ogradx, *grady, *ogrady; // gradient values
00334   int *count;                   // for histogram
00335   int gridsize;                 // total length of grid
00336   ArPose pathPoint;             // dx, dy point ahead of robot on path (shortcut)
00337   bool firstGoal;               // for defining 2 point goals
00338   int gcalcCycleN;              // current calculation cycle
00339   SfGrid *gridMap;              // set if we want to use a map from a grid
00340 
00341   // wave propagation arrays
00342   bool *done, *odone;           // where we've finished
00343   int wf1[WAVEMAX];             // wavefront array 
00344   int wf2[WAVEMAX];             // wavefront array 
00345   int wf3[WAVEMAX];             // wavefront array 
00346   int *wp, *w, *wshunt;         // wavefront array pts 
00347   int w_end, wp_end, wshunt_end; // end points of arrays
00348   bool obcost;                  // if we're doing obstacles
00349   float thresh;                 // for smoothing wavefront values
00350   float inc_thresh;             // how much to increment threshold
00351 
00352   // sync to Process()
00353   SfVector syncingSetSizeValue;
00354   double syncingSetSizeGridRes;
00355   bool needToDoSyncingSetSize;
00356   
00357   ArPose syncingSetGoalGoal;
00358   bool syncingSetGoalDoit;
00359   bool needToDoSyncingSetGoal;
00360 
00361   // updating GUI
00362   void update();
00363 };
00364 
00365 
00366 //
00367 // Aria action for moving robot along the gradient
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   // This gets called whenever there is a new goal
00396   virtual void newGoal(void); 
00397  protected:
00398   ArActionDesired myDesired;
00399   ArFunctorC<SfGradAction> newGoalCB;
00400   // things for the turn around
00401   bool isRadiusClear(int radius);
00402   double velBack;
00403   double turnRadius;
00404   ArPose lastBacked;
00405   bool backing;
00406 
00407   // if this is 0 there is no backing, if this is 1 there is backing
00408   // when appropriate, and 2 there is backing all the time
00409   int canBack;
00410   // movement velocities and params
00411   double velSlow, velMid, velTop;
00412   double lowAngle, highAngle;   // low is ok, high is off (in degrees)
00413   SfGradGrid *grad;
00414 
00415   ArPose pathPoint;             // dx, dy point ahead of robot on path (shortcut)
00416   double pathDist;              // how far ahead we look
00417   bool pathFlag;                // true if path shortcut ok
00418   double CurveSpeed(ArPose &begin, ArPose &end); // max velocity on this curve
00419   double MaxSpeed(double dth);  // max speed for this error angle
00420   double robotAcc;              // acceleration, in mm / sec ** 2
00421   int safeDegrees;  // degrees in which we'll check for good degrees
00422   double clearDist; // the amount we want to clear where the turn radius was bad by
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   // This gets called whenever there is a new goal
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 // Callback function for creating GUI items
00456 extern void (* sfGradFrameFn)(SfGradGrid *x, SfFr *f);
00457 extern void (* sfGradUpdateFn)(SfGradGrid *x, SfFr *f);
00458 
00459 // External grad window
00460 extern SfWin *my_grad_win;
00461 extern SfGradGrid *my_grad;             // globals for instances
00462 extern SfGradAction *theGradAction;     
00463 extern SfGradFinalApproachAction *theGradFinalApproachAction;   
00464 
00465 // C interface fns
00466 
00467 #if 0
00468 extern "C" {
00469   // sets up a single goal, starts robot
00470   void sfGradSetGoal(float x, float y);
00471 }
00472 #endif
00473 
00474 
00475 #endif  // SFGRAD_H

Generated on Tue Nov 12 17:49:34 2002 for Saphira by doxygen1.2.13.1 written by Dimitri van Heesch, © 1997-2001