BaseArnl: justPathPlanning.cpp

- Main Page
- Classes
- Related Pages
- Examples

# justPathPlanning.cpp

Example which does path planning and following only, but provides servers for remote operation with MobileEyes.This is a modified and simplified version of guiServer.cpp that ONLY does path planning and not localization. This is provided as an example to build on for developing your own localization techniques, as the path planning and following will become very innacurate without correction of the robot pose from a localization process (use ArRobot::moveTo() to correct the robot pose).

    00001 #include "Aria.h"
    00002 #include "ArNetworking.h"
    00003 #include "Arnl.h"
    00004
    00021 int
    00022 main(int argc, char *argv[])
    00023 {
    00024   // Initialize location of Aria, Arnl and their args.
    00025   Aria::init();
    00026   Arnl::init();
    00027
    00028   // To initialize, but log to a file, do this instead of the above:
    00029   //ArLog::init(ArLog::File, ArLog::Normal, "log.txt", true, true);
    00030   //ArLog::init(ArLog::File, ArLog::Verbose);
    00031
    00032   // The robot object
    00033   ArRobot robot;
    00034
    00035   // Our server
    00036   ArServerBase server;
    00037
    00038   // Parse the command line arguments.
    00039   ArArgumentParser parser(&argc, argv);
    00040
    00041   // Set up our simpleConnector
    00042   ArSimpleConnector simpleConnector(&parser);
    00043
    00044   // Set up our simpleOpener
    00045   ArServerSimpleOpener simpleOpener(&parser);
    00046
    00047   // Set up our client for the central server
    00048   ArClientSwitchManager clientSwitch(&server, &parser);
    00049
    00050   // Load default arguments for this computer (from /etc/Aria.args, environment
    00051   // variables, and other places)
    00052   parser.loadDefaultArguments();
    00053
    00054   // set up a gyro
    00055   ArAnalogGyro gyro(&robot);
    00056
    00057   // Parse arguments for the simple connector.
    00058   if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
    00059   {
    00060     ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]);
    00061     Aria::logOptions();
    00062     Aria::exit(1);
    00063   }
    00064
    00065
    00066   // The laser object, will be used if we have one
    00067   ArSick sick;
    00068
    00069   // Add the laser to the robot
    00070   robot.addRangeDevice(&sick);
    00071
    00072   // Sonar, must be added to the robot, used by teleoperation and wander to
    00073   // detect obstacles, and for localization if SONARNL
    00074   ArSonarDevice sonarDev;
    00075
    00076   // Add the sonar to the robot
    00077   robot.addRangeDevice(&sonarDev);
    00078
    00079   // Set up where we'll look for files
    00080   char fileDir[1024];
    00081   ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(),
    00082              "examples");
    00083   ArLog::log(ArLog::Normal, "Installation directory is: %s\nMaps directory is: %s\n", Aria::getDirectory(), fileDir);
    00084
    00085   // Set up the map, this will look for files in the examples
    00086   // directory (unless the file name starts with a /, \, or .
    00087   // You can take out the 'fileDir' argument to look in the current directory
    00088   // instead
    00089   ArMap arMap(fileDir);
    00090   // set it up to ignore empty file names (otherwise the parseFile
    00091   // on the config will fail)
    00092   arMap.setIgnoreEmptyFileName(true);
    00093
    00094   // Make the path task planning task
    00095   ArPathPlanningTask pathTask(&robot, &sick, &sonarDev, &arMap);
    00096
    00097   // Set up things so data can be logged (only do it with the laser
    00098   // since it can overrun a 9600 serial connection which the sonar is
    00099   // more likely to have)
    00100   ArDataLogger dataLogger(&robot);
    00101   dataLogger.addToConfig(Aria::getConfig());
    00102
    00103   // add our logging to the config
    00104   ArLog::addToConfig(Aria::getConfig());
    00105
    00106   // First open the server
    00107   if (!simpleOpener.open(&server, fileDir, 240))
    00108   {
    00109     if (simpleOpener.wasUserFileBad())
    00110       ArLog::log(ArLog::Normal, "Bad user file");
    00111     else
    00112       ArLog::log(ArLog::Normal, "Could not open server port");
    00113     exit(2);
    00114   }
    00115
    00116   // Connect the robot
    00117   if (!simpleConnector.connectRobot(&robot))
    00118   {
    00119     ArLog::log(ArLog::Normal, "Could not connect to robot... exiting");
    00120     Aria::exit(3);
    00121   }
    00122
    00123   // Set up a class that'll put the movement and gyro parameters into ArConfig
    00124   ArRobotConfig robotConfig(&robot);
    00125   robotConfig.addAnalogGyro(&gyro);
    00126
    00127   robot.enableMotors();
    00128   robot.clearDirectMotion();
    00129
    00130   // if we are connected to a simulator, reset it to its start position
    00131   robot.comInt(ArCommands::RESETSIMTOORIGIN, 1);
    00132   robot.moveTo(ArPose(0,0,0));
    00133
    00134
    00135   // Set up laser using connector (command line arguments, etc.)
    00136   simpleConnector.setupLaser(&sick);
    00137
    00138   // Start the robot thread.
    00139   robot.runAsync(true);
    00140
    00141   // Start the laser thread.
    00142   sick.runAsync();
    00143
    00144   // Try to connect the laser
    00145   if (!sick.blockingConnect())
    00146     ArLog::log(ArLog::Normal, "Warning: Couldn't connect to SICK laser, it won't be used");
    00147   else
    00148     ArLog::log(ArLog::Normal, "Connected to laser.");
    00149
    00150
    00151   // Add additional range devices to the robot and path planning task.
    00152   // IRs if the robot has them.
    00153   robot.lock();
    00154   ArIRs irs;
    00155   robot.addRangeDevice(&irs);
    00156   pathTask.addRangeDevice(&irs, ArPathPlanningTask::CURRENT);
    00157
    00158   // Bumpers.
    00159   ArBumpers bumpers;
    00160   robot.addRangeDevice(&bumpers);
    00161   pathTask.addRangeDevice(&bumpers, ArPathPlanningTask::CURRENT);
    00162
    00163   // Forbidden regions from the map
    00164   ArForbiddenRangeDevice forbidden(&arMap);
    00165   robot.addRangeDevice(&forbidden);
    00166   pathTask.addRangeDevice(&forbidden, ArPathPlanningTask::CURRENT);
    00167
    00168   // This is the place to add a range device which will hold sensor data
    00169   // and delete it appropriately to replan around blocked paths.
    00170   ArGlobalReplanningRangeDevice replanDev(&pathTask);
    00171
    00172   // Create objects that add network services:
    00173
    00174   // Drawing in the map display:
    00175   ArServerInfoDrawings drawings(&server);
    00176   drawings.addRobotsRangeDevices(&robot);
    00177   drawings.addRangeDevice(&replanDev);
    00178
    00179   /* If you want to draw the destination put this code back in:
    00180   ArServerDrawingDestination destination(
    00181       &drawings, &pathTask, "destination",
    00182       500, 500,
    00183       new ArDrawingData("polyDots",
    00184                 ArColor(0xff, 0xff, 0x0),
    00185                 800, // size
    00186                 49), // just below the robot
    00187   */
    00188
    00189   /* If you want to see the local path planning area use this
    00190     (You can enable this particular drawing from custom commands
    00191     which is set up down below in ArServerInfoPath)
    00192   ArDrawingData drawingDataP("polyLine", ArColor(200,200,200), 1, 75);
    00193   ArFunctor2C<ArPathPlanningTask, ArServerClient *, ArNetPacket *>
    00194   drawingFunctorP(pathTask, &ArPathPlanningTask::drawSearchRectangle);
    00195   drawings.addDrawing(&drawingDataP, "Local Plan Area", &drawingFunctorP);
    00196   */
    00197
    00198   /* If you want to see the points making up the local path in addition to the
    00199    * main path use this.
    00200   ArDrawingData drawingDataP2("polyDots", ArColor(0,128,0), 100, 70);
    00201   ArFunctor2C<ArPathPlanningTask, ArServerClient *, ArNetPacket *>
    00202   drawingFunctorP2(pathTask, &ArPathPlanningTask::drawPathPoints);
    00203   drawings.addDrawing(&drawingDataP2, "Path Points", &drawingFunctorP2);
    00204   */
    00205
    00206   // Misc. simple commands:
    00207   ArServerHandlerCommands commands(&server);
    00208
    00209
    00210   // These provide various kinds of information to the client:
    00211   ArServerInfoRobot serverInfoRobot(&server, &robot);
    00212   ArServerInfoSensor serverInfoSensor(&server, &robot);
    00213   ArServerInfoPath serverInfoPath(&server, &robot, &pathTask);
    00214   serverInfoPath.addSearchRectangleDrawing(&drawings);
    00215   serverInfoPath.addControlCommands(&commands);
    00216
    00217   // Provide the map to the client (and related controls):
    00218   // This uses both lines and points now, since everything except
    00219   // sonar localization uses both (path planning with sonar still uses both)
    00220   ArServerHandlerMap serverMap(&server, &arMap);
    00221
    00222
    00223
    00224   // Add some simple (custom) commands for testing and debugging:
    00225   ArServerSimpleComUC uCCommands(&commands, &robot);                   // Send any command to the microcontroller
    00226   ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // configure logging
    00227   ArServerSimpleComGyro gyroCommands(&commands, &robot, &gyro);        // monitor the gyro
    00228   ArServerSimpleComLogRobotConfig configCommands(&commands, &robot);   // trigger logging of the robot config parameters
    00229   ArServerSimpleServerCommands serverCommands(&commands, &server);     // monitor networking behavior (track packets sent etc.)
    00230
    00231
    00232   /* Set up the possible modes for remote control from a client such as
    00233    * MobileEyes:
    00234    */
    00235
    00236   // Mode To go to a goal or other specific point:
    00237   ArServerModeGoto modeGoto(&server, &robot, &pathTask, &arMap, ArPose(0,0,0));
    00238
    00239   // Add a simple (custom) command that allows you to give a list of
    00240   // goals to tour, instead of all. Useful for testing and debugging.
    00241   modeGoto.addTourGoalsInListSimpleCommand(&commands);
    00242
    00243   // Mode To stop and remain stopped:
    00244   ArServerModeStop modeStop(&server, &robot);
    00245
    00246   // cause the sonar to turn off automatically
    00247   // when the robot is stopped, and turn it back on when commands to move
    00248   // are sent. (Note, this should not be done if you need the sonar
    00249   // data to localize, or for other purposes while stopped)
    00250   ArSonarAutoDisabler sonarAutoDisabler(&robot);
    00251
    00252   // Teleoperation modes To drive by keyboard, joystick, etc:
    00253   ArServerModeRatioDrive modeRatioDrive(&server, &robot);  // New, improved mode
    00254   ArServerModeDrive modeDrive(&server, &robot);            // Older mode for compatability
    00255
    00256   // Drive mode's configuration and custom (simple) commands:
    00257   modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings");
    00258   modeDrive.addControlCommands(&commands);
    00259   modeRatioDrive.addControlCommands(&commands);
    00260
    00261   // Wander mode
    00262   ArServerModeWander modeWander(&server, &robot);
    00263
    00264
    00265   // This provides a small table of interesting information for the client
    00266   // to display to the operator:
    00267   ArServerInfoStrings stringInfo(&server);
    00268   Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor());
    00269
    00270   Aria::getInfoGroup()->addStringInt(
    00271       "Motor Packet Count", 10,
    00272       new ArConstRetFunctorC<int, ArRobot>(&robot,
    00273                            &ArRobot::getMotorPacCount));
    00274
    00275
    00276   // Make Stop mode the default (If current mode deactivates without entering
    00277   // a new mode, then Stop Mode will be selected)
    00278   modeStop.addAsDefaultMode();
    00279
    00280
    00281
    00282
    00283   /* File transfer services: */
    00284
    00285 #ifdef WIN32
    00286   // these server file things don't work under windows yet
    00287   ArLog::log(ArLog::Normal, "Note, file upload/download services are not implemented for Windows; not enabling them.");
    00288 #else
    00289   // This block will allow you to set up where you get and put files
    00290   // to/from, just comment them out if you don't want this to happen
    00291   // /*
    00292   ArServerFileLister fileLister(&server, fileDir);
    00293   ArServerFileToClient fileToClient(&server, fileDir);
    00294   ArServerFileFromClient fileFromClient(&server, fileDir, "/tmp");
    00295   ArServerDeleteFileOnServer deleteFileOnServer(&server, fileDir);
    00296   // */
    00297 #endif
    00298
    00299   // Create the service that allows the client to monitor the communication
    00300   // between the robot and the client.
    00301   //
    00302   ArServerHandlerCommMonitor handlerCommMonitor(&server);
    00303
    00304   // Create service that allows client to change configuration parameters in ArConfig
    00305   ArServerHandlerConfig handlerConfig(&server, Aria::getConfig(),
    00306                       Arnl::getTypicalDefaultParamFileName(),
    00307                       Aria::getDirectory());
    00308
    00309
    00310
    00311   // Read in parameter files.
    00312   Aria::getConfig()->useArgumentParser(&parser);
    00313   if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
    00314   {
    00315     ArLog::log(ArLog::Normal, "Trouble loading configuration file, exiting");
    00316     Aria::exit(5);
    00317   }
    00318
    00319   // Warn about unknown params.
    00320   if (!simpleOpener.checkAndLog() || !parser.checkHelpAndWarnUnparsed())
    00321   {
    00322     ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]);
    00323     simpleConnector.logOptions();
    00324     simpleOpener.logOptions();
    00325     Aria::exit(6);
    00326   }
    00327
    00328   // Warn if there is no map
    00329   if (arMap.getFileName() == NULL || strlen(arMap.getFileName()) <= 0)
    00330   {
    00331     ArLog::log(ArLog::Normal, "");
    00332     ArLog::log(ArLog::Normal, "### Warning, No map file is set up, you can make a map with sickLogger or arnlServer, and Mapper3; More info in docs/Mapping.txt and README.txt. Set the map with the -map command line option, or by changing the config with MobileEyes or by editing the config file.");
    00333     ArLog::log(ArLog::Normal, "");
    00334   }
    00335
    00336   // find out where we'll want to put files
    00337   ArLog::log(ArLog::Normal, "");
    00338   ArLog::log(ArLog::Normal,
    00339          "Directory for maps and file serving: %s", fileDir);
    00340
    00341   ArLog::log(ArLog::Normal, "See the ARNL README.txt for more information");
    00342   ArLog::log(ArLog::Normal, "");
    00343
    00344   // If you want MobileSim to try and load up the same map as you are
    00345   // using in guiServer then uncomment out the next line and this object
    00346   // will send a command to MobileSim to do so, but make sure you start
    00347   // MobileSim from the Arnl/examples directory or use the --cwd option,
    00348   // so that the map names used by MobileSim match  the map names used
    00349   // by guiServer
    00350   //ArSimMapSwitcher mapSwitcher(&robot, &arMap);
    00351
    00352
    00353
    00354   /* Finally, get ready to run the robot: */
    00355
    00356
    00357   robot.unlock();
    00358
    00359   server.runAsync();
    00360
    00361   // Add a key handler (mostly so that on windows you can exit by pressing
    00362   // escape.) This key handler, however, prevents this program from
    00363   // running in the background (e.g. as a system daemon or run from
    00364   // the shell with "&") -- it will lock up trying to read the keys;
    00365   // remove this if you wish to be able to run this program in the background.
    00366   ArKeyHandler *keyHandler;
    00367   if ((keyHandler = Aria::getKeyHandler()) == NULL)
    00368   {
    00369     keyHandler = new ArKeyHandler;
    00370     Aria::setKeyHandler(keyHandler);
    00371     robot.lock();
    00372     robot.attachKeyHandler(keyHandler);
    00373     robot.unlock();
    00374     printf("To exit, press escape.\n");
    00375   }
    00376
    00377   robot.waitForRunExit();
    00378   Aria::exit(0);
    00379 }