/* 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 * * This version includes the saliency code and controls for saliency motions. * * version 2.2 dml 7/11 * * 8/1 Added code to make corridor and open space salience motions different * TCM2 data now correct for now and old Aria * 8/3 Added code to dump landmarks in world rather than BB coordinates; other datasets still in BB */ #define OLDARIA // for the TCM2 stuff, but affects GPS as well #define VERSIONSTRING "version 2.2 Jul 2011" #define OPENCV //#define NOGPS #ifdef OLDARIA #ifndef NOGPS #define USESERIALGPS #endif #endif // must define one of these to get the rigth salience motions compiled // you will get a compile error else //#define CONFINEDSPACE // very small salience motions #define WIDEOPENSPACE // big (and better) salience motions #ifdef OPENCV #include #include #endif #include #include "stereoCamera.h" #include "checkSalience.h" #ifdef USESERIALGPS // this is the local serialGPS rather than the Aria gps #include "serialGPS.h" #endif #include "landmarkList.h" /* Globals so they can be accessed in the handlers */ ArDPPTU *myPTZ=0; ArRobot *myRobot=0; #ifdef OLDARIA ArTCM2 *myTCM2=0; #else ArTCMCompassRobot *myTCM2 = 0; #endif #define MAXSALMOVE 4 ArPose salMoveList[MAXSALMOVE]; ArPose salPTList[MAXSALMOVE]; int salMoveIndex = -1; ArActionGoto *salMove=0; ArActionGroup *salAction = 0; bool g_checkSalience = false; bool g_emergencyStop = false; #ifndef NOGPS #ifdef USESERIALGPS serialGPS *gps; // local serial GPS object #else ArGPS *gps=0; // aria GPS object #endif #endif stereoCam *myStereoCamera=0; time_t stereoScanTime = 0; int stereoCounter=0; int panStart=-20, panInc=40, panNum=1, panEnd=20, defTilt=1; int tiltStart=-20, tiltInc=20, tiltNum=3, tiltEnd=20, defPan=0; int savedPS, savedPE, savedPI; char stereoRoot[100], // the root name intialized to robot and date stereoLog[100]; #ifdef OPENCV IplImage *frame=0, *disparity=0; #endif salienceChecker *salCheck; // salience check object LandmarkList *landmarkList; void myStereoScanHandler(void); /*---------------------------------------------*/ #define D2R(x) ((double(x))*3.14159/180.0) #define R2D(x) ((double(x))*180.0/3.14159) void transformBumbleBee2World(double from[3], double to[3], double pan, double tilt, double rx, double ry, double rth) { double tx, ty, tz; printf("BB transform: BB(%5.1lf,%5.1lf,%5.1lf)->", from[0],from[1],from[2]); // move the point to the center of the PT unit to[0] = 1000*from[0] - 59.0; //Reye to Hcenter front to[1] = 1000*from[1] + 59.5; //Hcenter down Y to center PT front to[2] = 1000*from[2]; //PT center front to PT center center // do pan around Y tx = cos(D2R(pan))*to[0]-sin(D2R(pan))*to[2]; tz = sin(D2R(pan))*to[0]+cos(D2R(pan))*to[2]; to[0]=tx; to[2]=tz; // do tilt around X ty = cos(D2R(tilt))*to[1]-sin(D2R(tilt))*to[2]; tz = sin(D2R(tilt))*to[1]+cos(D2R(tilt))*to[2]; to[1]=ty; to[2]=tz; // move the point to the center of the robot proj on ground to[1] += 92.0 + 279.0; // PT center to topplate center to ground proj PT center center to[2] += 255; // ground proj PT center center to ground proj robot center center // rotate the axis from BB to robot ty = -to[0]; tx = to[2]; tz = to[1]; to[0]=tx; to[1]=ty; to[2]=-tz; printf("R(%5.1lf,%5.1lf,%5.1lf)->",to[0],to[1],to[2]); // apply the robot transformation // rotate the XY axis by -theta tx = cos(D2R(rth))*to[0]-sin(D2R(rth))*to[1]; ty = sin(D2R(rth))*to[0]+cos(D2R(rth))*to[1]; to[0]=tx; to[1]=ty; // transform by origin to[0] += rx; to[1] += ry; printf("W(%5.1lf,%5.1lf,%5.1lf).\n",to[0], to[1], to[2]); return; } void transformWorld2BumbleBee(double from[3], double to[3], double pan, double tilt, double rx, double ry, double rth) { double tx, ty, tz; printf("Inv BB transform: WW(%5.1lf,%5.1lf,%5.1lf)->", from[0],from[1],from[2]); // transform out robot origin to[0] = from[0] - rx; to[1] = from[1] - ry; to[2] = from[2]; // transform out robot rotation tx = cos(D2R(-rth))*to[0]-sin(D2R(-rth))*to[1]; ty = sin(D2R(-rth))*to[0]+cos(D2R(-rth))*to[1]; to[0]=tx; to[1]=ty; // rotate from robot to BB tx=to[0]; ty=to[1]; tz = -to[2]; to[1]=tz; to[2]=tx; to[0]=-ty; // move back up from the robot to the camera to[2] -= 255; to[1] -= (92.0+279.0); // reverse the tilt ty = cos(D2R(-tilt))*to[1]-sin(D2R(-tilt))*to[2]; tz = sin(D2R(-tilt))*to[1]+cos(D2R(-tilt))*to[2]; to[1]=ty; to[2]=tz; // reverse the pan tx = cos(D2R(-pan))*to[0]-sin(D2R(-pan))*to[2]; tz = sin(D2R(-pan))*to[0]+cos(D2R(-pan))*to[2]; to[0]=tx; to[2]=tz; // move to the center of the Reye to[0] = (to[0]-59.0)/1000.0; to[1] = (to[1]-59.5)/1000.0; to[2] /= 1000.0; printf("BB(%5.1lf,%5.1lf,%5.1lf).\n",to[0], to[1], to[2]); } /* make a unique name for this set of dat afiles and the log file */ void setStereoScanRoot(ArRobot *r) { time_t rawtime; struct tm *timeinfo; char filename[100]; FILE *log; myRobot = r; time(&rawtime); timeinfo=localtime(&rawtime); sprintf(stereoRoot,"%s-%d%d%d%d%d-",r->getName(), //robot name timeinfo->tm_year+1900, timeinfo->tm_mon+1, timeinfo->tm_mday, //date timeinfo->tm_hour+1, timeinfo->tm_min+1); 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+1900, timeinfo->tm_mon+1, timeinfo->tm_mday, //date timeinfo->tm_hour+1, timeinfo->tm_min+1, timeinfo->tm_sec); fclose(log); // create the landmark list sprintf(filename,"%sLandmarks.txt",stereoRoot); landmarkList = new LandmarkList(filename); 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) { myTCM2->commandContinuousPackets(); #ifdef NOGPS // fprintf(log,"%d %s %f, %f, %f, %d, %d\n", stop, fname, // r->getX(), r->getY(), r->getTh(), pan, tilt); fprintf(log,"%d, %s, %5.1f, %5.1f, %5.1f, %lf, %lf, %4.1f, %3.1f, %3.1f, %3.1f, %d, %d\n", stop, fname, r->getX(), r->getY(), r->getTh(), // odo 0.0, 0.0, // NO GPS myTCM2->getCompass(), myTCM2->getPitch(), myTCM2->getRoll(), myTCM2->getTemperature(), // TCM 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()); printf("Received TCM data is %f,%f,%f,%f\n", myTCM2->getCompass(), myTCM2->getPitch(), myTCM2->getRoll(), myTCM2->getTemperature() ); fprintf(log,"%d, %s, %5.1f, %5.1f, %5.1f, %lf, %lf, %4.1f, %3.1f, %3.1f, %3.1f, %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, %5.1f, %5.1f, %5.1f, %lf, %lf, %4.1f, %3.1f, %3.1f, %3.1f, %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=0; 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=0; 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); } /* toggle the checking of salience */ void mySalienceHandler(void) { if (!g_checkSalience) { g_checkSalience=true; printf("Salience checking enabled.\n"); } else { g_checkSalience=false; printf("Salience checking disabled\n"); } salCheck->resetSalList(); salMoveIndex=-1; } #ifdef WIDEOPENSPACE #define SALCLOSEDIST 0.4 /* goes 40% of distance towards object*/ #define SALCLOSEANGLE 45 /* degrees on each side of observation to go*/ #endif #ifdef CONFINEDSPACE #define SALCLOSEDIST 0.4 /* goes 10% of distance towards object*/ #define SALCLOSEANGLE 20 /* degrees on each side of observation to go*/ #endif void setSalMoveList(ArPose here, ArPose target) { printf("Robot at (%5.1f, %5.1f; %5.1f); Target at (%5.1f, %5.1f; %5.1f).\n", here.getX(), here.getY(), here.getTh(), target.getX(), target.getY(), target.getTh()); double orientAngle = R2D(atan2( target.getY()-here.getY(), target.getX()-here.getX())); double orientDistance = here.findDistanceTo( target ); printf("Distance: %5.1lf, Orient Angle %5.1lf (+ve=left)\n", orientDistance,orientAngle); // first move is just an orient float pcD=SALCLOSEDIST; /* percent of distance to approach, also = lead/lag distance, 0..1 */ float angle = R2D(atan2(pcD,1.0-pcD)), lD=0; double inspectAngle; salMoveList[0] = here; salMoveList[0].setTh( orientAngle ); //second move is +45 or whatever angle is lD = pcD * orientDistance / cos (D2R(SALCLOSEANGLE)); // need to keep the angle of the second move clockwise to face target inspectAngle = orientAngle;//+angle; salMoveList[1] = salMoveList[0] + ArPose( lD*cos( D2R(orientAngle-SALCLOSEANGLE) ), lD*sin( D2R(orientAngle-SALCLOSEANGLE) ), orientAngle); salMoveList[1].setTh( orientAngle+angle ); //third move is -45 or whatever angle is inspectAngle = orientAngle;//-angle; salMoveList[2] = salMoveList[0] + ArPose( lD*cos( D2R(orientAngle+SALCLOSEANGLE) ), lD*sin( D2R(orientAngle+SALCLOSEANGLE) ), orientAngle); salMoveList[2].setTh( orientAngle-angle ); // fourth move is back to orient salMoveList[3] = salMoveList[0]; printf("Salience Move List.\n"); for (int i=0; i=0 && salMoveIndex < MAXSALMOVE) { printf("SM %d. (%5.1f, %5.1f; %3.1f):(%3.1f,%3.1f)\n",salMoveIndex, salMoveList[salMoveIndex].getX(),salMoveList[salMoveIndex].getY(),salMoveList[salMoveIndex].getTh(), salPTList[salMoveIndex].getX(),salPTList[salMoveIndex].getY() ); myRobot->clearDirectMotion(); salMove->setGoal( salMoveList[salMoveIndex] ); g_emergencyStop = false; salAction->activate(); time_t nowtime,starttime; time(&starttime); time(&nowtime); while (!g_emergencyStop && !salMove->haveAchievedGoal() && (nowtime-starttime<20) ) { ArUtil::sleep(250); time(&nowtime); } salMove->cancelGoal(); if (nowtime-starttime>=20) /* too too long, give up*/{ printf("\nToo too long to make move, giving up\n"); } else { //myPTZ->setPanTilt(salPTList[salMoveIndex].getX(), salPTList[salMoveIndex].gety()); double th = salMoveList[salMoveIndex ].getTh(); printf("Cleaning up orientation....target %lf, current %lf\n",th,myRobot->getTh()); myRobot->setHeading(th); while (!g_emergencyStop && !myRobot->isHeadingDone(1)) ArUtil::sleep(250); printf(" \t\t .. done.\n"); } myRobot->clearDirectMotion(); printf("Salience Move %d Done.\n", salMoveIndex); if (salMoveIndex<=MAXSALMOVE-2) myStereoScanHandler(); if (salMoveIndex==MAXSALMOVE-2) { /* last move that was an inspection*/ printf("Processing the result of the inspection.\n"); panEnd=savedPE; panStart=savedPS; panInc=savedPI; salCheck->crossCorrelateSalList(); printf("Cross-correlated list:\n"); salCheck->printSalList(); float x,y,z,r; salCheck->mergedBest(&x,&y,&z,&r); printf("Merged best target: (%5.1f, %5.1f, %5.1f; %5.1f)\n",x,y,z,r); double orientAngle = R2D(atan2( y-salMoveList[0].getY(), x-salMoveList[0].getX())); printf("Angle: original pos to merged target: %3.1f\n",orientAngle); salMoveList[MAXSALMOVE-1].setTh(orientAngle); // reset last move } if (++salMoveIndex>=MAXSALMOVE) { char fname1[100], fname2[100]; FILE *out; float x,y,z,r; salMoveIndex=-1; /* save the landmark data */ printf("Saving and logging landmark data for landmark %d\n", landmarkList->getIndex()); sprintf(fname1,"%sS%dLandmark%d.txt", stereoRoot, stereoCounter, landmarkList->getIndex()); out=fopen(fname1,"w"); ArUtil::sleep(1000); // dump landmark in WORLD coordinates myStereoCamera->doStereoFrame(frame, disparity,0,out, double(defPan), double(defTilt), myRobot->getX(),myRobot->getY(), myRobot->getTh()); fclose(out); sprintf(fname2,"%sS%dLandmark%d.jpg", stereoRoot, stereoCounter, landmarkList->getIndex()); cvSaveImage(fname2,frame); salCheck->mergedBest(&x,&y,&z,&r); printf("Merged best target: (%5.1f, %5.1f, %5.1f; %5.1f)\n",x,y,z,r); landmarkList->addViewingLoc(myRobot->getPose()); landmarkList->addToList(x,y,z, myTCM2->getRoll(), myTCM2->getPitch(), myTCM2->getCompass(), #ifdef NOGPS 0,0, #else #ifdef USESERIALGPS gps->getNorth(), gps->getWest(), // gps #else gps->getLongitude(), gps->getLatitude(),// gps #endif #endif r ); landmarkList->logLandmark(fname2,fname1); // writes the info to the landmark log file salCheck->resetSalList(); } #ifdef CONFINEDSPACE else /* adjust the next salience move if this is a confined situation */ { double rth = myRobot->getTh(), tth = salMoveList[0].getTh(); if (rth<0) rth += 360; if (tth<0) tth += 360; // convert [=180..180] to [0..360] if (tth=rth && salMoveIndex==2) salMoveIndex++; // skipe -ve move } #endif printf("Next Sal Move will be number %d\n",salMoveIndex); ArUtil::sleep(2000); } else printf("No Sal Move queued up.\n"); } void myStopHandler(void) { printf("EMERGENCY STOP FLAG SET!\n"); salMove->cancelGoal(); g_emergencyStop = true; // but its up to someone to check it and reset it } /* 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]; 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) { if (g_emergencyStop) break; printf("Stereoscan: 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,0,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); if (g_emergencyStop) break; if (g_checkSalience) { salCheck->doSaliency(myPTZ->getPan(), myPTZ->getTilt(), myRobot); sprintf(filename,"%sS%dP%dSAL.jpg", stereoRoot, stereoCounter, i); salCheck->saveSalienceImage(filename); } }/* 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) { if (g_emergencyStop) break; 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,0,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); if (g_emergencyStop) break; if (g_checkSalience){ salCheck->doSaliency(myPTZ->getPan(), myPTZ->getTilt(), myRobot); sprintf(filename,"%sS%dT%dSAL.jpg", stereoRoot, stereoCounter, i); salCheck->saveSalienceImage(filename); } }/* end of for loop*/ } /*end of IF tilt */ myPTZ->panTilt(float(defPan),float(defTilt)); stereoCounter++; // Wrap up salience processing on this scan if (g_checkSalience) { salCheck->printSalList(); if ( salMoveIndex<0) {/* do salience but no list made yet */ float tx, ty, tz; int bI; bI = salCheck->bestSalList(&tx, &ty, &tz); if (bI>=0) { printf("Best target is %d at (%5.lf, %5.1f, %5.1f)\n",bI,tx, ty, tz); setSalMoveList(myRobot->getPose(), ArPose(tx,ty,0.0)); } else printf("No target salient enough here\n"); salCheck->resetSalList(); } } printf("Scan done.\n"); } /******* dml 10/10 ******/ ArGlobalFunctor myStereoScanFunctor(&myStereoScanHandler); ArGlobalFunctor2 mySetPanParamFunctor2(&mySetPanParamHandler2); ArGlobalFunctor2 mySetTiltParamFunctor2(&mySetTiltParamHandler2); ArGlobalFunctor mySalienceFunctor(&mySalienceHandler); ArGlobalFunctor mySalienceMoveFunctor(&mySalienceMoveHandler); ArGlobalFunctor myStopFunctor(&myStopHandler); /* 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); // 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 FRCV commands: doStereoScan, setStereoTilt, setStereoPan, \n"); printf("\t\t doSalience, doSalienceMove, doSTOP\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"); commands.addCommand("doSalience", "Toggles salience checking to the pan or tilt scan", &mySalienceFunctor,0); commands.addCommand("doSalienceMove", "Carries out a salience move, if any was flagged", &mySalienceMoveFunctor,0); commands.addCommand("doSTOP", "Stops salience related motion", &myStopFunctor,0); myStereoCamera = new stereoCam(); setStereoScanRoot(&robot); salCheck = new salienceChecker( myStereoCamera ); #ifdef OPENCV frame = cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,3); disparity = cvCreateImage(cvSize(320,240),IPL_DEPTH_8U,1); #endif /****************/ // set up a gyro, if installed ArAnalogGyro gyro(&robot); gyro.activate(); 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); } // TCM #ifdef OLDARIA ArTCM2 theTCM2(&robot); #else ArTCMCompassRobot theTCM2(&robot); #endif myTCM2 = &theTCM2;// new ArTCMCompassRobot(&robot); theTCM2.commandContinuousPackets(); // wake up the TCM for (int i=0; i<5; i++) { printf("TCM data: %3.1f, %3.1f, %3.1f %3.1f\n", theTCM2.getCompass(), theTCM2.getPitch(), theTCM2.getRoll(), theTCM2.getTemperature()); // TCM myTCM2->commandContinuousPackets(); ArUtil::sleep(250); } // 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"); printf("Adding action group for saliency move ....\n"); salAction = new ArActionGroup(myRobot); // if we're stalled we want to back up and recover salAction->addAction(new ArActionStallRecover, 100); // #ifdef CONFINEDSPACE // turn to avoid things closer to us salAction->addAction(new ArActionAvoidFront("Avoid Front Near", 30, 50,5), 51); #if 0 // turn to avoid things closer to us salAction->addAction(new ArActionAvoidFront("Avoid Front Near", 255, 50,15), 80); // turn avoid things further away salAction->addAction(new ArActionAvoidFront("Avoid Front Far", 455,200,15), 55); #endif salMove = new ArActionGoto(); // keep moving to goal point salAction->addAction(salMove, 50); printf ("\t\t...Action group added.\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); }