guiserver.cpp

sonarnl localization and path planning by mobile robots

  Aria::getInfoGroup()->addStringDouble(
	  "CPU", 8, 
	  new ArGlobalRetFunctor<double>(&ArSystemStatus::getCPUPercent),
	  "%.03f");
#else

  Aria::getInfoGroup()->addStringDouble(
	  "Localization Score", 8, 
	  new ArRetFunctorC<double, ArLocalizationTask>(
		  &locTask, &ArLocalizationTask::getLocalizationScore),
	  "%.03f");
   Aria::getInfoGroup()->addStringInt(
	  "Num Samples", 8, 
	  new ArRetFunctorC<int, ArLocalizationTask>(
		  &locTask, &ArLocalizationTask::getCurrentNumSamples),
	  "%4d");

  Aria::getInfoGroup()->addStringDouble(
	  "CPU", 8, 
	  new ArGlobalRetFunctor<double>(&ArSystemStatus::getCPUPercent),
	  "%.03f");


  Aria::getInfoGroup()->addStringInt(
	  "Laser Packet Count", 10, 
	  new ArRetFunctorC<int, ArSick>(&sick, 
					 &ArSick::getSickPacCount));
#endif

  Aria::getInfoGroup()->addStringInt(
	  "Motor Packet Count", 10, 
	  new ArConstRetFunctorC<int, ArRobot>(&robot, 
					       &ArRobot::getMotorPacCount));

  // Setup the dock if there is a docking system on board.
  // (But SONARNL can't dock, you need a laser to find it.)
#ifndef SONARNL
  ArServerModeDock *modeDock = NULL;
  modeDock = ArServerModeDock::createDock(&server, &robot, &locTask, 
					  &pathTask);
  if (modeDock != NULL)
  {
    modeDock->checkDock();
    modeDock->addAsDefaultMode();
    modeDock->addToConfig(Aria::getConfig());
    modeDock->addControlCommands(&commands);
  }
#endif  // ifndef SONARNL

  // Make Stop mode the default (If current mode deactivates without entering
  // a new mode, then Stop Mode will be selected)
  modeStop.addAsDefaultMode();


  /* Services that allow the client to initiate scanning with the laser to
     create maps in Mapper3 (So not possible with SONARNL): */
  
#ifndef SONARNL

  // this will allow you to create a map from this program instead of having to
  // use sickLogger (comment it out if you don't want it)
  ArServerHandlerMapping handlerMapping(&server, &robot, &sick, fileDir, "", true);

  // make localization stop while mapping
  handlerMapping.addMappingStartCallback(
	  new ArFunctor1C<ArLocalizationTask, bool>
	  (&locTask, &ArLocalizationTask::setIdleFlag, true));

  // and then make it start again when we're doine
  handlerMapping.addMappingEndCallback(
	  new ArFunctor1C<ArLocalizationTask, bool>
	  (&locTask, &ArLocalizationTask::setIdleFlag, false));
  
  // Make it so our "lost" actions don't stop us while mapping
  handlerMapping.addMappingStartCallback(actionLostPath.getDisableCB());
  handlerMapping.addMappingStartCallback(actionLostDrive.getDisableCB());
  handlerMapping.addMappingStartCallback(actionLostRatioDrive.getDisableCB());
  handlerMapping.addMappingStartCallback(actionLostWander.getDisableCB());

  // And then let them make us stop as usual when done mapping
  handlerMapping.addMappingEndCallback(actionLostPath.getEnableCB());
  handlerMapping.addMappingEndCallback(actionLostDrive.getEnableCB());
  handlerMapping.addMappingEndCallback(actionLostRatioDrive.getEnableCB());
  handlerMapping.addMappingEndCallback(actionLostWander.getEnableCB());

  // don't let forbidden lines show up as obstacles while mapping
  // (since localization is off)
  handlerMapping.addMappingStartCallback(forbidden.getDisableCB());

  // let forbidden lines show up as obstacles again as usual after mapping
  handlerMapping.addMappingEndCallback(forbidden.getEnableCB());
