void handleGetMap(ArNetPacket *packet)
{
  char buffer[512];

  if (packet->getDataReadLength() == packet->getDataLength())
  {
    printf("Empty packet signifying end of map (for central forward)\n");
    return;
  }
  
  packet->bufToStr(buffer, sizeof(buffer));
  // if we got an end of line char instead of a line it means the map is over
  if (buffer[0] == '\0')
  {
    printf("Map took %g seconds\n", start.mSecSince() / 1000.0);
    arMap.parsingComplete();
    arMap.writeFile("mapExample.map");
    //client.disconnect();
    //exit(0);
  }
  else
  {
    //printf("line '%s'\n", buffer);
    arMap.parseLine(buffer);
  }

}
int main ( int argc, char *argv[] ){
//cout << "running....\n";
try{
	// Create the socket
	ServerSocket server ( 30000 );

	Aria::init();
	Arnl::init();

	ArRobot robot;
	ArArgumentParser parser(&argc, argv);
	parser.loadDefaultArguments();
	ArSonarDevice sonar;
	ArSimpleConnector simpleConnector(&parser);

	// Our server for mobile eyes
	ArServerBase moServer;

	// Set up our simpleOpener
	ArServerSimpleOpener simpleOpener(&parser);

	parser.loadDefaultArguments();
  	if (!Aria::parseArgs () || !parser.checkHelpAndWarnUnparsed()){
    		Aria::logOptions ();
   		Aria::exit (1);
  	}

	//Add the sonar to the robot
	robot.addRangeDevice(&sonar);

  	// Look for map in the current directory
  	ArMap arMap;
  	// set it up to ignore empty file names (otherwise the parseFile
  	// on the config will fail)
  	arMap.setIgnoreEmptyFileName (true);

	// First open the server 
	if (!simpleOpener.open(&moServer)){
		if (simpleOpener.wasUserFileBad())
    			ArLog::log(ArLog::Normal, "Bad user file");
		else
    			ArLog::log(ArLog::Normal, "Could not open server port");
  		exit(2);
	}

        // Connect to the robot
        if (!simpleConnector.connectRobot (&robot)){
                ArLog::log (ArLog::Normal, "Could not connect to robot... exiting");
                Aria::exit (3);
        }

	// Create the localization task (it will start its own thread here)
	ArSonarLocalizationTask locTask(&robot, &sonar, &arMap);

	ArLocalizationManager locManager(&robot, &arMap);

	ArLog::log(ArLog::Normal, "Creating sonar localization task");
	locManager.addLocalizationTask(&locTask);




	// Set the initial pose to the robot's "Home" position from the map, or
	// (0,0,0) if none, then let the localization thread take over.
	locTask.localizeRobotAtHomeNonBlocking();

	//Create the path planning task
	ArPathPlanningTask pathTask(&robot,&sonar,&arMap);

	ArLog::log(ArLog::Normal, "Robot Server: Connected.");

	robot.enableMotors();
	robot.clearDirectMotion();

	// Start the robot processing cycle running in the background.
	// True parameter means that if the connection is lost, then the
	// run loop ends.
	robot.runAsync(true);

	// Read in parameter files.
  	Aria::getConfig ()->useArgumentParser (&parser);
  	if (!Aria::getConfig ()->parseFile (Arnl::getTypicalParamFileName ())){
		ArLog::log (ArLog::Normal, "Trouble loading configuration file, exiting");
		Aria::exit (5);
	}

	//Create the three states
	robot.lock();
	Follow follow = Follow(&robot,&sonar);
	GoTo goTo(&robot,&pathTask,&arMap);
	Search s(&robot,&sonar);

	// Bumpers.
  	ArBumpers bumpers;
  	robot.addRangeDevice(&bumpers);
  	pathTask.addRangeDevice(&bumpers, ArPathPlanningTask::CURRENT);

  	// Forbidden regions from the map
  	ArForbiddenRangeDevice forbidden(&arMap);
  	robot.addRangeDevice(&forbidden);
  	pathTask.addRangeDevice(&forbidden, ArPathPlanningTask::CURRENT);

  	// Mode To stop and remain stopped:
  	ArServerModeStop modeStop(&moServer, &robot);

  	// Action to slow down robot when localization score drops but not lost.
  	ArActionSlowDownWhenNotCertain actionSlowDown(&locTask);
  	pathTask.getPathPlanActionGroup()->addAction(&actionSlowDown, 140);

  	// Action to stop the robot when localization is "lost" (score too low)
  	ArActionLost actionLostPath(&locTask, &pathTask);
  	pathTask.getPathPlanActionGroup()->addAction(&actionLostPath, 150);

	// These provide various kinds of information to the client:
	ArServerInfoRobot serverInfoRobot(&moServer, &robot);
	ArServerInfoSensor serverInfoSensor(&moServer, &robot);
	ArServerInfoPath serverInfoPath(&moServer, &robot, &pathTask);
	
	// Provide the map to the client (and related controls):
	// This uses both lines and points now, since everything except
	// sonar localization uses both (path planning with sonar still uses both)
  	ArServerHandlerMap serverMap(&moServer, &arMap);
	
	// Provides localization info and allows the client (MobileEyes) to relocalize at a given
	// pose:
	ArServerInfoLocalization serverInfoLocalization(&moServer, &robot, &locTask);
	ArServerHandlerLocalization serverLocHandler(&moServer, &robot, &locTask);

	robot.unlock();

	moServer.runAsync();

	//Main loop
	while (true){
		//The socket to accept connection
		ServerSocket new_sock;
		server.accept ( new_sock );
		int state = 1;		//1 = Follow, 2 = Search, 3 = GoTo
		int lastPos[2];		//Storing last position of BB to search the target
		int data[2];		//matrix with X,Y of BB
		try{
			while ( true ){
				//receive data from tld
				new_sock >> data;
				//cout << data[0] << "," << data[1] << endl;

				if(data[0] != -1)
					lastPos[0] = data[0];

				//cout << state <<endl; //for debugging
				//Main logic
				switch(state){

					case 1:
						cout << "Following target\n";
						state = follow.run(data);
					break;
					case 2:
						cout << "Searching for target\n";
						state = s.seek(lastPos, data);
					break;
					case 3:
						cout << "Going to ...\n";
						state = goTo.run(data);
					break;
					default:
						cout << "Not a case for state\n";
					break;
				}
				std::cout << "Loc score: " << locTask.getLocalizationScore() << std::endl;
			}
		}
		catch ( SocketException& ) {
			cout << "Lost Connection" << endl;
			robot.lock();
			robot.stop();
			robot.unlock();
		}
	}
}
catch ( SocketException& e ){
	std::cout << "Exception was caught:" << e.description() << "\nExiting.\n";
}

ArLog::log(ArLog::Normal, "RobotServer: Exiting.");
return 0;
}
Beispiel #3
0
int main(int argc, char **argv)
{

  Aria::init();
  ArLog::init(ArLog::StdOut, ArLog::Verbose);

  ArMap testMap;

  ArTime timer;

  ArGlobalFunctor mapChangedCB(&mapChanged);
  testMap.addMapChangedCB(&mapChangedCB);


  if (argc <= 1)
  {
    printf("mapTest: Usage %s <map> <map2:optional>\n", argv[0]);
    Aria::exit(1);
  }

  timer.setToNow();
  if (!testMap.readFile(argv[1]))
  {
    printf("mapTest: Could not read map '%s'\n", argv[1]);
    Aria::exit(2);
  }
  printf("mapTest: Took %ld ms to read file\n", timer.mSecSince());
/*
  printf("mapTest: ChangeTimes (in ms): mapObjects %ld points %ld mapInfo %ld\n",
	 testMap.getMapObjectsChanged().mSecSince(),
	 testMap.getPointsChanged().mSecSince(),
	 testMap.getMapInfoChanged().mSecSince());
*/
  timer.setToNow();
  if(!testMap.writeFile("mapTest.map"))
  {
    printf("mapTest: Error could not write new map to mapTest.map");
    Aria::exit(3);
  }
  printf("mapTest: Took %ld ms to write file mapTest.map\n", timer.mSecSince());

  std::list<ArMapObject *>::iterator objIt;
  ArMapObject *obj;
  for (objIt = testMap.getMapObjects()->begin(); 
       objIt != testMap.getMapObjects()->end(); 
       objIt++)
  {
    obj = (*objIt);
    printf("mapTest: Map object: %s named \"%s\". Pose: %0.2f,%0.2f,%0.2f. ", obj->getType(), obj->getName(), obj->getPose().getX(), obj->getPose().getY(), obj->getPose().getTh());
    if(obj->hasFromTo())
      printf("mapTest: Extents: From %0.2f,%0.2f to %0.2f,%0.2f.", obj->getFromPose().getX(), obj->getFromPose().getY(), obj->getToPose().getX(), obj->getToPose().getY());
    printf("mapTest: \n");

/*
    if (strcasecmp(obj->getType(), "Goal") == 0 ||
	strcasecmp(obj->getType(), "GoalWithHeading") == 0)
    {
      printf("mapTest: Map object: Goal %s\n", obj->getName());
    }
    else if (strcasecmp(obj->getType(), "ForbiddenLine") == 0 &&
	obj->hasFromTo())
    {
      printf("mapTest: Map object: Forbidden line from %.0f %.0f to %.0f %.0f\n",
	     obj->getFromPose().getX(), obj->getFromPose().getY(),
	     obj->getToPose().getX(), obj->getToPose().getY());
    }

*/

  }

  std::list<ArArgumentBuilder*>* objInfo = testMap.getMapInfo();
  for(std::list<ArArgumentBuilder*>::const_iterator i = objInfo->begin();
      i != objInfo->end();
      i++)
  {
    printf("mapTest: MapInfo object definition:\n----\n");
    (*i)->log();
    printf("mapTest: ----\n");
  }

  printf("mapTest: First 5 data points:\n");
  std::vector<ArPose>::iterator pIt;
  ArPose pose;
  int n = 0;
  for (pIt = testMap.getPoints()->begin(); 
       pIt != testMap.getPoints()->end(); 
       pIt++)
  {
    pose = (*pIt);
    if (n > 5)
      exit(0);
    printf("mapTest: \t%.0f %.0f\n", pose.getX(), pose.getY());
    n++;
    // the points are gone through
  }

  if (argc >= 3)
  {
    timer.setToNow();
    if (!testMap.readFile(argv[2]))
    {
      printf("mapTest: Could not read map '%s'\n", argv[2]);
    }
    printf("mapTest: Took %ld ms to read file2\n", timer.mSecSince());
/*
    ArUtil::sleep(30);
    printf("mapTest: ChangeTimes (in ms): mapObjects %ld points %ld mapInfo %ld\n",
	   testMap.getMapObjectsChanged().mSecSince(),
	   testMap.getPointsChanged().mSecSince(),
	   testMap.getMapInfoChanged().mSecSince());
*/
    timer.setToNow();
    testMap.writeFile("mapTest2.map");
    printf("mapTest: Took %ld ms to write file2\n", timer.mSecSince());
  }  

  // now test it with the config stuff
  ArArgumentBuilder builder;
  builder.setExtraString("Map");
  builder.add(argv[1]);
  printf("mapTest: Trying config with map (%s)\n", argv[1]);
  Aria::getConfig()->parseArgument(&builder);
  Aria::getConfig()->callProcessFileCallBacks(true, NULL, 0);
  printf("mapTest: Trying config again with same map (%s)\n", argv[1]);
  Aria::getConfig()->parseArgument(&builder);
  Aria::getConfig()->callProcessFileCallBacks(true, NULL, 0);
  if (argc >= 3)
  {
    ArArgumentBuilder builder2;
    builder2.setExtraString("Map");
    builder2.add(argv[2]);
    printf("mapTest: Trying config with map2 (%s)\n", argv[2]);
    Aria::getConfig()->parseArgument(&builder2);
    Aria::getConfig()->callProcessFileCallBacks(true, NULL, 0);
  }
  
}
int main(int argc, char **argv)
{
  Aria::init();
  char *worldName;
  char *mapName;

  if (argc != 3)
  {
    ArLog::log(ArLog::Normal, "Usage: %s <WorldFile> <MapFile>", argv[0]);
    ArLog::log(ArLog::Normal, "Example: %s columbia.wld columbia.map", argv[0]);
    exit(1);
  }

  worldName = argv[1];
  mapName = argv[2];
  
  FILE *file;
  if ((file = ArUtil::fopen(worldName, "r")) == NULL)
  {
    ArLog::log(ArLog::Normal, "Could not open world file '%s' to convert", worldName);
    exit(1);
  }
  
  char line[10000];
  
  std::vector<ArLineSegment> lines;

  bool haveHome = false;
  ArPose homePose;

  // read until the end of the file
  while (fgets(line, sizeof(line), file) != NULL)
  {
    ArArgumentBuilder builder;
    builder.add(line);

    // Four ints is a line
    if (builder.getArgc() == 4 && builder.isArgInt(0) && 
	builder.isArgInt(1) && builder.isArgInt(2) && 
	builder.isArgInt(3))
    {
      lines.push_back(
	      ArLineSegment(builder.getArgInt(0), builder.getArgInt(1),
			    builder.getArgInt(2), builder.getArgInt(3)));
    }

    // "position X Y Th" becomes a RobotHome
    if( !strcmp(builder.getArg(0), "position") &&
        builder.getArgc() == 4 && builder.isArgInt(1) && 
        builder.isArgInt(2) && builder.isArgInt(3) )
    {
      haveHome = true;
      homePose.setX(builder.getArgInt(1));
      homePose.setY(builder.getArgInt(2));
      homePose.setTh(builder.getArgInt(3));
      printf("Will make a Home point out of start position: ");
      homePose.log();
    }
  }

    
  ArMap armap;
  armap.setLines(&lines);

  ArPose nopose;
  ArMapObject home("RobotHome", homePose, NULL, "ICON", "Home", false, nopose, nopose);
  std::list<ArMapObject*> objects;
  if(haveHome)
  {
    objects.push_back(&home);
    armap.setMapObjects(&objects);
  }

  if (!armap.writeFile(mapName))
  {
    ArLog::log(ArLog::Normal, "Could not save map file '%s'", mapName);
    exit(1);
  }

  ArLog::log(ArLog::Normal, "Converted %s world file to %s map file.", worldName, mapName);
  exit(0);
}