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