#endif // ifndef SONARNL



  /* File transfer services: */
  
#ifdef WIN32
  // these server file things don't work under windows yet
  ArLog::log(ArLog::Normal, "Note, file upload/download services are not implemented for Windows; not enabling them.");
#else
  // This block will allow you to set up where you get and put files
  // to/from, just comment them out if you don't want this to happen
  // /*
  ArServerFileLister fileLister(&server, fileDir);
  ArServerFileToClient fileToClient(&server, fileDir);
  ArServerFileFromClient fileFromClient(&server, fileDir, "/tmp");
  ArServerDeleteFileOnServer deleteFileOnServer(&server, fileDir);
  // */
#endif

  // Create the service that allows the client to monitor the communication 
  // between the robot and the client.
  //
  ArServerHandlerCommMonitor handlerCommMonitor(&server);

  // Create service that allows client to change configuration parameters in ArConfig 
  ArServerHandlerConfig handlerConfig(&server, Aria::getConfig(),
				      Arnl::getTypicalDefaultParamFileName(),
				      Aria::getDirectory());


  
  // 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);
  }

  // Warn about unknown params.
  if (!simpleOpener.checkAndLog() || !parser.checkHelpAndWarnUnparsed())
  {
    ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]);
    simpleConnector.logOptions();
    simpleOpener.logOptions();
    Aria::exit(6);
  }

  // Warn if there is no map
#ifndef SONARNL
  if (arMap.getFileName() == NULL || strlen(arMap.getFileName()) <= 0)
  {
    ArLog::log(ArLog::Normal, "");
    ArLog::log(ArLog::Normal, "### No map file is set up, you can make a map with the following procedure");
    ArLog::log(ArLog::Normal, "\t 0) You can find this information in README.txt or docs/Mapping.txt");
    ArLog::log(ArLog::Normal, "\t 1) Connect to this server with MobileEyes");
    ArLog::log(ArLog::Normal, "\t 2) Go to Tools->Map Creation->Start Scan");
    ArLog::log(ArLog::Normal, "\t 3) Give the map a name and hit okay");
    ArLog::log(ArLog::Normal, "\t 4) Drive the robot around your space (see docs/Mapping.txt");
    ArLog::log(ArLog::Normal, "\t 5) Go to Tools->Map Creation->Stop Scan");
    ArLog::log(ArLog::Normal, "\t 6) Start up Mapper3");
    ArLog::log(ArLog::Normal, "\t 7) Go to File->Open on Robot");
    ArLog::log(ArLog::Normal, "\t 8) Select the .2d you created");
    ArLog::log(ArLog::Normal, "\t 9) Create a .map");
    ArLog::log(ArLog::Normal, "\t10) Go to File->Save on Robot");
    ArLog::log(ArLog::Normal, "\t11) In MobileEyes, go to Tools->Robot Config");
    ArLog::log(ArLog::Normal, "\t12) Choose the Files section");
    ArLog::log(ArLog::Normal, "\t13) Enter the path and name of your new .map file for the value of the Map parameter.");
    ArLog::log(ArLog::Normal, "\t14) Press OK and your new map should become the map used");
    ArLog::log(ArLog::Normal, "");    
  }
