/* MobileRobots Advanced Robotics Interface for Applications (ARIA) Copyright (C) 2004, 2005 ActivMedia Robotics LLC Copyright (C) 2006, 2007 MobileRobots Inc. This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA If you wish to redistribute ARIA under different terms, contact MobileRobots for information about a commercial version of ARIA at robots@mobilerobots.com or MobileRobots Inc, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481 */ #include "Aria.h" #include "ArNetworking.h" /** @example serverDemo.cpp Example ArNetworking server providing teleoperation, * sonar data, control the camera, etc. * * This is a basic ArNetworking server. It connects to a robot or simulator, * including, if available, IRs, gyro, and bumpers. Give the option * "-connectLaser" on the command line to enable the laser rangefinder, * if available. * * Run "./serverDemo -help" for a full list of command line options. * * Once running, connect to this server with a a client such as * MobileEyes. * * This server provides the following services: * - User login (optional) * - Basic robot telemetry information * - Range sensor data values (not used by MobileEyes) * - Graphics representing range sensor reading positions * - Teleoperation modes (including safe/unsafe drive modes) * - Wander mode * - Various advanced "custom" commands to control logging, debugging, etc. * - If an ACTS or SAV server is running, forward the video stream * - Camera control (pan/tilt/zoom) if cameras are available * * Note that this program requires a terminal to run -- i.e. you can't run * it in the background in Linux. To modify it to allow that, remove the key * handler code in main(). */ /******* dml 10/10 ******/ /* * This version modified to take a nw command doStereoScan and to run * the bumblebee stereo through a predetermined set of PT angles, storing * the stereo data for each. * needs to work with a version of the client demo that produces this command. * Information is stored in a sequence of files named for the robot, the * time/date, the stop number and the pan/tilt combination. * * version 1.0 dml 10/10 * * This version modified to take pan and tilt parameter commands, to store * left/right images * and to store GPS data * * version 2.0 dml 06/11 * * This version has code for my serialGPS and the AriaGPS and no GPS. You need * to define NOGPS or USESERIALGPS, the ariaGPS us otherwise default. * This version also adds compass and inclinometer data to the log * * version 2.1 dml 7/11 */ #define VERSIONSTRING "version 2.1 Jun 2011" #define OPENCV #undef NOGPS #define USESERIALGPS #ifdef OPENCV #include #include #endif #include #include "stereoCamera.h" #ifdef USESERIALGPS // this is the local serialGPS rather than the Aria gps #include "serialGPS.h" #endif /* Globals so they can be accessed in the handlers */ ArDPPTU *myPTZ=0; ArRobot *myRobot=0; ArTCM2 *myTCM2 = 0; #ifdef USESERIALGPS serialGPS *gps; // local serial GPS object #else ArGPS *gps=0; // aria GPS object #endif stereoCam *myStereoCamera=0; time_t stereoScanTime = 0; int stereoCounter=0; int panStart=-20, panInc=20, panNum=3, panEnd=20, defTilt=20; int tiltStart=-20, tiltInc=20, tiltNum=3, tiltEnd=20, defPan=-1; char stereoRoot[100], // the root name intialized to robot and date stereoLog[100]; #ifdef OPENCV IplImage *frame=0, *disparity=0; #endif /* make a unique name for this set of dat afiles and the log file */ void setStereoScanRoot(ArRobot *r) { time_t rawtime; struct tm *timeinfo; FILE *log; myRobot = r; time(&rawtime); timeinfo=localtime(&rawtime); sprintf(stereoRoot,"%s-%d%d%d%d%d-",r->getName(), //robot name timeinfo->tm_year, timeinfo->tm_mon, timeinfo->tm_mday, //date timeinfo->tm_hour, timeinfo->tm_min); sprintf(stereoLog,"%slog.txt",stereoRoot); log = fopen(stereoLog,"w"); if (log==NULL) { printf("Can't open stereo log file %s\n",stereoLog); exit(0); } fprintf(log,"Stereo sequence. Robot %s Date %d:%d:%d Time: %d:%d:%d.\n", r->getName(), timeinfo->tm_year, timeinfo->tm_mon, timeinfo->tm_mday, //date timeinfo->tm_hour, timeinfo->tm_min, timeinfo->tm_sec); fclose(log); return; } /* write one line to the log file that describes this data set */ void logStereoScan(FILE *log, char *fname, ArRobot *r, int stop, int pan, int tilt) { #ifdef NOGPS fprintf(log,"%d %s %f, %f, %f, %d, %d\n", stop, fname, r->getX(), r->getY(), r->getTh(), pan, tilt); #else #ifdef USESERIALGPS // for (int i=0; i<20; i++) gps->readGPS(); printf("Received north is %lf and west is %lf\n",gps->getNorth(), gps->getWest()); myTCM2->commandContinuousPackets(); printf("Received TCM data is %f,%f,%f,%f\n", myTCM2->getCompass(), myTCM2->getPitch(), myTCM2->getRoll(), myTCM2->getTemperature() ); fprintf(log,"%d, %s, %f, %f, %f, %lf, %lf, %f, %f, %f, %f, %d, %d\n", stop, fname, r->getX(), r->getY(), r->getTh(), // odometry gps->getNorth(), gps->getWest(), // gps myTCM2->getCompass(), myTCM2->getPitch(), myTCM2->getRoll(), myTCM2->getTemperature(), // TCM pan, tilt); // PT #else fprintf(log,"%d, %s, %f, %f, %f, %lf, %lf, %lf, %lf, %lf, %lf, %d, %d\n", stop, fname, r->getX(), r->getY(), r->getTh(), // odo gps->getLongitude(), gps->getLatitude(),// gps myTCM2->getCompass(), myTCM2->getPitch(), myTCM2->getRoll(), myTCM2->getTemperature(), // TCM pan, tilt); #endif #endif return; } void mySetPanParamHandler2(ArServerClient *client, ArNetPacket *packet) { double a,b,c; a = packet->bufToDouble(); b = packet->bufToDouble(); c = packet->bufToDouble(); panInc = a; panNum=b; defTilt=c; if (defTilt>0) defPan=-1; panStart = -panInc*panNum/2; panEnd = panInc*panNum/2; printf("Resetting stereo scan pan parameters to [%d to %d] in %d inc of %d degrees\n", panStart, panEnd, panNum, panInc); } void mySetTiltParamHandler2(ArServerClient *client, ArNetPacket *packet) { double a,b,c; a = packet->bufToDouble(); b = packet->bufToDouble(); c = packet->bufToDouble(); tiltInc = a; tiltNum=b; defPan=c; if (defPan>0) defTilt=-1; panStart = -tiltInc*tiltNum/2; panEnd = tiltInc*tiltNum/2; printf("Resetting stereo scan tilt parameters to [%d to %d] in %d inc of %d degrees\n", tiltStart, tiltEnd, tiltNum, tiltInc); } /* Stereo scan handler will figure out what to do based on the pan and tilt variables. It will generate log files about whre and what it did as well as the data files. */ void myStereoScanHandler(void) { FILE *log, *out; char filename[100]; printf("Doing stereo scan\n"); if (myPTZ==0) { printf("PTZ unit not initialized\n"); return; } /// prevent joystick bounce //if ((time(NULL) - stereoScanTime)<20) return; stereoScanTime=time(NULL); if (defTilt>0) { // do a Pan myPTZ->panTilt(0.0,float(defTilt)); for (int i=panStart; i<=panEnd; i+=panInc) { printf("pan to %d\n",i); sprintf(filename,"%sS%dP%d.txt", stereoRoot, stereoCounter, i); out=fopen(filename,"w"); log=fopen(stereoLog,"a"); logStereoScan(log,filename,myRobot,stereoCounter,i,defTilt); if (out==NULL) { printf("Could not open %s for stereo dump.\n",filename); exit(0); } myPTZ->panTilt(double(i),myPTZ->getTilt()); ArUtil::sleep(3000); myStereoCamera->doStereoFrame(frame, disparity,out); printf("Actual pan tilt is %lf deg %lf deg\n", myPTZ->getPan(), myPTZ->getTilt()); sprintf(filename,"%sS%dP%d.jpg", stereoRoot, stereoCounter, i); cvSaveImage(filename,frame); fclose(log); fclose(out); }/* end of for loop*/ } /*end of IF */ if (defPan>0) { // do a Tilt myPTZ->panTilt(float(defPan),0.0); for (int i=tiltStart; i<=tiltEnd; i+=tiltInc) { printf("tilt to %d\n",i); sprintf(filename,"%sS%dT%d.txt", stereoRoot, stereoCounter, i); out=fopen(filename,"w"); log=fopen(stereoLog,"a"); logStereoScan(log,filename,myRobot,stereoCounter,defPan,i); if (out==NULL) { printf("Could not open %s for stereo dump.\n",filename); exit(0); } myPTZ->panTilt(myPTZ->getPan(),double(i)); ArUtil::sleep(2000); myStereoCamera->doStereoFrame(frame,disparity,out); printf("Actual pan tilt is %lf deg %lf deg\n", myPTZ->getPan(), myPTZ->getTilt()); sprintf(filename,"%sS%dT%d.jpg", stereoRoot, stereoCounter, i); cvSaveImage(filename,frame); fclose(log); fclose(out); }/* end of for loop*/ } /*end of IF tilt */ myPTZ->panTilt(0.0,0.0); stereoCounter++; printf("Scan done.\n"); } /******* dml 10/10 ******/ ArGlobalFunctor myStereoScanFunctor(&myStereoScanHandler); ArGlobalFunctor2 mySetPanParamFunctor2(&mySetPanParamHandler2); ArGlobalFunctor2 mySetTiltParamFunctor2(&mySetTiltParamHandler2); /* Key handler for the escape key: shutdown all of Aria. */ void escape(void) { printf("esc pressed, shutting down aria\n"); printf("FCRV Stereo Vision Server .. exiting.\n"); delete myStereoCamera; ArUtil::sleep(3000); myRobot->stopRunning(true); Aria::shutdown(); Aria::exit(0); } int main(int argc, char **argv) { Aria::init(); //ArLog::init(ArLog::StdErr, ArLog::Verbose); ArRobot robot; ArDPPTU thePTZ(&robot); // pan-tilt base, mode DPPU myPTZ = &thePTZ; printf("********************************************\n"); printf("FRCV Stereo Vision Server. "); printf("%s\n\n\n",VERSIONSTRING); myPTZ->reset(); ArUtil::sleep(1000); // our base server object ArServerBase server; ArArgumentParser parser(&argc, argv); ArSimpleConnector simpleConnector(&parser); ArServerSimpleOpener simpleOpener(&parser); // set up a gyro, if installed ArAnalogGyro gyro(&robot); // TCM ArTCM2 theTCM2(&robot); myTCM2 = &theTCM2; myTCM2->commandContinuousPackets(); // wake up the TCM for (int i=0; i<20; i++) {float x = myTCM2->getRoll();} myTCM2->commandContinuousPackets(); // load the default arguments parser.loadDefaultArguments(); 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); } // GPS #ifndef NOGPS #ifdef USESERIALGPS printf("GPS: using serialGPS.\n"); gps = new serialGPS(); #else printf("GPS: using Aria GPS.\n"); ArGPSConnector gpsConnector(&argParser); gps = gpsConnector.createGPS(); if(!gps || !gpsConnector.connectGPS(gps)) { ArLog::log(ArLog::Terse, "gpsExample: Error connecting to GPS device. Try -gpsType, -gpsPort, and/or -gpsBaud command-line arguments. Use -help for help."); return -1; } #endif #else printf("GPS: using no GPS.\n"); #endif // Range devices: ArSonarDevice sonarDev; robot.addRangeDevice(&sonarDev); ArIRs irs; robot.addRangeDevice(&irs); ArBumpers bumpers; robot.addRangeDevice(&bumpers); ArSick sick(361, 180); robot.addRangeDevice(&sick); // attach services to the server ArServerInfoRobot serverInfoRobot(&server, &robot); ArServerInfoSensor serverInfoSensor(&server, &robot); ArServerInfoDrawings drawings(&server); drawings.addRobotsRangeDevices(&robot); // this gets high-reflectance readings from the laser and draws them // in a different color. (requires special reflector beacons.) ArLaserReflectorDevice reflector(&sick, &robot); drawings.addRangeDevice(&reflector); // modes for controlling robot movement 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); /****dml 10/10 ***/ printf("Adding extra stereo command...\n"); commands.addCommand("doStereoScan", "Does a scan of the stereo", &myStereoScanFunctor,0); server.addData("setStereoTilt", "Sets the tilt range for a stereoscan", &mySetTiltParamFunctor2, "Tilt Inc, Number of tilts, default pan", "Nothing back; just sets the params for tilt stereo scan"); server.addData("setStereoPan","Sets the pan range for a stereoscan", &mySetPanParamFunctor2, "Pan Inc, Number of pans, default tilt", "Nothing back; just sets the params for pan stereo scan"); myStereoCamera = new stereoCam(); setStereoScanRoot(&robot); #ifdef OPENCV frame = cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,3); disparity = cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); #endif /****************/ 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 modeDrive.addControlCommands(&commands); // configure the drive modes (e.g. enable/disable safe drive) // 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(&robot, &ArRobot::getMotorPacCount)); Aria::getInfoGroup()->addStringInt( "Laser Packet Count", 10, new ArRetFunctorC(&sick, &ArSick::getSickPacCount)); // Connect to the robot. if (!simpleConnector.connectRobot(&robot)) { printf("Could not connect to robot... exiting\n"); delete myStereoCamera; Aria::shutdown(); return 1; exit(1); } // set up the laser before handing it to the laser mode simpleConnector.setupLaser(&sick); // start the robot running, true means that if we lose connection the run thread stops robot.enableMotors(); robot.runAsync(true); sick.runAsync(); // connect the laser if it was requested if (!simpleConnector.connectLaser(&sick)) { printf("Could not connect to laser... exiting\n"); delete myStereoCamera; Aria::exit(2); } // 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; ArGlobalFunctor escapeCB(&escape); if ((keyHandler = Aria::getKeyHandler()) == NULL) { keyHandler = new ArKeyHandler; Aria::setKeyHandler(keyHandler); keyHandler->addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB); robot.lock(); robot.attachKeyHandler(keyHandler); robot.unlock(); printf("To exit, press escape.\n"); } clientSwitchManager.runAsync(); robot.waitForRunExit(); printf("FCRV Stereo Vision Server .. exiting.\n"); delete myStereoCamera; ArUtil::sleep(3000); Aria::exit(0); }