int main(int argc, char **argv) { Aria::init(); ArServerBase server; if (!server.open(7272)) { printf("Could not open server port\n"); exit(1); } ArServerHandlerCommands commands(&server); commands.addCommand("Function1", "Call the function that is number 1!", new ArGlobalFunctor(&function1)); commands.addCommand("Function2", "Second function to call", new ArGlobalFunctor(&function2)); commands.addCommand("Function3", "Tree climb", new ArGlobalFunctor(&function3)); commands.addStringCommand("StringFunction4", "Print a string in function 4", new ArGlobalFunctor1< ArArgumentBuilder *>(&function4)); commands.addStringCommand("StringFunction5", "Prints a string given (5)", new ArGlobalFunctor1< ArArgumentBuilder *>(&function5)); commands.addStringCommand("StringFunction6", "Print out a value for function 6", new ArGlobalFunctor1< ArArgumentBuilder *>(&function6)); server.run(); }
int main(int argc, char **argv) { Aria::init(); ArServerBase server; //ArLog::init(ArLog::StdOut, ArLog::Verbose); ArConfig *config; config = Aria::getConfig(); ArRobotP3DX dx; //dx.writeFile("dx.txt"); *Aria::getConfig() = dx; //Aria::getConfig()->writeFile("dxcopy.txt"); if (!server.open(7272)) { printf("Could not open server port\n"); exit(1); } ArServerHandlerConfig configHandler(&server, Aria::getConfig()); server.run(); Aria::shutdown(); return 0; }
int main(int argc, char **argv) { Aria::init(); ArServerBase server; ArConfig *config; config = Aria::getConfig(); std::string section; char joy[512]; sprintf(joy, "Joy"); section = "section1"; config->addParam(ArConfigArg("int", new int, "fun int", 0), section.c_str(), ArPriority::NORMAL); config->addParam(ArConfigArg("double", new double, "fun double", 0, 1), section.c_str(), ArPriority::NORMAL); config->addParam(ArConfigArg("bool", new bool, "fun bool"), section.c_str(), ArPriority::IMPORTANT); config->addParam(ArConfigArg("string", joy, "fun string", sizeof(joy)), section.c_str(), ArPriority::TRIVIAL); section = "section8"; config->addParam(ArConfigArg("int", new int, "fun int", 0), section.c_str(), ArPriority::NORMAL); config->addParam(ArConfigArg("double", new double, "fun double", 0, 1), section.c_str(), ArPriority::NORMAL); config->addParam(ArConfigArg("doubleFOUR", (double).4, "fun double", 0.0, 1.0), section.c_str(), ArPriority::NORMAL); config->addParam(ArConfigArg("double three", new double, "fun double", 0, 1), section.c_str(), ArPriority::NORMAL); config->addParam(ArConfigArg("double two", new double, "fun double", 0, 1), section.c_str(), ArPriority::NORMAL); config->addParam(ArConfigArg("double one", new double, "fun double", 0, 1), section.c_str(), ArPriority::NORMAL); config->addParam(ArConfigArg("double", new double, "fun double", 0, 1), section.c_str(), ArPriority::NORMAL); config->addParam(ArConfigArg("bool", new bool, "fun bool"), section.c_str(), ArPriority::IMPORTANT); config->addParam(ArConfigArg("string", joy, "fun string", sizeof(joy)), section.c_str(), ArPriority::TRIVIAL); section = "some section"; config->setSectionComment("some section", "this is a random section with 4 ints"); config->addParam(ArConfigArg("int1", new int, "fun int"), section.c_str(), ArPriority::TRIVIAL); config->addParam(ArConfigArg("int2", new int, "fun int", -1, 1200), section.c_str(), ArPriority::NORMAL); config->addParam(ArConfigArg("int3", new int, "fun int"), section.c_str(), ArPriority::IMPORTANT); config->addParam(ArConfigArg("int4", new int, "fun int"), section.c_str(), ArPriority::NORMAL); config->addParam(ArConfigArg("int4", new int, "fun int"), section.c_str(), ArPriority::NORMAL); config->addParam(ArConfigArg("int1", new int, "fun int"), section.c_str(), ArPriority::TRIVIAL); section = "another section"; config->setSectionComment("another section", "this is another section with 1 of each type"); config->addParam(ArConfigArg("inta", new int, "fun int"), section.c_str(), ArPriority::NORMAL); config->addParam(ArConfigArg("doublea", new double, "fun double"), section.c_str(), ArPriority::NORMAL); config->addParam(ArConfigArg("boola", new bool, "fun bool"), section.c_str(), ArPriority::NORMAL); config->addParam(ArConfigArg("stringa", new char[512], "fun string", 512), section.c_str(), ArPriority::NORMAL); if (!server.open(7272)) { printf("Could not open server port\n"); exit(1); } config->setBaseDirectory("./"); config->writeFile("default.txt"); config->parseFile("modified.txt"); config->writeFile("modifiedModified.txt"); ArServerHandlerConfig configHandler(&server, Aria::getConfig(), "default.txt"); server.run(); Aria::shutdown(); return 0; }
int main(int argc, char **argv) { Aria::init(); ArGlobalFunctor2<ArServerClient *, ArNetPacket *> testCB(&testFunction); ArGlobalFunctor2<ArServerClient *, ArNetPacket *> setVelCB(&setVelFunction); ArServerBase server; //ArLog::init(ArLog::StdOut, ArLog::Verbose); ArNetPacket packet; server.addData("test", "some wierd test", &testCB, "none", "none"); server.addData("test2", "another wierd test", &testCB, "none", "none"); server.addData("test3", "yet another wierd test", &testCB, "none", "none"); server.addData("SetVelRequest", "yet another wierd test", &setVelCB, "none", "none"); if (!server.open(7273)) { printf("Could not open server port\n"); exit(1); } server.runAsync(); while (server.getRunningWithLock()) { ArUtil::sleep(1000); server.broadcastPacketTcp(&packet, "test3"); } Aria::shutdown(); return 0; }
int main(int argc, char **argv) { Aria::init(); ArServerBase server; if (!server.open(7272)) { printf("Could not open server port\n"); exit(1); } server.run(); return 0; }
int main(int argc, char **argv) { Aria::init(); ArServerBase server; if (!server.open(7272)) { printf("Could not open server port\n"); exit(1); } ArServerFileLister fileLister(&server, "."); ArServerFileToClient fileToClient(&server, "."); ArServerFileFromClient fileFromClient(&server, ".", "/tmp"); ArServerDeleteFileOnServer fileDeleter(&server, "."); server.run(); }
int main(int argc, char *argv[]) { Aria::init(); ArVideo::init(); ArArgumentParser argParser(&argc, argv); ArServerBase server; ArServerSimpleOpener openServer(&argParser); argParser.loadDefaultArguments(); if(!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(-1); } if(!openServer.open(&server)) { std::cout << "error opening ArNetworking server" << std::endl; Aria::exit(5); return 5; } server.runAsync(); std::cout << "ArNetworking server running on port " << server.getTcpPort() << std::endl; /* Set up ArNetworking services */ ArVideoRemoteForwarder forwarder(&server, "localhost", 7070); if(!forwarder.connected()) { std::cout << "ERror connecting to server localhost:7070" << std::endl; Aria::exit(1); } std::cout << "ArNetworking server running on port " << server.getTcpPort() << std::endl; while(true) ArUtil::sleep(10000); Aria::exit(0); return 0; }
int main(int argc, char **argv) { Aria::init(); ArServerBase server; ArGlobalFunctor2<ArServerClient *, ArNetPacket *> sendEmptyCB(&sendEmpty); if (!server.open(7272)) { printf("Could not open server port\n"); exit(1); } ArServerInfoDrawings drawing(&server); ArDrawingData arrows("polyarrow", ArColor(0, 0, 255), 5, 50); ArDrawingData dots("polydots", ArColor(0, 255, 0), 12, 50); drawing.addDrawing(&arrows, "arrows", &sendEmptyCB); drawing.addDrawing(&dots, "dots", &sendEmptyCB); server.run(); }
int main(int argc, char **argv) { Aria::init(); ArServerBase robotServer; if (!robotServer.open(5000)) { printf("Could not open robot server port\n"); Aria::exit(1); } ArServerBase clientServer; if (!clientServer.open(7272)) { printf("Could not open robot server port\n"); Aria::exit(1); } ArCentralManager switchManager(&robotServer, &clientServer); switchManager.addForwarderAddedCallback( new ArGlobalFunctor1<ArCentralForwarder *>(&forwarderAdded), 100); switchManager.addForwarderRemovedCallback( new ArGlobalFunctor1<ArCentralForwarder *>(&forwarderRemoved), 100); // Start a small handler to monitor communication between the server and // client. ArServerHandlerCommMonitor commMonitor(&clientServer); ArServerHandlerCommands commands(&clientServer); commands.setPrefix("CentralServer"); ArServerSimpleServerCommands serverCommands(&commands, &clientServer, false); commands.addCommand( "NetworkLogConnections", "Logs the connections to the central server, and to all the forwarded connections", new ArFunctorC<ArCentralManager> (&switchManager, &ArCentralManager::logConnections)); clientServer.runAsync(); robotServer.run(); Aria::exit(0); }
int main(int argc, char **argv) { // Initialize Aria and Arnl global information Aria::init(); Arnl::init(); // You can change default ArLog options in this call, but the settings in the parameter file // (arnl.p) which is loaded below (Aria::getConfig()->parseFile()) will override the options. //ArLog::init(ArLog::File, ArLog::Normal, "log.txt", true, true); // Used to parse the command line arguments. ArArgumentParser parser(&argc, argv); // Load default arguments for this computer (from /etc/Aria.args, environment // variables, and other places) parser.loadDefaultArguments(); #ifdef ARNL_LASER // Tell the laser connector to always connect the first laser since // this program always requires a laser. parser.addDefaultArgument("-connectLaser"); #endif // The robot object ArRobot robot; // handle messages from robot controller firmware and log the contents robot.addPacketHandler(new ArGlobalRetFunctor1<bool, ArRobotPacket*>(&handleDebugMessage)); // This object is used to connect to the robot, which can be configured via // command line arguments. ArRobotConnector robotConnector(&parser, &robot); // Connect to the robot if (!robotConnector.connectRobot()) { ArLog::log(ArLog::Normal, "Error: Could not connect to robot... exiting"); Aria::exit(3); } // Set up where we'll look for files. Arnl::init() set Aria's default // directory to Arnl's default directory; addDirectories() appends this // "examples" directory. char fileDir[1024]; ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(), "examples"); // To direct log messages to a file, or to change the log level, use these calls: //ArLog::init(ArLog::File, ArLog::Normal, "log.txt", true, true); //ArLog::init(ArLog::File, ArLog::Verbose); // Add a section to the configuration to change ArLog parameters ArLog::addToConfig(Aria::getConfig()); // set up a gyro (if the robot is older and its firmware does not // automatically incorporate gyro corrections, then this object will do it) ArAnalogGyro gyro(&robot); // Our networking server ArServerBase server; #ifdef ARNL_GPSLOC // GPS connector. ArGPSConnector gpsConnector(&parser); #endif // Set up our simpleOpener, used to set up the networking server ArServerSimpleOpener simpleOpener(&parser); #ifdef ARNL_LASER // the laser connector ArLaserConnector laserConnector(&parser, &robot, &robotConnector); #endif // used to connect to camera PTZ control ArPTZConnector ptzConnector(&parser, &robot); #ifdef ARNL_MULTIROBOT // Used to connect to a "central server" which can be used as a proxy // for multiple robot servers, and as a way for them to also communicate with // each other. (objects implementing some of these inter-robot communication // features are created below). // NOTE: If the central server is running on the same host as robot server(s), // then you must use the -serverPort argument to instruct these robot-control // server(s) to use different ports than the default 7272, since the central // server will use that port. ArClientSwitchManager clientSwitch(&server, &parser); #endif // Load default arguments for this computer (from /etc/Aria.args, environment // variables, and other places) parser.loadDefaultArguments(); // Parse arguments if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) { logOptions(argv[0]); Aria::exit(1); } // This causes Aria::exit(9) to be called if the robot unexpectedly // disconnects ArGlobalFunctor1<int> shutdownFunctor(&Aria::exit, 9); robot.addDisconnectOnErrorCB(&shutdownFunctor); // Create an ArSonarDevice object (ArRangeDevice subclass) and // connect it to the robot. ArSonarDevice sonarDev; robot.addRangeDevice(&sonarDev); // This object will allow robot's movement parameters to be changed through // a Robot Configuration section in the ArConfig global configuration facility. ArRobotConfig robotConfig(&robot); // Include gyro configuration options in the robot configuration section. robotConfig.addAnalogGyro(&gyro); // Start the robot thread. robot.runAsync(true); #ifdef ARNL_GPSLOC // On the Seekur, power to the GPS receiver is switched on by this command. // (A third argument of 0 would turn it off). On other robots this command is // ignored. If this fails, you may need to reset the port with ARIA demo or // seekurPower program (turn port off then on again). If the port is already // on, it will have no effect on the GPS (it will remain powered.) // Do this now before connecting to lasers to give it plenty of time to power // on, initialize, and find a good position before GPS localization begins. ArLog::log(ArLog::Normal, "Turning on GPS power... (Seekur/Seekur Jr. power port 6)"); robot.com2Bytes(116, 6, 1); #endif #ifdef ARNL_LASER // connect the laser(s) if it was requested, this adds them to the // robot too, and starts them running in their own threads ArLog::log(ArLog::Normal, "Connecting to laser(s) configured in parameters..."); if (!laserConnector.connectLasers()) { ArLog::log(ArLog::Normal, "Error: Could not connect to laser(s). Exiting."); Aria::exit(2); } ArLog::log(ArLog::Normal, "Done connecting to laser(s)."); #endif #if defined(ARNL_LASERLOC) || defined(ARNL_MAPPING) // find the laser we should use for localization and/or mapping, // which will be the first laser robot.lock(); ArLaser *firstLaser = robot.findLaser(1); if (firstLaser == NULL || !firstLaser->isConnected()) { ArLog::log(ArLog::Normal, "Did not have laser 1 or it is not connected, cannot start localization and/or mapping... exiting"); Aria::exit(2); } robot.unlock(); #endif /* Create and set up map object */ // Set up the map object, this will look for files in the examples // directory (unless the file name starts with a /, \, or . // You can take out the 'fileDir' argument to look in the program's current directory // instead. // When a configuration file is loaded into ArConfig later, if it specifies a // map file, then that file will be loaded as the map. ArMap map(fileDir); // set it up to ignore empty file names (otherwise if a configuration omits // the map file, the whole configuration change will fail) map.setIgnoreEmptyFileName(true); // ignore the case, so that if someone is using MobileEyes or // MobilePlanner from Windows and changes the case on a map name, // it will still work. map.setIgnoreCase(true); /* Create localization threads */ #ifdef ARNL_MULTILOC ArLocalizationManager locManager(&robot, &map); #define LOCTASK locManager #endif #ifdef ARNL_LASERLOC ArLog::log(ArLog::Normal, "Creating laser localization task"); // Laser Monte-Carlo Localization ArLocalizationTask locTask(&robot, firstLaser, &map); #ifdef ARNL_MULTILOC locManager.addLocalizationTask(&locTask); #else #define LOCTASK locTask #endif #endif #ifdef ARNL_SONARLOC ArLog::log(ArLog::Normal, "Creating sonar localization task"); ArSonarLocalizationTask locTask(&robot, &sonarDev, &map); #ifdef ARNL_MULTILOC locManager.addLocalizationTask(&locTask); #else #define LOCTASK locTask #endif #endif #ifndef ARNL_GPSLOC // A callback function, which is called if localization fails ArGlobalFunctor1<int> locFailedCB(&locFailed); locTask.setFailedCallBack(&locFailedCB); //, &locTask); #endif #ifdef ARNL_GPSLOC ArLog::log(ArLog::Normal, "Connecting to GPS..."); // Connect to GPS ArGPS *gps = gpsConnector.createGPS(&robot); if(!gps || !gps->connect()) { ArLog::log(ArLog::Terse, "Error connecting to GPS device." "Try -gpsType, -gpsPort, and/or -gpsBaud command-line arguments." "Use -help for help. Exiting."); Aria::exit(5); } // set up GPS localization task ArLog::log(ArLog::Normal, "Creating GPS localization task"); ArGPSLocalizationTask gpsLocTask(&robot, gps, &map); #ifdef ARNL_MULTILOC locManager.addLocalizationTask(&gpsLocTask); #else #define LOCTASK gpsLocTask #endif #endif #ifdef ARNL_LASER // Set some options and callbacks on each laser that the laser connector // connected to. std::map<int, ArLaser *>::iterator laserIt; for (laserIt = robot.getLaserMap()->begin(); laserIt != robot.getLaserMap()->end(); laserIt++) { int laserNum = (*laserIt).first; ArLaser *laser = (*laserIt).second; // Skip lasers that aren't connected if(!laser->isConnected()) continue; // add the disconnectOnError CB to shut things down if the laser // connection is lost laser->addDisconnectOnErrorCB(&shutdownFunctor); // set the number of cumulative readings the laser will take laser->setCumulativeBufferSize(200); // set the cumulative clean offset (so that they don't all fire at once) laser->setCumulativeCleanOffset(laserNum * 100); // reset the cumulative clean time (to make the new offset take effect) laser->resetLastCumulativeCleanTime(); // Add the packet count to the Aria info strings (It will be included in // MobileEyes custom details so you can monitor whether the laser data is // being received correctly) std::string laserPacketCountName; laserPacketCountName = laser->getName(); laserPacketCountName += " Packet Count"; Aria::getInfoGroup()->addStringInt( laserPacketCountName.c_str(), 10, new ArRetFunctorC<int, ArLaser>(laser, &ArLaser::getReadingCount)); } #endif /* Start the server */ // Open the networking server if (!simpleOpener.open(&server, fileDir, 240)) { ArLog::log(ArLog::Normal, "Error: Could not open server."); exit(2); } /* Create various services that provide network access to clients (such as * MobileEyes), as well as add various additional features to ARNL */ robot.unlock(); // Service to provide drawings of data in the map display : ArServerInfoDrawings drawings(&server); drawings.addRobotsRangeDevices(&robot); #ifdef ARNL_LASERLOC /* Show the sample points used by MCL */ ArDrawingData drawingDataL("polyDots", ArColor(0,255,0), 100, 75); ArFunctor2C<ArLocalizationTask, ArServerClient *, ArNetPacket *> drawingFunctorL(&locTask, &ArLocalizationTask::drawRangePoints); drawings.addDrawing(&drawingDataL, "Localization Points", &drawingFunctorL); #endif #ifdef ARNL_GPSLOC /* Show the positions calculated by GPS localization */ ArDrawingData drawingDataG("polyDots", ArColor(100,100,255), 130, 61); ArFunctor2C<ArGPSLocalizationTask, ArServerClient *, ArNetPacket *> drawingFunctorG(&gpsLocTask, &ArGPSLocalizationTask::drawGPSPoints); drawings.addDrawing(&drawingDataG, "GPS Points", &drawingFunctorG); ArDrawingData drawingDataG2("polyDots", ArColor(255,100,100), 100, 62); ArFunctor2C<ArGPSLocalizationTask, ArServerClient *, ArNetPacket *> drawingFunctorG2(&gpsLocTask, &ArGPSLocalizationTask::drawKalmanPoints); drawings.addDrawing(&drawingDataG2, "Kalman Points", &drawingFunctorG2); ArDrawingData drawingDataG3("polyDots", ArColor(100,255,100), 70, 63); ArFunctor2C<ArGPSLocalizationTask, ArServerClient *, ArNetPacket *> drawingFunctorG3(&gpsLocTask, &ArGPSLocalizationTask::drawOdoPoints); drawings.addDrawing(&drawingDataG3, "Odom. Points", &drawingFunctorG3); ArDrawingData drawingDataG4("polyDots", ArColor(255,50,50), 100, 75); ArFunctor2C<ArGPSLocalizationTask, ArServerClient *, ArNetPacket *> drawingFunctorG4(&gpsLocTask, &ArGPSLocalizationTask::drawKalmanRangePoints); drawings.addDrawing(&drawingDataG4, "KalRange Points", &drawingFunctorG4); ArDrawingData drawingDataG5("polySegments", ArColor(100,0,255), 1, 78); ArFunctor2C<ArGPSLocalizationTask, ArServerClient *, ArNetPacket *> drawingFunctorG5(&gpsLocTask, &ArGPSLocalizationTask::drawKalmanVariance); drawings.addDrawing(&drawingDataG5, "VarGPS", &drawingFunctorG5); #endif // "Custom" commands. You can add your own custom commands here, they will // be available in MobileEyes' custom commands (enable in the toolbar or // access through Robot Tools) ArServerHandlerCommands commands(&server); // These provide various kinds of information to the client: ArServerInfoRobot serverInfoRobot(&server, &robot); ArServerInfoSensor serverInfoSensor(&server, &robot); // Provides localization info and allows the client (MobileEyes) to relocalize at a given // pose: ArServerInfoLocalization serverInfoLocalization(&server, &robot, &LOCTASK); ArServerHandlerLocalization serverLocHandler(&server, &robot, &LOCTASK); // If you're using MobileSim, ArServerHandlerLocalization sends it a command // to move the robot's true pose if you manually do a localization through // MobileEyes. To disable that behavior, use this constructor call instead: // ArServerHandlerLocalization serverLocHandler(&server, &robot, true, false); // The fifth argument determines whether to send the command to MobileSim. // Provide the map to the client (and related controls): ArServerHandlerMap serverMap(&server, &map); // These objects add some simple (custom) commands to 'commands' for testing and debugging: ArServerSimpleComUC uCCommands(&commands, &robot); // Send any command to the microcontroller ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // configure logging ArServerSimpleComLogRobotConfig configCommands(&commands, &robot); // trigger logging of the robot config parameters // ArServerSimpleServerCommands serverCommands(&commands, &server); // monitor networking behavior (track packets sent etc.) // service that allows the client to monitor the communication link status // between the robot and the client. // ArServerHandlerCommMonitor handlerCommMonitor(&server); // service that allows client to change configuration parameters in ArConfig ArServerHandlerConfig handlerConfig(&server, Aria::getConfig(), Arnl::getTypicalDefaultParamFileName(), Aria::getDirectory()); // This service causes the client to show simple dialog boxes ArServerHandlerPopup popupServer(&server); /* Set up the possible modes for remote control from a client such as * MobileEyes: */ // Mode To stop and remain stopped: ArServerModeStop modeStop(&server, &robot); #ifndef ARNL_SONARLOC // Cause the sonar to turn off automatically // when the robot is stopped, and turn it back on when commands to move // are sent. (Note, if using SONARNL to localize, then don't do this // since localization may get lost) ArSonarAutoDisabler sonarAutoDisabler(&robot); #endif // Teleoperation modes To drive by keyboard, joystick, etc: ArServerModeRatioDrive modeRatioDrive(&server, &robot); // Prevent normal teleoperation driving if localization is lost using // a high-priority action, which enables itself when the particular mode is // active. // (You have to enter unsafe drive mode to drive when lost.) ArActionLost actionLostRatioDrive(&LOCTASK, NULL, &modeRatioDrive); modeRatioDrive.getActionGroup()->addAction(&actionLostRatioDrive, 110); // Add drive mode section to the configuration, and also some custom (simple) commands: modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings"); modeRatioDrive.addControlCommands(&commands); // Wander mode (also prevent wandering if lost): ArServerModeWander modeWander(&server, &robot); ArActionLost actionLostWander(&LOCTASK, NULL, &modeWander); modeWander.getActionGroup()->addAction(&actionLostWander, 110); // Tool to log data periodically to a file ArDataLogger dataLogger(&robot, "datalog.txt"); dataLogger.addToConfig(Aria::getConfig()); // make it configurable through ArConfig // Automatically add anything from the global info group to the data logger. Aria::getInfoGroup()->addAddStringCallback(dataLogger.getAddStringFunctor()); // This provides a small table of interesting information for the client // to display to the operator. You can add your own callbacks to show any // data you want. ArServerInfoStrings stringInfo(&server); Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor()); // The following statements add fields to a set of informational data called // the InfoGroup. These are served to MobileEyes for displayi (turn on by enabling Details // and Custom Details in the View menu of MobileEyes.) Aria::getInfoGroup()->addStringInt( "Motor Packet Count", 10, new ArConstRetFunctorC<int, ArRobot>(&robot, &ArRobot::getMotorPacCount)); #ifdef ARNL_LASERLOC Aria::getInfoGroup()->addStringDouble( "Laser Localization Score", 8, new ArRetFunctorC<double, ArLocalizationTask>( &locTask, &ArLocalizationTask::getLocalizationScore), "%.03f"); Aria::getInfoGroup()->addStringInt( "Laser Loc Num Samples", 8, new ArRetFunctorC<int, ArLocalizationTask>( &locTask, &ArLocalizationTask::getCurrentNumSamples), "%4d"); #elif defined(ARNL_SONARLOC) Aria::getInfoGroup()->addStringDouble( "Sonar Localization Score", 8, new ArRetFunctorC<double, ArSonarLocalizationTask>( &locTask, &ArSonarLocalizationTask::getLocalizationScore), "%.03f"); Aria::getInfoGroup()->addStringInt( "Sonar Loc Num Samples", 8, new ArRetFunctorC<int, ArSonarLocalizationTask>( &locTask, &ArSonarLocalizationTask::getCurrentNumSamples), "%4d"); #endif #ifdef ARNL_GPSLOC const char *dopfmt = "%2.4f"; const char *posfmt = "%2.8f"; const char *altfmt = "%3.6f m"; Aria::getInfoGroup()->addStringString( "GPS Fix Mode", 25, new ArConstRetFunctorC<const char*, ArGPS>(gps, &ArGPS::getFixTypeName) ); Aria::getInfoGroup()->addStringInt( "GPS Num. Satellites", 4, new ArConstRetFunctorC<int, ArGPS>(gps, &ArGPS::getNumSatellitesTracked) ); Aria::getInfoGroup()->addStringDouble( "GPS HDOP", 12, new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getHDOP), dopfmt ); Aria::getInfoGroup()->addStringDouble( "GPS VDOP", 5, new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getVDOP), dopfmt ); Aria::getInfoGroup()->addStringDouble( "GPS PDOP", 5, new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getPDOP), dopfmt ); Aria::getInfoGroup()->addStringDouble( "Latitude", 15, new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getLatitude), posfmt ); Aria::getInfoGroup()->addStringDouble( "Longitude", 15, new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getLongitude), posfmt ); Aria::getInfoGroup()->addStringDouble( "Altitude", 8, new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getAltitude), altfmt ); // only some GPS receivers provide these, but you can uncomment them // here to enable them if yours does. /* const char *errfmt = "%2.4f m"; Aria::getInfoGroup()->addStringDouble( "GPS Lat. Err.", 6, new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getLatitudeError), errfmt ); Aria::getInfoGroup()->addStringDouble( "GPS Lon. Err.", 6, new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getLongitudeError), errfmt ); Aria::getInfoGroup()->addStringDouble( "GPS Alt. Err.", 6, new ArConstRetFunctorC<double, ArGPS>(gps, &ArGPS::getAltitudeError), errfmt ); */ Aria::getInfoGroup()->addStringDouble( "MOGS Localization Score", 8, new ArRetFunctorC<double, ArGPSLocalizationTask>( &gpsLocTask, &ArGPSLocalizationTask::getLocalizationScore), "%.03f" ); #endif // Display gyro status if gyro is enabled and is being handled by the firmware (gyro types 2, 3, or 4). // (If the firmware detects an error communicating with the gyro or IMU it // returns a flag, and stops using it.) // (This gyro type parameter, and fault flag, are only in ARCOS, not Seekur firmware) if(robot.getOrigRobotConfig() && robot.getOrigRobotConfig()->getGyroType() > 1) { Aria::getInfoGroup()->addStringString( "Gyro/IMU Status", 10, new ArGlobalRetFunctor1<const char*, ArRobot*>(&getGyroStatusString, &robot) ); } // Display system CPU and wireless network status ArSystemStatus::startPeriodicUpdate(1000); // update every 1 second Aria::getInfoGroup()->addStringDouble("CPU Use", 10, ArSystemStatus::getCPUPercentFunctor(), "% 4.0f%%"); Aria::getInfoGroup()->addStringInt("Wireless Link Quality", 9, ArSystemStatus::getWirelessLinkQualityFunctor(), "%d"); Aria::getInfoGroup()->addStringInt("Wireless Link Noise", 9, ArSystemStatus::getWirelessLinkNoiseFunctor(), "%d"); Aria::getInfoGroup()->addStringInt("Wireless Signal", 9, ArSystemStatus::getWirelessLinkSignalFunctor(), "%d"); // stats on how far its driven since software started Aria::getInfoGroup()->addStringDouble("Distance Travelled (m)", 20, new ArRetFunctorC<double, ArRobot>(&robot, &ArRobot::getOdometerDistanceMeters), "%.2f"); Aria::getInfoGroup()->addStringDouble("Run time (min)", 20, new ArRetFunctorC<double, ArRobot>(&robot, &ArRobot::getOdometerTimeMinutes), "%.2f"); #ifdef ARNL_GPSLOC // Add some "custom commands" for setting up initial GPS offset and heading. gpsLocTask.addLocalizationInitCommands(&commands); // Add some commands for manually creating map objects based on GPS positions: // ArGPSMapTools gpsMapTools(gps, &robot, &commands, &map); // Add command to set simulated GPS position manually if(gpsConnector.getGPSType() == ArGPSConnector::Simulator) { ArSimulatedGPS *simGPS = dynamic_cast<ArSimulatedGPS*>(gps); // simGPS->setDummyPosition(42.80709, -71.579047, 100); commands.addStringCommand("GPS:setDummyPosition", "Manually set a new dummy position for simulated GPS. Provide latitude (required), longitude (required) and altitude (optional)", new ArFunctor1C<ArSimulatedGPS, ArArgumentBuilder*>(simGPS, &ArSimulatedGPS::setDummyPositionFromArgs) ); } #endif // Make Stop mode the default (If current mode deactivates without entering // a new mode, then Stop Mode will be selected) modeStop.addAsDefaultMode(); // TODO move up near where stop mode is created? #ifdef ARNL_MAPPING /* Services that allow the client to initiate scanning with the laser to create maps in Mapper3 (So not possible with SONARNL): */ ArServerHandlerMapping handlerMapping(&server, &robot, firstLaser, fileDir, "", true); #ifdef ARNL_LASERLOC // make laser localization stop while mapping handlerMapping.addMappingStartCallback( new ArFunctor1C<ArLocalizationTask, bool> (&locTask, &ArLocalizationTask::setIdleFlag, true)); // and then make it start again when we're doine handlerMapping.addMappingEndCallback( new ArFunctor1C<ArLocalizationTask, bool> (&locTask, &ArLocalizationTask::setIdleFlag, false)); #endif #ifdef ARNL_GPSLOC // Save GPS positions in the .2d scan log when making a map handlerMapping.addLocationData("robotGPS", gpsLocTask.getPoseInterpPositionCallback()); // add the starting latitude and longitude info to the .2d scan log handlerMapping.addMappingStartCallback( new ArFunctor1C<ArGPSLocalizationTask, ArServerHandlerMapping *> (&gpsLocTask, &ArGPSLocalizationTask::addScanInfo, &handlerMapping)); #endif // Make it so our "lost" actions don't stop us while mapping handlerMapping.addMappingStartCallback(actionLostRatioDrive.getDisableCB()); handlerMapping.addMappingStartCallback(actionLostWander.getDisableCB()); // And then let them make us stop as usual when done mapping handlerMapping.addMappingEndCallback(actionLostRatioDrive.getEnableCB()); handlerMapping.addMappingEndCallback(actionLostWander.getEnableCB()); #endif // ARNL_MAPPING /* // If we are on a simulator, move the robot back to its starting position, // and reset its odometry. // This will allow localizeRobotAtHomeBlocking() below will (probably) work (it // tries current odometry (which will be 0,0,0) and all the map // home points. // (Ignored by a real robot) //robot.com(ArCommands::SIM_RESET); */ // create a pose storage class, this will let the program keep track // of where the robot is between runs... after we try and restore // from this file it will start saving the robot's pose into the // file ArPoseStorage poseStorage(&robot); /// if we could restore the pose from then set the sim there (this /// won't do anything to the real robot)... if we couldn't restore /// the pose then just reset the position of the robot (which again /// won't do anything to the real robot) if (poseStorage.restorePose("robotPose")) serverLocHandler.setSimPose(robot.getPose()); //else // robot.com(ArCommands::SIM_RESET); /* File transfer services: */ #pragma GPP off #ifdef WIN32 // Not implemented for Windows yet. ArLog::log(ArLog::Normal, "Note, file upload/download services are not implemented for Windows; not enabling them."); #else // This block will allow you to set up where you get and put files // to/from, just comment them out if you don't want this to happen // /* ArServerFileLister fileLister(&server, fileDir); ArServerFileToClient fileToClient(&server, fileDir); ArServerFileFromClient fileFromClient(&server, fileDir, "/tmp"); ArServerDeleteFileOnServer deleteFileOnServer(&server, fileDir); // */ #endif #pragma GPP on /* Video image streaming, and camera controls (Requires SAVserver or ACTS) */ // Forward one video stream if either ACTS, ArVideo videoSubServer, // or SAV server are running. // ArHybridForwarderVideo allows this program to be separate from the ArVideo // library. You could replace videoForwarder and the PTZ connection code below // with a call to ArVideo::createVideoServers(), and link the program to the // ArVideo library if you want to include video capture in the same program // as robot control. ArHybridForwarderVideo videoForwarder(&server, "localhost", 7070); // connect to first configured camera PTZ controls (in robot parameter file and // command line options) ptzConnector.connect(); ArCameraCollection cameraCollection; ArPTZ *ptz = ptzConnector.getPTZ(0); if(ptz) { ArLog::log(ArLog::Normal, "Connected to PTZ Camera"); cameraCollection.addCamera("Camera1", ptz->getTypeName(), "Camera", ptz->getTypeName()); videoForwarder.setCameraName("Camera1"); videoForwarder.addToCameraCollection(cameraCollection); new ArServerHandlerCamera("Camera1", &server, &robot, ptz, &cameraCollection); } // Allows client to find any camera servers created above ArServerHandlerCameraCollection cameraCollectionServer(&server, &cameraCollection); /* Load configuration values, map, and begin! */ // When parsing the configuration file, also look at the program's command line options // from the command-line argument parser as well as the configuration file. // (So you can use any argument on the command line, namely -map.) Aria::getConfig()->useArgumentParser(&parser); // Read in parameter files. ArLog::log(ArLog::Normal, "Loading config file %s%s into ArConfig...", Aria::getDirectory(), Arnl::getTypicalParamFileName()); if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName())) { ArLog::log(ArLog::Normal, "Could not load ARNL configuration file. Set ARNL environment variable to use non-default installation director.y"); Aria::exit(5); } // Warn about unknown params. if (!simpleOpener.checkAndLog() || !parser.checkHelpAndWarnUnparsed()) { logOptions(argv[0]); Aria::exit(6); } // Warn if there is no map if (map.getFileName() == NULL || strlen(map.getFileName()) <= 0) { ArLog::log(ArLog::Normal, ""); ArLog::log(ArLog::Normal, "### No map file is set up, you can make a map with the following procedure"); #ifdef ARNL ArLog::log(ArLog::Normal, " 0) You can find this information in README.txt or docs/Mapping.txt"); ArLog::log(ArLog::Normal, " 1) Connect to this server with MobileEyes"); ArLog::log(ArLog::Normal, " 2) Go to Tools->Map Creation->Start Scan"); ArLog::log(ArLog::Normal, " 3) Give the map a name and hit okay"); ArLog::log(ArLog::Normal, " 4) Drive the robot around your space (see docs/Mapping.txt"); ArLog::log(ArLog::Normal, " 5) Go to Tools->Map Creation->Stop Scan"); ArLog::log(ArLog::Normal, " 6) Start up Mapper3"); ArLog::log(ArLog::Normal, " 7) Go to File->Open on Robot"); ArLog::log(ArLog::Normal, " 8) Select the .2d you created"); ArLog::log(ArLog::Normal, " 9) Create a .map"); ArLog::log(ArLog::Normal, " 10) Go to File->Save on Robot"); ArLog::log(ArLog::Normal, " 11) In MobileEyes, go to Tools->Robot Config"); ArLog::log(ArLog::Normal, " 12) Choose the Files section"); ArLog::log(ArLog::Normal, " 13) Enter the path and name of your new .map file for the value of the Map parameter."); ArLog::log(ArLog::Normal, " 14) Press OK and your new map should become the map used"); ArLog::log(ArLog::Normal, ""); #elif defined(SONARNL) ArLog::log(ArLog::Normal, " 0) You can find this information in README.txt or docs/SonarMapping.txt"); ArLog::log(ArLog::Normal, " 1) Start up Mapper3Basic"); ArLog::log(ArLog::Normal, " 2) Go to File->New"); ArLog::log(ArLog::Normal, " 3) Draw a line map of your area (make sure it is to scale)"); ArLog::log(ArLog::Normal, " 4) Go to File->Save on Robot"); ArLog::log(ArLog::Normal, " 5) In MobileEyes, go to Tools->Robot Config"); ArLog::log(ArLog::Normal, " 6) Choose the Files section"); ArLog::log(ArLog::Normal, " 7) Enter the path and name of your new .map file for the value of the Map parameter."); ArLog::log(ArLog::Normal, " 8) Press OK and your new map should become the map used"); ArLog::log(ArLog::Normal, ""); #endif #ifdef ARNL_GPSLOC ArLog::log(ArLog::Normal, "\n See docs/GPSMapping.txt for instructions on creating a map for GPS localization"); #endif } // Print a log message notifying user of the directory for map files ArLog::log(ArLog::Normal, ""); ArLog::log(ArLog::Normal, "Directory for maps and file serving: %s", fileDir); ArLog::log(ArLog::Normal, "See the ARNL README.txt for more information"); ArLog::log(ArLog::Normal, ""); // Do an initial localization of the robot. ARNL and SONARNL try all the home points // in the map, as well as the robot's current odometric position, as possible // places the robot is likely to be at startup. If successful, it will // also save the position it found to be the best localized position as the // "Home" position, which can be obtained from the localization task (and is // used by the "Go to home" network request). // MOGS instead just initializes at the current GPS position. // (You will stil have to drive the robot so it can determine the robot's // heading, however. See GPS Mapping instructions.) LOCTASK.localizeRobotAtHomeBlocking(); #ifdef ARNL_MULTIROBOT // Let the client switch manager (for multirobot) spin off into its own thread // TODO move to multirobot example? clientSwitch.runAsync(); #endif // Start the networking server's thread server.runAsync(); ArLog::log(ArLog::Normal, "Server running. To exit, press CTRL-C."); // Enable the motors and wait until the robot exits (disconnection, etc.) or this program is // canceled. robot.enableMotors(); robot.waitForRunExit(); Aria::exit(0); }
int main(int argc, char **argv) { //----------------------initialized robot server------------------------------------------ Aria::init(); Arnl::init(); ArServerBase server; //----------------------------------------------------------------------------------- VCCHandler ptz(&robot); //create keyboard for control vcc50i G_PTZHandler->reset(); ArUtil::sleep(300); G_PTZHandler->panSlew(30); //----------------------------------------------------------------------------------- argc = 2 ; argv[0] = "-map"; argv[1] = "map20121111.map"; // Parse the command line arguments. ArArgumentParser parser(&argc, argv); // Set up our simpleConnector ArSimpleConnector simpleConnector(&parser); // Set up our simpleOpener ArServerSimpleOpener simpleOpener(&parser); //******* // Set up our client for the central server // ArClientSwitchManager clientSwitch(&server, &parser); //************ // Load default arguments for this computer (from /etc/Aria.args, environment // variables, and other places) // parser.loadDefaultArguments(); // set up a gyro ArAnalogGyro gyro(&robot); //gyro.activate(); // Parse arguments for the simple connector. // if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) // { // ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]); // Aria::logOptions(); // Aria::exit(1); // } // The laser object, will be used if we have one // Add the laser to the robot robot.addRangeDevice(&sick); // Sonar, must be added to the robot, used by teleoperation and wander to // detect obstacles, and for localization if SONARNL ArSonarDevice sonarDev; // Add the sonar to the robot robot.addRangeDevice(&sonarDev); // Set up where we'll look for files char fileDir[1024]; ArUtil::addDirectories(fileDir, sizeof(fileDir), "./", "maps"); ArLog::log(ArLog::Normal, "Installation directory is: %s\nMaps directory is: %s\n", Aria::getDirectory(), fileDir); // Set up the map, this will look for files in the examples // directory (unless the file name starts with a /, \, or . // You can take out the 'fileDir' argument to look in the current directory // instead ArMap arMap(fileDir); // set it up to ignore empty file names (otherwise the parseFile // on the config will fail) arMap.setIgnoreEmptyFileName(true); //******************************** //Localization //******************************** ArLocalizationManager locManager(&robot, &arMap); ArLog::log(ArLog::Normal, "Creating laser localization task"); ArLocalizationTask locTask (&robot, &sick, &arMap); locManager.addLocalizationTask(&locTask); //******************************* //Path planning //******************************* // Make the path task planning task ArPathPlanningTask pathTask(&robot, &sick, &sonarDev, &arMap); G_PathPlanning = &pathTask; // Set up things so data can be logged (only do it with the laser // since it can overrun a 9600 serial connection which the sonar is // more likely to have) ArDataLogger dataLogger(&robot); dataLogger.addToConfig(Aria::getConfig()); // add our logging to the config // ArLog::addToConfig(Aria::getConfig()); // First open the server if (!simpleOpener.open(&server, fileDir, 240)) { if (simpleOpener.wasUserFileBad()) ArLog::log(ArLog::Normal, "Bad user file"); else ArLog::log(ArLog::Normal, "Could not open server port"); exit(2); } // Connect the robot if (!simpleConnector.connectRobot(&robot)) { ArLog::log(ArLog::Normal, "Could not connect to robot... exiting"); Aria::exit(3); } //----------------------------------------------- //************************** // Set up a class that'll put the movement and gyro parameters into ArConfig ArRobotConfig robotConfig(&robot); robotConfig.addAnalogGyro(&gyro); //***************************** robot.enableMotors(); robot.clearDirectMotion(); // if we are connected to a simulator, reset it to its start position robot.comInt(ArCommands::RESETSIMTOORIGIN, 1); robot.moveTo(ArPose(0,0,0)); // Set up laser using connector (command line arguments, etc.) simpleConnector.setupLaser(&sick); // Start the robot thread. robot.runAsync(true); // Start the laser thread. sick.runAsync(); // Try to connect the laser if (!sick.blockingConnect()) ArLog::log(ArLog::Normal, "Warning: Couldn't connect to SICK laser, it won't be used"); else ArLog::log(ArLog::Normal, "Connected to laser."); //*************************************** // Add additional range devices to the robot and path planning task. // IRs if the robot has them. robot.lock(); // ArIRs irs; // robot.addRangeDevice(&irs); // pathTask.addRangeDevice(&irs, ArPathPlanningTask::CURRENT); //****************************************** // Forbidden regions from the map ArForbiddenRangeDevice forbidden(&arMap); robot.addRangeDevice(&forbidden); // This is the place to add a range device which will hold sensor data // and delete it appropriately to replan around blocked paths. ArGlobalReplanningRangeDevice replanDev(&pathTask); // Create objects that add network services: // Drawing in the map display: ArServerInfoDrawings drawings(&server); drawings.addRobotsRangeDevices(&robot); drawings.addRangeDevice(&replanDev); /* If you want to draw the destination put this code back in: ArServerDrawingDestination destination( &drawings, &pathTask, "destination", 500, 500, new ArDrawingData("polyDots", ArColor(0xff, 0xff, 0x0), 800, // size 49), // just below the robot */ /* If you want to see the local path planning area use this (You can enable this particular drawing from custom commands which is set up down below in ArServerInfoPath) ArDrawingData drawingDataP("polyLine", ArColor(200,200,200), 1, 75); ArFunctor2C<ArPathPlanningTask, ArServerClient *, ArNetPacket *> drawingFunctorP(pathTask, &ArPathPlanningTask::drawSearchRectangle); drawings.addDrawing(&drawingDataP, "Local Plan Area", &drawingFunctorP); */ /* If you want to see the points making up the local path in addition to the * main path use this. ArDrawingData drawingDataP2("polyDots", ArColor(0,128,0), 100, 70); ArFunctor2C<ArPathPlanningTask, ArServerClient *, ArNetPacket *> drawingFunctorP2(pathTask, &ArPathPlanningTask::drawPathPoints); drawings.addDrawing(&drawingDataP2, "Path Points", &drawingFunctorP2); */ // Misc. simple commands: ArServerHandlerCommands commands(&server); // These provide various kinds of information to the client: ArServerInfoRobot serverInfoRobot(&server, &robot); ArServerInfoSensor serverInfoSensor(&server, &robot); ArServerInfoPath serverInfoPath(&server, &robot, &pathTask); serverInfoPath.addSearchRectangleDrawing(&drawings); serverInfoPath.addControlCommands(&commands); //-------------------------receive commands or events from client--------------------- // ArServerHandlerCommands commands(&server); server.addData("RobotVideo" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&VideoServerBase::RobotVideoCB) ,"",""); server.addData("turn" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&turn_func) ,"",""); server.addData("RobotMotion" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_RobotMotion) ,"",""); server.addData("CameraMotion" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_CameraMotion) ,"",""); server.addData("RobotTurnLeft" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_RobotTurnLeft) ,"",""); server.addData("RobotTurnRight" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_RobotTurnRight) ,"",""); server.addData("TargetApproach" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_TargetApproach) ,"",""); server.addData("TargetApproachObstacles" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_TargetApproach_Obstacles) ,"",""); server.addData("GlassesCancel" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_GlassesCancel) ,"",""); server.addData("Calibration" ,"", new ArGlobalFunctor2<ArServerClient *, ArNetPacket*>(&S_Calibration) ,"",""); server.addClientRemovedCallback(new ArGlobalFunctor1< ArServerClient * >(&clientCloseCallback)); //-------------------------receive commands or events from client--------------------- //*********************** ArServerInfoLocalization serverInfoLocalization (&server, &robot, &locManager); ArServerHandlerLocalization serverLocHandler (&server, &robot, &locManager); // Provide the map to the client (and related controls): // This uses both lines and points now, since everything except // sonar localization uses both (path planning with sonar still uses both) ArServerHandlerMap serverMap(&server, &arMap); // Add some simple (custom) commands for testing and debugging: ArServerSimpleComUC uCCommands(&commands, &robot); // Send any command to the microcontroller ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // configure logging ArServerSimpleComGyro gyroCommands(&commands, &robot, &gyro); // monitor the gyro ArServerSimpleComLogRobotConfig configCommands(&commands, &robot); // trigger logging of the robot config parameters ArServerSimpleServerCommands serverCommands(&commands, &server); // monitor networking behavior (track packets sent etc.) /* Set up the possible modes for remote control from a client such as * MobileEyes: */ // Mode To go to a goal or other specific point: ArServerModeGoto modeGoto(&server, &robot, &pathTask, &arMap, ArPose(0,0,0)); // Add a simple (custom) command that allows you to give a list of // goals to tour, instead of all. Useful for testing and debugging. modeGoto.addTourGoalsInListSimpleCommand(&commands); // Mode To stop and remain stopped: ArServerModeStop modeStop(&server, &robot); // cause the sonar to turn off automatically // when the robot is stopped, and turn it back on when commands to move // are sent. (Note, this should not be done if you need the sonar // data to localize, or for other purposes while stopped) ArSonarAutoDisabler sonarAutoDisabler(&robot); // Teleoperation modes To drive by keyboard, joystick, etc: ArServerModeRatioDrive modeRatioDrive(&server, &robot); // New, improved mode ArServerModeDrive modeDrive(&server, &robot); // Older mode for compatability // Drive mode's configuration and custom (simple) commands: modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings"); modeDrive.addControlCommands(&commands); modeRatioDrive.addControlCommands(&commands); // Wander mode // ArServerModeWander modeWander(&server, &robot); //********************************* // Prevent driving if localization is lost: ArActionLost actionLostRatioDrive (&locManager, NULL, &modeRatioDrive); modeRatioDrive.getActionGroup ()->addAction (&actionLostRatioDrive, 110); // Prevent wandering if lost: // ArActionLost // actionLostWander (&locManager, NULL, &modeWander); // modeWander.getActionGroup ()->addAction (&actionLostWander, 110); // This provides a small table of interesting information for the client // to display to the operator: ArServerInfoStrings stringInfo(&server); Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor()); Aria::getInfoGroup()->addStringInt( "Motor Packet Count", 10, new ArConstRetFunctorC<int, ArRobot>(&robot, &ArRobot::getMotorPacCount)); // Make Stop mode the default (If current mode deactivates without entering // a new mode, then Stop Mode will be selected) modeStop.addAsDefaultMode(); // Create the service that allows the client to monitor the communication // between the robot and the client. // ArServerHandlerCommMonitor handlerCommMonitor(&server); // Create service that allows client to change configuration parameters in ArConfig ArServerHandlerConfig handlerConfig(&server, Aria::getConfig(), Arnl::getTypicalDefaultParamFileName(), Aria::getDirectory()); // Read in parameter files. read the paras from input Aria::getConfig()->useArgumentParser(&parser); if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName())) { ArLog::log(ArLog::Normal, "Trouble loading configuration file, exiting"); Aria::exit(5); } // Warn about unknown params. if (!simpleOpener.checkAndLog() || !parser.checkHelpAndWarnUnparsed()) { ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]); simpleConnector.logOptions(); simpleOpener.logOptions(); Aria::exit(6); } // Warn if there is no map if (arMap.getFileName() == NULL || strlen(arMap.getFileName()) <= 0) { ArLog::log(ArLog::Normal, ""); 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."); ArLog::log(ArLog::Normal, ""); } // find out where we'll want to put files ArLog::log(ArLog::Normal, ""); ArLog::log(ArLog::Normal, "Directory for maps and file serving: %s", fileDir); ArLog::log(ArLog::Normal, "See the ARNL README.txt for more information"); ArLog::log(ArLog::Normal, ""); // If you want MobileSim to try and load up the same map as you are // using in guiServer then uncomment out the next line and this object // will send a command to MobileSim to do so, but make sure you start // MobileSim from the Arnl/examples directory or use the --cwd option, // so that the map names used by MobileSim match the map names used // by guiServer //ArSimMapSwitcher mapSwitcher(&robot, &arMap); /****************************************************** * **************************************************** * Camera * **************************************************** * *****************************************************/ robot.unlock(); // Localize robot at home. locTask.localizeRobotAtHomeBlocking(); locTask.forceUpdatePose(ArPose(0,0,0)); resetMotion(); // robot.enableMotors(); // robot.runAsync(true); //locTask.localizeRobotAtHomeBlocking(); server.runAsync(); VideoServerBase videoserver; videoserver.runAsync(); robot.comInt(ArCommands::SOUNDTOG, 0); //G_PathPlanning->pathPlanToPose(ArPose(1500, -1500 , -32),true,true); //while(G_PathPlanning->getState() != ArPathPlanningTask::REACHED_GOAL ); //S_TargetApproach_Obstacles1(); //ArUtil::sleep(5000); // G_PTZHandler->panRel(60); // ArUtil::sleep(3000); //while(1) //S_TargetApproach1(); cout << "done!!!!!!!---------------------" <<endl; robot.waitForRunExit(); Aria::exit(0); }
int main(int argc, char **argv) { Aria::init(); ArRobot robot; ArSonarDevice sonar; ArSick sick; robot.addRangeDevice(&sonar); ArServerBase server; // Argument parser: ArArgumentParser parser(&argc, argv); parser.loadDefaultArguments(); // Connector and server opener: ArRobotConnector robotConnector(&parser, &robot); if(!robotConnector.connectRobot()) { ArLog::log(ArLog::Terse, "popupExample: Could not connect to the robot."); if(parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); } Aria::exit(1); } ArLaserConnector laserConnector(&parser, &robot, &robotConnector); ArServerSimpleOpener simpleOpener(&parser); // Get command-line and other parameters if(!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(1); } robot.runAsync(true); // connect to the laser if(!laserConnector.connectLasers()) { ArLog::log(ArLog::Normal, "popupExample: Warning: Could not connect to lasers."); } // Open the server if(!simpleOpener.open(&server)) { ArLog::log(ArLog::Terse, "popupExample: Error, could not open server."); return 1; } server.runAsync(); ArLog::log(ArLog::Normal, "popupExample: Server running. Press control-C to exit."); ArLog::log(ArLog::Normal, "popupExample: Each time an obstacle is detected near the robot, a new popup message will be created. Connect with MobileEyes to see them."); // Sends robot position etc. ArServerInfoRobot robotInfoServer(&server, &robot); // This service sends drawings e.g. showing range device positions ArServerInfoDrawings drawingsServer(&server); drawingsServer.addRobotsRangeDevices(&robot); // This service can send messages to clients to display as popup dialogs: ArServerHandlerPopup popupServer(&server); // This object contains the robot sensor interpretation task and creates // popups: SensorDetectPopup(&robot, &popupServer); // modes for controlling robot movement ArServerModeStop modeStop(&server, &robot); ArServerModeRatioDrive modeRatioDrive(&server, &robot); ArServerModeWander modeWander(&server, &robot); modeStop.addAsDefaultMode(); modeStop.activate(); // allow configuration of driving and other settings ArServerHandlerConfig serverHandlerConfig(&server, Aria::getConfig()); // make a config handler ArLog::addToConfig(Aria::getConfig()); // let people configure logging modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings"); // able to configure teleop settings robot.enableMotors(); robot.waitForRunExit(); Aria::exit(0); }
int main ( int argc, char *argv[] ){ //cout << "running....\n"; try{ // Create the socket ServerSocket server ( 30000 ); Aria::init(); Arnl::init(); ArRobot robot; ArArgumentParser parser(&argc, argv); parser.loadDefaultArguments(); ArSonarDevice sonar; ArSimpleConnector simpleConnector(&parser); // Our server for mobile eyes ArServerBase moServer; // Set up our simpleOpener ArServerSimpleOpener simpleOpener(&parser); parser.loadDefaultArguments(); if (!Aria::parseArgs () || !parser.checkHelpAndWarnUnparsed()){ Aria::logOptions (); Aria::exit (1); } //Add the sonar to the robot robot.addRangeDevice(&sonar); // Look for map in the current directory ArMap arMap; // set it up to ignore empty file names (otherwise the parseFile // on the config will fail) arMap.setIgnoreEmptyFileName (true); // First open the server if (!simpleOpener.open(&moServer)){ if (simpleOpener.wasUserFileBad()) ArLog::log(ArLog::Normal, "Bad user file"); else ArLog::log(ArLog::Normal, "Could not open server port"); exit(2); } // Connect to the robot if (!simpleConnector.connectRobot (&robot)){ ArLog::log (ArLog::Normal, "Could not connect to robot... exiting"); Aria::exit (3); } // Create the localization task (it will start its own thread here) ArSonarLocalizationTask locTask(&robot, &sonar, &arMap); ArLocalizationManager locManager(&robot, &arMap); ArLog::log(ArLog::Normal, "Creating sonar localization task"); locManager.addLocalizationTask(&locTask); // Set the initial pose to the robot's "Home" position from the map, or // (0,0,0) if none, then let the localization thread take over. locTask.localizeRobotAtHomeNonBlocking(); //Create the path planning task ArPathPlanningTask pathTask(&robot,&sonar,&arMap); ArLog::log(ArLog::Normal, "Robot Server: Connected."); robot.enableMotors(); robot.clearDirectMotion(); // Start the robot processing cycle running in the background. // True parameter means that if the connection is lost, then the // run loop ends. robot.runAsync(true); // Read in parameter files. Aria::getConfig ()->useArgumentParser (&parser); if (!Aria::getConfig ()->parseFile (Arnl::getTypicalParamFileName ())){ ArLog::log (ArLog::Normal, "Trouble loading configuration file, exiting"); Aria::exit (5); } //Create the three states robot.lock(); Follow follow = Follow(&robot,&sonar); GoTo goTo(&robot,&pathTask,&arMap); Search s(&robot,&sonar); // Bumpers. ArBumpers bumpers; robot.addRangeDevice(&bumpers); pathTask.addRangeDevice(&bumpers, ArPathPlanningTask::CURRENT); // Forbidden regions from the map ArForbiddenRangeDevice forbidden(&arMap); robot.addRangeDevice(&forbidden); pathTask.addRangeDevice(&forbidden, ArPathPlanningTask::CURRENT); // Mode To stop and remain stopped: ArServerModeStop modeStop(&moServer, &robot); // Action to slow down robot when localization score drops but not lost. ArActionSlowDownWhenNotCertain actionSlowDown(&locTask); pathTask.getPathPlanActionGroup()->addAction(&actionSlowDown, 140); // Action to stop the robot when localization is "lost" (score too low) ArActionLost actionLostPath(&locTask, &pathTask); pathTask.getPathPlanActionGroup()->addAction(&actionLostPath, 150); // These provide various kinds of information to the client: ArServerInfoRobot serverInfoRobot(&moServer, &robot); ArServerInfoSensor serverInfoSensor(&moServer, &robot); ArServerInfoPath serverInfoPath(&moServer, &robot, &pathTask); // Provide the map to the client (and related controls): // This uses both lines and points now, since everything except // sonar localization uses both (path planning with sonar still uses both) ArServerHandlerMap serverMap(&moServer, &arMap); // Provides localization info and allows the client (MobileEyes) to relocalize at a given // pose: ArServerInfoLocalization serverInfoLocalization(&moServer, &robot, &locTask); ArServerHandlerLocalization serverLocHandler(&moServer, &robot, &locTask); robot.unlock(); moServer.runAsync(); //Main loop while (true){ //The socket to accept connection ServerSocket new_sock; server.accept ( new_sock ); int state = 1; //1 = Follow, 2 = Search, 3 = GoTo int lastPos[2]; //Storing last position of BB to search the target int data[2]; //matrix with X,Y of BB try{ while ( true ){ //receive data from tld new_sock >> data; //cout << data[0] << "," << data[1] << endl; if(data[0] != -1) lastPos[0] = data[0]; //cout << state <<endl; //for debugging //Main logic switch(state){ case 1: cout << "Following target\n"; state = follow.run(data); break; case 2: cout << "Searching for target\n"; state = s.seek(lastPos, data); break; case 3: cout << "Going to ...\n"; state = goTo.run(data); break; default: cout << "Not a case for state\n"; break; } std::cout << "Loc score: " << locTask.getLocalizationScore() << std::endl; } } catch ( SocketException& ) { cout << "Lost Connection" << endl; robot.lock(); robot.stop(); robot.unlock(); } } } catch ( SocketException& e ){ std::cout << "Exception was caught:" << e.description() << "\nExiting.\n"; } ArLog::log(ArLog::Normal, "RobotServer: Exiting."); return 0; }
int main(int argc, char *argv[]) { // Initialize location of Aria, Arnl and their args. Aria::init(); Arnl::init(); // The robot object ArRobot robot; #ifndef SONARNL // The laser object ArSick sick(181, 361); #endif // Parse them command line arguments. ArArgumentParser parser(&argc, argv); // Set up our simpleConnector ArSimpleConnector simpleConnector(&parser); // Load default arguments for this computer parser.loadDefaultArguments(); // Parse its arguments for the simple connector. simpleConnector.parseArgs(); // sonar, must be added to the robot, for teleop and wander ArSonarDevice sonarDev; // add the sonar to the robot robot.addRangeDevice(&sonarDev); ArMap arMap; #ifndef SONARNL // Initialize the localization ArLocalizationTask locTask(&robot, &sick, &arMap); // Make the path task ArPathPlanningTask pathTask(&robot, &sick, &sonarDev, &arMap); #else // Initialize the localization ArSonarLocalizationTask locTask(&robot, &sonarDev, &arMap); // Make the path task ArPathPlanningTask pathTask(&robot, NULL, &sonarDev, &arMap); #endif // Stop the robot as soon as localization fails. ArFunctor1C<ArPathPlanningTask, int> locaFailed(&pathTask, &ArPathPlanningTask::trackingFailed); locTask.addFailedLocalizationCB(&locaFailed); // Read in param files. Aria::getConfig()->useArgumentParser(&parser); if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName())) { printf("Trouble loading configuration file, exiting\n"); exit(1); } // Warn about unknown params. if (!parser.checkHelpAndWarnUnparsed()) { printf("\nUsage: %s -map mapfilename\n\n", argv[0]); simpleConnector.logOptions(); exit(2); } // Our server ArServerBase server; // First open the server up if (!server.open(7272)) { printf("Could not open server port\n"); exit(1); } // Connect the robot if (!simpleConnector.connectRobot(&robot)) { printf("Could not connect to robot... exiting\n"); Aria::shutdown(); return 1; } robot.com2Bytes(31, 14, 0); robot.com2Bytes(31, 15, 0); ArUtil::sleep(100); robot.com2Bytes(31, 14, 1); robot.com2Bytes(31, 15, 1); robot.enableMotors(); robot.clearDirectMotion(); #ifndef SONARNL // Set up the laser before handing it to the laser mode simpleConnector.setupLaser(&sick); // Add the laser to the robot robot.addRangeDevice(&sick); #endif // Start the robot thread. robot.runAsync(true); #ifndef SONARNL // Start the laser thread. sick.runAsync(); // Connect the laser if (!sick.blockingConnect()) { printf("Couldn't connect to sick, exiting\n"); Aria::shutdown(); return 1; } #endif ArUtil::sleep(300); // If you want to set the number of samples change the line below locTask.setNumSamples(2000); // Localize the robot to home if(locTask.localizeRobotAtHomeBlocking()) { printf("Successfully localized at home.\n"); } else { printf("WARNING: Unable to localize at home position!\n"); } robot.lock(); // attach stuff to the server ArServerInfoRobot serverInfoRobot(&server, &robot); ArServerInfoSensor serverInfoSensor(&server, &robot); ArServerInfoPath serverInfoPath(&server, &robot, &pathTask); ArServerInfoLocalization serverInfoLocalization(&server, &robot, &locTask); #ifndef SONARNL // Set it up to handle maps. ArServerHandlerMap serverMap(&server, &arMap, ArServerHandlerMap::POINTS); #else ArServerHandlerMap serverMap(&server, &arMap, ArServerHandlerMap::LINES); #endif // Set up a service that allows the client to monitor the communication // between the robot and the client. // ArServerHandlerCommMonitor serverCommMonitor(&server); //ArServerModeGoto modeGoto(&server, &robot, &pathTask); //ArServerModeStop modeStop(&server, &robot); //ArServerModeDrive modeDrive(&server, &robot); SimpleTask simpleTask(&robot, &pathTask); robot.unlock(); // Read in param files. Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()); // Now let it spin off in its own thread server.run(); exit(0); }
int main(int argc, char **argv) { Aria::init(); ArRobot robot; ArServerBase server; ArArgumentParser parser(&argc, argv); ArSimpleConnector simpleConnector(&parser); ArServerSimpleOpener simpleOpener(&parser); // parse the command line... fail and print the help if the parsing fails // or if help was requested parser.loadDefaultArguments(); if (!simpleConnector.parseArgs() || !simpleOpener.parseArgs() || !parser.checkHelpAndWarnUnparsed()) { simpleConnector.logOptions(); simpleOpener.logOptions(); exit(1); } // Set up where we'll look for files such as config file, user/password file, // etc. char fileDir[1024]; ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(), "ArNetworking/examples"); // first open the server up if (!simpleOpener.open(&server, fileDir, 240)) { if (simpleOpener.wasUserFileBad()) printf("Error: Bad user/password/permissions file.\n"); else printf("Error: Could not open server port. Use -help to see options.\n"); exit(1); } // Devices ArAnalogGyro gyro(&robot); ArSonarDevice sonarDev; robot.addRangeDevice(&sonarDev); ArIRs irs; robot.addRangeDevice(&irs); ArBumpers bumpers; robot.addRangeDevice(&bumpers); ArSick sick(361, 180); robot.addRangeDevice(&sick); ArServerInfoRobot serverInfoRobot(&server, &robot); ArServerInfoSensor serverInfoSensor(&server, &robot); // This is the service that provides drawing data to the client. ArServerInfoDrawings drawings(&server); // Convenience function that sets up drawings for all the robot's current // range devices (using default shape and color info) drawings.addRobotsRangeDevices(&robot); // Add our custom drawings drawings.addDrawing( // shape: color: size: layer: new ArDrawingData("polyLine", ArColor(255, 0, 0), 2, 49), "exampleDrawing_Home", new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleHomeDrawingNetCallback) ); drawings.addDrawing( new ArDrawingData("polyDots", ArColor(0, 255, 0), 250, 48), "exampleDrawing_Dots", new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleDotsDrawingNetCallback) ); drawings.addDrawing( new ArDrawingData("polySegments", ArColor(0, 0, 0), 4, 52), "exampleDrawing_XMarksTheSpot", new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleXDrawingNetCallback) ); drawings.addDrawing( new ArDrawingData("polyArrows", ArColor(255, 0, 255), 500, 100), "exampleDrawing_Arrows", new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleArrowsDrawingNetCallback) ); // modes for moving the robot ArServerModeStop modeStop(&server, &robot); ArServerModeDrive modeDrive(&server, &robot); ArServerModeRatioDrive modeRatioDrive(&server, &robot); ArServerModeWander modeWander(&server, &robot); modeStop.addAsDefaultMode(); modeStop.activate(); // Connect to the robot. if (!simpleConnector.connectRobot(&robot)) { printf("Error: Could not connect to robot... exiting\n"); Aria::shutdown(); return 1; } // set up the laser before handing it to the laser mode simpleConnector.setupLaser(&sick); robot.enableMotors(); // start the robot cycle running in a background thread robot.runAsync(true); // start the laser processing cycle in a background thread sick.runAsync(); // connect the laser if it was requested if (!simpleConnector.connectLaser(&sick)) { printf("Error: Could not connect to laser... exiting\n"); Aria::shutdown(); return 1; } // log whatever we wanted to before the runAsync simpleOpener.checkAndLog(); // run the server thread in the background server.runAsync(); printf("Server is now running...\n"); // Add a key handler mostly that windows can exit by pressing // escape, note that the key handler prevents you from running this program // in the background on Linux. ArKeyHandler *keyHandler; if ((keyHandler = Aria::getKeyHandler()) == NULL) { keyHandler = new ArKeyHandler; Aria::setKeyHandler(keyHandler); robot.lock(); robot.attachKeyHandler(keyHandler); robot.unlock(); printf("To exit, press escape.\n"); } robot.waitForRunExit(); Aria::shutdown(); exit(0); }
int main(int argc, char **argv) { // Initialize Aria and Arnl global information Aria::init(); Arnl::init(); // The robot object ArRobot robot; // Parse the command line arguments. ArArgumentParser parser(&argc, argv); // Set up our simpleConnector, to connect to the robot and laser //ArSimpleConnector simpleConnector(&parser); ArRobotConnector robotConnector(&parser, &robot); // Connect to the robot if (!robotConnector.connectRobot()) { ArLog::log(ArLog::Normal, "Error: Could not connect to robot... exiting"); Aria::exit(3); } // Set up where we'll look for files. Arnl::init() set Aria's default // directory to Arnl's default directory; addDirectories() appends this // "examples" directory. char fileDir[1024]; ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(), "examples"); // To direct log messages to a file, or to change the log level, use these calls: //ArLog::init(ArLog::File, ArLog::Normal, "log.txt", true, true); //ArLog::init(ArLog::File, ArLog::Verbose); // Add a section to the configuration to change ArLog parameters ArLog::addToConfig(Aria::getConfig()); // set up a gyro (if the robot is older and its firmware does not // automatically incorporate gyro corrections, then this object will do it) ArAnalogGyro gyro(&robot); // Our networking server ArServerBase server; // Set up our simpleOpener, used to set up the networking server ArServerSimpleOpener simpleOpener(&parser); // the laser connector ArLaserConnector laserConnector(&parser, &robot, &robotConnector); // Tell the laser connector to always connect the first laser since // this program always requires a laser. parser.addDefaultArgument("-connectLaser"); // Load default arguments for this computer (from /etc/Aria.args, environment // variables, and other places) parser.loadDefaultArguments(); // Parse arguments if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) { logOptions(argv[0]); Aria::exit(1); } // This causes Aria::exit(9) to be called if the robot unexpectedly // disconnects ArGlobalFunctor1<int> shutdownFunctor(&Aria::exit, 9); robot.addDisconnectOnErrorCB(&shutdownFunctor); // Create an ArSonarDevice object (ArRangeDevice subclass) and // connect it to the robot. ArSonarDevice sonarDev; robot.addRangeDevice(&sonarDev); // This object will allow robot's movement parameters to be changed through // a Robot Configuration section in the ArConfig global configuration facility. ArRobotConfig robotConfig(&robot); // Include gyro configuration options in the robot configuration section. robotConfig.addAnalogGyro(&gyro); // Start the robot thread. robot.runAsync(true); // connect the laser(s) if it was requested, this adds them to the // robot too, and starts them running in their own threads if (!laserConnector.connectLasers()) { ArLog::log(ArLog::Normal, "Could not connect to all lasers... exiting\n"); Aria::exit(2); } // find the laser we should use for localization and/or mapping, // which will be the first laser robot.lock(); ArLaser *firstLaser = robot.findLaser(1); if (firstLaser == NULL || !firstLaser->isConnected()) { ArLog::log(ArLog::Normal, "Did not have laser 1 or it is not connected, cannot start localization and/or mapping... exiting"); Aria::exit(2); } robot.unlock(); /* Create and set up map object */ // Set up the map object, this will look for files in the examples // directory (unless the file name starts with a /, \, or . // You can take out the 'fileDir' argument to look in the program's current directory // instead. // When a configuration file is loaded into ArConfig later, if it specifies a // map file, then that file will be loaded as the map. ArMap map(fileDir); // set it up to ignore empty file names (otherwise if a configuration omits // the map file, the whole configuration change will fail) map.setIgnoreEmptyFileName(true); // ignore the case, so that if someone is using MobileEyes or // MobilePlanner from Windows and changes the case on a map name, // it will still work. map.setIgnoreCase(true); /* Create localization and path planning threads */ ArPathPlanningTask pathTask(&robot, &sonarDev, &map); ArLog::log(ArLog::Normal, "Creating laser localization task"); // Laser Monte-Carlo Localization ArLocalizationTask locTask(&robot, firstLaser, &map); // Set some options on each laser that the laser connector // connected to. std::map<int, ArLaser *>::iterator laserIt; for (laserIt = robot.getLaserMap()->begin(); laserIt != robot.getLaserMap()->end(); laserIt++) { int laserNum = (*laserIt).first; ArLaser *laser = (*laserIt).second; // Skip lasers that aren't connected if(!laser->isConnected()) continue; // add the disconnectOnError CB to shut things down if the laser // connection is lost laser->addDisconnectOnErrorCB(&shutdownFunctor); // set the number of cumulative readings the laser will take laser->setCumulativeBufferSize(200); // add the lasers to the path planning task pathTask.addRangeDevice(laser, ArPathPlanningTask::BOTH); // set the cumulative clean offset (so that they don't all fire at once) laser->setCumulativeCleanOffset(laserNum * 100); // reset the cumulative clean time (to make the new offset take effect) laser->resetLastCumulativeCleanTime(); // Add the packet count to the Aria info strings (It will be included in // MobileEyes custom details so you can monitor whether the laser data is // being received correctly) std::string laserPacketCountName; laserPacketCountName = laser->getName(); laserPacketCountName += " Packet Count"; Aria::getInfoGroup()->addStringInt( laserPacketCountName.c_str(), 10, new ArRetFunctorC<int, ArLaser>(laser, &ArLaser::getReadingCount)); } // Used for optional multirobot features (see below) (TODO move to multirobot // example?) ArClientSwitchManager clientSwitch(&server, &parser); /* Start the server */ // Open the networking server if (!simpleOpener.open(&server, fileDir, 240)) { ArLog::log(ArLog::Normal, "Error: Could not open server."); exit(2); } /* Create various services that provide network access to clients (such as * MobileEyes), as well as add various additional features to ARNL */ // ARNL can optionally get information about the positions of other robots from a // "central server" (see central server example program), if command // line options specifying the address of the central server was given. // If there is no central server, then the address of each other robot // can instead be given in the configuration, and the multirobot systems // will connect to each robot (or "peer") individually. // TODO move this to multirobot example? bool usingCentralServer = false; if(clientSwitch.getCentralServerHostName() != NULL) usingCentralServer = true; // if we're using the central server then we want to create the // multiRobot central classes if (usingCentralServer) { // Make the handler for multi robot information (this sends the // information to the central server) //ArServerHandlerMultiRobot *handlerMultiRobot = new ArServerHandlerMultiRobot(&server, &robot, &pathTask, &locTask, &map); // Normally each robot, and the central server, must all have // the same map name for the central server to share robot // information. (i.e. they are operating in the same space). // This changes the map name that ArServerHandlerMutliRobot // reports to the central server, in case you want this individual // robot to load a different map file name, but still report // the common map file to the central server. //handlerMultiRobot->overrideMapName("central.map"); // the range device that gets the multi robot information from // the central server and presents it as virtual range readings // to ARNL ArMultiRobotRangeDevice *multiRobotRangeDevice = new ArMultiRobotRangeDevice(&server); robot.addRangeDevice(multiRobotRangeDevice); pathTask.addRangeDevice(multiRobotRangeDevice, ArPathPlanningTask::BOTH); // Set up options for drawing multirobot information in MobileEyes. multiRobotRangeDevice->setCurrentDrawingData( new ArDrawingData("polyDots", ArColor(125, 125, 0), 100, 73, 1000), true); multiRobotRangeDevice->setCumulativeDrawingData( new ArDrawingData("polyDots", ArColor(125, 0, 125), 100, 72, 1000), true); // This sets up the localization to use the known poses of other robots // for its localization in cases where numerous robots crowd out the map. locTask.setMultiRobotCallback(multiRobotRangeDevice->getOtherRobotsCB()); } // if we're not using a central server then create the multirobot peer classes else { // set the path planning so it uses the explicit collision range for how far its planning pathTask.setUseCollisionRangeForPlanningFlag(true); // make our thing that gathers information from the other servers ArServerHandlerMultiRobotPeer *multiRobotPeer = NULL; ArMultiRobotPeerRangeDevice *multiRobotPeerRangeDevice = NULL; multiRobotPeerRangeDevice = new ArMultiRobotPeerRangeDevice(&map); // make our thing that sends information to the other servers multiRobotPeer = new ArServerHandlerMultiRobotPeer(&server, &robot, &pathTask, &locTask); // hook the two together so they both know what priority this robot is multiRobotPeer->setNewPrecedenceCallback( multiRobotPeerRangeDevice->getSetPrecedenceCallback()); // hook the two together so they both know what priority this // robot's fingerprint is multiRobotPeer->setNewFingerprintCallback( multiRobotPeerRangeDevice->getSetFingerprintCallback()); // hook the two together so that the range device can call on the // server handler to change its fingerprint multiRobotPeerRangeDevice->setChangeFingerprintCB( multiRobotPeer->getChangeFingerprintCB()); // then add the robot to the places it needs to be robot.addRangeDevice(multiRobotPeerRangeDevice); pathTask.addRangeDevice(multiRobotPeerRangeDevice, ArPathPlanningTask::BOTH); // Set the range device so that we can see the information its using // to avoid, you can comment these out in order to not see them multiRobotPeerRangeDevice->setCurrentDrawingData( new ArDrawingData("polyDots", ArColor(125, 125, 0), 100, 72, 1000), true); multiRobotPeerRangeDevice->setCumulativeDrawingData( new ArDrawingData("polyDots", ArColor(125, 0, 125), 100, 72, 1000), true); // This sets up the localization to use the known poses of other robots // for its localization in cases where numerous robots crowd out the map. locTask.setMultiRobotCallback( multiRobotPeerRangeDevice->getOtherRobotsCB()); } /* Add additional range devices to the robot and path planning task (so it avoids obstacles detected by these devices) */ // Add IR range device to robot and path planning task (so it avoids obstacles // detected by this device) robot.lock(); ArIRs irs; robot.addRangeDevice(&irs); pathTask.addRangeDevice(&irs, ArPathPlanningTask::CURRENT); // Add bumpers range device to robot and path planning task (so it avoids obstacles // detected by this device) ArBumpers bumpers; robot.addRangeDevice(&bumpers); pathTask.addRangeDevice(&bumpers, ArPathPlanningTask::CURRENT); // Add range device which uses forbidden regions given in the map to give virtual // range device readings to ARNL. (so it avoids obstacles // detected by this device) ArForbiddenRangeDevice forbidden(&map); robot.addRangeDevice(&forbidden); pathTask.addRangeDevice(&forbidden, ArPathPlanningTask::CURRENT); robot.unlock(); // Action to slow down robot when localization score drops but not lost. ArActionSlowDownWhenNotCertain actionSlowDown(&locTask); pathTask.getPathPlanActionGroup()->addAction(&actionSlowDown, 140); // Action to stop the robot when localization is "lost" (score too low) ArActionLost actionLostPath(&locTask, &pathTask); pathTask.getPathPlanActionGroup()->addAction(&actionLostPath, 150); // Arnl uses this object when it must replan its path because its // path is completely blocked. It will use an older history of sensor // readings to replan this new path. This should not be used with SONARNL // since sonar readings are not accurate enough and may prevent the robot // from planning through space that is actually clear. ArGlobalReplanningRangeDevice replanDev(&pathTask); // Service to provide drawings of data in the map display : ArServerInfoDrawings drawings(&server); drawings.addRobotsRangeDevices(&robot); drawings.addRangeDevice(&replanDev); /* Draw a box around the local path planning area use this (You can enable this particular drawing from custom commands which is set up down below in ArServerInfoPath) */ ArDrawingData drawingDataP("polyLine", ArColor(200,200,200), 1, 75); ArFunctor2C<ArPathPlanningTask, ArServerClient *, ArNetPacket *> drawingFunctorP(&pathTask, &ArPathPlanningTask::drawSearchRectangle); drawings.addDrawing(&drawingDataP, "Local Plan Area", &drawingFunctorP); /* Show the sample points used by MCL */ ArDrawingData drawingDataL("polyDots", ArColor(0,255,0), 100, 75); ArFunctor2C<ArLocalizationTask, ArServerClient *, ArNetPacket *> drawingFunctorL(&locTask, &ArLocalizationTask::drawRangePoints); drawings.addDrawing(&drawingDataL, "Localization Points", &drawingFunctorL); // "Custom" commands. You can add your own custom commands here, they will // be available in MobileEyes' custom commands (enable in the toolbar or // access through Robot Tools) ArServerHandlerCommands commands(&server); // These provide various kinds of information to the client: ArServerInfoRobot serverInfoRobot(&server, &robot); ArServerInfoSensor serverInfoSensor(&server, &robot); ArServerInfoPath serverInfoPath(&server, &robot, &pathTask); serverInfoPath.addSearchRectangleDrawing(&drawings); serverInfoPath.addControlCommands(&commands); // Provides localization info and allows the client (MobileEyes) to relocalize at a given // pose: ArServerInfoLocalization serverInfoLocalization(&server, &robot, &locTask); ArServerHandlerLocalization serverLocHandler(&server, &robot, &locTask); // If you're using MobileSim, ArServerHandlerLocalization sends it a command // to move the robot's true pose if you manually do a localization through // MobileEyes. To disable that behavior, use this constructor call instead: // ArServerHandlerLocalization serverLocHandler(&server, &robot, true, false); // The fifth argument determines whether to send the command to MobileSim. // Provide the map to the client (and related controls): ArServerHandlerMap serverMap(&server, &map); // These objects add some simple (custom) commands to 'commands' for testing and debugging: ArServerSimpleComUC uCCommands(&commands, &robot); // Send any command to the microcontroller ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // configure logging ArServerSimpleComLogRobotConfig configCommands(&commands, &robot); // trigger logging of the robot config parameters // ArServerSimpleServerCommands serverCommands(&commands, &server); // monitor networking behavior (track packets sent etc.) // service that allows the client to monitor the communication link status // between the robot and the client. // ArServerHandlerCommMonitor handlerCommMonitor(&server); // service that allows client to change configuration parameters in ArConfig ArServerHandlerConfig handlerConfig(&server, Aria::getConfig(), Arnl::getTypicalDefaultParamFileName(), Aria::getDirectory()); /* Set up the possible modes for remote control from a client such as * MobileEyes: */ // Mode To go to a goal or other specific point: ArServerModeGoto modeGoto(&server, &robot, &pathTask, &map, locTask.getRobotHome(), locTask.getRobotHomeCallback()); // Mode To stop and remain stopped: ArServerModeStop modeStop(&server, &robot); // Cause the sonar to turn off automatically // when the robot is stopped, and turn it back on when commands to move // are sent. (Note, if using SONARNL to localize, then don't do this // since localization may get lost) ArSonarAutoDisabler sonarAutoDisabler(&robot); // Teleoperation modes To drive by keyboard, joystick, etc: ArServerModeRatioDrive modeRatioDrive(&server, &robot); // ArServerModeDrive modeDrive(&server, &robot); // Older mode for compatability // Prevent normal teleoperation driving if localization is lost using // a high-priority action, which enables itself when the particular mode is // active. // (You have to enter unsafe drive mode to drive when lost.) ArActionLost actionLostRatioDrive(&locTask, &pathTask, &modeRatioDrive); modeRatioDrive.getActionGroup()->addAction(&actionLostRatioDrive, 110); // Add drive mode section to the configuration, and also some custom (simple) commands: modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings"); modeRatioDrive.addControlCommands(&commands); // Wander mode (also prevent wandering if lost): ArServerModeWander modeWander(&server, &robot); ArActionLost actionLostWander(&locTask, &pathTask, &modeWander); modeWander.getActionGroup()->addAction(&actionLostWander, 110); // This provides a small table of interesting information for the client // to display to the operator. You can add your own callbacks to show any // data you want. ArServerInfoStrings stringInfo(&server); Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor()); // Provide a set of informational data (turn on in MobileEyes with // View->Custom Details) Aria::getInfoGroup()->addStringInt( "Motor Packet Count", 10, new ArConstRetFunctorC<int, ArRobot>(&robot, &ArRobot::getMotorPacCount)); Aria::getInfoGroup()->addStringDouble( "Laser Localization Score", 8, new ArRetFunctorC<double, ArLocalizationTask>( &locTask, &ArLocalizationTask::getLocalizationScore), "%.03f"); Aria::getInfoGroup()->addStringInt( "Laser Loc Num Samples", 8, new ArRetFunctorC<int, ArLocalizationTask>( &locTask, &ArLocalizationTask::getCurrentNumSamples), "%4d"); // Display gyro status if gyro is enabled and is being handled by the firmware (gyro types 2, 3, or 4). // (If the firmware detects an error communicating with the gyro or IMU it // returns a flag, and stops using it.) // (This gyro type parameter, and fault flag, are only in ARCOS, not Seekur firmware) if(robot.getOrigRobotConfig() && robot.getOrigRobotConfig()->getGyroType() > 1) { Aria::getInfoGroup()->addStringString( "Gyro/IMU Status", 10, new ArGlobalRetFunctor1<const char*, ArRobot*>(&getGyroStatusString, &robot) ); } // Setup the dock if there is a docking system on board. ArServerModeDock *modeDock = NULL; modeDock = ArServerModeDock::createDock(&server, &robot, &locTask, &pathTask); if (modeDock != NULL) { modeDock->checkDock(); modeDock->addAsDefaultMode(); modeDock->addToConfig(Aria::getConfig()); modeDock->addControlCommands(&commands); } // Make Stop mode the default (If current mode deactivates without entering // a new mode, then Stop Mode will be selected) modeStop.addAsDefaultMode(); // TODO move up near where stop mode is created? /* Services that allow the client to initiate scanning with the laser to create maps in Mapper3 (So not possible with SONARNL): */ ArServerHandlerMapping handlerMapping(&server, &robot, firstLaser, fileDir, "", true); // make laser localization stop while mapping handlerMapping.addMappingStartCallback( new ArFunctor1C<ArLocalizationTask, bool> (&locTask, &ArLocalizationTask::setIdleFlag, true)); // and then make it start again when we're doine handlerMapping.addMappingEndCallback( new ArFunctor1C<ArLocalizationTask, bool> (&locTask, &ArLocalizationTask::setIdleFlag, false)); // Make it so our "lost" actions don't stop us while mapping handlerMapping.addMappingStartCallback(actionLostPath.getDisableCB()); handlerMapping.addMappingStartCallback(actionLostRatioDrive.getDisableCB()); handlerMapping.addMappingStartCallback(actionLostWander.getDisableCB()); // And then let them make us stop as usual when done mapping handlerMapping.addMappingEndCallback(actionLostPath.getEnableCB()); handlerMapping.addMappingEndCallback(actionLostRatioDrive.getEnableCB()); handlerMapping.addMappingEndCallback(actionLostWander.getEnableCB()); // don't let forbidden lines show up as obstacles while mapping // (they'll just interfere with driving while mapping, and localization is off anyway) handlerMapping.addMappingStartCallback(forbidden.getDisableCB()); // let forbidden lines show up as obstacles again as usual after mapping handlerMapping.addMappingEndCallback(forbidden.getEnableCB()); /* // If we are on a simulator, move the robot back to its starting position, // and reset its odometry. // This will allow localizeRobotAtHomeBlocking() below will (probably) work (it // tries current odometry (which will be 0,0,0) and all the map // home points. // (Ignored by a real robot) //robot.com(ArCommands::SIM_RESET); */ // create a pose storage class, this will let the program keep track // of where the robot is between runs... after we try and restore // from this file it will start saving the robot's pose into the // file ArPoseStorage poseStorage(&robot); /// if we could restore the pose from then set the sim there (this /// won't do anything to the real robot)... if we couldn't restore /// the pose then just reset the position of the robot (which again /// won't do anything to the real robot) if (poseStorage.restorePose("robotPose")) serverLocHandler.setSimPose(robot.getPose()); else robot.com(ArCommands::SIM_RESET); /* File transfer services: */ #ifdef WIN32 // Not implemented for Windows yet. ArLog::log(ArLog::Normal, "Note, file upload/download services are not implemented for Windows; not enabling them."); #else // This block will allow you to set up where you get and put files // to/from, just comment them out if you don't want this to happen // /* ArServerFileLister fileLister(&server, fileDir); ArServerFileToClient fileToClient(&server, fileDir); ArServerFileFromClient fileFromClient(&server, fileDir, "/tmp"); ArServerDeleteFileOnServer deleteFileOnServer(&server, fileDir); // */ #endif /* Video image streaming, and camera controls (Requires SAVserver or ACTS) */ // Forward any video if either ACTS or SAV server are running. // You can find out more about SAV and ACTS on our website // http://robots.activmedia.com. ACTS is for color tracking and is // a seperate product. SAV just does software A/V transmitting and is // free to all our customers. Just run ACTS or SAV server before you // start this program and this class here will forward video from the // server to the client. ArHybridForwarderVideo videoForwarder(&server, "localhost", 7070); // make a camera to use in case we have video. the camera collection collects // multiple ptz cameras ArPTZ *camera = NULL; ArServerHandlerCamera *handlerCamera = NULL; ArCameraCollection *cameraCollection = NULL; // if we have video then set up a camera if (videoForwarder.isForwardingVideo()) { cameraCollection = new ArCameraCollection(); cameraCollection->addCamera("Cam1", "PTZ", "Camera", "PTZ"); videoForwarder.setCameraName("Cam1"); videoForwarder.addToCameraCollection(*cameraCollection); camera = new ArVCC4(&robot); //, invertedCamera, ArVCC4::COMM_UNKNOWN, true, true); // To use an RVision SEE camera instead: // camera = new ArRVisionPTZ(&robot); camera->init(); handlerCamera = new ArServerHandlerCamera("Cam1", &server, &robot, camera, cameraCollection); pathTask.addGoalFinishedCB( new ArFunctorC<ArServerHandlerCamera>( handlerCamera, &ArServerHandlerCamera::cameraModeLookAtGoalClearGoal)); } // After all of the cameras / videos have been created and added to the collection, // then start the collection server. // if (cameraCollection != NULL) { new ArServerHandlerCameraCollection(&server, cameraCollection); } /* Load configuration values, map, and begin! */ // When parsing the configuration file, also look at the program's command line options // from the command-line argument parser as well as the configuration file. // (So you can use any argument on the command line, namely -map.) Aria::getConfig()->useArgumentParser(&parser); puts("xxx");puts("aaa"); fflush(stdout); // Read in parameter files. ArLog::log(ArLog::Normal, "Loading config file %s into ArConfig (base directory %s)...", Arnl::getTypicalParamFileName(), Aria::getConfig()->getBaseDirectory()); if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName())) { ArLog::log(ArLog::Normal, "Trouble loading configuration file, exiting"); Aria::exit(5); } // Warn about unknown params. if (!simpleOpener.checkAndLog() || !parser.checkHelpAndWarnUnparsed()) { logOptions(argv[0]); Aria::exit(6); } // Warn if there is no map if (map.getFileName() == NULL || strlen(map.getFileName()) <= 0) { ArLog::log(ArLog::Normal, ""); ArLog::log(ArLog::Normal, "### No map file is set up, you can make a map with the following procedure"); ArLog::log(ArLog::Normal, " 0) You can find this information in README.txt or docs/Mapping.txt"); ArLog::log(ArLog::Normal, " 1) Connect to this server with MobileEyes"); ArLog::log(ArLog::Normal, " 2) Go to Tools->Map Creation->Start Scan"); ArLog::log(ArLog::Normal, " 3) Give the map a name and hit okay"); ArLog::log(ArLog::Normal, " 4) Drive the robot around your space (see docs/Mapping.txt"); ArLog::log(ArLog::Normal, " 5) Go to Tools->Map Creation->Stop Scan"); ArLog::log(ArLog::Normal, " 6) Start up Mapper3"); ArLog::log(ArLog::Normal, " 7) Go to File->Open on Robot"); ArLog::log(ArLog::Normal, " 8) Select the .2d you created"); ArLog::log(ArLog::Normal, " 9) Create a .map"); ArLog::log(ArLog::Normal, " 10) Go to File->Save on Robot"); ArLog::log(ArLog::Normal, " 11) In MobileEyes, go to Tools->Robot Config"); ArLog::log(ArLog::Normal, " 12) Choose the Files section"); ArLog::log(ArLog::Normal, " 13) Enter the path and name of your new .map file for the value of the Map parameter."); ArLog::log(ArLog::Normal, " 14) Press OK and your new map should become the map used"); ArLog::log(ArLog::Normal, ""); } // Print a log message notifying user of the directory for map files ArLog::log(ArLog::Normal, ""); ArLog::log(ArLog::Normal, "Directory for maps and file serving: %s", fileDir); ArLog::log(ArLog::Normal, "See the ARNL README.txt for more information"); ArLog::log(ArLog::Normal, ""); // Do an initial localization of the robot. It tries all the home points // in the map, as well as the robot's current odometric position, as possible // places the robot is likely to be at startup. If successful, it will // also save the position it found to be the best localized position as the // "Home" position, which can be obtained from the localization task (and is // used by the "Go to home" network request). locTask.localizeRobotAtHomeBlocking(); // Let the client switch manager (for multirobot) spin off into its own thread // TODO move to multirobot example? clientSwitch.runAsync(); // Start the networking server's thread server.runAsync(); // Add a key handler so that you can exit by pressing // escape. Note that this key handler, however, prevents this program from // running in the background (e.g. as a system daemon or run from // the shell with "&") -- it will lock up trying to read the keys; // remove this if you wish to be able to run this program in the background. ArKeyHandler *keyHandler; if ((keyHandler = Aria::getKeyHandler()) == NULL) { keyHandler = new ArKeyHandler; Aria::setKeyHandler(keyHandler); robot.lock(); robot.attachKeyHandler(keyHandler); robot.unlock(); puts("Server running. To exit, press escape."); } ArnlASyncTaskExample asyncTaskExample(&pathTask, &robot, &modeGoto, &parser); // Enable the motors and wait until the robot exits (disconnection, etc.) or this program is // canceled. robot.enableMotors(); robot.waitForRunExit(); Aria::exit(0); }
int main(int argc, char **argv) { Aria::init(); ArRobot robot; ArArgumentParser parser(&argc, argv); ArSimpleConnector simpleConnector(&parser); // The base server object, manages all connections to clients. ArServerBase server; // This object simplifies configuration and opening of the ArServerBase // object. ArServerSimpleOpener simpleOpener(&parser); // parse the command line. fail and print the help if the parsing fails // or if the help was requested with -help parser.loadDefaultArguments(); if (!simpleConnector.parseArgs() || !simpleOpener.parseArgs() || !parser.checkHelpAndWarnUnparsed()) { simpleConnector.logOptions(); simpleOpener.logOptions(); exit(1); } // Use the ArSimpleOpener to open the server port if (!simpleOpener.open(&server)) { ArLog::log(ArLog::Terse, "Error: Could not open server on port %d", simpleOpener.getPort()); exit(1); } // // Create services attached to the base server: // // Robot position etc.: ArServerInfoRobot serverInfoRobot(&server, &robot); // Robot control modes (only one mode can be active at once): ArServerModeStop modeStop(&server, &robot); // old ArServerModeDrive modeDrive(&server, &robot); ArServerModeRatioDrive modeRatioDrive(&server, &robot); ArServerModeWander modeWander(&server, &robot); modeStop.addAsDefaultMode(); modeStop.activate(); // This provides a simple way to add new commands. ArServerHandlerCommands commands(&server); // Add our custom command. ArServerHandlerCommands also has other methods // for adding commands taht take different kinds of arguments, or no // arguments. ArGlobalFunctor1<ArArgumentBuilder*> customCommandFunctor(&customCommandHandler); commands.addStringCommand("ExampleCustomCommand", "Example of a custom command. simpleServerExample will print out the text sent with the command.", &customCommandFunctor); // These objects provide various debugging and diagnostic custom commands: ArServerSimpleComUC uCCommands(&commands, &robot); // Get information about the robot ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // Control logging modeRatioDrive.addControlCommands(&commands); // Drive mode diagnostics // This provides the client (e.g. MobileEyes) with a simple table of string values // (called an InfoGroup). An InfoGroup is kept globally by Aria. // The values in the table sent to clients are retrieved periodically by calling a // functor. ArServerInfoStrings stringInfo(&server); Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor()); // Here are some example entries in the InfoGroup: Aria::getInfoGroup()->addStringInt( "Motor Packet Count", 10, new ArConstRetFunctorC<int, ArRobot>(&robot, &ArRobot::getMotorPacCount)); // // Connect to the robot: // if (!simpleConnector.connectRobot(&robot)) { printf("Error: Could not connect to robot... exiting\n"); Aria::shutdown(); return 1; } robot.enableMotors(); robot.runAsync(true); // The simple opener might have information to display right before starting // the server thread: simpleOpener.checkAndLog(); // now let the server base run in a new thread, accepting client connections. server.runAsync(); ArLog::log(ArLog::Normal, "Server is now running... Press Ctrl-C to exit."); robot.waitForRunExit(); Aria::shutdown(); exit(0); }
int main(int argc, char **argv) { Aria::init(); //ArLog::init(ArLog::StdOut, ArLog::Verbose); // robot ArRobot robot; /// our server ArServerBase server; // set up our parser ArArgumentParser parser(&argc, argv); // set up our simple connector ArSimpleConnector simpleConnector(&parser); // set up a gyro ArAnalogGyro gyro(&robot); // load the default arguments parser.loadDefaultArguments(); // parse the command line... fail and print the help if the parsing fails // or if the help was requested if (!simpleConnector.parseArgs() || !parser.checkHelpAndWarnUnparsed()) { simpleConnector.logOptions(); exit(1); } if (!server.loadUserInfo("userServerTest.userInfo")) { printf("Could not load user info, exiting\n"); exit(1); } server.logUsers(); // first open the server up if (!server.open(7272)) { printf("Could not open server port\n"); exit(1); } // sonar, must be added to the robot ArSonarDevice sonarDev; // add the sonar to the robot robot.addRangeDevice(&sonarDev); ArIRs irs; robot.addRangeDevice(&irs); ArBumpers bumpers; robot.addRangeDevice(&bumpers); // a laser in case one is used ArSick sick(361, 180); // add the laser to the robot robot.addRangeDevice(&sick); // attach stuff to the server ArServerInfoRobot serverInfoRobot(&server, &robot); ArServerInfoSensor serverInfoSensor(&server, &robot); ArServerInfoDrawings drawings(&server); drawings.addRobotsRangeDevices(&robot); // ways of moving the robot ArServerModeStop modeStop(&server, &robot); ArServerModeDrive modeDrive(&server, &robot); ArServerModeRatioDrive modeRatioDrive(&server, &robot); ArServerModeWander modeWander(&server, &robot); modeStop.addAsDefaultMode(); modeStop.activate(); // set up the simple commands ArServerHandlerCommands commands(&server); // add the commands for the microcontroller ArServerSimpleComUC uCCommands(&commands, &robot); // add the commands for logging ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // add the commands for the gyro ArServerSimpleComGyro gyroCommands(&commands, &robot, &gyro); // add the commands to enable and disable safe driving to the simple commands modeDrive.addControlCommands(&commands); // Forward any video if we have some to forward.. this will forward // from SAV or ACTS, you can find both on our website // http://robots.activmedia.com, ACTS is for color tracking and is // charged for but SAV just does software A/V transmitting and is // free to all our customers... just run ACTS or SAV before you // start this program and this class here will forward video from it // to MobileEyes ArHybridForwarderVideo videoForwarder(&server, "localhost", 7070); // make a camera to use in case we have video ArPTZ *camera = NULL; ArServerHandlerCamera *handlerCamera = NULL; // if we have video then set up a camera if (videoForwarder.isForwardingVideo()) { bool invertedCamera = false; camera = new ArVCC4(&robot, invertedCamera, ArVCC4::COMM_UNKNOWN, true, true); camera->init(); handlerCamera = new ArServerHandlerCamera(&server, &robot, camera); } server.logCommandGroups(); server.logCommandGroupsToFile("userServerTest.commandGroups"); // now let it spin off in its own thread server.runAsync(); // set up the robot for connecting if (!simpleConnector.connectRobot(&robot)) { printf("Could not connect to robot... exiting\n"); Aria::shutdown(); return 1; } // set up the laser before handing it to the laser mode simpleConnector.setupLaser(&sick); robot.enableMotors(); // start the robot running, true so that if we lose connection the run stops robot.runAsync(true); sick.runAsync(); // connect the laser if it was requested if (!simpleConnector.connectLaser(&sick)) { printf("Could not connect to laser... exiting\n"); Aria::shutdown(); return 1; } robot.waitForRunExit(); // now exit Aria::shutdown(); exit(0); }
int main(int argc, char **argv) { // mandatory init Aria::init(); //ArLog::init(ArLog::StdOut, ArLog::Verbose); // set up our parser ArArgumentParser parser(&argc, argv); // load the default arguments parser.loadDefaultArguments(); // robot ArRobot robot; // set up our simple connector ArRobotConnector robotConnector(&parser, &robot); // add a gyro, it'll see if it should attach to the robot or not ArAnalogGyro gyro(&robot); // set up the robot for connecting if (!robotConnector.connectRobot()) { printf("Could not connect to robot... exiting\n"); Aria::exit(1); } ArDataLogger dataLogger(&robot, "dataLog.txt"); dataLogger.addToConfig(Aria::getConfig()); // our base server object ArServerBase server; ArLaserConnector laserConnector(&parser, &robot, &robotConnector); ArServerSimpleOpener simpleOpener(&parser); ArClientSwitchManager clientSwitchManager(&server, &parser); // parse the command line... fail and print the help if the parsing fails // or if the help was requested if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(1); } // Set up where we'll look for files such as user/password char fileDir[1024]; ArUtil::addDirectories(fileDir, sizeof(fileDir), Aria::getDirectory(), "ArNetworking/examples"); // first open the server up if (!simpleOpener.open(&server, fileDir, 240)) { if (simpleOpener.wasUserFileBad()) printf("Bad user/password/permissions file\n"); else printf("Could not open server port\n"); exit(1); } // Range devices: ArSonarDevice sonarDev; robot.addRangeDevice(&sonarDev); ArIRs irs; robot.addRangeDevice(&irs); ArBumpers bumpers; robot.addRangeDevice(&bumpers); // attach services to the server ArServerInfoRobot serverInfoRobot(&server, &robot); ArServerInfoSensor serverInfoSensor(&server, &robot); ArServerInfoDrawings drawings(&server); // modes for controlling robot movement ArServerModeStop modeStop(&server, &robot); ArServerModeRatioDrive modeRatioDrive(&server, &robot); ArServerModeWander modeWander(&server, &robot); modeStop.addAsDefaultMode(); modeStop.activate(); // set up the simple commands ArServerHandlerCommands commands(&server); ArServerSimpleComUC uCCommands(&commands, &robot); // send commands directly to microcontroller ArServerSimpleComMovementLogging loggingCommands(&commands, &robot); // control debug logging ArServerSimpleComGyro gyroCommands(&commands, &robot, &gyro); // configure gyro ArServerSimpleComLogRobotConfig configCommands(&commands, &robot); // control more debug logging ArServerSimpleServerCommands serverCommands(&commands, &server); // control ArNetworking debug logging ArServerSimpleLogRobotDebugPackets logRobotDebugPackets(&commands, &robot, "."); // debugging tool // ArServerModeDrive is an older drive mode. ArServerModeRatioDrive is newer and generally performs better, // but you can use this for old clients if neccesary. //ArServerModeDrive modeDrive(&server, &robot); //modeDrive.addControlCommands(&commands); // configure the drive modes (e.g. enable/disable safe drive) ArServerHandlerConfig serverHandlerConfig(&server, Aria::getConfig()); // make a config handler ArLog::addToConfig(Aria::getConfig()); // let people configure logging modeRatioDrive.addToConfig(Aria::getConfig(), "Teleop settings"); // able to configure teleop settings modeRatioDrive.addControlCommands(&commands); // Forward video if either ACTS or SAV server are running. // You can find out more about SAV and ACTS on our website // http://robots.activmedia.com. ACTS is for color tracking and is // a separate product. SAV just does software A/V transmitting and is // free to all our customers. Just run ACTS or SAV server before you // start this program and this class here will forward video from the // server to the client. ArHybridForwarderVideo videoForwarder(&server, "localhost", 7070); // Control a pan/tilt/zoom camera, if one is installed, and the video // forwarder was enabled above. ArPTZ *camera = NULL; ArServerHandlerCamera *handlerCamera = NULL; ArCameraCollection *cameraCollection = NULL; if (videoForwarder.isForwardingVideo()) { bool invertedCamera = false; camera = new ArVCC4(&robot, invertedCamera, ArVCC4::COMM_UNKNOWN, true, true); camera->init(); cameraCollection = new ArCameraCollection(); cameraCollection->addCamera("Cam1", "VCC4", "Camera", "VCC4"); handlerCamera = new ArServerHandlerCamera("Cam1", &server, &robot, camera, cameraCollection); } // You can use this class to send a set of arbitrary strings // for MobileEyes to display, this is just a small example ArServerInfoStrings stringInfo(&server); Aria::getInfoGroup()->addAddStringCallback(stringInfo.getAddStringFunctor()); Aria::getInfoGroup()->addStringInt( "Motor Packet Count", 10, new ArConstRetFunctorC<int, ArRobot>(&robot, &ArRobot::getMotorPacCount)); /* Aria::getInfoGroup()->addStringInt( "Laser Packet Count", 10, new ArRetFunctorC<int, ArSick>(&sick, &ArSick::getSickPacCount)); */ // start the robot running, true means that if we lose connection the run thread stops robot.runAsync(true); // connect the laser(s) if it was requested if (!laserConnector.connectLasers()) { printf("Could not connect to lasers... exiting\n"); Aria::exit(2); } drawings.addRobotsRangeDevices(&robot); // log whatever we wanted to before the runAsync simpleOpener.checkAndLog(); // now let it spin off in its own thread server.runAsync(); printf("Server is now running...\n"); // Add a key handler so that you can exit by pressing // escape. Note that a key handler prevents you from running // a program in the background on Linux, since it expects an // active terminal to read keys from; remove this if you want // to run it in the background. ArKeyHandler *keyHandler; if ((keyHandler = Aria::getKeyHandler()) == NULL) { keyHandler = new ArKeyHandler; Aria::setKeyHandler(keyHandler); robot.lock(); robot.attachKeyHandler(keyHandler); robot.unlock(); printf("To exit, press escape.\n"); } // Read in parameter files. std::string configFile = "serverDemoConfig.txt"; Aria::getConfig()->setBaseDirectory("./"); if (Aria::getConfig()->parseFile(configFile.c_str(), true, true)) { ArLog::log(ArLog::Normal, "Loaded config file %s", configFile.c_str()); } else { if (ArUtil::findFile(configFile.c_str())) { ArLog::log(ArLog::Normal, "Trouble loading configuration file %s, continuing", configFile.c_str()); } else { ArLog::log(ArLog::Normal, "No configuration file %s, will try to create if config used", configFile.c_str()); } } clientSwitchManager.runAsync(); robot.lock(); robot.enableMotors(); robot.unlock(); robot.waitForRunExit(); Aria::exit(0); }
int main(int argc, char **argv) { Aria::init(); ArServerBase server; ArArgumentParser parser(&argc, argv); ArServerSimpleOpener simpleOpener(&parser); // parse the command line... fail and print the help if the parsing fails // or if help was requested parser.loadDefaultArguments(); if (!simpleOpener.parseArgs() || !parser.checkHelpAndWarnUnparsed()) { simpleOpener.logOptions(); exit(1); } // first open the server up if (!simpleOpener.open(&server)) { if (simpleOpener.wasUserFileBad()) printf("Error: Bad user/password/permissions file.\n"); else printf("Error: Could not open server port. Use -help to see options.\n"); exit(1); } // This is the service that provides drawing data to the client. ArServerInfoDrawings drawings(&server); // Add our custom drawings drawings.addDrawing( // shape: color: size: layer: new ArDrawingData("polyLine", ArColor(255, 0, 0), 2, 49), "exampleDrawing_Home", new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleHomeDrawingNetCallback) ); drawings.addDrawing( new ArDrawingData("polyDots", ArColor(0, 255, 0), 250, 48), "exampleDrawing_Dots", new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleDotsDrawingNetCallback) ); drawings.addDrawing( new ArDrawingData("polySegments", ArColor(0, 0, 0), 4, 52), "exampleDrawing_XMarksTheSpot", new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleXDrawingNetCallback) ); drawings.addDrawing( new ArDrawingData("polyArrows", ArColor(255, 0, 255), 500, 100), "exampleDrawing_Arrows", new ArGlobalFunctor2<ArServerClient*, ArNetPacket*>(&exampleArrowsDrawingNetCallback) ); Circle circle(&drawings, "exampleDrawing_circle", new ArDrawingData("polySegments", ArColor(255, 150, 0), 3, 120)); circle.setPos(ArPose(0, -5000)); circle.setRadius(1000); circle.setNumPoints(360); // log whatever we wanted to before the runAsync simpleOpener.checkAndLog(); // run the server thread in the background server.runAsync(); printf("Server is now running...\n"); // Add a key handler mostly that windows can exit by pressing // escape, note that the key handler prevents you from running this program // in the background on Linux. ArKeyHandler *keyHandler; if ((keyHandler = Aria::getKeyHandler()) == NULL) { keyHandler = new ArKeyHandler; Aria::setKeyHandler(keyHandler); printf("To exit, press escape.\n"); } double circleRadius = 1000; while(true) { ArUtil::sleep(100); circleRadius += 50; if(circleRadius > 5000) circleRadius = 0; circle.setRadius(circleRadius); } Aria::shutdown(); exit(0); }