main.cpp

一个简单使用的控制器

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <iostream>

#include "ipc/ipc.h"
#include "msg_types.h"
#include "OdometryState.h"
#include "StallState.h"
#include "RFIDState.h"
#include "RFIDAction.h"
#include "RobotAction.h"
#include "RobotSkill.h"
#include "TargetDrive.h"
#include "VFHState.h"
#include "com/ipc_com.h"
#include "controller/src/reactiveState.h"
#include "logger/src/logger.h"
#include "util/timeutil.h"

using namespace std;

#define DEBUG  0

#define DEBUG_TAG  0 

#define STALL_PERSISTENCE_RANDOM 30  //mal 100 s
#define STALL_PERSISTENCE_BEST 30  

#define SCAN3D 0




enum CONTROLLER_MODE {
   ACTION_IDLE,
   ACTION_BEST_ANGLE_FROM_STALL,
   ACTION_RANDOM_FREE_FROM_STALL,
   ACTION_FREE_GRADIENT,
   ACTION_FOLLOW_PATH,
   ACTION_FOLLOW_WALL,
   ACTION_REACH_GOAL,
   ACTION_REACH_GOAL_WITH_OBSTACLES,
   ACTION_ACTIVE_PERCEPTION_MOVE,
   ACTION_ACTIVE_PERCEPTION_3DSCAN,
   ACTION_STOP,
   ACTION_JOYSTICK,
   ACTION_RELEASE_TAG
};
static CONTROLLER_MODE controllerMode = ACTION_FOLLOW_PATH;
//static enum VFH::GRID2D_TYPE gridType = VFH::PROBABILITY_GRID;


