ARNL: arnlJustLocalization.cpp


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

#include "ArLocalizationTask.h"



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);
  ArAnalogGyro gyro (&robot);

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

  ArSick sick;
  robot.addRangeDevice (&sick);

  ArSonarDevice sonarDev;
  robot.addRangeDevice (&sonarDev);

  ArBumpers bumpers;
  robot.addRangeDevice (&bumpers);

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


  ArLocalizationManager locManager(&robot, &arMap);
  ArLocalizationTask locTask (&robot, &sick, &arMap);
  locManager.addLocalizationTask(&locTask);



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

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

  simpleConnector.setupLaser (&sick);

  // run robot task cycle
  robot.runAsync (true);

  // Run SICK thread and try to connect
  sick.runAsync ();
  if (!sick.blockingConnect ())
  {
    ArLog::log (ArLog::Normal, "Couldn't connect to laser, exiting");
    Aria::exit (4);
  }

  ArUtil::sleep (300);

  // 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, &locManager);
  ArServerHandlerLocalization serverLocHandler (&server, &robot, &locManager);
  ArServerHandlerMap serverMap (&server, &arMap);

  // 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);
  modeStop.addAsDefaultMode ();

  // Disable the sonar automatically if stopped and enable when we
  // move
  ArSonarAutoDisabler sonarAutoDisabler (&robot);

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

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

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

  // Prevent wandering if lost:
  ArActionLost
  actionLostWander (&locManager, 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 more
  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));


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


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

  // 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  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  README.txt for more information");


  robot.unlock();


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

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


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

}