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

00001 /*************
00002  * Vector Mover
00003  * Frederick Heckel
00004  * May 2006
00005  *
00006  * Port of the SMRT vector mover. I'm not really happy with this
00007  * code; we move in straight lines, turn, move in straight lines, turn,
00008  * ad infinitum. We should be able to turn and move at the same time.
00009  * This is the fastest port, though. Later I'll try to port my old
00010  * velocity space code from Swarthmore.
00011  */
00012 
00013 #include <CommsManager.H>
00014 #include <RobotDrive.H>
00015 #include <Egomotion.H>
00016 #include <VectorMoverTransport.H>
00017 #include <cmath>
00018 #include <stdlib.h>
00019 #include <RobotUtil.H>
00020 
00021 using namespace WURDE;
00022 using namespace std;
00023 
00024 static const float MAX_TRANS_VEL_DEFAULT = 0.4;
00025 static const float MAX_ANG_VEL_DEFAULT = 0.9;
00026 
00027 static const float MAX_TRANS_ACCEL_DEFAULT = 0.8;
00028 static const float MAX_TRANS_DECEL_DEFAULT = 0.9;
00029 static const float MAX_ANG_ACCEL_DEFAULT = 10.0;
00030 static const float MAX_ANG_DECEL_DEFAULT = 10.0;
00031 
00032 static const float CLOSE_ENOUGH_DISTANCE = 0.06;
00033 static const float CLOSE_ENOUGH_ANGLE = M_PI/180;
00034 
00035 static const float DISPLACEMENT_ENVELOPE = 0.5;
00036 //static const float DELTA_THETA_ENVELOPE = 0.5;
00037 static const float DELTA_THETA_ENVELOPE = 1.2;
00038 
00039 double max_trans_vel=MAX_TRANS_VEL_DEFAULT;
00040 double max_ang_vel=MAX_ANG_VEL_DEFAULT;
00041 
00042 enum VMState { VM_IDLE, VM_MOVING, VM_SPINNING, VM_STOPPING, VM_HOLDING };
00043 
00044 double calcRotVelocity(double & delta_theta);
00045 double calcTransVelocity(double & displacement);
00046 void translate(double dist, Pose & currPose, Pose &goalPose);
00047 void rotateRelative(double theta, Pose &currPose, Pose &goalPose);
00048 void rotateAbsolute(double theta, Pose &currPose, Pose &goalPose);
00049 void moveRelative(Pose & goalPose, Pose &currPose, Pose &goalPose);
00050 void moveAbsolute(Pose & goalPose, Pose &currPose, Pose &goalPose);
00051 
00052 int main(int argc, char *argv[]){
00053         CommsManager myManager("SynchroMover");
00054         Logger myLogger("VectorMoverLogger");
00055         myManager.setRealName("SynchroMover");
00056         myManager.setHelpString("\nThis module abstracts the dynamics of a Synchro/Diff drive robot system to allow movement along an arbitrary vector.\n\n");
00057         myManager.parseOptions(argc,argv);
00058         RobotDrive myDrive(STRAT_ASSIGNED,"DriveClient");
00059         Egomotion myEgomotion(STRAT_ASSIGNED,"EgoClient");
00060         //  RobotDrive myDrive("rFlexDrive");
00061         //Egomotion myEgomotion("rFlexMotion");
00062         VectorMoverTransport myNavigator("SynchroMover");
00063         
00064         char logbuf[1024];
00065         
00066         bool haveGoalTheta, hasGoal;
00067         Pose goalPose(0,0,0),currPose(0,0,0),requestedPose(0,0,0);
00068         double trans=0, steer=0, displacement=0,delta_theta=0;
00069         
00070         double desired_trans_vel=0, desired_ang_vel=0;
00071         double lasttrans=0,laststeer=0;
00072         VMState state=VM_IDLE;
00073         
00074         myManager.registerConsumer(&myDrive);
00075         myManager.registerConsumer(&myEgomotion);
00076         myManager.registerSupplier(&myNavigator);
00077         
00078         myDrive.requests.trans.setValue(0);
00079         myDrive.requests.angular.setValue(0);
00080         myDrive.requests.brake.setValue(false);
00081         myDrive.requests.angularLimit.setValue(MAX_ANG_VEL_DEFAULT);
00082         myDrive.requests.transLimit.setValue(MAX_TRANS_VEL_DEFAULT);
00083         myDrive.publishRequest();
00084         
00085         myManager.setSleep(0.03);
00086         
00087         trans=0;
00088         steer=0;
00089         
00090         while(myManager.runUpdate()==STATE_RUN||
00091              myManager.getState()==STATE_IDLE){
00092 
00093 
00094               // Update the pose if we've got new readings
00095               if(myEgomotion.newData()){
00096                      myEgomotion.getNextData();
00097                      currPose=myEgomotion.data.location;
00098                      g_logdebug << "Current: " << currPose.x() << " " << currPose.y() << endl;
00099               }
00100 
00101 
00102               // Update the current goal if we have a new request
00103               if(myNavigator.newRequest()){
00104                      hasGoal=true;
00105                      myNavigator.getNextRequest();
00106                      if(myNavigator.requests.brake.getValue()){
00107                             state=VM_HOLDING;
00108                      }else{
00109                             requestedPose=myNavigator.requests.goalPose.getValue();
00110                             if(myNavigator.requests.absolute.getValue()){
00111                                    // absolute position request
00112                                    if(myNavigator.requests.goalHasLocation.getValue()){
00113                                           //translate
00114                                           state=VM_MOVING;                                        
00115                                           moveAbsolute(requestedPose,currPose,goalPose);
00116 
00117                                           sprintf(logbuf,"Received request to move to %f, %f",requestedPose.x(),requestedPose.y());
00118                                           g_debug(logbuf);
00119                                           if(!myNavigator.requests.goalHasTheta.getValue()){
00120                                                  goalPose.theta(currPose.theta());
00121                                           }
00122                                    }else{
00123                                           state=VM_SPINNING;
00124                                           //rotate
00125                                           rotateAbsolute(requestedPose.theta(),currPose,goalPose);
00126                                           sprintf(logbuf,"Received request to rotate to %f",requestedPose.theta());
00127                                           g_debug(logbuf);
00128                                    }
00129                                    
00130                             }else{
00131                                    // relative position request
00132                                    if(myNavigator.requests.goalHasLocation.getValue()){
00133                                           //move to a relative x,y position
00134                                           moveRelative(requestedPose,currPose,goalPose);
00135                                           if(myNavigator.requests.goalHasTheta.getValue()){
00136                                                  //end theta matters : deal with this later...
00137                                                  
00138                                           }else{
00139                                                  
00140                                                  goalPose.theta(currPose.theta());
00141                                           }
00142                                           sprintf(logbuf,"Received request to move to %f, %f, %f",requestedPose.x(),requestedPose.y(),requestedPose.theta());
00143                                           g_debug(logbuf);
00144                                           state=VM_MOVING;
00145                                           
00146                                    }else if(myNavigator.requests.goalHasTheta.getValue()){
00147                                           //rotate a relative amount
00148                                           double temptheta=requestedPose.theta();
00149                                           sprintf(logbuf,"Received request to rotate relative %f",requestedPose.theta());
00150                                           g_debug(logbuf);
00151 
00152                                           rotateRelative(temptheta,currPose,goalPose);
00153                                           state=VM_SPINNING;
00154                                    }else{
00155                                           double tempx=requestedPose.x();
00156                                           //just translate some distance
00157                                           translate(tempx,currPose,goalPose);
00158                                           sprintf(logbuf,"Received request to translate %f",tempx);
00159                                           g_debug(logbuf);
00160 
00161                                           state=VM_MOVING;
00162                                    }
00163                                    
00164                             }
00165                      }
00166               }
00167 
00168               // Calculate/publish the distance from the current position to the goal point
00169               // and the angular distance from our current heading to the goal
00170               if (state == VM_MOVING){
00171                      displacement = hypot(goalPose.x() - currPose.x(), goalPose.y() - currPose.y());
00172                      if (displacement > CLOSE_ENOUGH_DISTANCE) {
00173                             delta_theta = normalizeAngle(atan2(goalPose.y() - currPose.y(), goalPose.x() - currPose.x()) - currPose.theta());
00174                      } else {
00175                             delta_theta = normalizeAngle(goalPose.theta() - currPose.theta());
00176                             //                      delta_theta = 0.0;
00177                      }
00178                      //              cout << "Displacement of " << displacement << " with delta theta " << delta_theta << endl;
00179               } else if (state == VM_SPINNING) {
00180                      displacement = 0.0;
00181                      //cout << "In spin...\n";
00182                      delta_theta = normalizeAngle(goalPose.theta() - currPose.theta());
00183                      if (fabs(delta_theta) < CLOSE_ENOUGH_ANGLE) {
00184                             //      cout << "Not large enough angle." << delta_theta << "\n";
00185                             delta_theta = 0.0;
00186                      }
00187               } else {
00188                      displacement = 0.0;
00189                      delta_theta = 0.0;
00190               }
00191 
00192               
00193               
00194               switch(state){
00195               case VM_MOVING:
00196                      hasGoal=true;
00197                      // only move if roughly pointed at the goal
00198                      if(fabs(delta_theta) < M_PI/4){
00199                             desired_trans_vel = calcTransVelocity(displacement);
00200                      } else {
00201                             //   cout << "trans of 0.\n";
00202                             desired_trans_vel = 0.0;
00203                      }
00204                      // rotate regardless
00205                      if(fabs(delta_theta) > CLOSE_ENOUGH_ANGLE){
00206                             desired_ang_vel = calcRotVelocity(delta_theta);
00207                      }
00208                      
00209                      if ((fabs(displacement) < CLOSE_ENOUGH_DISTANCE)){
00210                             state = VM_STOPPING;
00211                             hasGoal=false;
00212                      }
00213                      break;
00214                      
00215               case VM_SPINNING:
00216                      hasGoal=true;
00217                      desired_trans_vel = 0.0;
00218                      desired_ang_vel = calcRotVelocity(delta_theta);
00219                      
00220                      if (fabs(delta_theta) < CLOSE_ENOUGH_ANGLE) {
00221                             //                      cout << "Ending the spin.\n";
00222                             state = VM_STOPPING;
00223                             hasGoal=false;
00224                      }
00225                      break;
00226                      
00227               case VM_STOPPING:
00228                      desired_trans_vel = 0.0;
00229                      desired_ang_vel = 0.0;
00230                      hasGoal=false;
00231                      if ((fabs(trans) < 1e-6) && (fabs(steer) < 1e-6)){
00232                             state = VM_IDLE;
00233                      }
00234                      
00235                      break;
00236                      
00237               case VM_HOLDING:
00238                      hasGoal=false;
00239                      desired_trans_vel = 0.0;
00240                      desired_ang_vel = 0.0;
00241                      break;
00242                      
00243               case VM_IDLE:
00244                      break;
00245                      
00246               }
00247               
00248               //              if (state==VM_SPINNING)
00249                      //              dprintf_verbose("Velocities (cur %.1f %.1f) (desired %.1f %.1f)\n",trans_vel,ang_vel,desired_trans_vel,desired_ang_vel);
00250               
00251               //dprintf_verbose("current trans_vel is %f\n", trans_vel);
00252               //dprintf_verbose("current ang_vel is %f\n", ang_vel);
00253               
00254               //dprintf_verbose("desired trans_vel is %f\n", desired_trans_vel);
00255               //dprintf_verbose("desired ang_vel is %f\n", desired_ang_vel);
00256               
00257               if(state != VM_IDLE) {
00258                      trans=desired_trans_vel;
00259                      steer=desired_ang_vel;
00260               }else{
00261                      trans=0;
00262                      steer=0;
00263               }
00264 
00265               myNavigator.data.hasGoal=hasGoal;
00266               myNavigator.data.distanceToGoal=displacement;
00267               myNavigator.data.angleToGoal=delta_theta;
00268               myNavigator.publishData();
00269 
00270               lasttrans=trans;
00271               laststeer=steer;
00272 
00273               
00274               //              cout << "Current request: " << goalPose.x() << "," << goalPose.y() << "," << goalPose.theta() << ". current location: " << currPose.x() << "," << currPose.y() << ", " << currPose.theta() << " speed: " << trans << "," << steer<< endl;
00275               
00276               myDrive.requests.brake.setValue(false);
00277               myDrive.requests.trans.setValue(trans);
00278               myDrive.requests.angular.setValue(steer);
00279               myDrive.requests.angularLimit.setValue(max_ang_vel);
00280               myDrive.requests.transLimit.setValue(max_trans_vel);
00281               myDrive.publishRequest();       
00282        }
00283 
00284        myDrive.requests.trans.setValue(0);
00285        myDrive.requests.angular.setValue(0);
00286        myDrive.publishRequest();
00287 
00288        myManager.runUpdate();
00289        sleep(1);
00290        myManager.cleanUp();
00291 
00292        return 0;
00293 }
00294 
00295 void moveAbsolute(Pose &requestedPose, Pose &currPose, Pose &goalPose){
00296        goalPose=requestedPose;
00297 }
00298 
00299 void moveRelative(Pose &requestedPose, Pose &currPose, Pose &goalPose){
00300 
00301        double dist=sqrt(pow(requestedPose.x(),2)+pow(requestedPose.y(),2));
00302        double angle=currPose.theta()+atan2(requestedPose.y(),requestedPose.x());
00303        requestedPose.x(dist*cos(angle)+currPose.x());
00304        requestedPose.y(dist*sin(angle)+currPose.y());
00305        requestedPose.theta(requestedPose.theta()+currPose.theta());
00306        goalPose=requestedPose;
00307        /*
00308        requestedPose.x(requestedPose.x()*cos(currPose.theta())+currPose.x());
00309        requestedPose.y(requestedPose.y()*sin(currPose.theta())+currPose.y());
00310        requestedPose.theta(requestedPose.theta()+currPose.theta());
00311        goalPose=requestedPose;
00312        */
00313 }
00314 
00315 void rotateAbsolute(double theta, Pose &currPose, Pose &goalPose){
00316        Pose requestedPose;
00317        goalPose.x(currPose.x());
00318        goalPose.y(currPose.y());
00319        goalPose.theta(theta);
00320 }
00321 
00322 void rotateRelative(double theta, Pose &currPose, Pose &goalPose){
00323        Pose requestedPose;
00324        requestedPose.x(currPose.x());
00325        requestedPose.y(currPose.y());
00326        requestedPose.theta(theta+currPose.theta());
00327        goalPose=requestedPose;       
00328 }
00329 
00330 void translate(double dist, Pose &currPose, Pose &goalPose){
00331        goalPose.x(currPose.x()+cos(currPose.theta())*dist);
00332        goalPose.y(currPose.y()+sin(currPose.theta())*dist);
00333 
00334 }
00335 
00336 double calcTransVelocity(double & displacement){
00337        double v;
00338        if(displacement < CLOSE_ENOUGH_DISTANCE){
00339               return 0;
00340        }
00341        if (displacement < DISPLACEMENT_ENVELOPE)
00342               v = max_trans_vel * displacement / DISPLACEMENT_ENVELOPE;
00343        else
00344               v = max_trans_vel;
00345        return v;
00346 }
00347 
00348 double calcRotVelocity(double & delta_theta){
00349        double w;
00350        if(fabs(delta_theta) < CLOSE_ENOUGH_ANGLE){
00351               //              cout << "delta_theta is too close to get a value\n";
00352               return 0;
00353        }
00354 
00355        if (fabs(delta_theta) < DELTA_THETA_ENVELOPE){
00356          w = max_ang_vel * delta_theta / DELTA_THETA_ENVELOPE;
00357        }else{
00358          w=max_ang_vel;
00359          if (delta_theta < 0) 
00360            w = -w;
00361        }
00362        
00363 
00364        return w;
00365 }