int main(int argc, char *argv[])
{
   bool _useKalmanPoses = true;
   bool _competitionMode = false;
   bool _simulationMode = false;
   bool _automaticallyDeployTags = false;
   int bumps=0;
   initLogger("CONTROLLER");
   char c;
   string fakeName = "";
   while((c = getopt(argc, argv, "scjkor:")) != EOF)
   {
      switch(c)
      {
         case 'k':
            _useKalmanPoses=true;
            break;
         case 'o':
            _useKalmanPoses=false;
            break;
         case 't':
            //FIXME: NOT USED NOW!
            _automaticallyDeployTags=true;
            rlogNote("Automatically distributing RFID tags\n");
            break;
         case 's':
            _simulationMode=true;
            break;
         case 'c':
            _competitionMode = true;
            rlogNote("Running in COMPETITION Mode \n");
            break;
         case 'r':
            fakeName = optarg;
            break;
         default:
            rlogNote("\nOptions:\n");
            rlogNote("--------------\n");
            rlogNote("-t Automatically distribute RFID tags (default is OFF)\n");
            rlogNote("-k Use Kalmanposes (default)\n");
            rlogNote("-o Use Odometry poses\n");
            rlogNote("-s Activate simulation mode: If true, controll is optimized for simulation (default=false)\n");
            rlogNote("-c Competition Mode (don't start initially)\n");
            rlogNote("-r robot name (z.B. zerg1). This opt set FAKED_HOSTNAE=zerg1\n");
            rlogNote("-[h|?] this help\n\n");
            exitLogger();
            exit(0);
            break;
      }
   }

   if (strcmp(fakeName.c_str(),"")) {
      setenv("FAKED_HOSTNAME",fakeName.c_str(),true);
   }
   rlogNote("Publishing for robot %s\n",getenv("FAKED_HOSTNAME"));


   if (!_useKalmanPoses)
      rlogNote("Using Odometry poses\n");
   else
      rlogNote("Using Kalman poses\n");

   // Prepare client for reading
   string clientName = "Controller for " + fakeName;
   rlogNote("clientname %s\n",clientName.c_str());
   ComInit((char*)clientName.c_str());

   VFHState* vfhState = new VFHState();

   RobotSkill *robotSkill = new RobotSkill(_useKalmanPoses, 
         _simulationMode);
   ReactiveState* reactState=new  ReactiveState(robotSkill->robot);

   RobotSkill::Pos goal;
   goal.x = -5000;
   goal.z =100000;
   goal.th = 0;

   bool done = false;
   bool bump=false;
   struct timeval bump_time=getCurrentTime();
   usleep(1000*1000);
   while (1) {
      ComListenClear(0);
      done = false;



      if(reactState->reactiveFreeBump(goal)) 
      { 
         if(DEBUG)
         {
            rlogDebug("BUMP!\n");
         }
         if(!bump)
         {
            bump=true;
            bump_time=getCurrentTime();
         }
         bumps+=10;
      }
      else
         bumps--;
      
      struct timeval now=getCurrentTime();
      
      if(bump)
        if(fabs(TimevalDiff(&now,&bump_time))>0.5)
             bump=false;

      // Action selection

      if(robotSkill->robotInStall(bumps))                   // Level 0 
      {     
         if(DEBUG || 1)
            rlogWarning("STALL!\n");
         controllerMode = ACTION_RANDOM_FREE_FROM_STALL;
         robotSkill->ActionRandomFreeFromStall(true, vfhState);
      }
      else if(reactState->reactiveFreeGradient(goal))        //Level 1 (false if the robot can plan from curr location)
      { 
         if(DEBUG)
            rlogDebug("GRADIENT!\n");
         controllerMode=ACTION_FREE_GRADIENT;
         //robotSkill->setReachedDist(-10);
      }
      else if(bump)                                        //Level 2
         robotSkill->ActionStop();
      else if(reactState->requestReleaseTag())              //Level 3
      { if(DEBUG_TAG)
         {
            rlogDebug("TAG_REQ!\n");
         }
         controllerMode=ACTION_RELEASE_TAG;
         //doneTag=false;
      }
      else{	
         controllerMode =ACTION_FOLLOW_PATH;                //Level 4
         robotSkill->setReachedDist(100.0);
      }
      // Action execution

      switch (controllerMode) {
         case ACTION_REACH_GOAL:
            if (DEBUG)
               rlogDebug("executing ACTION_REACH_GOAL\n");
            done = robotSkill->ActionReachGoal(false, goal, vfhState);//FIXME
            if (DEBUG)
               rlogDebug("Goal: %f  %f  %f \n",goal.x,goal.z,goal.th);              


            break;

         case ACTION_FOLLOW_PATH:
            if (DEBUG)
               rlogDebug("executing ACTION_FOLLOW_PATH\n");
            try{
               done = robotSkill->ActionFollowPath(false, vfhState);
            }catch(const char* ex) {
               if(strncmp("nopath",ex,6)==0){
                  if (DEBUG)
                     rlogDebug("executing ACTION_STOP\n");
                  done = robotSkill->ActionStop();
                  //if(DEBUG)
                  //rlogDebug("noPathPersistence %d\n",noPathPersistence);
               }
            }

            break;
         case ACTION_RELEASE_TAG:
            if (DEBUG_TAG)
               rlogDebug("handling tag release request\n"); 
            robotSkill->rfidA->deployTag();
            break;
         case ACTION_RANDOM_FREE_FROM_STALL:
            if (DEBUG)
               rlogDebug("executing ACTION_FREE_FROM_STALL\n");
            done = robotSkill->ActionRandomFreeFromStall(false, vfhState);
            if (done) {
               controllerMode = ACTION_IDLE;
            }
            break;
          case ACTION_FREE_GRADIENT:
            if (DEBUG)
               rlogDebug("executing ACTION_FREE_GRADIENT\n");
               done = robotSkill->ActionFreeGradient(goal);
          break;

         default:
            rlogWarning("WARNING: executing ACTION_IDLE\n");
            break;
      }
      //END REMOVE 
      if (!done) {
         usleep(50000);
      } else {

         usleep(50000);
      }
   }
   exitLogger();
   return 0;
}

/*********************************************************************
 * (C) Copyright 2006 Albert Ludwigs University Freiburg
 *     Institute of Computer Science
 *
 * All rights reserved. Use of this software is permitted for
 * non-commercial research purposes, and it may be copied only
 * for that use. All copies must include this copyright message.
 * This software is made available AS IS, and neither the authors
 * nor the Albert Ludwigs University Freiburg make any warranty
 * about the software or its performance.
 *********************************************************************/