sonarnlServer.cpp

example of almost all SONARNL features

00001 
00031 #include "Aria.h"
00032 #include "ArNetworking.h"
00033 #include "Arnl.h"
00034 
00035 #include "ArSonarLocalizationTask.h"
00036 
00037 void logOptions(const char *progname)
00038 {
00039   ArLog::log(ArLog::Normal, "Usage: %s [options]\n", progname);
00040   ArLog::log(ArLog::Normal, "[options] are any program options listed below, or any ARNL configuration");
00041   ArLog::log(ArLog::Normal, "parameters as -name <value>, see params/arnl.p for list.");
00042   ArLog::log(ArLog::Normal, "For example, -map <map file>.");
00043   Aria::logOptions();
00044 }
00045 
00046 bool gyroErrored = false;
00047 const char* getGyroStatusString(ArRobot* robot)
00048 {
00049   if(!robot || !robot->getOrigRobotConfig() || robot->getOrigRobotConfig()->getGyroType() < 2) return "N/A";
00050   if(robot->getFaultFlags() & ArUtil::BIT4)
00051   {
00052     gyroErrored = true;
00053     return "ERROR/OFF";
00054   }
00055   if(gyroErrored)
00056   {
00057     return "OK but error before";
00058   }
00059   return "OK";
00060 }
00061 
00062 int main(int argc, char **argv)
00063 {
00064   // Initialize Aria and Arnl global information
00065   Aria::init();
00066   Arnl::init();
00067 
00068 
00069   // The robot object
00070   ArRobot robot;
00071 
00072   // Parse the command line arguments.
00073   ArArgumentParser parser(&argc, argv);
00074 
00075   // Set up our simpleConnector, to connect to the robot and laser
00076   //ArSimpleConnector simpleConnector(&parser);
00077   ArRobotConnector robotConnector(&parser, &robot);
00078 
00079   // Connect to the robot
00080   if (!robotConnector.connectRobot())
00081   {
00082     ArLog::log(ArLog::Normal, "Error: Could not connect to robot... exiting");
00083     Aria::exit(3);
00084   }
00085 
00086 
00087 
00088   // Set up where we'll look for files. Arnl::init() set Aria's default
00089   // directory to Arnl's default directory; addDirectories() appends this
00090   // "examples" directory.
00091   char fileDir[1024];
00092   ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(), 
00093              "examples");
00094   
00095   
00096   // To direct log messages to a file, or to change the log level, use these  calls:
00097   //ArLog::init(ArLog::File, ArLog::Normal, "log.txt", true, true);
00098   //ArLog::init(ArLog::File, ArLog::Verbose);
00099  
00100   // Add a section to the configuration to change ArLog parameters
00101   ArLog::addToConfig(Aria::getConfig());
00102 
00103   // set up a gyro (if the robot is older and its firmware does not
00104   // automatically incorporate gyro corrections, then this object will do it)
00105   ArAnalogGyro gyro(&robot);
00106 
00107   // Our networking server
00108   ArServerBase server;
00109   
00110 
00111   // Set up our simpleOpener, used to set up the networking server
00112   ArServerSimpleOpener simpleOpener(&parser);
00113 
00114   
00115   // Load default arguments for this computer (from /etc/Aria.args, environment
00116   // variables, and other places)
00117   parser.loadDefaultArguments();
00118 
00119   // Parse arguments 
00120   if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
00121   {
00122     logOptions(argv[0]);
00123     Aria::exit(1);
00124   }
00125   
00126 
00127   // This causes Aria::exit(9) to be called if the robot unexpectedly
00128   // disconnects
00129   ArGlobalFunctor1<int> shutdownFunctor(&Aria::exit, 9);
00130   robot.addDisconnectOnErrorCB(&shutdownFunctor);
00131 
00132 
00133   // Create an ArSonarDevice object (ArRangeDevice subclass) and 
00134   // connect it to the robot.
00135   ArSonarDevice sonarDev;
00136   robot.addRangeDevice(&sonarDev);
00137 
00138 
00139 
00140   // This object will allow robot's movement parameters to be changed through
00141   // a Robot Configuration section in the ArConfig global configuration facility.
00142   ArRobotConfig robotConfig(&robot);
00143 
00144   // Include gyro configuration options in the robot configuration section.
00145   robotConfig.addAnalogGyro(&gyro);
00146 
00147   // Start the robot thread.
00148   robot.runAsync(true);
00149   
00150 
00151 
00152 
00153     /* Create and set up map object */
00154   
00155   // Set up the map object, this will look for files in the examples
00156   // directory (unless the file name starts with a /, \, or .
00157   // You can take out the 'fileDir' argument to look in the program's current directory
00158   // instead.
00159   // When a configuration file is loaded into ArConfig later, if it specifies a
00160   // map file, then that file will be loaded as the map.
00161   ArMap map(fileDir);
00162   // set it up to ignore empty file names (otherwise if a configuration omits
00163   // the map file, the whole configuration change will fail)
00164   map.setIgnoreEmptyFileName(true);
00165   // ignore the case, so that if someone is using MobileEyes or
00166   // MobilePlanner from Windows and changes the case on a map name,
00167   // it will still work.
00168   map.setIgnoreCase(true);
00169 
00170     
00171     /* Create localization and path planning threads */
00172 
00173 
00174   ArPathPlanningTask pathTask(&robot, &sonarDev, &map);
00175 
00176 
00177   ArLog::log(ArLog::Normal, "Creating sonar localization task");
00178   ArSonarLocalizationTask locTask(&robot, &sonarDev, &map);
00179 
00180 
00181 
00182 
00183 
00184 
00185 
00186 
00187     /* Start the server */
00188 
00189   // Open the networking server
00190   if (!simpleOpener.open(&server, fileDir, 240))
00191   {
00192     ArLog::log(ArLog::Normal, "Error: Could not open server.");
00193     exit(2);
00194   }
00195 
00196 
00197 
00198     /* Create various services that provide network access to clients (such as
00199      * MobileEyes), as well as add various additional features to ARNL */
00200 
00201 
00202 
00203 
00204 
00205   /* Add additional range devices to the robot and path planning task (so it
00206      avoids obstacles detected by these devices) */
00207   
00208   // Add IR range device to robot and path planning task (so it avoids obstacles
00209   // detected by this device)
00210   robot.lock();
00211   ArIRs irs;
00212   robot.addRangeDevice(&irs);
00213   pathTask.addRangeDevice(&irs, ArPathPlanningTask::CURRENT);
00214 
00215   // Add bumpers range device to robot and path planning task (so it avoids obstacles
00216   // detected by this device)
00217   ArBumpers bumpers;
00218   robot.addRangeDevice(&bumpers);
00219   pathTask.addRangeDevice(&bumpers, ArPathPlanningTask::CURRENT);
00220 
00221   // Add range device which uses forbidden regions given in the map to give virtual
00222   // range device readings to ARNL.  (so it avoids obstacles
00223   // detected by this device)
00224   ArForbiddenRangeDevice forbidden(&map);
00225   robot.addRangeDevice(&forbidden);
00226   pathTask.addRangeDevice(&forbidden, ArPathPlanningTask::CURRENT);
00227 
00228   robot.unlock();
00229 
00230 
00231   // Action to slow down robot when localization score drops but not lost.
00232   ArActionSlowDownWhenNotCertain actionSlowDown(&locTask);
00233   pathTask.getPathPlanActionGroup()->addAction(&actionSlowDown, 140);
00234 
00235   // Action to stop the robot when localization is "lost" (score too low)
00236   ArActionLost actionLostPath(&locTask, &pathTask);
00237   pathTask.getPathPlanActionGroup()->addAction(&actionLostPath, 150);
00238 
00239 
00240   
00241   // Service to provide drawings of data in the map display :
00242   ArServerInfoDrawings drawings(&server);
00243   drawings.addRobotsRangeDevices(&robot);
00244 
00245   /* Draw a box around the local path planning area use this 
00246     (You can enable this particular drawing from custom commands 
00247     which is set up down below in ArServerInfoPath) */
00248   ArDrawingData drawingDataP("polyLine", ArColor(200,200,200), 1, 75);
00249   ArFunctor2C<ArPathPlanningTask, ArServerClient *, ArNetPacket *> 
00250     drawingFunctorP(&pathTask, &ArPathPlanningTask::drawSearchRectangle);
00251   drawings.addDrawing(&drawingDataP, "Local Plan Area", &drawingFunctorP); 
00252 
00253 
00254 
00255   // "Custom" commands. You can add your own custom commands here, they will
00256   // be available in MobileEyes' custom commands (enable in the toolbar or
00257   // access through Robot Tools)
00258   ArServerHandlerCommands commands(&server);
00259 
00260 
00261   // These provide various kinds of information to the client:
00262   ArServerInfoRobot serverInfoRobot(&server, &robot);
00263   ArServerInfoSensor serverInfoSensor(&server, &robot);
00264   ArServerInfoPath serverInfoPath(&server, &robot, &pathTask);
00265   serverInfoPath.addSearchRectangleDrawing(&drawings);
00266   serverInfoPath.addControlCommands(&commands);
00267 
00268   // Provides localization info and allows the client (MobileEyes) to relocalize at a given
00269   // pose:
00270   ArServerInfoLocalization serverInfoLocalization(&server, &robot, &locTask);
00271   ArServerHandlerLocalization serverLocHandler(&server, &robot, &locTask);
00272 
00273   // If you're using MobileSim, ArServerHandlerLocalization sends it a command
00274   // to move the robot's true pose if you manually do a localization through 
00275   // MobileEyes.  To disable that behavior, use this constructor call instead:
00276   // ArServerHandlerLocalization serverLocHandler(&server, &robot, true, false);
00277   // The fifth argument determines whether to send the command to MobileSim.
00278 
00279   // Provide the map to the client (and related controls):
00280   ArServerHandlerMap serverMap(&server, &map);
00281 
00282   // These objects add some simple (custom) commands to 'commands' for testing and debugging:
00283   ArServerSimpleComUC uCCommands(&commands, &robot);                   // Send any command to the microcontroller
00284   ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // configure logging
00285   ArServerSimpleComLogRobotConfig configCommands(&commands, &robot);   // trigger logging of the robot config parameters
00286 //  ArServerSimpleServerCommands serverCommands(&commands, &server);     // monitor networking behavior (track packets sent etc.)
00287 
00288 
00289   // service that allows the client to monitor the communication link status
00290   // between the robot and the client.
00291   //
00292   ArServerHandlerCommMonitor handlerCommMonitor(&server);
00293 
00294 
00295 
00296   // service that allows client to change configuration parameters in ArConfig 
00297   ArServerHandlerConfig handlerConfig(&server, Aria::getConfig(),
00298                       Arnl::getTypicalDefaultParamFileName(),
00299                       Aria::getDirectory());
00300 
00301 
00302 
00303   /* Set up the possible modes for remote control from a client such as
00304    * MobileEyes:
00305    */
00306 
00307   // Mode To go to a goal or other specific point:
00308   ArServerModeGoto modeGoto(&server, &robot, &pathTask, &map,
00309                 locTask.getRobotHome(),
00310                 locTask.getRobotHomeCallback());
00311 
00312 
00313   // Mode To stop and remain stopped:
00314   ArServerModeStop modeStop(&server, &robot);
00315 
00316 
00317   // Teleoperation modes To drive by keyboard, joystick, etc:
00318   ArServerModeRatioDrive modeRatioDrive(&server, &robot);  
00319 //  ArServerModeDrive modeDrive(&server, &robot);            // Older mode for compatability
00320 
00321 
00322 
00323   // Prevent normal teleoperation driving if localization is lost using
00324   // a high-priority action, which enables itself when the particular mode is
00325   // active.
00326   // (You have to enter unsafe drive mode to drive when lost.)
00327   ArActionLost actionLostRatioDrive(&locTask, &pathTask, &modeRatioDrive);
00328   modeRatioDrive.getActionGroup()->addAction(&actionLostRatioDrive, 110);
00329 
00330   // Add drive mode section to the configuration, and also some custom (simple) commands:
00331   modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings");
00332   modeRatioDrive.addControlCommands(&commands);
00333 
00334   // Wander mode (also prevent wandering if lost):
00335   ArServerModeWander modeWander(&server, &robot);
00336   ArActionLost actionLostWander(&locTask, &pathTask, &modeWander);
00337   modeWander.getActionGroup()->addAction(&actionLostWander, 110);
00338 
00339 
00340   // This provides a small table of interesting information for the client
00341   // to display to the operator. You can add your own callbacks to show any
00342   // data you want.
00343   ArServerInfoStrings stringInfo(&server);
00344   Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor());
00345   
00346   // Provide a set of informational data (turn on in MobileEyes with
00347   // View->Custom Details)
00348 
00349   Aria::getInfoGroup()->addStringInt(
00350       "Motor Packet Count", 10, 
00351       new ArConstRetFunctorC<int, ArRobot>(&robot, 
00352                            &ArRobot::getMotorPacCount));
00353 
00354   Aria::getInfoGroup()->addStringDouble(
00355       "Sonar Localization Score", 8, 
00356       new ArRetFunctorC<double, ArSonarLocalizationTask>(
00357           &locTask, 
00358       &ArSonarLocalizationTask::getLocalizationScore),
00359       "%.03f");
00360   Aria::getInfoGroup()->addStringInt(
00361       "Sonar Loc Num Samples", 8, 
00362       new ArRetFunctorC<int, ArSonarLocalizationTask>(
00363           &locTask, &ArSonarLocalizationTask::getCurrentNumSamples),
00364       "%4d");
00365 
00366 
00367   // Display gyro status if gyro is enabled and is being handled by the firmware (gyro types 2, 3, or 4).
00368   // (If the firmware detects an error communicating with the gyro or IMU it
00369   // returns a flag, and stops using it.)
00370   // (This gyro type parameter, and fault flag, are only in ARCOS, not Seekur firmware)
00371   if(robot.getOrigRobotConfig() && robot.getOrigRobotConfig()->getGyroType() > 1)
00372   {
00373     Aria::getInfoGroup()->addStringString(
00374           "Gyro/IMU Status", 10,
00375           new ArGlobalRetFunctor1<const char*, ArRobot*>(&getGyroStatusString, &robot)
00376       );
00377   }
00378 
00379 
00380 
00381 
00382 
00383   // Make Stop mode the default (If current mode deactivates without entering
00384   // a new mode, then Stop Mode will be selected)
00385   modeStop.addAsDefaultMode();
00386     // TODO move up near where stop mode is created?
00387 
00388 
00389 
00390 
00391 
00392   /*
00393   // If we are on a simulator, move the robot back to its starting position,
00394   // and reset its odometry.
00395   // This will allow localizeRobotAtHomeBlocking() below will (probably) work (it
00396   // tries current odometry (which will be 0,0,0) and all the map
00397   // home points.
00398   // (Ignored by a real robot)
00399   //robot.com(ArCommands::SIM_RESET);
00400   */
00401 
00402 
00403   // create a pose storage class, this will let the program keep track
00404   // of where the robot is between runs...  after we try and restore
00405   // from this file it will start saving the robot's pose into the
00406   // file
00407   ArPoseStorage poseStorage(&robot);
00412   if (poseStorage.restorePose("robotPose"))
00413     serverLocHandler.setSimPose(robot.getPose());
00414   else
00415     robot.com(ArCommands::SIM_RESET);
00416 
00417 
00418 
00419   /* File transfer services: */
00420   
00421 #ifdef WIN32
00422   // Not implemented for Windows yet.
00423   ArLog::log(ArLog::Normal, "Note, file upload/download services are not implemented for Windows; not enabling them.");
00424 #else
00425   // This block will allow you to set up where you get and put files
00426   // to/from, just comment them out if you don't want this to happen
00427   // /*
00428   ArServerFileLister fileLister(&server, fileDir);
00429   ArServerFileToClient fileToClient(&server, fileDir);
00430   ArServerFileFromClient fileFromClient(&server, fileDir, "/tmp");
00431   ArServerDeleteFileOnServer deleteFileOnServer(&server, fileDir);
00432   // */
00433 #endif
00434 
00435     /* Video image streaming, and camera controls (Requires SAVserver or ACTS) */
00436 
00437   // Forward any video if either ACTS or SAV server are running.
00438   // You can find out more about SAV and ACTS on our website
00439   // http://robots.activmedia.com. ACTS is for color tracking and is
00440   // a seperate product. SAV just does software A/V transmitting and is
00441   // free to all our customers. Just run ACTS or SAV server before you
00442   // start this program and this class here will forward video from the
00443   // server to the client.
00444   ArHybridForwarderVideo videoForwarder(&server, "localhost", 7070);
00445   
00446   // make a camera to use in case we have video. the camera collection collects
00447   // multiple ptz cameras 
00448   ArPTZ *camera = NULL;
00449   ArServerHandlerCamera *handlerCamera = NULL;
00450   ArCameraCollection *cameraCollection = NULL;
00451 
00452   // if we have video then set up a camera 
00453   if (videoForwarder.isForwardingVideo())
00454   {
00455 
00456     cameraCollection = new ArCameraCollection();
00457     cameraCollection->addCamera("Cam1", "VCC4", "Camera", "VCC4");
00458 
00459     videoForwarder.setCameraName("Cam1");
00460     videoForwarder.addToCameraCollection(*cameraCollection);
00461 
00462     bool invertedCamera = false;
00463     camera = new ArVCC4(&robot, invertedCamera, 
00464                               ArVCC4::COMM_UNKNOWN, true, true);
00465     camera->init();
00466 
00467     handlerCamera = new ArServerHandlerCamera("Cam1", 
00468                                                    &server, 
00469                                                          &robot,
00470                                                          camera, 
00471                                                          cameraCollection);
00472 
00473     pathTask.addGoalFinishedCB(
00474         new ArFunctorC<ArServerHandlerCamera>(
00475             handlerCamera, 
00476             &ArServerHandlerCamera::cameraModeLookAtGoalClearGoal));
00477   }
00478 
00479   // After all of the cameras / videos have been created and added to the collection,
00480   // then start the collection server.
00481   //
00482   if (cameraCollection != NULL) {
00483     new ArServerHandlerCameraCollection(&server, cameraCollection);
00484   }
00485 
00486 
00487 
00488 
00489     /* Load configuration values, map, and begin! */
00490 
00491   
00492   // When parsing the configuration file, also look at the program's command line options 
00493   // from the command-line argument parser as well as the configuration file.
00494   // (So you can use any argument on the command line, namely -map.) 
00495   Aria::getConfig()->useArgumentParser(&parser);
00496 
00497   // Read in parameter files.
00498   ArLog::log(ArLog::Normal, "Loading config file %s into ArConfig...", Arnl::getTypicalParamFileName());
00499   if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
00500   {
00501     ArLog::log(ArLog::Normal, "Trouble loading configuration file, exiting");
00502     Aria::exit(5);
00503   }
00504 
00505   // Warn about unknown params.
00506   if (!simpleOpener.checkAndLog() || !parser.checkHelpAndWarnUnparsed())
00507   {
00508     logOptions(argv[0]);
00509     Aria::exit(6);
00510   }
00511 
00512   // Warn if there is no map
00513   if (map.getFileName() == NULL || strlen(map.getFileName()) <= 0)
00514   {
00515     ArLog::log(ArLog::Normal, "");
00516     ArLog::log(ArLog::Normal, "### No map file is set up, you can make a map with the following procedure");
00517     ArLog::log(ArLog::Normal, "   0) You can find this information in README.txt or docs/SonarMapping.txt");
00518     ArLog::log(ArLog::Normal, "   1) Start up Mapper3Basic");
00519     ArLog::log(ArLog::Normal, "   2) Go to File->New");
00520     ArLog::log(ArLog::Normal, "   3) Draw a line map of your area (make sure it is to scale)");
00521     ArLog::log(ArLog::Normal, "   4) Go to File->Save on Robot");
00522     ArLog::log(ArLog::Normal, "   5) In MobileEyes, go to Tools->Robot Config");
00523     ArLog::log(ArLog::Normal, "   6) Choose the Files section");
00524     ArLog::log(ArLog::Normal, "   7) Enter the path and name of your new .map file for the value of the Map parameter.");
00525     ArLog::log(ArLog::Normal, "   8) Press OK and your new map should become the map used");
00526     ArLog::log(ArLog::Normal, "");    
00527   }
00528 
00529   // Print a log message notifying user of the directory for map files
00530   ArLog::log(ArLog::Normal, "");
00531   ArLog::log(ArLog::Normal, 
00532          "Directory for maps and file serving: %s", fileDir);
00533   
00534   ArLog::log(ArLog::Normal, "See the ARNL README.txt for more information");
00535   ArLog::log(ArLog::Normal, "");
00536 
00537   // Do an initial localization of the robot. It tries all the home points
00538   // in the map, as well as the robot's current odometric position, as possible
00539   // places the robot is likely to be at startup.   If successful, it will
00540   // also save the position it found to be the best localized position as the
00541   // "Home" position, which can be obtained from the localization task (and is
00542   // used by the "Go to home" network request).
00543   locTask.localizeRobotAtHomeBlocking();
00544   
00545 
00546   // Start the networking server's thread
00547   server.runAsync();
00548 
00549 
00550   // Add a key handler so that you can exit by pressing
00551   // escape. Note that this key handler, however, prevents this program from
00552   // running in the background (e.g. as a system daemon or run from 
00553   // the shell with "&") -- it will lock up trying to read the keys; 
00554   // remove this if you wish to be able to run this program in the background.
00555   ArKeyHandler *keyHandler;
00556   if ((keyHandler = Aria::getKeyHandler()) == NULL)
00557   {
00558     keyHandler = new ArKeyHandler;
00559     Aria::setKeyHandler(keyHandler);
00560     robot.lock();
00561     robot.attachKeyHandler(keyHandler);
00562     robot.unlock();
00563     puts("Server running. To exit, press escape.");
00564   }
00565 
00566   // Enable the motors and wait until the robot exits (disconnection, etc.) or this program is
00567   // canceled.
00568   robot.enableMotors();
00569   robot.waitForRunExit();
00570   Aria::exit(0);
00571 }
00572 

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