// the important function
void KeyPTU::drive(void)
{
// if the PTU isn't initialized, initialize it here... it has to be
// done here instead of above because it needs to be done when the
// robot is connected
if (!myPTUInitRequested && !myPTU.isInitted() && myRobot->isConnected())
{
printf("\nWaiting for Camera to Initialize\n");
myAbsolute = true;
myPTUInitRequested = true;
myPTU.init();
}
// if the camera hasn't initialized yet, then just return
if (myPTUInitRequested && !myPTU.isInitted())
{
return;
}
if (myPTUInitRequested && myPTU.isInitted())
{
myPTUInitRequested = false;
myPanSlew = myPTU.getPanSlew();
myTiltSlew = myPTU.getTiltSlew();
printf("Done.\n");
question();
}
if (myExerciseTime.secSince() > 5 && myExercise)
{
int pan,tilt;
if (ArMath::random()%2)
pan = ArMath::random()%((int)myPTU.getMaxPosPan());
else
pan = -ArMath::random()%((int)myPTU.getMaxNegPan());
if (ArMath::random()%2)
tilt = ArMath::random()%((int)myPTU.getMaxPosTilt());
else
tilt = -ArMath::random()%((int)myPTU.getMaxNegTilt());
myPTU.panTilt(pan, tilt);
//printf("** %d\n", myRobot->getEstop());
//printf("--> %x\n", myRobot->getFlags());
myExerciseTime.setToNow();
}
}
// this is the function called in the new thread
void *Joydrive::runThread(void *arg)
{
threadStarted();
int trans, rot;
// only run while running, ie play nice and pay attention to the thread
//being shutdown
while (myRunning)
{
// lock the robot before touching it
myRobot->lock();
if (!myRobot->isConnected())
{
myRobot->unlock();
break;
}
// print out some information about the robot
printf("\rx %6.1f y %6.1f tth %6.1f vel %7.1f mpacs %3d ",
myRobot->getX(), myRobot->getY(), myRobot->getTh(),
myRobot->getVel(), myRobot->getMotorPacCount());
fflush(stdout);
// if one of the joystick buttons is pushed, drive the robot
if (myJoyHandler.haveJoystick() && (myJoyHandler.getButton(1) ||
myJoyHandler.getButton(2)))
{
// get out the values from the joystick
myJoyHandler.getAdjusted(&rot, &trans);
// drive the robot
myRobot->setVel(trans);
myRobot->setRotVel(-rot);
}
// if no buttons are pushed stop the robot
else
{
myRobot->setVel(0);
myRobot->setRotVel(0);
}
// unlock the robot, so everything else can run
myRobot->unlock();
// now take a little nap
ArUtil::sleep(50);
}
// return out here, means the thread is done
return NULL;
}
int main(int argc, char *argv[])
{
// Initialize location of Aria, Arnl and their args.
Aria::init();
Arnl::init();
// The robot object
ArRobot robot;
// Parse the command line arguments.
ArArgumentParser parser(&argc, argv);
// Read data_index if exists
int data_index;
bool exist_data_index;
parser.checkParameterArgumentInteger("dataIndex",&data_index,&exist_data_index);
// Load default arguments for this computer (from /etc/Aria.args, environment
// variables, and other places)
parser.loadDefaultArguments();
// Object that connects to the robot or simulator using program options
ArRobotConnector robotConnector(&parser, &robot);
// set up a gyro
ArAnalogGyro gyro(&robot);
ArLog::init(ArLog::File,ArLog::Normal,"run.log",false,true,true);
// Parse arguments for the simple connector.
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
ArLog::log(ArLog::Normal, "\nUsage: %s -map mapfilename\n", argv[0]);
Aria::logOptions();
Aria::exit(1);
}
// Collision avoidance actions at higher priority
ArActionAvoidFront avoidAction("avoid",200);
ArActionLimiterForwards limiterAction("speed limiter near", 150, 500, 150);
ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400);
ArActionLimiterTableSensor tableLimiterAction;
//robot.addAction(&tableLimiterAction, 100);
//robot.addAction(&avoidAction,100);
//robot.addAction(&limiterAction, 95);
//robot.addAction(&limiterFarAction, 90);
// Goto action at lower priority
ArActionGoto gotoPoseAction("goto");
//robot.addAction(&gotoPoseAction, 50);
gotoPoseAction.setCloseDist(750);
// Stop action at lower priority, so the robot stops if it has no goal
ArActionStop stopAction("stop");
//robot.addAction(&stopAction, 40);
// Connect the robot
if (!robotConnector.connectRobot())
{
ArLog::log(ArLog::Normal, "Could not connect to robot... exiting");
Aria::exit(3);
}
if(!robot.isConnected())
{
ArLog::log(ArLog::Terse, "Internal error: robot connector succeeded but ArRobot::isConnected() is false!");
}
// Connector for laser rangefinders
ArLaserConnector laserConnector(&parser, &robot, &robotConnector);
// Sonar, must be added to the robot, used by teleoperation and wander to
// detect obstacles, and for localization if SONARNL
ArSonarDevice sonarDev;
// Add the sonar to the robot
robot.addRangeDevice(&sonarDev);
// Start the robot thread.
robot.runAsync(true);
// Connect to the laser(s) if lasers were configured in this robot's parameter
// file or on the command line, and run laser processing thread if applicable
// for that laser class. For the purposes of this demo, add all
// possible lasers to ArRobot's list rather than just the ones that were
// connected by this call so when you enter laser mode, you
// can then interactively choose which laser to use from that list of all
// lasers mentioned in robot parameters and on command line. Normally,
// only connected lasers are put in ArRobot's list.
if (!laserConnector.connectLasers(
false, // continue after connection failures
true, // add only connected lasers to ArRobot
true // add all lasers to ArRobot
))
{
ArLog::log(ArLog::Normal ,"Could not connect to lasers... exiting\n");
Aria::exit(2);
}
// Puntero a laser
ArSick* sick=(ArSick*)robot.findLaser(1);
// Conectamos el laser
sick->asyncConnect();
//Esperamos a que esté encendido
while(!sick->isConnected())
{
ArUtil::sleep(100);
}
ArLog::log(ArLog::Normal ,"Laser conectado\n");
// Set up things so data can be logged (only do it with the laser
// since it can overrun a 9600 serial connection which the sonar is
// more likely to have)
ArDataLogger dataLogger(&robot);
dataLogger.addToConfig(Aria::getConfig());
// add our logging to the config
//ArLog::addToConfig(Aria::getConfig());
// Set up a class that'll put the movement and gyro parameters into ArConfig
ArRobotConfig robotConfig(&robot);
robotConfig.addAnalogGyro(&gyro);
// Add additional range devices to the robot and path planning task.
// IRs if the robot has them.
robot.lock();
ArIRs irs;
robot.addRangeDevice(&irs);
// Bumpers.
ArBumpers bumpers;
robot.addRangeDevice(&bumpers);
// cause the sonar to turn off automatically
// when the robot is stopped, and turn it back on when commands to move
// are sent. (Note, this should not be done if you need the sonar
// data to localize, or for other purposes while stopped)
ArSonarAutoDisabler sonarAutoDisabler(&robot);
// Read in parameter files.
Aria::getConfig()->useArgumentParser(&parser);
if (!Aria::getConfig()->parseFile(Arnl::getTypicalParamFileName()))
{
ArLog::log(ArLog::Normal, "Trouble loading configuration file, exiting");
Aria::exit(5);
}
//Configuracion del laser
sick->setMinDistBetweenCurrent(0);
robot.enableMotors();
robot.setAbsoluteMaxTransVel(1000);
/* Finally, get ready to run the robot: */
robot.unlock();
Controlador driver(&robot);
if(exist_data_index){
driver.setDataIndex(data_index);
}
driver.runAsync();
ControlHandler handler(&driver,&robot);
// Use manual key handler
//handler.addKeyHandlers(&robot);
robot.addSensorInterpTask("ManualKeyHandler",50,handler.getFunctor());
ArLog::log(ArLog::Normal ,"Añadido manejador teclado\n");
// double x,y,dist,angle;
// ArPose punto;
// ArPose origen=robot.getPose();
// sick->lockDevice();
// for(int i=-90;i<90;i++){
// // Obtengo la medida de distancia y angulo
// dist=sick->currentReadingPolar(i,i+1,&angle);
// // Obtengo coordenadas del punto usando el laser como referencia
// x=dist*ArMath::cos(angle);
// y=dist*ArMath::sin(angle);
// //Roto los puntos
// ArMath::pointRotate(&x,&y,-origen.getTh());
// punto.setX(x);
// punto.setY(y);
// punto=punto + origen;
// printf("Medida: %d\t Angulo:%.2f\t Angulo:%.2f\t Distancia:%0.2f\t X:%0.2f\t Y:%0.2f\n",i,angle,angle+origen.getTh(),dist,punto.getX(),punto.getY());
// }
// printf("Medidas adquiridas\n");
// sick->unlockDevice();
robot.waitForRunExit();
//ArUtil::sleep(10000);
return 0;
}
int main(int argc, char **argv)
{
std::string str;
int ret;
int successes = 0, failures = 0;
int action;
bool exitOnFailure = true;
ArSerialConnection con;
ArRobot robot;
//ArLog::init(ArLog::StdOut, ArLog::Verbose);
srand(time(NULL));
robot.runAsync(false);
// if (!exitOnFailure)
// ArLog::init(ArLog::None, ArLog::Terse);
//else
//ArLog::init(ArLog::None);
while (1)
{
if (con.getStatus() != ArDeviceConnection::STATUS_OPEN &&
(ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
++failures;
if (exitOnFailure)
{
printf("Failed\n");
exit(0);
}
else
{
ArUtil::sleep(200);
robot.unlock();
continue;
}
}
robot.lock();
robot.setDeviceConnection(&con);
robot.unlock();
ArUtil::sleep((rand() % 5) * 100);
if (robot.asyncConnect())
{
robot.waitForConnectOrConnFail();
robot.lock();
if (!robot.isConnected())
{
if (exitOnFailure)
{
printf("Failed after %d tries.\n", successes);
exit(0);
}
printf("Failed to connect successfully");
++failures;
}
robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::SOUNDTOG, 0);
//robot.comInt(ArCommands::PLAYLIST, 0);
robot.comInt(ArCommands::ENCODER, 1);
ArUtil::sleep(((rand() % 20) + 3) * 100);
++successes;
// okay, now try to leave it in a messed up state
action = rand() % 8;
robot.dropConnection();
switch (action) {
case 0:
printf("Discon 0 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(0);
break;
case 1:
printf("Discon 1 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(0);
ArUtil::sleep(100);
robot.com(1);
break;
case 2:
printf("Discon 2 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(0);
ArUtil::sleep(100);
robot.com(1);
ArUtil::sleep(100);
robot.com(2);
break;
case 3:
printf("Discon 10 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(10);
break;
case 4:
printf("Discon ");
robot.disconnect();
break;
default:
printf("Leave ");
break;
}
robot.unlock();
}
else
{
if (exitOnFailure)
{
printf("Failed after %d tries.\n", successes);
exit(0);
}
printf("Failed to start connect ");
++failures;
}
if ((rand() % 2) == 0)
{
printf(" ! RadioDisconnect ! ");
con.write("|||\15", strlen("!!!\15"));
ArUtil::sleep(100);
con.write("WMD\15", strlen("WMD\15"));
ArUtil::sleep(200);
}
if ((rand() % 2) == 0)
{
printf(" ! ClosePort !\n");
con.close();
}
else
printf("\n");
printf("#### %d successes %d failures, %% %.2f success\n", successes, failures,
(float)successes/(float)(successes+failures)*100);
ArUtil::sleep((rand() % 2)* 1000);
}
return 0;
}
// the important function
void KeyPTU::drive(void)
{
// if the PTU isn't initialized, initialize it here... it has to be
// done here instead of above because it needs to be done when the
// robot is connected
if (!myPTUInited && myRobot->isConnected())
{
myPTU.init();
myPTU.resetCalib();
myPTU.awaitExec();
myPTU.regStatPower();
myPTU.regMotPower();
mySlew = myPTU.getPanSlew(); //uses only pan slew rate
myPTU.awaitExec();
myPTUInited = true;
myInit = false;
myAbsolute = true;
}
if (myInit == true)
{
myPTU.init();
myInit = false;
myDesiredPanPos = myPTU.getPan();
myDesiredTiltPos = myPTU.getTilt();
mySlew = myPTU.getPanSlew(); //uses only pan slew rate
myReset = false;
}
if (myReset == true)
{
myPTU.resetCalib();
myPTU.awaitExec();
myDesiredPanPos = myPTU.getPan();
myDesiredTiltPos = myPTU.getTilt();
myReset = false;
}
else
{
if (myDesiredPanPos != myPTU.getPan())
{
if (myAbsolute)
myPTU.pan(myDesiredPanPos);
else
myPTU.panRel(myDesiredPanPos - myPTU.getPan());
}
if (myDesiredTiltPos != myPTU.getTilt())
{
if (myAbsolute)
myPTU.tilt(myDesiredTiltPos);
else
myPTU.tiltRel(myDesiredTiltPos - myPTU.getTilt());
}
if (mySlew != myPTU.getPanSlew())
{
myPTU.panSlew(mySlew);
myPTU.tiltSlew(mySlew);
}
}
}
int main(int argc, char **argv)
{
Aria::init();
ArRobot robot;
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobotConnector robotConnector(&parser, &robot);
ArLaserConnector laserConnector(&parser, &robot, &robotConnector);
// Connect to the robot, get some initial data from it such as type and name,
// and then load parameter files for this robot.
if(!robotConnector.connectRobot())
{
ArLog::log(ArLog::Terse, "lasersExample: Could not connect to the robot.");
if(parser.checkHelpAndWarnUnparsed())
{
// -help not given
Aria::logOptions();
Aria::exit(1);
}
}
if (!Aria::parseArgs())
{
Aria::logOptions();
Aria::exit(2);
return 2;
}
ArLog::log(ArLog::Normal, "lasersExample: Connected to robot.");
// Start the robot processing cycle running in the background.
// True parameter means that if the connection is lost, then the
// run loop ends.
robot.runAsync(true);
// 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;
}
// Allow some time to read laser data
ArUtil::sleep(500);
ArLog::log(ArLog::Normal, "Connected to all lasers.");
// Print out some data from each connected laser.
while(robot.isConnected())
{
int numLasers = 0;
// Get a pointer to ArRobot's list of connected lasers. We will lock the robot while using it to prevent changes by tasks in the robot's background task thread or any other threads. Each laser has an index. You can also store the laser's index or name (laser->getName()) and use that to get a reference (pointer) to the laser object using ArRobot::findLaser().
robot.lock();
std::map<int, ArLaser*> *lasers = robot.getLaserMap();
for(std::map<int, ArLaser*>::const_iterator i = lasers->begin(); i != lasers->end(); ++i)
{
int laserIndex = (*i).first;
ArLaser* laser = (*i).second;
if(!laser)
continue;
++numLasers;
laser->lockDevice();
// The current readings are a set of obstacle readings (with X,Y positions as well as other attributes) that are the most recent set from teh laser.
std::list<ArPoseWithTime*> *currentReadings = laser->getCurrentBuffer(); // see ArRangeDevice interface doc
// The raw readings are just range or other data supplied by the sensor. It may also include some device-specific extra values associated with each reading as well. (e.g. Reflectance for LMS200)
const std::list<ArSensorReading*> *rawReadings = laser->getRawReadings();
// There is a utility to find the closest reading wthin a range of degrees around the laser, here we use this laser's full field of view (start to end)
// If there are no valid closest readings within the given range, dist will be greater than laser->getMaxRange().
double angle = 0;
double dist = laser->currentReadingPolar(laser->getStartDegrees(), laser->getEndDegrees(), &angle);
ArLog::log(ArLog::Normal, "Laser #%d (%s): %s.\n\tHave %d 'current' readings.\n\tHave %d 'raw' readings.\n\tClosest reading is at %3.0f degrees and is %2.4f meters away.",
laserIndex, laser->getName(), (laser->isConnected() ? "connected" : "NOT CONNECTED"),
currentReadings->size(),
rawReadings->size(),
angle, dist/1000.0);
laser->unlockDevice();
}
if(numLasers == 0)
ArLog::log(ArLog::Normal, "No lasers.");
else
ArLog::log(ArLog::Normal, "");
// Unlock robot and sleep for 5 seconds before next loop.
robot.unlock();
ArUtil::sleep(5000);
}
ArLog::log(ArLog::Normal, "lasersExample: exiting.");
Aria::exit(0);
return 0;
}
int main(int argc, char** argv)
{
// Initialize some global data
Aria::init();
// If you want ArLog to print "Verbose" level messages uncomment this:
//ArLog::init(ArLog::StdOut, ArLog::Verbose);
// This object parses program options from the command line
ArArgumentParser parser(&argc, argv);
// Load some default values for command line arguments from /etc/Aria.args
// (Linux) or the ARIAARGS environment variable.
parser.loadDefaultArguments();
// Central object that is an interface to the robot and its integrated
// devices, and which manages control of the robot by the rest of the program.
ArRobot robot;
// Object that connects to the robot or simulator using program options
ArRobotConnector robotConnector(&parser, &robot);
// If the robot has an Analog Gyro, this object will activate it, and
// if the robot does not automatically use the gyro to correct heading,
// this object reads data from it and corrects the pose in ArRobot
ArAnalogGyro gyro(&robot);
// Connect to the robot, get some initial data from it such as type and name,
// and then load parameter files for this robot.
if (!robotConnector.connectRobot())
{
// Error connecting:
// if the user gave the -help argumentp, then just print out what happened,
// and continue so options can be displayed later.
if (!parser.checkHelpAndWarnUnparsed())
{
ArLog::log(ArLog::Terse, "Could not connect to robot, will not have parameter file so options displayed later may not include everything");
}
// otherwise abort
else
{
ArLog::log(ArLog::Terse, "Error, could not connect to robot.");
Aria::logOptions();
Aria::exit(1);
}
}
if(!robot.isConnected())
{
ArLog::log(ArLog::Terse, "Internal error: robot connector succeeded but ArRobot::isConnected() is false!");
}
// Connector for laser rangefinders
ArLaserConnector laserConnector(&parser, &robot, &robotConnector);
// Connector for compasses
ArCompassConnector compassConnector(&parser);
// Parse the command line options. Fail and print the help message if the parsing fails
// or if the help was requested with the -help option
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
return 1;
}
// Used to access and process sonar range data
ArSonarDevice sonarDev;
// Used to perform actions when keyboard keys are pressed
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
// ArRobot contains an exit action for the Escape key. It also
// stores a pointer to the keyhandler so that other parts of the program can
// use the same keyhandler.
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");
// Attach sonarDev to the robot so it gets data from it.
robot.addRangeDevice(&sonarDev);
// Start the robot task loop running in a new background thread. The 'true' argument means if it loses
// connection the task loop stops and the thread exits.
robot.runAsync(true);
// Connect to the laser(s) if lasers were configured in this robot's parameter
// file or on the command line, and run laser processing thread if applicable
// for that laser class. For the purposes of this demo, add all
// possible lasers to ArRobot's list rather than just the ones that were
// connected by this call so when you enter laser mode, you
// can then interactively choose which laser to use from that list of all
// lasers mentioned in robot parameters and on command line. Normally,
// only connected lasers are put in ArRobot's list.
if (!laserConnector.connectLasers(
false, // continue after connection failures
false, // add only connected lasers to ArRobot
true // add all lasers to ArRobot
))
{
printf("Could not connect to lasers... exiting\n");
Aria::exit(2);
}
/* not needed, robot connector will do it by default
if (!sonarConnector.connectSonars(
false, // continue after connection failures
false, // add only connected lasers to ArRobot
true // add all lasers to ArRobot
))
{
printf("Could not connect to sonars... exiting\n");
Aria::exit(2);
}
*/
// Create and connect to the compass if the robot has one.
ArTCM2 *compass = compassConnector.create(&robot);
if(compass && !compass->blockingConnect()) {
compass = NULL;
}
// Sleep for a second so some messages from the initial responses
// from robots and cameras and such can catch up
ArUtil::sleep(1000);
// We need to lock the robot since we'll be setting up these modes
// while the robot task loop thread is already running, and they
// need to access some shared data in ArRobot.
robot.lock();
// now add all the modes for this demo
// these classes are defined in ArModes.cpp in ARIA's source code.
if(robot.getOrigRobotConfig()->getHasGripper())
new ArModeGripper(&robot, "gripper", 'g', 'G');
else
ArLog::log(ArLog::Normal, "Robot does not indicate that it has a gripper.");
ArModeActs actsMode(&robot, "acts", 'a', 'A');
ArModeTCM2 tcm2(&robot, "tcm2", 'm', 'M', compass);
ArModeIO io(&robot, "io", 'i', 'I');
ArModeConfig cfg(&robot, "report robot config", 'o' , 'O');
ArModeCommand command(&robot, "command", 'd', 'D');
ArModeCamera camera(&robot, "camera", 'c', 'C');
ArModePosition position(&robot, "position", 'p', 'P', &gyro);
ArModeSonar sonar(&robot, "sonar", 's', 'S');
ArModeBumps bumps(&robot, "bumps", 'b', 'B');
ArModeLaser laser(&robot, "laser", 'l', 'L');
ArModeWander wander(&robot, "wander", 'w', 'W');
ArModeUnguardedTeleop unguardedTeleop(&robot, "unguarded teleop", 'u', 'U');
ArModeTeleop teleop(&robot, "teleop", 't', 'T');
// activate the default mode
teleop.activate();
// turn on the motors
robot.comInt(ArCommands::ENABLE, 1);
robot.unlock();
// Block execution of the main thread here and wait for the robot's task loop
// thread to exit (e.g. by robot disconnecting, escape key pressed, or OS
// signal)
robot.waitForRunExit();
Aria::exit(0);
return 0;
}
// the important function
void KeyPTU::drive(void)
{
// if the PTU isn't initialized, initialize it here... it has to be
// done here instead of above because it needs to be done when the
// robot is connected
if (!myPTUInited && myRobot->isConnected())
{
ArLog::log(ArLog::Normal, "Initializing ArDPPTU...");
myPTU.init();
ArLog::log(ArLog::Normal, "Resetting PTU and performing self-calibration...");
myPTU.resetCalib();
myPTU.awaitExec(); // DPPTU will wait for self-calibration to end before executing the following commands (though they will still be sent)
mySlew = myPTU.getPanSlew(); //uses only pan slew rate
myPTU.awaitExec();
myPTUInited = true;
myInit = false;
myAbsolute = true;
}
if (myInit == true) // User hit initialization key
{
ArLog::log(ArLog::Normal, "Initializing PTU...");
myPTU.init();
myInit = false;
myDesiredPanPos = myPTU.getPan();
myDesiredTiltPos = myPTU.getTilt();
mySlew = myPTU.getPanSlew(); //uses only pan slew rate
myReset = false;
}
if (myReset == true) // User hit reset key
{
ArLog::log(ArLog::Normal, "Resetting PTU and performing self-calibration...");
myPTU.resetCalib();
myPTU.awaitExec();
myDesiredPanPos = myPTU.getPan();
myDesiredTiltPos = myPTU.getTilt();
myReset = false;
}
else // User did nothing, or hit a key that changed myDesiredPanPos, myDesiredTiltPos, or mySlew (so request PTU to move if those changed since last request)
{
// Some PTUs can determine their current position (with encoders, etc) and return that.
// canGetRealPanTilt() will return true in this case, and getPan() and
// getTilt() will return those received values. Otherwise, getPan() and
// getTilt() return the last commanded values. getLastPanRequest() and
// getLastTiltRequest() will always return the last commanded values sent by
// ArDPPTU (so in the case that canGetRealPanTilt() is false, getPan() and
// getTilt() return the same pair of values as getLastPanRequest() and
// getLastTiltRequest(). ArDPPTU::canGetRealPanTilt() is initialally false,
// but once the first set of pan and tilt positions is read back from the
// PTU device, it becomes true.
if(myPTU.canGetRealPanTilt())
printf("Position (%.1f deg, %.1f deg) [Incr. %d deg] Press ? for help \r", myPTU.getPan(), myPTU.getTilt(), myPosIncrement);
else
printf("Requested (%.1f deg, %.1f deg) [Incr. %d deg] Press ? for help \r", myPTU.getPan(), myPTU.getTilt(), myPosIncrement);
if (myDesiredPanPos != myPTU.getLastPanRequest())
{
if (myAbsolute)
myPTU.pan(myDesiredPanPos);
else
myPTU.panRel(myDesiredPanPos - myPTU.getPan());
}
if (myDesiredTiltPos != myPTU.getLastTiltRequest())
{
if (myAbsolute)
myPTU.tilt(myDesiredTiltPos);
else
myPTU.tiltRel(myDesiredTiltPos - myPTU.getTilt());
}
if (mySlew != myPTU.getPanSlew())
{
myPTU.panSlew(mySlew);
myPTU.tiltSlew(mySlew);
}
}
}
void Joydrive::drive(void)
{
int trans, rot;
ArPose pose;
ArPose rpose;
ArTransform transform;
ArRangeDevice *dev;
ArSensorReading *son;
if (!myRobot->isConnected())
{
printf("Lost connection to the robot, exiting\n");
exit(0);
}
printf("\rx %6.1f y %6.1f th %6.1f",
myRobot->getX(), myRobot->getY(), myRobot->getTh());
fflush(stdout);
if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1))
{
if (ArMath::fabs(myRobot->getVel()) < 10.0)
myRobot->comInt(ArCommands::ENABLE, 1);
myJoyHandler.getAdjusted(&rot, &trans);
myRobot->setVel(trans);
myRobot->setRotVel(-rot);
}
else
{
myRobot->setVel(0);
myRobot->setRotVel(0);
}
if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(2) &&
time(NULL) - myLastPress > 1)
{
myLastPress = time(NULL);
printf("\n");
switch (myTest)
{
case 1:
printf("Moving back to the origin.\n");
pose.setPose(0, 0, 0);
myRobot->moveTo(pose);
break;
case 2:
printf("Moving over a meter.\n");
pose.setPose(myRobot->getX() + 1000, myRobot->getY(), 0);
myRobot->moveTo(pose);
break;
case 3:
printf("Doing a transform test....\n");
printf("\nOrigin should be transformed to the robots coords.\n");
transform = myRobot->getToGlobalTransform();
pose.setPose(0, 0, 0);
pose = transform.doTransform(pose);
rpose = myRobot->getPose();
printf("Pos: ");
pose.log();
printf("Robot: ");
rpose.log();
if (pose.findDistanceTo(rpose) < .1)
printf("Success\n");
else
printf("#### FAILURE\n");
printf("\nRobot coords should be transformed to the origin.\n");
transform = myRobot->getToLocalTransform();
pose = myRobot->getPose();
pose = transform.doTransform(pose);
rpose.setPose(0, 0, 0);
printf("Pos: ");
pose.log();
printf("Robot: ");
rpose.log();
if (pose.findDistanceTo(rpose) < .1)
printf("Success\n");
else
printf("#### FAILURE\n");
break;
case 4:
printf("Doing a tranform test...\n");
printf("A point 1 meter to the -x from the robot (in local coords) should be transformed into global coordinates.\n");
transform = myRobot->getToGlobalTransform();
pose.setPose(-1000, 0, 0);
pose = transform.doTransform(pose);
rpose = myRobot->getPose();
printf("Pos: ");
pose.log();
printf("Robot: ");
rpose.log();
if (ArMath::fabs(pose.findDistanceTo(rpose) - 1000.0) < .1)
printf("Probable Success\n");
else
printf("#### FAILURE\n");
break;
case 5:
printf("Doing a transform test on range devices..\n");
printf("Moving the robot +4 meters x and +4 meters y and seeing if the moveTo will move the sonar readings along with it.\n");
dev = myRobot->findRangeDevice("sonar");
if (dev == NULL)
{
printf("No sonar on the robot, can't do the test.\n");
break;
}
printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
printf("Sonar 0 reading is at ");
son = myRobot->getSonarReading(0);
if (son != NULL)
{
pose = son->getPose();
pose.log();
}
pose = myRobot->getPose();
pose.setX(pose.getX() + 4000);
pose.setY(pose.getY() + 4000);
myRobot->moveTo(pose);
printf("Moved robot.\n");
printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
printf("Sonar 0 reading is at ");
son = myRobot->getSonarReading(0);
if (son != NULL)
{
pose = son->getPose();
pose.log();
}
break;
case 6:
printf("Robot position now is:\n");
pose = myRobot->getPose();
pose.log();
printf("Disconnecting from the robot, then reconnecting.\n");
myRobot->disconnect();
myRobot->blockingConnect();
printf("Robot position now is:\n");
pose = myRobot->getPose();
pose.log();
break;
default:
printf("No test for second button.\n");
break;
}
}
}