justlocalizationguiserver.cpp

sonarnl localization and path planning by mobile robots

/*

MobileRobots Advanced Robotics Navigation and Localization (ARNL)
Copyright (C) 2004, 2005, ActivMedia Robotics LLC.
Copyright (C) 2006, 2007, MobileRobots Inc.
All Rights Reserved

MobileRobots Inc does not make any representations about the
suitability of this software for any purpose.  It is provided "as is"
without express or implied warranty.

The license for this software is distributed as LICENSE.txt in the top
level directory.

[e-mail]
MobileRobots
19 Columbia Drive
Amherst, NH 03031
800-639-9481

*/

#include "Aria.h"
#include "ArNetworking.h"
#include "Arnl.h"

/** @example justLocalizationGuiServer.cpp Example which does localization only,
  but provides servers for remote operation with MobileEyes.
 
 
This is a modified and simplified version of guiServer.cpp that ONLY does
localization and not path planning.  To test this you can start up
MobileEyes and connect to the robot then just drive the robot around
with teleoperation commands.

Note, to make your own custom planning/navigation information and controls
available in MobileEyes you can look at ArServerClasses.cpp and make your own class
that adds the same server commands as ArServerInfoPath and
ArServerModeGoto.

**/

int main(int argc, char *argv[])
{
  Aria::init();
  Arnl::init();

  ArRobot robot;
  ArServerBase server;
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();
  ArSimpleConnector simpleConnector(&parser);
  ArServerSimpleOpener simpleOpener (&parser);

  // set up a gyro
  ArAnalogGyro gyro (&robot);

  // Parse arguments for the simple connector.
  if (!Aria::parseArgs ())
  {
    ArLog::log (ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]);
    Aria::logOptions ();
    Aria::exit (1);
  }

  // If not using Sonarnl, then use the SICK laser
#ifndef SONARNL
  ArSick sick;
  robot.addRangeDevice (&sick);
#endif // ifndef SONARNL

  // Sonar, used by Sonarnl, as well as
  // teleoperation and wander modes.
  ArSonarDevice sonarDev;
  robot.addRangeDevice (&sonarDev);

  // Use the "examples" directory as a place to keep map files
  char fileDir[1024];
  ArUtil::addDirectories (fileDir, sizeof (fileDir), Aria::getDirectory (),
			"examples");

  // Set up the map, this will look for files in the examples
  // directory (unless the file name starts with a / \ or .
  // You can take out the argument to look in the current directory
  ArMap arMap (fileDir);
  // set it up to ignore empty file names (otherwise the parseFile
  // on the config will fail)
  arMap.setIgnoreEmptyFileName (true);

#ifndef SONARNL
  // Initialize Arnl for laser localization
  ArLocalizationTask locTask (&robot, &sick, &arMap);
#else
  // Initialize Sonarnl for sonar localization
  ArSonarLocalizationTask locTask (&robot, &sonarDev, &arMap);
#endif // ifndef SONARNL

  // Add log controls to configuration
  ArLog::addToConfig (Aria::getConfig ());

  // Open the server port
  if (!simpleOpener.open (&server, fileDir, 240))
  {
    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);
  }

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

  // reset the simulator to its start position
  robot.comInt (ArCommands::RESETSIMTOORIGIN, 1);

#ifndef SONARNL
  // Set up the laser before the robot uses it
  simpleConnector.setupLaser (&sick);
#endif // ifndef SONARNL


  // Start the robot thread.
  robot.runAsync (true);

#ifndef SONARNL
  // Start the laser thread
  sick.runAsync ();

  // Connect to the laser
  if (!sick.blockingConnect ())
  {
    ArLog::log (ArLog::Normal, "Couldn't connect to sick, exiting");
    Aria::exit (4);
  }
#endif // ifndef SONARNL

  ArUtil::sleep (300);

  // Bumpers.
  ArBumpers bumpers;
  robot.addRangeDevice (&bumpers);

  // Forbidden regions from the map.
  ArForbiddenRangeDevice forbidden (&arMap);
  robot.addRangeDevice (&forbidden);

  // Objects that provide network services:
  ArServerInfoRobot serverInfoRobot (&server, &robot);
  ArServerInfoSensor serverInfoSensor (&server, &robot);
  ArServerInfoLocalization serverInfoLocalization (&server, &robot, &locTask);
  ArServerHandleLocalization serverLocHandler (&server, &robot, &locTask);
