本例程演示瞭如何創建並使用新動作：前進 和 轉向 ，並且本例程還從Aria中加入了一個預先定義的動作：通過後退和轉向從熄火中恢復。
Note that actions must take a small amount of time to execute, to avoid delaying the robot synchronization cycle.
【無註釋版】：
#include "Aria.h"
/*
通過設定一個停止距離，距離障礙物達到停止距離後轉彎。
*/
//============= class definition =============//
class ActionGo : public ArAction
{
public:
ActionGo(double maxSpeed, double stopDistance);
virtual ~ActionGo(void) {};
virtual ArActionDesired *fire(ArActionDesired currentDesired);
virtual void setRobot(ArRobot *robot);
protected:
ArRangeDevice *mySonar;
ArActionDesired myDesired;
double myMaxSpeed;
double myStopDistance;
};
class ActionTurn : public ArAction
{
public:
ActionTurn(double turnThreshold, double turnAmount);
virtual ~ActionTurn(void) {};
virtual ArActionDesired *fire(ArActionDesired currentDesired);
virtual void setRobot(ArRobot *robot);
protected:
ArRangeDevice *mySonar;
ArActionDesired myDesired;
double myTurnThreshold;
double myTurnAmount;
int myTurning; // -1 == left, 1 == right, 0 == none
};
//==================== go ====================//
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."));
}
void ActionGo::setRobot(ArRobot *robot)
{
ArAction::setRobot(robot);
mySonar = robot->findRangeDevice("sonar");
if (robot == NULL)
{
ArLog::log(ArLog::Terse, "actionExample: ActionGo: Warning: I found no sonar, deactivating.");
deactivate();
}
}
ArActionDesired *ActionGo::fire(ArActionDesired currentDesired)
{
double range;
double speed;
myDesired.reset();
if (mySonar == NULL)
{
deactivate();
return NULL;
}
range = mySonar->currentReadingPolar(-70, 70) - myRobot->getRobotRadius();
if (range > myStopDistance)
{
speed = range * .3;
if (speed > myMaxSpeed)
speed = myMaxSpeed;
myDesired.setVel(speed);
}
else
{
myDesired.setVel(0);
}
return &myDesired;
}
//=================== turn ====================//
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;
}
void ActionTurn::setRobot(ArRobot *robot)
{
ArAction::setRobot(robot);
mySonar = robot->findRangeDevice("sonar");
if (mySonar == NULL)
{
ArLog::log(ArLog::Terse, "actionExample: ActionTurn: Warning: I found no sonar, deactivating.");
deactivate();
}
}
ArActionDesired *ActionTurn::fire(ArActionDesired currentDesired)
{
double leftRange, rightRange;
myDesired.reset();
if (mySonar == NULL)
{
deactivate();
return NULL;
}
leftRange = (mySonar->currentReadingPolar(0, 100) - myRobot->getRobotRadius());//(開始角度，結束角度)
rightRange = (mySonar->currentReadingPolar(-100, 0) - myRobot->getRobotRadius());
if (leftRange > myTurnThreshold && rightRange > myTurnThreshold)
{
myTurning = 0;
myDesired.setDeltaHeading(0);
}
else if (myTurning)
{
myDesired.setDeltaHeading(myTurnAmount * myTurning);
}
else if (leftRange < rightRange) //實際中如何理解？
{
myTurning = -1;
myDesired.setDeltaHeading(myTurnAmount * myTurning);
}
else
{
myTurning = 1;
myDesired.setDeltaHeading(myTurnAmount * myTurning);
}
return &myDesired;
}
//=================== main ====================//
int main(int argc, char** argv)
{
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobot robot;
ArSonarDevice sonar;
ArRobotConnector robotConnector(&parser, &robot);
if (!robotConnector.connectRobot())
{
ArLog::log(ArLog::Terse, "actionExample: 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, "actionExample: Connected to robot.");
ActionGo go(1000, 350);
ActionTurn turn(400, 10);
ArActionStallRecover recover;
robot.addRangeDevice(&sonar);
robot.addAction(&recover, 100); //(動作，優先級)
robot.addAction(&go, 50);
robot.addAction(&turn, 49);
robot.enableMotors();
robot.run(true);
Aria::exit(0);
}
【註釋版】：
#include "Aria.h"
/*
* Action that drives the robot forward, but stops if obstacles are
* detected by sonar.
*/
class ActionGo : public ArAction //ActionGo是基類ArAction的派生類
/*
類ActionGo公有繼承（public）了類ArAction，在這種繼承方式下，父類（ArAction）所有的成員變量和成員函數的訪問屬性
不變的被子類（ActionGo）繼承，在ActionGo中可以訪問父類的protected和public修飾的成員。
*/
{
public:
// constructor, sets myMaxSpeed and myStopDistance
ActionGo(double maxSpeed, double stopDistance);
// destructor. does not need to do anything
// 派生類必須在其內部對所有重新定義的虛函數進行聲明（使用virtual關鍵字）
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)
{
ArAction::setRobot(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.current Desired 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)
{
ArAction::setRobot(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)
{
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobot robot;
ArSonarDevice sonar;
// Connect to the robot, get some initial data from it such as type and name,
// and then load parameter files for this robot.
ArRobotConnector robotConnector(&parser, &robot);
if (!robotConnector.connectRobot())
{
ArLog::log(ArLog::Terse, "actionExample: 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, "actionExample: Connected to robot.");
// Create instances of the actions defined above, plus ArActionStallRecover,
// a predefined action from Aria.
ActionGo go(500, 350);
ActionTurn turn(400, 10);
ArActionStallRecover recover;
// 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);
Aria::exit(0);
}
