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