#ifdef SONARNL
  ArServerHandlerMap serverMap (&server, &arMap, ArServerHandlerMap::LINES);
#else
  ArServerHandlerMap serverMap (&server, &arMap, ArServerHandlerMap::BOTH);
#endif

  // Drawing services in the map display:
  ArServerInfoDrawings drawings (&server);
  drawings.addRobotsRangeDevices (&robot);

  // Misc. "custom " commands:
  ArServerHandlerCommands commands (&server);
  ArServerSimpleComUC uCCommands (&commands, &robot);
  ArServerSimpleComMovementLogging loggingCommands (&commands, &robot);
  ArServerSimpleComGyro gyroCommands (&commands, &robot, &gyro);
  ArServerSimpleComLogRobotConfig configCommands (&commands, &robot);

  // Set up the possible modes for remote control from a client such as
  // MobileEyes:

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

  // If we have a laser we'll create this class which'll disable the
  // sonar automatically if we're stopped and turn them on when we
  // move (if we're using sonar to localize, then we cannot turn sonar
  // off since localization may get lost)
#ifndef SONARNL
  ArSonarAutoDisabler sonarAutoDisabler (&robot);
#endif

  // Teleoperate by keyboard, joystick, etc:
  ArServerModeRatioDrive modeRatioDrive (&server, &robot);

  // Teloperation mode's configuration and special commands:
  modeRatioDrive.addControlCommands (&commands);
  modeRatioDrive.addToConfig (Aria::getConfig (), "Teleop settings");

  // Prevent normal driving if localization is lost:
  ArActionLost actionLostRatioDrive (&locTask, NULL, &modeRatioDrive);
  modeRatioDrive.getActionGroup ()->addAction (&actionLostRatioDrive, 110);

  // Wander mode:
  ArServerModeWander
  modeWander (&server, &robot);

  // Prevent wandering if lost:
  ArActionLost
  actionLostWander (&locTask, NULL, &modeWander);
  modeWander.getActionGroup ()->addAction (&actionLostWander, 110);

  // This provides a small table of interesting information for the client
  // to display to the operator:
     ArServerInfoStrings
     stringInfo (&server);
  Aria::getInfoGroup ()->addAddStringCallback (stringInfo.
					     getAddStringFunctor ());

  // Display localization score, and laser communication statistic if
  // not SonArnl:
#ifdef SONARNL
  Aria::getInfoGroup ()->addStringDouble ("Localization Score", 8,
	new ArRetFunctorC < double, ArSonarLocalizationTask > (&locTask,
							   &ArSonarLocalizationTask::
							   getLocalizationScore),
					  		"%.03f");
#else

  Aria::getInfoGroup ()->addStringDouble ("Localization Score", 8,
					new ArRetFunctorC < double,
					ArLocalizationTask > (&locTask,
							      &ArLocalizationTask::
							      getLocalizationScore),
					"%.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));



#ifndef SONARNL
  // Setup the dock if there is a docking system on board.
  ArServerModeDock *modeDock = NULL;
  if (modeDock != NULL)
  {
    modeDock->checkDock ();
    modeDock->addAsDefaultMode ();
    modeDock->addToConfig (Aria::getConfig ());
    modeDock->addControlCommands (&commands);
  }
#endif // ifndef SONARNL

  modeStop.addAsDefaultMode ();


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

  // Error if there is no map
  if (arMap.getFileName () == NULL || strlen (arMap.getFileName ()) <= 0)
  {
    ArLog::log (ArLog::Terse,
		"Warning, no map given. Use the -map command-line argument or modify the config using MobileEyes or by editing the parameter file.");
    ArLog::log (ArLog::Terse,
		"See the ARNL documentation, including MAPPING.txt or SONAR_MAPPING.txt.");
  }

  ArLog::log (ArLog::Normal, "Directory for maps and file serving: %s", fileDir);

  ArLog::log (ArLog::Normal, "See the ARNL README.txt for more information");


  robot.unlock();


  // Localize robot at home.
  locTask.localizeRobotAtHomeBlocking();

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

  ArLog::log(ArLog::Normal, "Server now running on port %d. Press Control-C to exit.", server.getTcpPort());


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

}