/*!
* Interact with user on the terminal.
*/
void
interact()
{
ArMap* ariamap = advancedptr->myMap;
sleep(1);
advancedptr->getAllGoals(ariamap);
advancedptr->getAllRobotHomes(ariamap);
/// MPL
// lkeyCB();
advancedptr->myLocaTask->localizeRobotAtHomeNonBlocking();
//
// Interact with user using keyboard.
//
ArGlobalFunctor lCB(&lkeyCB);
ArGlobalFunctor pCB(&pkeyCB);
ArGlobalFunctor hCB(&hkeyCB);
ArGlobalFunctor rCB(&rkeyCB);
ArGlobalFunctor qCB(&quitCB);
ArGlobalFunctor escapeCB(&quitCB);
keyHandler.addKeyHandler('l', &lCB);
keyHandler.addKeyHandler('p', &pCB);
keyHandler.addKeyHandler('h', &hCB);
keyHandler.addKeyHandler('r', &rCB);
keyHandler.addKeyHandler('q', &qCB);
keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);
printf("Put robot at RobotHome and press 'l' to localize first.\n\
Press 'p' to move to the next goal\n\
Press 'h' to move to the first home\n\
Press 'r' to move to the goals in order\n\
Press 'q' to quit\n");
while (advancedptr->myLocaTask->getRunning() &&
advancedptr->myPathPlanningTask->getRunning()){
keyHandler.checkKeys();
ArUtil::sleep(250);
advancedptr->myRobot->lock();
ArPose rpose = advancedptr->myRobot->getPose();
double lvel = advancedptr->myRobot->getVel();
double avel = advancedptr->myRobot->getRotVel();
double volts = advancedptr->myRobot->getBatteryVoltage();
advancedptr->myRobot->unlock();
if(advancedptr->myLocaTask->getInitializedFlag()){
printf("\r%5.2f %5.2f %5.2f: %5.2f %5.2f: %4.1f\r",
rpose.getX(), rpose.getY(), rpose.getTh(), lvel, avel, volts);
fflush(stdout);
}
}
}
int main(int argc, char **argv)
{
char* host = "localhost";
if(argc > 1)
host = argv[1];
Aria::init();
ArClientBase client;
ArGlobalFunctor escapeCB(&escape);
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
printf("Connecting to standaloneServerDemo at %s:%d...\n", host, 7272);
if (!client.blockingConnect(host, 7272))
{
printf("Could not connect to server, exiting\n");
exit(1);
}
InputHandler inputHandler(&client, &keyHandler);
OutputHandler outputHandler(&client);
keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);
client.runAsync();
while (client.getRunningWithLock())
{
keyHandler.checkKeys();
ArUtil::sleep(1);
}
keyHandler.restore();
Aria::shutdown();
return 0;
}
void addKeyHandlers()
{
ArKeyHandler *keyHandler = Aria::getKeyHandler();
if(keyHandler == NULL)
{
keyHandler = new ArKeyHandler();
Aria::setKeyHandler(keyHandler);
robot->attachKeyHandler(keyHandler);
}
//keyHandler->addKeyHandler('g', &myGoCB);
//keyHandler->addKeyHandler('c', &myGoHomeCB);
keyHandler->addKeyHandler('p', &myStartCB);
keyHandler->addKeyHandler('s', &myStopCB);
keyHandler->addKeyHandler('m', &myPrintCB);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "ariaClientDriverNode"); //ROS Initialization
Aria::init(); //Aria Initialization
ArClientBase client; //setup client
ArArgumentParser parser(&argc, argv); //command line argument handler
ArClientSimpleConnector clientConnector(&parser); //connect to Arserver
parser.loadDefaultArguments();
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
exit(0);
}
if (!clientConnector.connectClient(&client))
{
if (client.wasRejected())
printf("Server '%s' rejected connection, exiting\n", client.getHost());
else
printf("Could not connect to server '%s', exiting\n", client.getHost());
exit(1);
}
printf("Connected to server.\n");
client.setRobotName(client.getHost()); // include server name in log messages
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
ArGlobalFunctor escapeCB(&escape);
keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);
client.runAsync();
if(!client.dataExists("ratioDrive") )
printf("Warning: server does not have ratioDrive command, can not use drive commands!\n");
else
printf("Keys are:\nUP: Forward\nDOWN: Backward\nLEFT: Turn Left\nRIGHT: Turn Right\n");
printf("s: Enable safe drive mode (if supported).\nu: Disable safe drive mode (if supported).\nl: list all data requests on server\n\nDrive commands use 'ratioDrive'.\nt: logs the network tracking tersely\nv: logs the network tracking verbosely\nr: resets the network tracking\n\n");
AriaClientDriver ariaClientDriver(&client,&keyHandler,"");
//while (ros::ok() && client.getRunningWithLock()) //the main loop
while (client.getRunningWithLock()) //the main loop
{
keyHandler.checkKeys(); //addthis if teleop from node required
ariaClientDriver.controlloop();
//Input output handling callback threads implemented in ariaClientDriver Class
ArUtil::sleep(100); //noneed
}
client.disconnect();
Aria::shutdown();
return 0;
}
int main(int argc, char **argv)
{
Aria::init();
ArClientBase client;
ArArgumentParser parser(&argc, argv);
ArClientSimpleConnector clientConnector(&parser);
parser.loadDefaultArguments();
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
exit(0);
}
if (!clientConnector.connectClient(&client))
{
if (client.wasRejected())
printf("Server '%s' rejected connection, exiting\n", client.getHost());
else
printf("Could not connect to server '%s', exiting\n", client.getHost());
exit(1);
}
printf("Connected to server.\n");
/* Create a key handler and also tell Aria about it */
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
/* Global escape-key handler to shut everythnig down */
ArGlobalFunctor escapeCB(&escape);
keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);
client.runAsync();
while (client.getRunningWithLock())
{
keyHandler.checkKeys();
ArUtil::sleep(100);
}
Aria::shutdown();
return 0;
}
AREXPORT void ArActionKeydrive::takeKeys(void)
{
ArKeyHandler *keyHandler;
if ((keyHandler = Aria::getKeyHandler()) == NULL)
{
ArLog::log(ArLog::Terse,
"ArActionKeydrive::takeKeys: There is no key handler, keydrive will not work.");
}
// now that we have one, add our keys as callbacks, print out big
// warning messages if they fail
if (!keyHandler->addKeyHandler(ArKeyHandler::UP, &myUpCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for up, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::DOWN, &myDownCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for down, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::LEFT, &myLeftCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for left, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::RIGHT, &myRightCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for right, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::SPACE, &mySpaceCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for space, keydrive will not work correctly.");
}
AREXPORT void ArRatioInputKeydrive::takeKeys(void)
{
myHaveKeys = true;
ArKeyHandler *keyHandler;
if ((keyHandler = Aria::getKeyHandler()) == NULL)
{
ArLog::log(ArLog::Terse,
"ArRatioInputKeydrive::takeKeys: There is no key handler, keydrive will not work.");
}
// now that we have one, add our keys as callbacks, print out big
// warning messages if they fail
if (!keyHandler->addKeyHandler(ArKeyHandler::UP, &myUpCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for up, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::DOWN, &myDownCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for down, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::LEFT, &myLeftCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for left, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::RIGHT, &myRightCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for right, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::SPACE, &mySpaceCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for space, keydrive will not work correctly.");
if (myRobot != NULL && myRobot->hasLatVel())
{
if (!keyHandler->addKeyHandler('z', &myZCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for z, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('Z', &myZCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for Z, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('x', &myXCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for x, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('X', &myXCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for x, keydrive will not work correctly.");
}
}
/**
@param robot the robot we're attaching to
@param name the name of this mode
@param key the primary key to switch to this mode on... it can be
'\\0' if you don't want to use this
@param key2 an alternative key to switch to this mode on... it can be
'\\0' if you don't want a second alternative key
**/
AREXPORT ArMode::ArMode(ArRobot *robot, const char *name, char key,
char key2) :
myActivateCB(this, &ArMode::activate),
myDeactivateCB(this, &ArMode::deactivate),
myUserTaskCB(this, &ArMode::userTask)
{
ArKeyHandler *keyHandler;
myName = name;
myRobot = robot;
myKey = key;
myKey2 = key2;
// see if there is already a keyhandler, if not make one for ourselves
if ((keyHandler = Aria::getKeyHandler()) == NULL)
{
keyHandler = new ArKeyHandler;
Aria::setKeyHandler(keyHandler);
if (myRobot != NULL)
myRobot->attachKeyHandler(keyHandler);
else
ArLog::log(ArLog::Terse, "ArMode: No robot to attach a keyHandler to, keyHandling won't work... either make your own keyHandler and drive it yourself, make a keyhandler and attach it to a robot, or give this a robot to attach to.");
}
if (ourHelpCB == NULL)
{
ourHelpCB = new ArGlobalFunctor(&ArMode::baseHelp);
if (!keyHandler->addKeyHandler('h', ourHelpCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for 'h', ArMode will not be invoked on an 'h' keypress.");
if (!keyHandler->addKeyHandler('H', ourHelpCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for 'H', ArMode will not be invoked on an 'H' keypress.");
if (!keyHandler->addKeyHandler('?', ourHelpCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for '?', ArMode will not be invoked on an '?' keypress.");
if (!keyHandler->addKeyHandler('/', ourHelpCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for '/', ArMode will not be invoked on an '/' keypress.");
}
// now that we have one, add our keys as callbacks, print out big
// warning messages if they fail
if (myKey != '\0')
if (!keyHandler->addKeyHandler(myKey, &myActivateCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for '%c', ArMode will not work correctly.", myKey);
if (myKey2 != '\0')
if (!keyHandler->addKeyHandler(myKey2, &myActivateCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for '%c', ArMode will not work correctly.", myKey2);
// toss this mode into our list of modes
ourModes.push_front(this);
}
void addKeyHandlers(ArRobot *robot)
{
ArKeyHandler *keyHandler = Aria::getKeyHandler();
if(keyHandler == NULL)
{
keyHandler = new ArKeyHandler();
Aria::setKeyHandler(keyHandler);
robot->attachKeyHandler(keyHandler);
}
keyHandler->addKeyHandler(ArKeyHandler::PAGEUP, &myUpCB);
keyHandler->addKeyHandler('u', &myUpCB);
keyHandler->addKeyHandler(ArKeyHandler::PAGEDOWN, &myDownCB);
keyHandler->addKeyHandler('d', &myDownCB);
keyHandler->addKeyHandler('o', &myOpenCB);
keyHandler->addKeyHandler('c', &myCloseCB);
keyHandler->addKeyHandler('s', &myStopCB);
}
/*
Constructor, sets the robot pointer, and some initial values, also note the
use of constructor chaining on myPTU and myDriveCB.
*/
KeyPTU::KeyPTU(ArRobot *robot) :
myUpCB(this, &KeyPTU::up),
myDownCB(this, &KeyPTU::down),
myLeftCB(this, &KeyPTU::left),
myRightCB(this, &KeyPTU::right),
myPlusCB(this, &KeyPTU::plus),
myMinusCB(this, &KeyPTU::minus),
myGreaterCB(this, &KeyPTU::greater),
myLessCB(this, &KeyPTU::less),
myQuestionCB(this, &KeyPTU::question),
mySCB(this, &KeyPTU::status),
myECB(this, &KeyPTU::exercise),
myACB(this, &KeyPTU::autoupdate),
myCCB(this, &KeyPTU::c),
myHCB(this, &KeyPTU::h),
myICB(this, &KeyPTU::i),
myPCB(this, &KeyPTU::p),
myXCB(this, &KeyPTU::x),
myZCB(this, &KeyPTU::z),
myPTU(robot),
myDriveCB(this, &KeyPTU::drive)
{
// set the robot pointer and add the KeyPTU as user task
ArKeyHandler *keyHandler;
myRobot = robot;
myRobot->addSensorInterpTask("KeyPTU", 50, &myDriveCB);
myExerciseTime.setToNow();
myExercise = true;
// SETPORT Uncomment the following to run the camera off
// of the computer's serial port, rather than the microcontroller
// uncomment below here
/*
#ifdef WIN32
myCon.setPort("COM2");
#else
myCon.setPort("/dev/ttyS0");
#endif
myPTU.setDeviceConnection(&myCon);
*/
// to here
// or use this next line to set the aux port
//myPTU.setAuxPort(2);
if ((keyHandler = Aria::getKeyHandler()) == NULL)
{
keyHandler = new ArKeyHandler;
Aria::setKeyHandler(keyHandler);
myRobot->attachKeyHandler(keyHandler);
}
if (!keyHandler->addKeyHandler(ArKeyHandler::UP, &myUpCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for up, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::DOWN, &myDownCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for down, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::LEFT, &myLeftCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for left, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::RIGHT, &myRightCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for right, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('+', &myPlusCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for '+', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('-', &myMinusCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for '-', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('>', &myGreaterCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for '>', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('<', &myLessCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for '<', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('?', &myQuestionCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for '?', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('c', &myCCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'C', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('h', &myHCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'H', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('i', &myICB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'I', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('p', &myPCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'P', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('s', &mySCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'S', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('x', &myXCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'X', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('z', &myZCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'Z', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('a', &myACB))
ArLog::log(ArLog::Terse, "The key handler already has a key for 'A', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('e', &myECB))
ArLog::log(ArLog::Terse, "The key handler already has a key for 'E', keydrive will not work correctly.");
// initialize some variables
myPTUInitRequested = false;
myPosIncrement = 1;
mySlewIncrement = 5;
myZoomIncrement = 50;
}
int main(int argc, char **argv)
{
Aria::init();
// parse our args and make sure they were all accounted for
ArSimpleConnector connector(&argc, argv);
ArRobot robot;
// the laser. ArActionTriangleDriveTo will use this laser object since it is
// named "laser" when added to the ArRobot.
ArSick sick;
if (!connector.parseArgs() || argc > 1)
{
connector.logOptions();
exit(1);
}
// a key handler so we can do our key handling
ArKeyHandler keyHandler;
// let the global aria stuff know about it
Aria::setKeyHandler(&keyHandler);
// toss it on the robot
robot.attachKeyHandler(&keyHandler);
// add the laser to the robot
robot.addRangeDevice(&sick);
ArSonarDevice sonar;
robot.addRangeDevice(&sonar);
ArActionTriangleDriveTo triangleDriveTo;
ArFunctorC<ArActionTriangleDriveTo> lineGoCB(&triangleDriveTo,
&ArActionTriangleDriveTo::activate);
keyHandler.addKeyHandler('g', &lineGoCB);
keyHandler.addKeyHandler('G', &lineGoCB);
ArFunctorC<ArActionTriangleDriveTo> lineStopCB(&triangleDriveTo,
&ArActionTriangleDriveTo::deactivate);
keyHandler.addKeyHandler('s', &lineStopCB);
keyHandler.addKeyHandler('S', &lineStopCB);
ArActionLimiterForwards limiter("limiter", 150, 0, 0, 1.3);
robot.addAction(&limiter, 70);
ArActionLimiterBackwards limiterBackwards;
robot.addAction(&limiterBackwards, 69);
robot.addAction(&triangleDriveTo, 60);
ArActionKeydrive keydrive;
robot.addAction(&keydrive, 55);
ArActionStop stopAction;
robot.addAction(&stopAction, 50);
// try to connect, if we fail exit
if (!connector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
robot.comInt(ArCommands::SONAR, 1);
robot.comInt(ArCommands::ENABLE, 1);
// start the robot running, true so that if we lose connection the run stops
robot.runAsync(true);
// now set up the laser
connector.setupLaser(&sick);
sick.runAsync();
if (!sick.blockingConnect())
{
printf("Could not connect to SICK laser... exiting\n");
Aria::shutdown();
return 1;
}
printf("If you press the 'g' key it'll go find a triangle, if you press 's' it'll stop.\n");
robot.waitForRunExit();
return 0;
}
int main(int argc, char **argv)
{
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobot robot;
ArRobotConnector robotConnector(&parser, &robot);
ArLaserConnector laserConnector(&parser, &robot, &robotConnector);
if(!robotConnector.connectRobot())
{
ArLog::log(ArLog::Terse, "lineFinderExample: Could not connect to the robot.");
if(parser.checkHelpAndWarnUnparsed())
{
// -help not given
Aria::logOptions();
Aria::exit(1);
}
}
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
ArLog::log(ArLog::Normal, "lineFinderExample: Connected to robot.");
robot.runAsync(true);
// Connect to laser(s) as defined in parameter files.
// (Some flags are available as arguments to connectLasers() to control error behavior and to control which lasers are put in the list of lasers stored by ArRobot. See docs for details.)
if(!laserConnector.connectLasers())
{
ArLog::log(ArLog::Terse, "Could not connect to configured lasers. Exiting.");
Aria::exit(3);
return 3;
}
ArLog::log(ArLog::Normal, "lineFinderExample: Connected to laser");
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
// Create the ArLineFinder object. Set it to log lots of information about its
// processing.
ArLaser *laser = robot.findLaser(1);
if(!laser)
{
ArLog::log(ArLog::Terse, "lineFinderExample: No laser device connected, exiting.");
Aria::exit(4);
return 4;
}
ArLineFinder lineFinder(laser);
lineFinder.setVerbose(true);
// Add key callbacks that simply call the ArLineFinder::getLinesAndSaveThem()
// function, which searches for lines in the current set of laser sensor
// readings, and saves them in files with the names 'points' and 'lines'.
ArFunctorC<ArLineFinder> findLineCB(&lineFinder,
&ArLineFinder::getLinesAndSaveThem);
keyHandler.addKeyHandler('f', &findLineCB);
keyHandler.addKeyHandler('F', &findLineCB);
printf("If you press the 'f' key the points and lines found will be saved\n");
printf("Into the 'points' and 'lines' file in the current working directory\n");
robot.waitForRunExit();
Aria::exit(0);
return 0;
}
/**
Make sure you have called ArSick::configure or
ArSick::configureShort on your laser before you make this class
@param robot The robot to attach to
@param sick the laser to log from
@param distDiff the distance traveled at which to take a new reading
@param degDiff the degrees turned at which to take a new reading
@param fileName the file name in which to put the log
@param addGoals whether to add goals automatically or... if true
then the sick logger puts hooks into places it needs this to
happen, into any keyhandler thats around (for a keypress of G), it
pays attention to the flag bit of the robot, and it puts in a
button press callback for the joyhandler passed in (if any)
**/
AREXPORT ArSickLogger::ArSickLogger(ArRobot *robot, ArSick *sick,
double distDiff, double degDiff,
const char *fileName, bool addGoals,
ArJoyHandler *joyHandler,
const char *baseDirectory,
bool useReflectorValues,
ArRobotJoyHandler *robotJoyHandler) :
mySectors(18),
myTaskCB(this, &ArSickLogger::robotTask),
myGoalKeyCB(this, &ArSickLogger::goalKeyCallback),
myLoopPacketHandlerCB(this, &ArSickLogger::loopPacketHandler)
{
ArKeyHandler *keyHandler;
ArSick::Degrees degrees;
ArSick::Increment increment;
double deg, incr;
myOldReadings = false;
myNewReadings = true;
myUseReflectorValues = useReflectorValues;
myWrote = false;
myRobot = robot;
mySick = sick;
if (baseDirectory != NULL && strlen(baseDirectory) > 0)
myBaseDirectory = baseDirectory;
else
myBaseDirectory = "";
std::string realFileName;
if (fileName[0] == '/' || fileName[0] == '\\')
{
realFileName = fileName;
}
else
{
realFileName = myBaseDirectory;
realFileName += fileName;
}
myFileName = realFileName;
myFile = fopen(realFileName.c_str(), "w+");
degrees = mySick->getDegrees();
increment = mySick->getIncrement();
if (degrees == ArSick::DEGREES180)
deg = 180;
else
deg = 100;
if (increment == ArSick::INCREMENT_ONE)
incr = 1;
else
incr = .5;
if (myFile != NULL)
{
const ArRobotParams *params;
params = robot->getRobotParams();
fprintf(myFile, "LaserOdometryLog\n");
fprintf(myFile, "#Created by ARIA's ArSickLogger\n");
fprintf(myFile, "version: 2\n");
fprintf(myFile, "sick1pose: %d %d %.2f\n", params->getLaserX(),
params->getLaserY(), params->getLaserTh());
fprintf(myFile, "sick1conf: %d %d %d\n",
ArMath::roundInt(0.0 - deg / 2.0),
ArMath::roundInt(deg / 2.0), ArMath::roundInt(deg / incr + 1.0));
}
else
ArLog::log(ArLog::Terse, "ArSickLogger cannot write to file %s",
myFileName.c_str());
myDistDiff = distDiff;
myDegDiff = degDiff;
myFirstTaken = false;
myScanNumber = 0;
myLastVel = 0;
myStartTime.setToNow();
myRobot->addUserTask("Sick Logger", 1, &myTaskCB);
char uCFileName[15];
strncpy(uCFileName, fileName, 14);
uCFileName[14] = '\0';
myRobot->comStr(94, uCFileName);
myLoopPacketHandlerCB.setName("ArSickLogger");
myRobot->addPacketHandler(&myLoopPacketHandlerCB);
myAddGoals = addGoals;
myJoyHandler = joyHandler;
myRobotJoyHandler = robotJoyHandler;
myTakeReadingExplicit = false;
myAddGoalExplicit = false;
myAddGoalKeyboard = false;
myLastAddGoalKeyboard = false;
myLastJoyButton = false;
myLastRobotJoyButton = false;
myFirstGoalTaken = false;
myNumGoal = 1;
myLastLoops = 0;
// only add goals from the keyboard if there's already a keyboard handler
if (myAddGoals && (keyHandler = Aria::getKeyHandler()) != NULL)
{
// now that we have a key handler, add our keys as callbacks, print out big
// warning messages if they fail
if (!keyHandler->addKeyHandler('g', &myGoalKeyCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for g, sick logger goal handling will not work correctly.");
if (!keyHandler->addKeyHandler('G', &myGoalKeyCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for g, sick logger goal handling will not work correctly.");
}
}
/*
Constructor, sets the robot pointer, and some initial values, also note the
use of constructor chaining on myPTU and myDriveCB.
*/
KeyPTU::KeyPTU(ArRobot *robot) :
myUpCB(this, &KeyPTU::up),
myDownCB(this, &KeyPTU::down),
myLeftCB(this, &KeyPTU::left),
myRightCB(this, &KeyPTU::right),
mySpaceCB(this, &KeyPTU::space),
myICB(this, &KeyPTU::i),
myPlusCB(this, &KeyPTU::plus),
myMinusCB(this, &KeyPTU::minus),
myGreaterCB(this, &KeyPTU::greater),
myLessCB(this, &KeyPTU::less),
myQuestionCB(this, &KeyPTU::question),
mySCB(this, &KeyPTU::status),
myACB(this, &KeyPTU::a),
myZCB(this, &KeyPTU::z),
myMCB(this, &KeyPTU::m),
myHCB(this, &KeyPTU::h),
myRCB(this, &KeyPTU::r),
myPTU(robot),
myDriveCB(this, &KeyPTU::drive),
mySerialConnection(NULL)
{
#ifdef SERIAL_PORT
mySerialConnection = new ArSerialConnection;
ArLog::log(ArLog::Normal, "dpptuExample: connecting to DPPTU over computer serial port %s.", SERIAL_PORT);
if(mySerialConnection->open(SERIAL_PORT) != 0)
{
ArLog::log(ArLog::Terse, "dpptuExample: Error: Could not open computer serial port %s for DPPTU!", SERIAL_PORT);
Aria::exit(5);
}
myPTU.setDeviceConnection(mySerialConnection);
#endif
// set the robot pointer and add the KeyPTU as user task
ArKeyHandler *keyHandler;
myRobot = robot;
myRobot->addSensorInterpTask("KeyPTU", 50, &myDriveCB);
if ((keyHandler = Aria::getKeyHandler()) == NULL)
{
keyHandler = new ArKeyHandler;
Aria::setKeyHandler(keyHandler);
myRobot->attachKeyHandler(keyHandler);
}
if (!keyHandler->addKeyHandler(ArKeyHandler::UP, &myUpCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for up, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::DOWN, &myDownCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for down, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::LEFT, &myLeftCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for left, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::RIGHT, &myRightCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for right, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::SPACE, &mySpaceCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for space, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('i', &myICB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'i', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('+', &myPlusCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for '+', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('-', &myMinusCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for '-', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('>', &myGreaterCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for '>', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('<', &myLessCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for '<', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('?', &myQuestionCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for '?', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('s', &mySCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'S', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('a', &myACB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'A', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('z', &myZCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'Z', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('m', &myMCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'M', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('h', &myHCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'H', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('r', &myRCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'R', keydrive will not work correctly.");
// initialize some variables
myReset = false;
myInit = true;
myDesiredPanPos = 0;
myDesiredTiltPos = 0;
myPosIncrement = 1;
mySlewIncrement = 1;
myPTUInited = false;
posIncIncrement = 1;
myMonitor = false;
}
int main(int argc, char **argv)
{
bool done;
double distToTravel = 3000;
int spinTime = 0;
// set up our simpleConnector
ArSimpleConnector simpleConnector(&argc, argv);
// set up a key handler so escape exits and attach to the robot
ArKeyHandler keyHandler;
Aria::init();
robot = new ArRobot;
printf("You can press the escape key to exit this program\n");
// parse its arguments
if (simpleConnector.parseArgs())
{
simpleConnector.logOptions();
exit(1);
}
// if there are more arguments left then it means we didn't
// understand an option
/*
if (argc > 1)
{
simpleConnector.logOptions();
keyHandler.restore();
exit(1);
}
*/
ArGlobalFunctor exitCB(&hardExit);
ArGlobalFunctor printerCB(&printer);
keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &exitCB);
robot->attachKeyHandler(&keyHandler);
Aria::setKeyHandler(&keyHandler);
// set up the robot for connecting
if (!simpleConnector.connectRobot(robot))
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
keyHandler.restore();
return 1;
}
//robot->addUserTask("printer", 50, &printerCB);
// run the robot, true here so that the run will exit if connection lost
robot->runAsync(true);
#ifdef WIN32
// wait until someone pushes the motor button to go
printf("Press the motor button to start the robot moving\n");
while (1)
{
robot->lock();
if (!robot->isRunning())
hardExit();
if (robot->areMotorsEnabled())
{
robot->unlock();
break;
}
robot->unlock();
ArUtil::sleep(100);
}
#endif
ArAnalogGyro *gyro;
if (argc == 1)
{
printf("Gyro\n");
gyro = new ArAnalogGyro(robot);
}
printf("Waiting for inertial to stabilize for 5 seconds.\n");
// wait a bit for the inertial to warm up
ArUtil::sleep(5000);
// basically from here on down the robot just cruises around a bit
robot->lock();
// enable the motors, disable amigobot sounds
robot->comInt(ArCommands::SONAR, 0);
robot->comInt(ArCommands::ENABLE, 1);
robot->setMoveDoneDist(200);
// move a couple meters
printf("Driving out\n");
robot->move(distToTravel);
robot->setHeading(0);
robot->unlock();
do {
ArUtil::sleep(100);
robot->lock();
//robot->setHeading(0);
done = robot->isMoveDone(200);
robot->unlock();
} while (!done);
if (spinTime != 0)
{
printf("Spinning a while\n");
// rotate a few times
robot->lock();
robot->setRotVel(200);
robot->unlock();
ArUtil::sleep(spinTime * 1000);
}
printf("Pointing back\n");
robot->lock();
robot->setHeading(180);
robot->unlock();
do {
ArUtil::sleep(100);
robot->lock();
//robot->setHeading(180);
done = robot->isHeadingDone(5);
robot->unlock();
} while (!done);
printf("Driving back\n");
// move a couple meters
robot->lock();
robot->move(distToTravel);
robot->setHeading(180);
robot->unlock();
do {
ArUtil::sleep(100);
robot->lock();
//robot->setHeading(180);
done = robot->isMoveDone(200);
robot->unlock();
} while (!done);
printf("Pointing back in original direction.\n");
robot->lock();
robot->setHeading(0);
robot->unlock();
do {
ArUtil::sleep(100);
robot->lock();
//robot->setHeading(0);
done = robot->isHeadingDone(5);
robot->unlock();
} while (!done);
robot->lock();
printf("Final heading %.2f\n", robot->getTh());
robot->disconnect();
robot->unlock();
// now exit
Aria::shutdown();
return 0;
}
/*
Constructor, sets the robot pointer, and some initial values, also note the
use of constructor chaining on myCam and myDriveCB.
*/
Joydrive::Joydrive(ArRobot *robot, int LRAmount, int UDAmount, int zoomAmount) :
myCam(robot),
myDriveCB(this, &Joydrive::drive),
myUpCB(this, &Joydrive::up),
myDownCB(this, &Joydrive::down),
myLeftCB(this, &Joydrive::left),
myRightCB(this, &Joydrive::right),
myInCB(this, &Joydrive::in),
myOutCB(this, &Joydrive::out),
myCenterCB(this, &Joydrive::center)
{
// set the robot pointer and add the joydrive as a user task
myRobot = robot;
myRobot->addUserTask("joydrive", 50, &myDriveCB);
// initalize some variables
myRotValRobot = 100;
myTransValRobot = 700;
myPanValCamera = 8;
myTiltValCamera = 3;
myZoomValCamera = 50;
myLRAmount = LRAmount;
myUDAmount = UDAmount;
myZoomAmount = zoomAmount;
ArKeyHandler *keyHandler;
myRobot = robot;
// see if there is already a keyhandler, if not make one for ourselves
if ((keyHandler = Aria::getKeyHandler()) == NULL)
{
keyHandler = new ArKeyHandler;
Aria::setKeyHandler(keyHandler);
myRobot->attachKeyHandler(keyHandler);
}
// now that we have one, add our keys as callbacks, print out big
// warning messages if they fail
if (!keyHandler->addKeyHandler(ArKeyHandler::UP, &myUpCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for up, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::DOWN, &myDownCB))
ArLog::log(ArLog::Terse, "The key handler already has a key for down, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::LEFT, &myLeftCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for left, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler(ArKeyHandler::RIGHT, &myRightCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for right, keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('z', &myInCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'z', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('Z', &myInCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'Z', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('x', &myOutCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'x', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('X', &myOutCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'X', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('c', &myCenterCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'c', keydrive will not work correctly.");
if (!keyHandler->addKeyHandler('C', &myCenterCB))
ArLog::log(ArLog::Terse,
"The key handler already has a key for 'C', keydrive will not work correctly.");
// initialize the joystick
myJoyHandler.init();
// see if we have a joystick, and let the usre know the results
if (myJoyHandler.haveJoystick())
{
printf("Have a joystick\n\n");
}
// we don't have a joystic, so get out
else
{
printf("Do not have a joystick, only the keyboard will work.\n");
}
}
void ArMode::addKeyHandler(int keyToHandle, ArFunctor *functor)
{
ArKeyHandler *keyHandler;
std::string charStr;
// see if there is already a keyhandler, if not something is wrong
// (since constructor should make one if there isn't one yet
if ((keyHandler = Aria::getKeyHandler()) == NULL)
{
ArLog::log(ArLog::Terse,"ArMode '%s'::keyHandler: There should already be a key handler, but there isn't... mode won't work right.", getName());
return;
}
if (!keyHandler->addKeyHandler(keyToHandle, functor))
{
switch (keyToHandle) {
case ArKeyHandler::UP:
charStr = "Up";
break;
case ArKeyHandler::DOWN:
charStr = "Down";
break;
case ArKeyHandler::LEFT:
charStr = "Left";
break;
case ArKeyHandler::RIGHT:
charStr = "Right";
break;
case ArKeyHandler::ESCAPE:
charStr = "Escape";
break;
case ArKeyHandler::F1:
charStr = "F1";
break;
case ArKeyHandler::F2:
charStr = "F2";
break;
case ArKeyHandler::F3:
charStr = "F3";
break;
case ArKeyHandler::F4:
charStr = "F4";
break;
case ArKeyHandler::SPACE:
charStr = "Space";
break;
case ArKeyHandler::TAB:
charStr = "Tab";
break;
case ArKeyHandler::ENTER:
charStr = "Enter";
break;
case ArKeyHandler::BACKSPACE:
charStr = "Backspace";
break;
default:
charStr = (char)keyToHandle;
break;
}
ArLog::log(ArLog::Terse,
"ArMode '%s': The key handler has a duplicate key for '%s' so the mode may not work right.", getName(), charStr.c_str());
}
}
int main(int argc, char **argv)
{
desiredVels.resize(5);
puts("Connecting... press ctrl-c to cancel");
if(!arm.open())
{
puts("Error opening arm");
return 1;
}
puts("\nConnected to arm.\nPress spacebar or exit program (ESC) to stop arm.\n");
puts(" b/1/2 Select base joints (joint 1 rotation, joint 2 pivot)");
puts(" e/2/3 Select elbow joints (joint 3 pivot, joint 4 rotation)");
puts(" w/4/5 Select wrist joints (joint 5 pivot, joint 6 rotation)");
puts(" UP/DOWN Change pivot speed");
puts(" LEFT/RIGHT Change rotate speed");
puts(" SPACE Stop");
puts(" g/G Gripper");
puts("\npress ESC or q to exit program\n");
ArKeyHandler keys;
keys.addKeyHandler(ArKeyHandler::SPACE, new ArFunctorC<Arm>(&arm, &Arm::haltAll));
keys.addKeyHandler(ArKeyHandler::UP, new ArGlobalFunctor(&increasePivot));
keys.addKeyHandler(ArKeyHandler::DOWN, new ArGlobalFunctor(&decreasePivot));
keys.addKeyHandler(ArKeyHandler::LEFT, new ArGlobalFunctor(&decreaseRotate));
keys.addKeyHandler(ArKeyHandler::RIGHT, new ArGlobalFunctor(&increaseRotate));
keys.addKeyHandler('G', new ArGlobalFunctor(&increaseGrip));
keys.addKeyHandler('g', new ArGlobalFunctor(&decreaseGrip));
keys.addKeyHandler('b', new ArGlobalFunctor(&selectBase));
keys.addKeyHandler('e', new ArGlobalFunctor(&selectElbow));
keys.addKeyHandler('w', new ArGlobalFunctor(&selectWrist));
keys.addKeyHandler('1', new ArGlobalFunctor(&selectBase));
keys.addKeyHandler('2', new ArGlobalFunctor(&selectBase));
keys.addKeyHandler('3', new ArGlobalFunctor(&selectElbow));
keys.addKeyHandler('4', new ArGlobalFunctor(&selectElbow));
keys.addKeyHandler('5', new ArGlobalFunctor(&selectWrist));
keys.addKeyHandler('6', new ArGlobalFunctor(&selectWrist));
keys.addKeyHandler(ArKeyHandler::ESCAPE, new ArGlobalFunctor1<int>(&Aria::exit, 0));
keys.addKeyHandler('q', new ArGlobalFunctor1<int>(&Aria::exit, 0));
std::vector<float> pos;
int ctr = 0;
while(true)
{
ArUtil::sleep(20);
keys.checkKeys();
arm.sync();
if(DISPLAY_POSITIONS && ++ctr > 500)
{
pos = arm.getJointPositions();
for(std::vector<float>::const_iterator i = pos.begin(); i != pos.end(); ++i)
{
int n = (i - pos.begin());
if(n == selectedPivotJoint || n == selectedRotateJoint)
printf("[% 3.1f]", *i);
else
printf(" % 3.1f ", *i);
}
printf(" press ESC to exit, SPACE to halt arm\r");
ctr = 0;
}
}
return 0;
}
int main(int argc, char **argv)
{
/* Aria initialization: */
Aria::init();
//ArLog::init(ArLog::StdErr, ArLog::Verbose);
/* Create our client object. This is the object which connects with a remote
* server over the network, and which manages all of our communication with it
* once connected by sending data "requests". Requests may be sent once, or
* may be repeated at any frequency. Requests and replies to requsets contain
* payload "packets", into which various data types may be packed (when making a
* request), and from which they may also be extracted (when handling a reply).
* See the InputHandler and OutputHandler classes above for
* examples of making requests and reading/writing the data in packets.
*/
ArClientBase client;
/* Aria components use this to get options off the command line: */
ArArgumentParser parser(&argc, argv);
/* This will be used to connect our client to the server, including
* various bits of handshaking (e.g. sending a password, retrieving a list
* of data requests and commands...)
* It will get the hostname from the -host command line argument: */
ArClientSimpleConnector clientConnector(&parser);
parser.loadDefaultArguments();
/* Check for -help, and unhandled arguments: */
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
exit(0);
}
/* Connect our client object to the remote server: */
if (!clientConnector.connectClient(&client))
{
if (client.wasRejected())
printf("Server '%s' rejected connection, exiting\n", client.getHost());
else
printf("Could not connect to server '%s', exiting\n", client.getHost());
exit(1);
}
printf("Connected to server.\n");
client.setRobotName(client.getHost()); // include server name in log messages
/* Create a key handler and also tell Aria about it */
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
/* Global escape-key handler to shut everythnig down */
ArGlobalFunctor escapeCB(&escape);
keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);
/* Now that we're connected, we can run the client in a background thread,
* sending requests and receiving replies. When a reply to a request arrives,
* or the server makes a request of the client, a handler functor is invoked.
* The handlers for this program are registered with the client by the
* InputHandler and OutputHandler classes (in their constructors, above) */
client.runAsync();
/* Create the InputHandler object and request safe-drive mode */
InputHandler inputHandler(&client, &keyHandler);
inputHandler.safeDrive();
/* Use ArClientBase::dataExists() to see if the "ratioDrive" request is available on the
* currently connected server. */
if(!client.dataExists("ratioDrive") )
printf("Warning: server does not have ratioDrive command, can not use drive commands!\n");
else
printf("Keys are:\nUP: Forward\nDOWN: Backward\nLEFT: Turn Left\nRIGHT: Turn Right\n");
printf("s: Enable safe drive mode (if supported).\nu: Disable safe drive mode (if supported).\nl: list all data requests on server\n\nDrive commands use 'ratioDrive'.\nt: logs the network tracking tersely\nv: logs the network tracking verbosely\nr: resets the network tracking\n\n");
/* Create the OutputHandler object. It will begin printing out data from the
* server. */
OutputHandler outputHandler(&client);
/* Begin capturing keys into the key handler. Callbacks will be called
* asyncrosously from this main thread when pressed. */
/* While the client is still running (getRunningWithLock locks the "running"
* flag until it returns), check keys on the key handler (which will call
* our callbacks), then tell the input handler to send drive commands.
* Sleep a fraction of a second as well to avoid using
* too much CPU time, and give other threads time to work.
*/
while (client.getRunningWithLock())
{
keyHandler.checkKeys();
inputHandler.sendInput();
ArUtil::sleep(100);
}
/* The client stopped running, due to disconnection from the server, general
* Aria shutdown, or some other reason. */
client.disconnect();
Aria::shutdown();
return 0;
}
int main(int argc, char **argv)
{
// Initialize Aria and Arnl global information
/* Aria initialization: */
Aria::init();
ArLog::init(ArLog::File, ArLog::Verbose,"c:\\temp\\AmbifluxRobot.log",true);
ArLog::log(ArLog::Verbose, "Ambiflux Starting");
// Create the sound queue.
ArSoundsQueue soundQueue;
// Set WAV file callbacks
soundQueue.setPlayWavFileCallback(ArSoundPlayer::getPlayWavFileCallback());
soundQueue.setInterruptWavFileCallback(ArSoundPlayer::getStopPlayingCallback());
// Notifications when the queue goes empty or non-empty.
soundQueue.addQueueEmptyCallback(new ArGlobalFunctor(&queueNowEmpty));
soundQueue.addQueueNonemptyCallback(new ArGlobalFunctor(&queueNowNonempty));
// Run the sound queue in a new thread
soundQueue.runAsync();
/* Pool de messages en provenance de la tablette
Issu de l'implementation d'un modèle producteur/consommateur
pour les messages entrants. Plusieurs thread y accèdent
Tread-safe (mutex)*/
//Pool<Frame> messagePool;
/* Pool de messages en provenance d'un client TCP
Issu de l'implementation d'un modèle producteur/consommateur
pour les messages entrants. Plusieurs thread y accèdent
Tread-safe (mutex)*/
/*TODO : A remplacer par tcpReceivedPool */
//Pool<Frame> tcpMessagePool;
/* Pool de messages en provenance d'un client TCP
Issu de l'implementation d'un modèle producteur/consommateur
pour les messages entrants. Plusieurs thread y accèdent
Tread-safe (mutex)*/
Pool<TCPReceivedRequest> tcpReceivedPool;
/*Create our thread to communicate with iPad
Server start on port 7171 to receive requests from ipad
A client is created on port 7474 to request iPad
*/
//IhmCommunicationThread ihm(7171, &messagePool);
IhmCommunicationThread ihm(7171, &tcpReceivedPool);
//On s'abonne à la réception de message par la classe IhmCommunicationThread
//Todo : A supprimer ?
//ArGlobalFunctor1<Frame> functMessageReceived(&CallbackIhmReceived);
//ihm.setCallback(&functMessageReceived);
ihm.runAsync();
//soundQueue.play("c:\\temp\\let_me_out.wav");
//while(true);
/* Create our client object. This is the object which connects with a remote
* server over the network, and which manages all of our communication with it
* once connected by sending data "requests". Requests may be sent once, or
* may be repeated at any frequency. Requests and replies to requsets contain
* payload "packets", into which various data types may be packed (when making a
* request), and from which they may also be extracted (when handling a reply).
* See the InputHandler and OutputHandler classes above for
* examples of making requests and reading/writing the data in packets.
*/
ArClientBase client;
/* Aria components use this to get options off the command line: */
ArArgumentParser parser(&argc, argv);
/* This will be used to connect our client to the server, including
* various bits of handshaking (e.g. sending a password, retrieving a list
* of data requests and commands...)
* It will get the hostname from the -host command line argument: */
ArClientSimpleConnector clientConnector(&parser);
parser.loadDefaultArguments();
/* Check for -help, and unhandled arguments: */
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
exit(0);
}
/* Connect our client object to the remote server: */
if (!clientConnector.connectClient(&client))
{
if (client.wasRejected())
printf("Server '%s' rejected connection, exiting\n", client.getHost());
else
printf("Could not connect to server '%s', exiting\n", client.getHost());
exit(1);
}
printf("Connected to server.\n");
client.setRobotName(client.getHost()); // include server name in log messages
///* Create a key handler and also tell Aria about it */
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
/* Global escape-key handler to shut everythnig down */
ArGlobalFunctor escapeCB(&escape);
keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);
/* Now that we're connected, we can run the client in a background thread,
* sending requests and receiving replies. When a reply to a request arrives,
* or the server makes a request of the client, a handler functor is invoked.
* The handlers for this program are registered with the client by the
* InputHandler and OutputHandler classes (in their constructors, above) */
client.runAsync();
///* Create the InputHandler object and request safe-drive mode */
//InputHandler inputHandler(&client);
//inputHandler.gotoGoal("215");
////inputHandler.safeDrive();
// Mode goto
if(!client.dataExists("gotoGoal") )
printf("Warning: Pas de mode goto!\n");
else
printf("Mode goto accepte");
//ArFunctor1<ArNetPacket*>
//client.addHandler("pathPlannerStatus",);
/* Create the OutputHandler object. It will begin printing out data from the
* server. */
OutputHandler outputHandler(&client);
//On s'abonne à la réception de message par la classe IhmCommunicationThread
//Todo : A supprimer ?
//ArGlobalFunctor1<Frame> functMessageReceived(&CallbackIhmReceived);
//ihm.setCallback(&functMessageReceived);
//ihm.runAsync();
//pour tester IHM
// ArUtil::sleep(1000);
// ihm.testCommunication();
//SRMA object
string strSRMA = DALRest::getResourceById("9");
SRMA srma(strSRMA,client, outputHandler, ihm, &soundQueue);
//Loop du mode Ambiant
MainLoop myLoop(srma, &tcpReceivedPool);
myLoop.runAsync();
//Thread loop : TCP commands
//Produces messages in tcpMessagePool
//ServerLoop myServerLoop(srma, &tcpReceivedPool);
//myServerLoop.runAsync();
//Traitement des requetes TCP
//Consulmes messages in tcpMessagePool
//TCPRequestsLoop myTCPRequestsLoop(srma, &tcpReceivedPool);
//myTCPRequestsLoop.runAsync();
/* While the client is still running (getRunningWithLock locks the "running"
* flag until it returns), check keys on the key handler (which will call
* our callbacks), then tell the input handler to send drive commands.
* Sleep a fraction of a second as well to avoid using
* too much CPU time, and give other threads time to work.
*/
while (client.getRunningWithLock())
{
//keyHandler.checkKeys();
//inputHandler.sendInput();
ArUtil::sleep(100);
}
/* The client stopped running, due to disconnection from the server, general
* Aria shutdown, or some other reason. */
client.disconnect();
Aria::shutdown();
return 0;
}
int main(int argc, char** argv)
{
Aria::init();
ArArgumentParser argParser(&argc, argv);
ArSimpleConnector con(&argParser);
ArRobot robot;
ArSonarDevice sonar;
argParser.loadDefaultArguments();
if(!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
return 1;
}
/* - the action group for teleoperation actions: */
teleop = new ArActionGroup(&robot);
// don't hit any tables (if the robot has IR table sensors)
teleop->addAction(new ArActionLimiterTableSensor, 100);
// limiter for close obstacles
teleop->addAction(new ArActionLimiterForwards("speed limiter near",
300, 600, 250), 95);
// limiter for far away obstacles
teleop->addAction(new ArActionLimiterForwards("speed limiter far",
300, 1100, 400), 90);
// limiter so we don't bump things backwards
teleop->addAction(new ArActionLimiterBackwards, 85);
// the joydrive action (drive from joystick)
ArActionJoydrive joydriveAct("joydrive", 400, 15);
teleop->addAction(&joydriveAct, 50);
// the keydrive action (drive from keyboard)
teleop->addAction(new ArActionKeydrive, 45);
/* - the action group for wander actions: */
wander = new ArActionGroup(&robot);
// if we're stalled we want to back up and recover
wander->addAction(new ArActionStallRecover, 100);
// react to bumpers
wander->addAction(new ArActionBumpers, 75);
// turn to avoid things closer to us
wander->addAction(new ArActionAvoidFront("Avoid Front Near", 225, 0), 50);
// turn avoid things further away
wander->addAction(new ArActionAvoidFront, 45);
// keep moving
wander->addAction(new ArActionConstantVelocity("Constant Velocity", 400), 25);
/* - use key commands to switch modes, and use keyboard
* and joystick as inputs for teleoperation actions. */
// create key handler if Aria does not already have one
ArKeyHandler *keyHandler = Aria::getKeyHandler();
if (keyHandler == NULL)
{
keyHandler = new ArKeyHandler;
Aria::setKeyHandler(keyHandler);
robot.attachKeyHandler(keyHandler);
}
// set the callbacks
ArGlobalFunctor teleopCB(&teleopMode);
ArGlobalFunctor wanderCB(&wanderMode);
keyHandler->addKeyHandler('w', &wanderCB);
keyHandler->addKeyHandler('W', &wanderCB);
keyHandler->addKeyHandler('t', &teleopCB);
keyHandler->addKeyHandler('T', &teleopCB);
// if we don't have a joystick, let 'em know
if (!joydriveAct.joystickInited())
printf("Note: Do not have a joystick, only the arrow keys on the keyboard will work.\n");
// set the joystick so it won't do anything if the button isn't pressed
joydriveAct.setStopIfNoButtonPressed(false);
/* - connect to the robot, then enter teleoperation mode. */
robot.addRangeDevice(&sonar);
if(!con.connectRobot(&robot))
{
ArLog::log(ArLog::Terse, "actionGroupExample: Could not connect to the robot.");
Aria::shutdown();
return 1;
}
robot.enableMotors();
teleopMode();
robot.run(true);
Aria::shutdown();
return 0;
}
int main(int argc, char **argv)
{
Aria::init();
ArSimpleConnector connector(&argc, argv);
ArRobot robot;
ArSick sick;
if (!Aria::parseArgs() || argc > 1)
{
Aria::logOptions();
Aria::exit(1); // exit program with error code 1
return 1;
}
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
robot.addRangeDevice(&sick);
// Create the ArLineFinder object. Set it to log lots of information about its
// processing.
ArLineFinder lineFinder(&sick);
lineFinder.setVerbose(true);
// Add key callbacks that simply call the ArLineFinder::getLinesAndSaveThem()
// function, which searches for lines in the current set of laser sensor
// readings, and saves them in files with the names 'points' and 'lines'.
ArFunctorC<ArLineFinder> findLineCB(&lineFinder,
&ArLineFinder::getLinesAndSaveThem);
keyHandler.addKeyHandler('f', &findLineCB);
keyHandler.addKeyHandler('F', &findLineCB);
ArLog::log(ArLog::Normal, "lineFinderExample: connecting to robot...");
if (!connector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::exit(1); // exit program with error code 1
return 1;
}
robot.runAsync(true);
// now set up the laser
ArLog::log(ArLog::Normal, "lineFinderExample: connecting to SICK laser...");
connector.setupLaser(&sick);
sick.runAsync();
if (!sick.blockingConnect())
{
printf("Could not connect to SICK laser... exiting\n");
Aria::exit(1);
return 1;
}
printf("If you press the 'f' key the points and lines found will be saved\n");
printf("Into the 'points' and 'lines' file in the current working directory\n");
robot.waitForRunExit();
Aria::exit(0);
return 0;
}