#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 0enum 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. *********************************************************************/