Aria  2.8.0
actionExample.cpp

An example program demonstrating how to make and use new actions.This example program creates two new actions, Go and Turn. Go will drive the robot forward safely, while Turn will avoid obstacles detected by the sonar by turning. This program also adds a predefined action from Aria which tries to recover from stalls (hit something and can't move forward) by backing and turning.

Each of these actions have the normal constructor and destructor, note that the constructors use constructor chaining to create their ArAction part correctly. Each action then also implements the essential virtual method, fire(). This fire function is called by the action resolver, and returns values that, in combination with other actions' desired behavior, determine the driving commands sent to the robot.

Also note that each of these actions override the setRobot function; these implementations obtain the sonar device from the robot in addition to doing the needed caching of the robot pointer. This is what you should do if you care about the presence or absence of a particular sensor. If you don't care about any particular sensor you could just use one of the checkRangeDevice... methods in ArRobot (there are four of them). Also note that these are very naive actions, they are simply an example of how to use actions.

See the Actions Actions section of the Aria reference manual overview for more details about actions.

Note that actions must take a small amount of time to execute, to avoid delaying the robot synchronization cycle.

/*
Adept MobileRobots Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007, 2008, 2009, 2010 MobileRobots Inc.
Copyright (C) 2011, 2012, 2013 Adept Technology
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
Adept MobileRobots for information about a commercial version of ARIA at
robots@mobilerobots.com or
Adept MobileRobots, 10 Columbia Drive, Amherst, NH 03031; +1-603-881-7960
*/
#include "Aria.h"
/*
* Action that drives the robot forward, but stops if obstacles are
* detected by sonar.
*/
class ActionGo : public ArAction
{
public:
// constructor, sets myMaxSpeed and myStopDistance
ActionGo(double maxSpeed, double stopDistance);
// destructor. does not need to do anything
virtual ~ActionGo(void) {};
// called by the action resolver to obtain this action's requested behavior
virtual ArActionDesired *fire(ArActionDesired currentDesired);
// store the robot pointer, and it's ArSonarDevice object, or deactivate this action if there is no sonar.
virtual void setRobot(ArRobot *robot);
protected:
// the sonar device object obtained from the robot by setRobot()
ArRangeDevice *mySonar;
/* Our current desired action: fire() modifies this object and returns
to the action resolver a pointer to this object.
This object is kept as a class member so that it persists after fire()
returns (otherwise fire() would have to create a new object each invocation,
but would never be able to delete that object).
*/
ArActionDesired myDesired;
double myMaxSpeed;
double myStopDistance;
};
/* Action that turns the robot away from obstacles detected by the
* sonar. */
class ActionTurn : public ArAction
{
public:
// constructor, sets the turnThreshold, and turnAmount
ActionTurn(double turnThreshold, double turnAmount);
// destructor, its just empty, we don't need to do anything
virtual ~ActionTurn(void) {};
// fire, this is what the resolver calls to figure out what this action wants
virtual ArActionDesired *fire(ArActionDesired currentDesired);
// sets the robot pointer, also gets the sonar device, or deactivates this action if there is no sonar.
virtual void setRobot(ArRobot *robot);
protected:
// this is to hold the sonar device form the robot
ArRangeDevice *mySonar;
// what the action wants to do; used by the action resover after fire()
ArActionDesired myDesired;
// distance at which to start turning
double myTurnThreshold;
// amount to turn when turning is needed
double myTurnAmount;
// remember which turn direction we requested, to help keep turns smooth
int myTurning; // -1 == left, 1 == right, 0 == none
};
/*
Note the use of constructor chaining with
ArAction(actionName). Also note how it uses setNextArgument, which makes it so that
other parts of the program could find out what parameters this action has, and possibly modify them.
*/
ActionGo::ActionGo(double maxSpeed, double stopDistance) :
ArAction("Go")
{
mySonar = NULL;
myMaxSpeed = maxSpeed;
myStopDistance = stopDistance;
setNextArgument(ArArg("maximum speed", &myMaxSpeed, "Maximum speed to go."));
setNextArgument(ArArg("stop distance", &myStopDistance, "Distance at which to stop."));
}
/*
Override ArAction::setRobot() to get the sonar device from the robot, or deactivate this action if it is missing.
You must also call ArAction::setRobot() to properly store
the ArRobot pointer in the ArAction base class.
*/
void ActionGo::setRobot(ArRobot *robot)
{
mySonar = robot->findRangeDevice("sonar");
if (robot == NULL)
{
ArLog::log(ArLog::Terse, "actionExample: ActionGo: Warning: I found no sonar, deactivating.");
deactivate();
}
}
/*
This fire is the whole point of the action.
currentDesired is the combined desired action from other actions
previously processed by the action resolver. In this case, we're
not interested in that, we will set our desired
forward velocity in the myDesired member, and return it.
Note that myDesired must be a class member, since this method
will return a pointer to myDesired to the caller. If we had
declared the desired action as a local variable in this method,
the pointer we returned would be invalid after this method
returned.
*/
ArActionDesired *ActionGo::fire(ArActionDesired currentDesired)
{
double range;
double speed;
// reset the actionDesired (must be done), to clear
// its previous values.
myDesired.reset();
// if the sonar is null we can't do anything, so deactivate
if (mySonar == NULL)
{
deactivate();
return NULL;
}
// get the range of the sonar
range = mySonar->currentReadingPolar(-70, 70) - myRobot->getRobotRadius();
// if the range is greater than the stop distance, find some speed to go
if (range > myStopDistance)
{
// just an arbitrary speed based on the range
speed = range * .3;
// if that speed is greater than our max, cap it
if (speed > myMaxSpeed)
speed = myMaxSpeed;
// now set the velocity
myDesired.setVel(speed);
}
// the range was less than the stop distance, so request stop
else
{
myDesired.setVel(0);
}
// return a pointer to the actionDesired to the resolver to make our request
return &myDesired;
}
/*
This is the ActionTurn constructor, note the use of constructor chaining
with the ArAction. also note how it uses setNextArgument, which makes
it so that other things can see what parameters this action has, and
set them. It also initializes the classes variables.
*/
ActionTurn::ActionTurn(double turnThreshold, double turnAmount) :
ArAction("Turn")
{
myTurnThreshold = turnThreshold;
myTurnAmount = turnAmount;
setNextArgument(ArArg("turn threshold (mm)", &myTurnThreshold, "The number of mm away from obstacle to begin turnning."));
setNextArgument(ArArg("turn amount (deg)", &myTurnAmount, "The number of degress to turn if turning."));
myTurning = 0;
}
/*
Sets the myRobot pointer (all setRobot overloaded functions must do this),
finds the sonar device from the robot, and if the sonar isn't there,
then it deactivates itself.
*/
void ActionTurn::setRobot(ArRobot *robot)
{
mySonar = robot->findRangeDevice("sonar");
if (mySonar == NULL)
{
ArLog::log(ArLog::Terse, "actionExample: ActionTurn: Warning: I found no sonar, deactivating.");
deactivate();
}
}
/*
This is the guts of the Turn action.
*/
ArActionDesired *ActionTurn::fire(ArActionDesired currentDesired)
{
double leftRange, rightRange;
// reset the actionDesired (must be done)
myDesired.reset();
// if the sonar is null we can't do anything, so deactivate
if (mySonar == NULL)
{
deactivate();
return NULL;
}
// Get the left readings and right readings off of the sonar
leftRange = (mySonar->currentReadingPolar(0, 100) -
myRobot->getRobotRadius());
rightRange = (mySonar->currentReadingPolar(-100, 0) -
myRobot->getRobotRadius());
// if neither left nor right range is within the turn threshold,
// reset the turning variable and don't turn
if (leftRange > myTurnThreshold && rightRange > myTurnThreshold)
{
myTurning = 0;
myDesired.setDeltaHeading(0);
}
// if we're already turning some direction, keep turning that direction
else if (myTurning)
{
myDesired.setDeltaHeading(myTurnAmount * myTurning);
}
// if we're not turning already, but need to, and left is closer, turn right
// and set the turning variable so we turn the same direction for as long as
// we need to
else if (leftRange < rightRange)
{
myTurning = -1;
myDesired.setDeltaHeading(myTurnAmount * myTurning);
}
// if we're not turning already, but need to, and right is closer, turn left
// and set the turning variable so we turn the same direction for as long as
// we need to
else
{
myTurning = 1;
myDesired.setDeltaHeading(myTurnAmount * myTurning);
}
// return a pointer to the actionDesired, so resolver knows what to do
return &myDesired;
}
int main(int argc, char** argv)
{
ArSimpleConnector conn(&argc, argv);
ArRobot robot;
// Create instances of the actions defined above, plus ArActionStallRecover,
// a predefined action from Aria.
ActionGo go(500, 350);
ActionTurn turn(400, 10);
// Parse all command-line arguments
{
return 1;
}
// Connect to the robot
if(!conn.connectRobot(&robot))
{
ArLog::log(ArLog::Terse, "actionExample: Could not connect to robot! Exiting.");
return 2;
}
// Add the range device to the robot. You should add all the range
// devices and such before you add actions
robot.addRangeDevice(&sonar);
// Add our actions in order. The second argument is the priority,
// with higher priority actions going first, and possibly pre-empting lower
// priority actions.
robot.addAction(&recover, 100);
robot.addAction(&go, 50);
robot.addAction(&turn, 49);
// Enable the motors, disable amigobot sounds
robot.enableMotors();
// Run the robot processing cycle.
// 'true' means to return if it loses connection,
// after which we exit the program.
robot.run(true);
}
ArActionStallRecover
Action to recover from a stall.
Definition: ArActionStallRecover.h:40
ArAction::setRobot
virtual void setRobot(ArRobot *robot)
Sets the robot this action is driving.
Definition: ArAction.cpp:100
ArAction
Base class for actions.
Definition: ArAction.h:63
ArLog::Terse
Use terse logging.
Definition: ArLog.h:61
ArSimpleConnector
Legacy connector for robot and laser.
Definition: ArSimpleConnector.h:51
ArRobot::enableMotors
void enableMotors()
Enables the motors on the robot.
Definition: ArRobot.cpp:6521
ArRobot::addRangeDevice
void addRangeDevice(ArRangeDevice *device)
Adds a rangeDevice to the robot's list of them, and set the ArRangeDevice object's robot pointer to t...
Definition: ArRobot.cpp:5757
Aria::exit
static void exit(int exitCode=0)
Shutdown all Aria processes/threads, call exit callbacks, and exit the program.
Definition: Aria.cpp:367
ArLog::log
static void log(LogLevel level, const char *str,...)
Log a message, with formatting and variable number of arguments.
Definition: ArLog.cpp:93
ArActionDesired
Contains values returned by ArAction objects expressing desired motion commands to resolver.
Definition: ArActionDesired.h:225
ArRangeDevice
The base class for all sensing devices which return range information from the device (mounted on a r...
Definition: ArRangeDevice.h:79
Aria::init
static void init(SigHandleMethod method=SIGHANDLE_THREAD, bool initSockets=true, bool sigHandleExitNotShutdown=true)
Initialize Aria global data struture and perform OS-specific initialization, including adding OS sign...
Definition: Aria.cpp:128
ArRobot
Central class for communicating with and operating the robot.
Definition: ArRobot.h:82
ArActionDesired::reset
virtual void reset(void)
Resets the strengths to 0.
Definition: ArActionDesired.h:510
ArRobot::findRangeDevice
const ArRangeDevice * findRangeDevice(const char *name, bool ignoreCase=false) const
Finds a rangeDevice in the robot's list.
Definition: ArRobot.cpp:5823
Aria::logOptions
static void logOptions(void)
Logs all the options for the program (Calls all the callbacks added with addLogOptionsCB())
Definition: Aria.cpp:794
ArSonarDevice
Keep track of recent sonar readings from a robot as an ArRangeDevice.
Definition: ArSonarDevice.h:51
ArAction::fire
virtual ArActionDesired * fire(ArActionDesired currentDesired)=0
Fires the action, returning what the action wants to do.
ArRobot::addAction
bool addAction(ArAction *action, int priority)
Adds an action to the list with the given priority.
Definition: ArRobot.cpp:3278
Aria::parseArgs
static bool parseArgs(void)
Parses the arguments for the program (calls all the callbacks added with addParseArgsCB())
Definition: Aria.cpp:759
ArRobot::run
void run(bool stopRunIfNotConnected, bool runNonThreaded=false)
Starts the instance to do processing in this thread.
Definition: ArRobot.cpp:255
ArArg
Argument class, mostly for actions, could be used for other things.
Definition: ArArg.h:44