#include "Aria.h" #include "ArGPS.h" #include "ArGPSConnector.h" /* * This class encapsulates an ArRobot sensor interpretation task that prints the * last set of GPS data along with a local timestamp and current robot pose to * standard output. * * This class also contains a mutex, which it locks during the task (while * accessing the ArGPS object). If another thread is also accessing the GPS, * you can lock this mutex. */ class GPSLogTask { public: GPSLogTask(ArRobot *robot, ArGPS *gps, ArJoyHandler *joy = NULL) : myRobot(robot), myGPS(gps), myTaskFunctor(this, &GPSLogTask::doTask), myJoyHandler(joy), myButtonDown(false) { myRobot->addSensorInterpTask("GPS", ArListPos::LAST, &myTaskFunctor); puts("RobotX\tRobotY\tRobotTh\tRobotVel\tRobotRotVel\tRobotLatVel\tLatitude\tLongitude\tAltitude\tSpeed\tGPSTimeSec\tGPSTimeMSec\tFixType\tNumSats\tPDOP\tHDOP\tVDOP\tGPSDataReceived"); } void lock() { myMutex.lock(); } void unlock() { myMutex.unlock(); } protected: void doTask() { // print a mark if a joystick button is pressed (other than 1, which is // needed to drive) if(myJoyHandler) { for(unsigned int b = 2; b <= myJoyHandler->getNumButtons(); ++b) if(myJoyHandler->getButton(b)) { if(!myButtonDown) printf("--------------- Joystick button %d pressed.\n", b); myButtonDown = true; } else myButtonDown = false; } lock(); int f = myGPS->read(50); printf("%.4f\t%.4f\t%.4f\t%.4f\t%.4f\t%.4f" "\t%2.8f\t%2.8f\t%4.4f\t%4.4f" "\t%lu\t%lu\t%s" "\t%u\t%2.4f\t%2.4f\t%2.4f" "\t%s\n", myRobot->getX(), myRobot->getY(), myRobot->getTh(), myRobot->getVel(), myRobot->getRotVel(), (myRobot->hasLatVel())?(myRobot->getLatVel()):0, myGPS->getLatitude(), myGPS->getLongitude(), myGPS->getAltitude(), myGPS->getSpeed(), myGPS->getGPSPositionTimestamp().getSec(), myGPS->getGPSPositionTimestamp().getMSec(), myGPS->getFixTypeName(), myGPS->getNumSatellitesTracked(), myGPS->getPDOP(), myGPS->getHDOP(), myGPS->getVDOP(), ((f&ArGPS::ReadUpdated)?"yes":"no") ); unlock(); } private: ArRobot *myRobot; ArGPS *myGPS; ArFunctorC<GPSLogTask> myTaskFunctor; ArMutex myMutex; ArJoyHandler *myJoyHandler; bool myButtonDown; }; int main(int argc, char** argv) { Aria::init(); ArLog::init(ArLog::StdErr, ArLog::Normal); ArArgumentParser argParser(&argc, argv); ArSimpleConnector connector(&argParser); ArGPSConnector gpsConnector(&argParser); ArRobot robot; ArActionLimiterForwards nearLimitAction("limit near", 300, 600, 250); ArActionLimiterForwards farLimitAction("limit far", 300, 1100, 400); ArActionLimiterBackwards limitBackwardsAction; ArActionJoydrive joydriveAction; ArActionKeydrive keydriveAction; ArSonarDevice sonar; ArSick laser; argParser.loadDefaultArguments(); if(!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); return -1; } robot.addRangeDevice(&sonar); robot.addRangeDevice(&laser); ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Connecting to robot..."); if(!connector.connectRobot(&robot)) { ArLog::log(ArLog::Terse, "gpsRobotTaskExample: Could not connect to the robot. Exiting."); return -2; } ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Connected to the robot."); // Connect to GPS ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Connecting to GPS, it may take a few seconds..."); ArGPS *gps = gpsConnector.createGPS(); if(!gps || !gps->connect()); { ArLog::log(ArLog::Terse, "gpsRobotTaskExample: Error connecting to GPS device. Try -gpsType, -gpsPort, and/or -gpsBaud command-line arguments. Use -help for help. Exiting."); return -3; } // Create an GPSLogTask which will register a task with the robot. GPSLogTask gpsTask(&robot, gps, joydriveAction.getJoyHandler()->haveJoystick() ? joydriveAction.getJoyHandler() : NULL); // Add actions robot.addAction(&nearLimitAction, 100); robot.addAction(&farLimitAction, 90); robot.addAction(&limitBackwardsAction, 80); robot.addAction(&joydriveAction, 50); robot.addAction(&keydriveAction, 40); // allow keydrive action to drive robot even if joystick button isn't pressed joydriveAction.setStopIfNoButtonPressed(false); // Start the robot running robot.runAsync(true); // Connect to the laser connector.setupLaser(&laser); laser.runAsync(); if(!laser.blockingConnect()) ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Warning, could not connect to SICK laser, will not use it."); robot.lock(); robot.enableMotors(); robot.comInt(47, 1); // enable joystick driving on some robots // Add exit callback to reset/unwrap steering wheels on seekur (critical if the robot doesn't have sliprings); does nothing for other robots Aria::addExitCallback(new ArRetFunctor1C<bool, ArRobot, unsigned char>(&robot, &ArRobot::com, (unsigned char)120)); Aria::addExitCallback(new ArRetFunctor1C<bool, ArRobot, unsigned char>(&robot, &ArRobot::com, (unsigned char)120)); robot.unlock(); ArLog::log(ArLog::Normal, "gpsRobotTaskExample: Running... (drive robot with joystick or arrow keys)"); robot.waitForRunExit(); return 0; }