This program creates two action groups, a teleoperation group and a wander group. Each group contains actions that together effect the desired behavior: in teleoperation mode, input actions allow the robot to be driven by keyboard or joystick, and higher-priority limiter actions help avoid obstacles. In wander mode, a constant-velocity action drives the robot forward, but higher-priority avoidance actions make the robot turn away from obstacles, or back up if an obstacle is hit or the motors stall. Keyboard commands (the T and W keys) are used to switch between the two modes, by activating the action group for the chosen mode.
Actions overview
#include "Aria.h" ArActionGroup *teleop; ArActionGroup *wander; // Activate the teleop action group. activateExlcusive() causes // all other active action groups to be deactivated. void teleopMode(void) { teleop->activateExclusive(); printf("\n== Teleoperation Mode ==\n"); printf(" Use the arrow keys to drive, and the spacebar to stop.\n For joystick control hold the trigger button.\n Press 'w' to switch to wander mode.\n Press escape to exit.\n"); } // Activate the wander action group. activateExlcusive() causes // all other active action groups to be deactivated. void wanderMode(void) { wander->activateExclusive(); printf("\n== Wander Mode ==\n"); printf(" The robot will now just wander around avoiding things.\n Press 't' to switch to teleop mode.\n Press escape to exit.\n"); } 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; }