#else // ifndef SONARNL
  if (arMap.getFileName() == NULL || strlen(arMap.getFileName()) <= 0)
  {
    ArLog::log(ArLog::Normal, "");
    ArLog::log(ArLog::Normal, "### No map file is set up, you can make a map with the following procedure");
    ArLog::log(ArLog::Normal, "\t 0) You can find this information in README.txt or docs/SonarMapping.txt");
    ArLog::log(ArLog::Normal, "\t 1) Start up Mapper3Basic");
    ArLog::log(ArLog::Normal, "\t 2) Go to File->New");
    ArLog::log(ArLog::Normal, "\t 3) Draw a line map of your area (make sure it is to scale)");
    ArLog::log(ArLog::Normal, "\t 4) Go to File->Save on Robot");
    ArLog::log(ArLog::Normal, "\t 5) In MobileEyes, go to Tools->Robot Config");
    ArLog::log(ArLog::Normal, "\t 6) Choose the Files section");
    ArLog::log(ArLog::Normal, "\t 7) Enter the path and name of your new .map file for the value of the Map parameter.");
    ArLog::log(ArLog::Normal, "\t 8) Press OK and your new map should become the map used");
    ArLog::log(ArLog::Normal, "");    
}
#endif // ifndef SONARNL

  // find out where we'll want to put files
  ArLog::log(ArLog::Normal, "");
  ArLog::log(ArLog::Normal, 
	     "Directory for maps and file serving: %s", fileDir);
  
  ArLog::log(ArLog::Normal, "See the ARNL README.txt for more information");
  ArLog::log(ArLog::Normal, "");

  // If you want MobileSim to try and load up the same map as you are
  // using in guiServer then uncomment out the next line and this object
  // will send a command to MobileSim to do so, but make sure you start 
  // MobileSim from the Arnl/examples directory or use the --cwd option, 
  // so that the map names used by MobileSim match  the map names used 
  // by guiServer
  //ArSimMapSwitcher mapSwitcher(&robot, &arMap);


  /* If using a "central server" to manage multiple robots, 
   * then set up how configuration is recieved from the central
   * server:
   */

  // Set one of these two flags to what you want to do... note if you
  // separate all sections from the central config 
  // then you'll have to manage your own map on each robot,
  // which'll mean you can't use central avoidance
  bool enableSeparateAllSections = false;
  bool enableSeparateAllSectionsButFiles = false;

  if (clientSwitch.getCentralServerHostName() != NULL && 
      (enableSeparateAllSections || enableSeparateAllSectionsButFiles))
  {
    std::list<ArConfigSection *> *sections;
    std::list<ArConfigSection *>::iterator sIt;
    ArConfigSection *section;
    
    sections = Aria::getConfig()->getSections();
    for (sIt = sections->begin(); sIt != sections->end(); sIt++)
    {
      section = (*sIt);
      // if this section is already separated then just continue
      if (section->hasFlag("SEPARATE_SECTION"))
	continue;
      // otherwise make sure we should separate it, and if so, do it
      if (enableSeparateAllSections || 
	  (enableSeparateAllSectionsButFiles && 
	   strcasecmp(section->getName(), "Files") != 0))
      {
	Aria::getConfig()->addSectionFlags(section->getName(), 
					   "SEPARATE_SECTION");
      }
    }
  }

  // if our map file has a separate local section then make a special
  // command on the server to tell the central server
  if (Aria::getConfig()->findSection("Files") != NULL &&
      Aria::getConfig()->findSection("Files")->hasFlag("SEPARATE_SECTION"))
  {
    server.addData(
	    "centralServerDoNotPushMap", 
    "Data to tell the central server not to push maps at this program",
	    NULL, "none", "none", "Disallowed", "MAIN_SERVER_ONLY");
  }



  /* Finally, get ready to run the robot: */


  robot.unlock();

  // Localize robot at home.
  locTask.localizeRobotAtHomeBlocking();
  
  // Let the client switch manager spin off into its own thread
  clientSwitch.runAsync();

  // Now let it spin off in its own thread
  server.runAsync();

  // Add a key handler (mostly so that on windows you can exit by pressing
  // escape.) This key handler, however, prevents this program from
  // running in the background (e.g. as a system daemon or run from 
  // the shell with "&") -- it will lock up trying to read the keys; 
  // remove this if you wish to be able to run this program in the background.
  ArKeyHandler *keyHandler;
  if ((keyHandler = Aria::getKeyHandler()) == NULL)
  {
    keyHandler = new ArKeyHandler;
    Aria::setKeyHandler(keyHandler);
    robot.lock();
    robot.attachKeyHandler(keyHandler);
    robot.unlock();
    printf("To exit, press escape.\n");
  }

  robot.waitForRunExit();
  Aria::exit(0);
}