sonarnlJustLocalization.cpp

Example which does localization only, but provides servers for remote operation with MobileEyes.

00001 
00002 
00021 #include "Aria.h"
00022 #include "ArNetworking.h"
00023 #include "Arnl.h"
00024 
00025 
00026 
00027 #include "ArSonarLocalizationTask.h"
00028 
00029 int main(int argc, char *argv[])
00030 {
00031   Aria::init();
00032   Arnl::init();
00033 
00034   ArRobot robot;
00035   ArServerBase server;
00036   ArArgumentParser parser(&argc, argv);
00037   parser.loadDefaultArguments();
00038   ArSimpleConnector simpleConnector(&parser);
00039   ArServerSimpleOpener simpleOpener (&parser);
00040   ArAnalogGyro gyro (&robot);
00041 
00042   parser.loadDefaultArguments();
00043   if (!Aria::parseArgs () || !parser.checkHelpAndWarnUnparsed())
00044   {
00045     Aria::logOptions ();
00046     Aria::exit (1);
00047   }
00048 
00049 
00050   ArSonarDevice sonarDev;
00051   robot.addRangeDevice (&sonarDev);
00052 
00053   ArBumpers bumpers;
00054   robot.addRangeDevice (&bumpers);
00055 
00056   // Use the "examples" directory as a place to keep map files
00057   char fileDir[1024];
00058   ArUtil::addDirectories (fileDir, sizeof (fileDir), Aria::getDirectory (),
00059             "examples");
00060 
00061   // Set up the map, this will look for files in the examples
00062   // directory (unless the file name starts with a / \ or .
00063   // You can take out the argument to look in the current directory
00064   ArMap arMap (fileDir);
00065   // set it up to ignore empty file names (otherwise the parseFile
00066   // on the config will fail)
00067   arMap.setIgnoreEmptyFileName (true);
00068 
00069 
00070   ArLocalizationManager locManager(&robot, &arMap);
00071 
00072   ArSonarLocalizationTask locTask (&robot, &sonarDev, &arMap);
00073   locManager.addLocalizationTask(&locTask);
00074 
00075 
00076   // Add log controls to configuration
00077   ArLog::addToConfig (Aria::getConfig ());
00078 
00079   // Open the server port
00080   if (!simpleOpener.open (&server, fileDir, 240))
00081   {
00082     ArLog::log (ArLog::Normal, "Could not open server port");
00083     exit (2);
00084   }
00085 
00086   // Connect to the robot
00087   if (!simpleConnector.connectRobot (&robot))
00088   {
00089     ArLog::log (ArLog::Normal, "Could not connect to robot... exiting");
00090     Aria::exit (3);
00091   }
00092 
00093   robot.enableMotors ();
00094   robot.clearDirectMotion ();
00095 
00096   // reset the simulator to its start position
00097   robot.com(ArCommands::SIM_RESET);
00098 
00099 
00100   // run robot task cycle
00101   robot.runAsync (true);
00102 
00103 
00104   ArUtil::sleep (300);
00105 
00106   // Forbidden regions from the map.
00107   ArForbiddenRangeDevice forbidden (&arMap);
00108   robot.addRangeDevice (&forbidden);
00109 
00110   // Objects that provide network services:
00111   ArServerInfoRobot serverInfoRobot (&server, &robot);
00112   ArServerInfoSensor serverInfoSensor (&server, &robot);
00113   ArServerInfoLocalization serverInfoLocalization (&server, &robot, &locManager);
00114   ArServerHandlerLocalization serverLocHandler (&server, &robot, &locManager);
00115   ArServerHandlerMap serverMap (&server, &arMap);
00116 
00117   // Drawing services in the map display:
00118   ArServerInfoDrawings drawings (&server);
00119   drawings.addRobotsRangeDevices (&robot);
00120 
00121   // Misc. "custom " commands:
00122   ArServerHandlerCommands commands (&server);
00123   ArServerSimpleComUC uCCommands (&commands, &robot);
00124   ArServerSimpleComMovementLogging loggingCommands (&commands, &robot);
00125   ArServerSimpleComGyro gyroCommands (&commands, &robot, &gyro);
00126   ArServerSimpleComLogRobotConfig configCommands (&commands, &robot);
00127 
00128   // Set up the possible modes for remote control from a client such as
00129   // MobileEyes:
00130 
00131   // To stop and remain stopped:
00132   ArServerModeStop modeStop (&server, &robot);
00133   modeStop.addAsDefaultMode ();
00134 
00135 
00136   // Teleoperate by keyboard, joystick, etc:
00137   ArServerModeRatioDrive modeRatioDrive (&server, &robot);
00138 
00139   // Teloperation mode's configuration and special commands:
00140   modeRatioDrive.addControlCommands (&commands);
00141   modeRatioDrive.addToConfig (Aria::getConfig (), "Teleop settings");
00142 
00143   // Wander mode:
00144   ArServerModeWander
00145   modeWander (&server, &robot);
00146 
00147   // Prevent driving if localization is lost:
00148   ArActionLost actionLostRatioDrive (&locManager, NULL, &modeRatioDrive);
00149   modeRatioDrive.getActionGroup ()->addAction (&actionLostRatioDrive, 110);
00150 
00151   // Prevent wandering if lost:
00152   ArActionLost
00153   actionLostWander (&locManager, NULL, &modeWander);
00154   modeWander.getActionGroup ()->addAction (&actionLostWander, 110);
00155 
00156   // This provides a small table of interesting information for the client
00157   // to display to the operator:
00158      ArServerInfoStrings
00159      stringInfo (&server);
00160   Aria::getInfoGroup ()->addAddStringCallback (stringInfo.
00161                          getAddStringFunctor ());
00162 
00163   // Display localization score and more
00164   Aria::getInfoGroup ()->addStringDouble ("Localization Score", 8,
00165     new ArRetFunctorC < double, ArSonarLocalizationTask > (&locTask,
00166                                &ArSonarLocalizationTask::
00167                                getLocalizationScore),
00168                             "%.03f");
00169 
00170 
00171 Aria::getInfoGroup ()->addStringInt ("Motor Packet Count", 10,
00172                      new ArConstRetFunctorC < int,
00173                      ArRobot > (&robot,
00174                         &ArRobot::getMotorPacCount));
00175 
00176 
00177   // Create service that allows client to change configuration parameters in ArConfig 
00178   ArServerHandlerConfig handlerConfig (&server, Aria::getConfig (),
00179                      Arnl::getTypicalDefaultParamFileName (),
00180                      Aria::getDirectory ());
00181 
00182 
00183 
00184   // Read in parameter files.
00185   Aria::getConfig ()->useArgumentParser (&parser);
00186   if (!Aria::getConfig ()->parseFile (Arnl::getTypicalParamFileName ()))
00187   {
00188     ArLog::log (ArLog::Normal, "Trouble loading configuration file, exiting");
00189     Aria::exit (5);
00190   }
00191 
00192   // Error if there is no map
00193   if (arMap.getFileName () == NULL || strlen (arMap.getFileName ()) <= 0)
00194   {
00195     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.");
00196     ArLog::log(ArLog::Terse, "See the ARNL documentation, including MAPPING.txt or SONAR_MAPPING.txt.");
00197   }
00198 
00199   ArLog::log (ArLog::Normal, "Directory for maps and file serving: %s", fileDir);
00200 
00201   ArLog::log (ArLog::Normal, "See the ARNL README.txt for more information");
00202 
00203 
00204   robot.unlock();
00205 
00206 
00207   // Localize robot at home.
00208   locTask.localizeRobotAtHomeBlocking();
00209   
00210   server.runAsync();
00211 
00212   ArLog::log(ArLog::Normal, "Server now running on port %d. Press Control-C to exit.", server.getTcpPort());
00213 
00214 
00215   robot.waitForRunExit();
00216   Aria::exit(0);
00217 
00218 }
00219 

Generated on Thu Apr 23 10:19:39 2009 for SONARNL by  doxygen 1.5.1