WURDE: /home/fwph/code/wurde/modules/laserAvoider/laserAvoider.cpp Source File

00001 /*************
00002  * Obstacle Avoider
00003  * Frederick Heckel & Chris Wilson
00004  * May 2006
00005  *
00006  * Port of the SMRT laser avoider. 
00007  */
00008 
00009 #include <CommsManager.H>
00010 #include <Egomotion.H>
00011 #include <VectorMover.H>
00012 #include <RangeFinder.H>
00013 #include <ObstacleAvoiderTransport.H>
00014 #include <cmath>
00015 #include <stdlib.h>
00016 #include <values.h>
00017 #include <RobotUtil.H>
00018 
00019 using namespace WURDE;
00020 using namespace std;
00021 
00022 static const float AVOIDANCE_RADIUS_DEFAULT = 0.40;
00023 static const float MIN_AVOIDANCE_RADIUS = 0.40;
00024 
00025 static const float FRONTAL_AVOIDANCE_DEPTH = 0.40;
00026 
00027 static const float CLOSE_ENOUGH_DISTANCE = 0.07;
00028 static const float CLOSE_ENOUGH_ANGLE = 0.02;
00029 
00030 static const float WAYPOINT_DISTANCE_THRESHOLD = 0.3;
00031 static const float WAYPOINT_ANGLE_THRESHOLD = M_PI/4;
00032 static const float WAYPOINT_DIFF_THRESHOLD = 0.5;
00033 static const float WAYPOINT_DIFF_SENSITIVITY = 0.45;
00034 
00035 vector<double> closeLaser(vector<double> &ranges, RangeFinderInfoStruct &info, double r);
00036 void updateWaypoint(RangeFinderInfoStruct &info,Pose currPose,Pose &goalPose,Pose &waypoint,vector<double> &closedReadings,bool angleRequest,bool &path_obstructed);
00037 
00038 enum OAState {IDLE, MOVING, ROTATE};
00039 
00040 int main(int argc, char *argv[]){
00041        CommsManager myManager("laserAvoider");
00042        
00043        Logger myLogger("laserAvoiderLogger");
00044        
00045        VectorMover myNav(STRAT_ASSIGNED,"VectorMoverClient");
00046        Egomotion myEgomotion(STRAT_ASSIGNED,"EgoClient");
00047        RangeFinder myLaser(STRAT_ASSIGNED,"LaserClient");
00048        ObstacleAvoiderTransport myAvoider("SimpleAvoider");
00049        
00050        Pose currPose(0,0,0);
00051        Pose goalPose(0,0,0); //Global Coordinate Frame
00052        Pose waypoint(0,0,0); //Local Coordinate Frame
00053        vector<double> closedRanges;
00054        double avoidRadius = AVOIDANCE_RADIUS_DEFAULT;
00055        OAState state=IDLE;
00056        bool newData = false;
00057        bool angleRequest = false;
00058        bool path_obstructed = false;
00059        
00060        myManager.setRealName("laserAvoider");
00061        myManager.setHelpString("\nBasic laser avoider module. Attempts to travel to a point, and if it cannot, sets a waypoint to another nearby point in the world. May not be completely safe for ATRV Jr. robots.\n\n");
00062        myManager.parseOptions(argc,argv);
00063        
00064        myManager.registerConsumer(&myLaser);
00065        myManager.registerConsumer(&myEgomotion);
00066        myManager.registerConsumer(&myNav);
00067        myManager.registerSupplier(&myAvoider);
00068        
00069        myManager.setSleep(0.05);
00070        
00071        myLaser.doPing();
00072        
00073        myNav.stop();
00074        
00075        while(myManager.runUpdate()==STATE_RUN||
00076              myManager.getState()==STATE_IDLE){
00077               if(myEgomotion.newData()){
00078                      myEgomotion.getNextData();
00079                      currPose=myEgomotion.data.location;
00080                      if(state==MOVING || state==ROTATE){
00081                             float dist = hypot(currPose.x()-goalPose.x(),currPose.y()-goalPose.y());
00082                             float angle = goalPose.theta()-currPose.theta();
00083                             if(angle > M_PI)
00084                                    angle = angle - (2*M_PI);
00085                             if(angle < -M_PI)
00086                                    angle = angle + (2*M_PI);
00087                             if((dist<CLOSE_ENOUGH_DISTANCE)){
00088                                    if(angleRequest){
00089                                           state = ROTATE;
00090                                           if(abs(angle)<CLOSE_ENOUGH_ANGLE){
00091                                                  g_logdebug<<"Goal Achieved"<<endl;
00092                                                  myNav.stop();
00093                                                  state = IDLE;
00094                                                  waypoint.x(0);
00095                                                  waypoint.y(0);
00096                                                  waypoint.theta(currPose.theta());
00097                                                  path_obstructed=false;
00098                                           }
00099                                    }
00100                                    else{
00101                                           g_logdebug<<"Goal Achieved"<<endl;
00102                                           myNav.stop();
00103                                           state = IDLE;
00104                                           waypoint.x(0);
00105                                           waypoint.y(0);
00106                                           waypoint.theta(currPose.theta());
00107                                    }
00108                             }
00109                             if(state==ROTATE && abs(waypoint.theta()-currPose.theta())<CLOSE_ENOUGH_ANGLE && dist>CLOSE_ENOUGH_DISTANCE){
00110                                    state = MOVING;
00111                             }
00112                             myAvoider.data.distanceToGoal = dist;
00113                             myAvoider.data.angleToGoal = angle;
00114                      }
00115                      else{
00116                             myAvoider.data.distanceToGoal = 0.0;
00117                             myAvoider.data.angleToGoal = 0.0;
00118                      }
00119                      myAvoider.data.pathObstructed = path_obstructed;
00120                      myAvoider.publishData();
00121                      newData = true;
00122               }
00123               
00124               if(myLaser.newData()){
00125                      myLaser.getNextData();
00126                      closedRanges=closeLaser(myLaser.data.ranges, myLaser.info,avoidRadius);
00127                      newData = true;
00128               }
00129               
00130               if(myLaser.newInfo()){
00131                      myLaser.getNextInfo();
00132                      newData = true;
00133               }
00134               
00135               //HANDLE REQUESTS
00136               if(myAvoider.newRequest()){
00137                      newData = true;
00138                      myAvoider.getNextRequest();
00139                      if(myAvoider.requests.brake.getValue()){
00140                             myNav.stop();
00141                             state = IDLE;
00142                             goalPose=currPose;
00143                             waypoint.x(0);
00144                             waypoint.y(0);
00145                             waypoint.theta(0);
00146                             path_obstructed=false;
00147                             angleRequest=false;
00148                             newData=false;
00149                      } 
00150                      else{
00151                             if(myAvoider.requests.avoidanceRadius.getValue() >= MIN_AVOIDANCE_RADIUS)
00152                                    avoidRadius = myAvoider.requests.avoidanceRadius.getValue();
00153                             if(myAvoider.requests.absolute.getValue()){
00154                                    //WE HAVE AN ABSOLUTE GOAL POSE REQUEST
00155                                    if(myAvoider.requests.goalHasLocation.getValue()){
00156                                           if(myAvoider.requests.goalHasTheta.getValue()){
00157                                                  //COMPLETE GOAL POSE REQUESTED
00158                                                  goalPose = myAvoider.requests.goalPose.getValue();
00159                                                  state = MOVING;
00160                                                  angleRequest = true;
00161                                                  g_logdebug<<"Received Request: "<<goalPose.x()<<","<<goalPose.y()<<","<<goalPose.theta()<<endl;
00162                                           }
00163                                           else{
00164                                                  //ONLY X,Y LOCATION REQUESTED
00165                                                  goalPose.x(myAvoider.requests.goalPose.getValue().x());
00166                                                  goalPose.y(myAvoider.requests.goalPose.getValue().y());
00167                                                  goalPose.theta(currPose.theta());
00168                                                  state = MOVING;
00169                                                  angleRequest = false;
00170                                                  g_logdebug<<"Received Request: "<<goalPose.x()<<","<<goalPose.y()<<endl;
00171                                           }
00172                                    }
00173                                    else {
00174                                           if(myAvoider.requests.goalHasTheta.getValue()){
00175                                                  //ONLY ANGLE REQUESTED
00176                                                  goalPose.x(currPose.x());
00177                                                  goalPose.y(currPose.y());
00178                                                  goalPose.theta(myAvoider.requests.goalPose.getValue().theta());
00179                                                  angleRequest = true;
00180                                                  state = ROTATE;
00181                                                  g_logdebug<<"Received Request: "<<goalPose.theta()<<endl;
00182                                           }
00183                                    }
00184                             }
00185                             else {
00186                                    //WE HAVE A RELATIVE GOAL POSE REQUEST
00187                                    if(myAvoider.requests.goalHasLocation.getValue()){
00188                                           if(myAvoider.requests.goalHasTheta.getValue()){
00189                                                  //COMPLETE GOAL POSE REQUESTED
00190                                                  double dist=hypot(myAvoider.requests.goalPose.getValue().x(),myAvoider.requests.goalPose.getValue().y());
00191                                                  double angle=currPose.theta()+atan2(myAvoider.requests.goalPose.getValue().y(),myAvoider.requests.goalPose.getValue().x());
00192                                                  goalPose.x(dist*cos(angle)+currPose.x());
00193                                                  goalPose.y(dist*sin(angle)+currPose.y());
00194                                                  goalPose.theta(myAvoider.requests.goalPose.getValue().theta()+currPose.theta());
00195                                                  state = MOVING;
00196                                                  angleRequest = true;
00197                                                  g_logdebug<<"Received Request: "<<goalPose.x()<<","<<goalPose.y()<<","<<goalPose.theta()<<endl;
00198                                           }
00199                                           else{
00200                                                  //ONLY X,Y LOCATION REQUESTED
00201                                                  double dist=hypot(myAvoider.requests.goalPose.getValue().x(),myAvoider.requests.goalPose.getValue().y());
00202                                                  double angle=currPose.theta()+atan2(myAvoider.requests.goalPose.getValue().y(),myAvoider.requests.goalPose.getValue().x());
00203                                                  goalPose.x(dist*cos(angle)+currPose.x());
00204                                                  goalPose.y(dist*sin(angle)+currPose.y());
00205                                                  goalPose.theta(currPose.theta());
00206                                                  state = MOVING;
00207                                                  angleRequest = false;
00208                                                  g_logdebug<<"Received Request: "<<goalPose.x()<<","<<goalPose.y()<<endl;
00209                                           }
00210                                    }
00211                                    else {
00212                                           if(myAvoider.requests.goalHasTheta.getValue()){
00213                                                  //ONLY THETA REQUESTED
00214                                                  goalPose.x(currPose.x());
00215                                                  goalPose.y(currPose.y());
00216                                                  goalPose.theta(currPose.theta()+myAvoider.requests.goalPose.getValue().theta());
00217                                                  state = ROTATE;
00218                                                  angleRequest = true;
00219                                                  g_logdebug<<"Recieved Request: "<<goalPose.theta()<<endl;
00220                                           }
00221                                    }
00222                             }
00223                             if(state==MOVING || state==ROTATE){
00224                                    double dist = hypot(currPose.x()-goalPose.x(),currPose.y()-goalPose.y());
00225                                    if(dist<CLOSE_ENOUGH_DISTANCE){
00226                                           if(!angleRequest){
00227                                                  g_logdebug<<"Already There"<<endl;
00228                                                  myNav.stop();
00229                                                  state = IDLE;
00230                                                  waypoint.x(0);
00231                                                  waypoint.y(0);
00232                                                  waypoint.theta(currPose.theta());
00233                                                  angleRequest = false;
00234                                           }
00235                                           else{
00236                                                  if(abs(currPose.theta()-goalPose.theta())<CLOSE_ENOUGH_ANGLE){
00237                                                         g_logdebug<<"Already There"<<endl;
00238                                                         myNav.stop();
00239                                                         state = IDLE;
00240                                                         waypoint.x(0);
00241                                                         waypoint.y(0);
00242                                                         waypoint.theta(currPose.theta());
00243                                                         angleRequest = false;
00244                                                  }
00245                                                  else{
00246                                                         state = ROTATE;
00247                                                         waypoint.x(0);
00248                                                         waypoint.y(0);
00249                                                         waypoint.theta(goalPose.theta());
00250                                                  }
00251                                           }
00252                                    }  
00253                             }
00254                      }
00255               }
00256               
00257               //MOVE
00258               if(newData){
00259                      if(state == MOVING){
00260                             updateWaypoint(myLaser.info,currPose,goalPose,waypoint,closedRanges,angleRequest,path_obstructed);
00261                             if(!path_obstructed){
00262                                    if(waypoint.x()==0 && waypoint.y()==0){
00263                                           myNav.rotateToAngle(waypoint.theta());
00264                                           path_obstructed = false;
00265                                           state = ROTATE;
00266                                    }
00267                                    else{
00268                                           myNav.moveToRelativePose(waypoint);
00269                                    }
00270                             }
00271                             else {
00272                                    myNav.stop();
00273                                    g_logdebug <<"Path Blocked"<< endl;
00274                             }
00275                      }
00276                      else if(state == ROTATE){
00277                             myNav.rotateToAngle(goalPose.theta());
00278                             path_obstructed=false;
00279                      }
00280                      else if(state == IDLE){
00281                             myNav.stop();
00282                      }
00283               }
00284        }
00285        
00286        myNav.stop();
00287        //       myManager.runUpdate();
00288        myManager.cleanUp();
00289        
00290        return 0;
00291 }
00292 
00297 vector<double> closeLaser(vector<double> &ranges, RangeFinderInfoStruct &info, double r){
00298        vector<double> outranges = ranges;
00299        float angleMin=info.angle_min.getValue();
00300        float angleSep=info.theta_separation.getValue();
00301        float maxAng;
00302        int maxIndex;            //theta is the angle between laser i and i(+/-)j.
00303        float d;                //tempRange is the distance from the robot to the
00304        float theta;            //intersect of ranges[i(+/-)j] and the circle
00305        float tempRange;        //with center at ranges[i] and radius r.
00306        
00307        for(unsigned int i=0; i < ranges.size()-1; i++){
00308               maxAng = asin(r/ranges[i]);
00309               maxIndex = (int)(maxAng/angleSep);
00310               if(maxIndex > 90)                         //in case maxIndex becomes to large
00311                      maxIndex = 90;                     //and bogs down computation
00312               for(int j=0;j<maxIndex;++j){
00313                      d = ranges[i];
00314                      
00315                      theta = j*angleSep;
00316                      
00317                      if(i+j >=0 && i+j < ranges.size()-1){
00318                             tempRange = d*cos(theta) - sqrt(r*r - pow(d*sin(theta),2));
00319                             tempRange = tempRange-FRONTAL_AVOIDANCE_DEPTH>0?tempRange-FRONTAL_AVOIDANCE_DEPTH:0;
00320                             if(tempRange < outranges[i+j]){
00321                                    outranges[i+j] = tempRange;
00322                             }
00323                      }
00324                      if(i-j >=0 && i-j < ranges.size()-1){
00325                             tempRange = d*cos(-theta) - sqrt(r*r - pow(d*sin(-theta),2));
00326                             tempRange = tempRange-FRONTAL_AVOIDANCE_DEPTH>0?tempRange-FRONTAL_AVOIDANCE_DEPTH:0;
00327                             if(tempRange < outranges[i-j]){
00328                                    outranges[i-j] = tempRange;
00329                             }
00330                      }
00331               }    
00332        }
00333        return outranges;
00334 }
00335 
00336 void updateWaypoint (RangeFinderInfoStruct &info,Pose currPose,Pose &goalPose,Pose &waypoint,vector<double> &closedRanges,bool angleRequest,bool &path_obstructed){
00337        
00338        g_logdebug << "Goal Pose (x,y):  ("<<goalPose.x()<<","<<goalPose.y()<<")"<<endl;
00339        
00340        float dx = goalPose.x() - currPose.x();
00341        float dy = goalPose.y() - currPose.y();
00342        float distance = hypot(dx, dy);
00343        
00344        g_logdebug << "(dx,dy): ("<<dx<<","<<dy<<")   dist: "<<distance<<endl;
00345        
00346        Pose localGoal;
00347        Pose oldWaypoint = waypoint;
00348        localGoal.theta(normalizeAngle(atan2(dy,dx) - currPose.theta()));
00349        localGoal.x(distance * cos(localGoal.theta()));
00350        localGoal.y(distance * sin(localGoal.theta()));
00351        
00352        float angle_min = info.angle_min.getValue();
00353        float theta_separation = info.theta_separation.getValue();
00354        int goal_index = (int)(normalizeAngle(localGoal.theta()-(angle_min))/theta_separation);
00355        
00356        if(distance<CLOSE_ENOUGH_DISTANCE){
00357               // Just turn
00358               waypoint.x(0.0);
00359               waypoint.y(0.0);
00360               if(angleRequest){
00361                      waypoint.theta(goalPose.theta());
00362               }
00363               else{
00364                      waypoint.theta(currPose.theta());
00365               }
00366               path_obstructed = false;
00367        }
00368        else if ((goal_index < 0) || (goal_index >= closedRanges.size())){
00369               // goal is behind
00370               waypoint.x(0.0);
00371               waypoint.y(0.0);
00372               waypoint.theta(normalizeAngle(atan2(dy,dx)));
00373               path_obstructed = false;
00374        } 
00375        else if (distance<closedRanges[goal_index]) {
00376               // goal is within horizon
00377               waypoint.x(localGoal.x());
00378               waypoint.y(localGoal.y());
00379               waypoint.theta(localGoal.theta());
00380               path_obstructed = false;
00381        } 
00382        else {
00383               // select the waypoint from within the horizon that is closest to the goal
00384               float min_dist = MAXFLOAT;
00385               for(int i=0;i<closedRanges.size();i++){ 
00386                      // compute u and v 
00387                      // u ratio of range[i] and dist to point along range[i] closest to localGoal
00388                      // v is the distance between intersect and localGoal
00389                      
00390                      Point laser(closedRanges[i] * cos(i*theta_separation+(angle_min)),
00391                                  closedRanges[i] * sin(i*theta_separation+(angle_min)));
00392                      
00393                      float u = (localGoal.x()*laser.x() + localGoal.y()*laser.y()) / pow(closedRanges[i],2);
00394                      
00395                      Point intersect(u*laser.x(),u*laser.y());
00396                      float v = hypot(localGoal.x()-intersect.x(), localGoal.y()-intersect.y());
00397                      
00398                      float dist;
00399                      Point temp;
00400                      
00401                      if (u < 0.0) {
00402                             // goal is behind this reading
00403                             // v > range[goalIndex]
00404                             dist = hypot(localGoal.x(),localGoal.y());
00405                             temp.x(0.0);
00406                             temp.y(0.0);
00407                      }else if (u < 1.0) {
00408                             // closest point to goal is along the beam (not on an endpoint)
00409                             dist = v;
00410                             temp=intersect;
00411                      }else {
00412                             // closest point to goal is on the endpoint of the beam
00413                             dist = hypot(laser.x()-localGoal.x(), laser.y()-localGoal.y());
00414                             temp=laser;
00415                      }
00416                      
00417                      if (dist < min_dist) {
00418                             min_dist = dist;
00419                             waypoint.x(temp.x());
00420                             waypoint.y(temp.y());
00421                             waypoint.theta(atan2(temp.y(),temp.x()));
00422                      }
00423                      path_obstructed = ((atan2(waypoint.y(),waypoint.x()) < WAYPOINT_ANGLE_THRESHOLD) &&
00424                                         (hypot(waypoint.x(),waypoint.y()) < WAYPOINT_DISTANCE_THRESHOLD));
00425               }
00426               //fringe case hack
00427               //If anything wierd happens, check here first
00428               int i = (int)(normalizeAngle(oldWaypoint.theta()+(M_PI/2))/theta_separation);
00429               if((abs(oldWaypoint.theta()-waypoint.theta())>WAYPOINT_DIFF_THRESHOLD) && (oldWaypoint.y()*waypoint.y()<0) && (closedRanges[i]>WAYPOINT_DIFF_SENSITIVITY)){
00430                      waypoint = oldWaypoint;
00431               }
00432        }
00433        g_logdebug<<"Waypoint: "<<waypoint.x()<<","<<waypoint.y()<<","<<waypoint.theta()<<endl;
00434 }
00435 
00436