ArPose 和ArActionGoto 的典型用法：
#include "Aria.h"
int main(int argc, char **argv)
{
Aria::init();
ArArgumentParser parser(&argc, argv); //初始化命令行参数
parser.loadDefaultArguments(); //导入默认参数，这个默认参数就是.p参数文件，从电脑导入！！
ArRobot robot; // Instance 代表机器人底盘本体
ArAnalogGyro gyro(&robot);
ArSonarDevice sonar;
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.
robotConnector.connectRobot(); //同步建立连接，syn123
ArLog::log(ArLog::Normal, "gotoActionExample: Connected to robot."); //输出方式，其实是printf cout 没区别，前面是 等级，用于调试提示！！！
robot.addRangeDevice(&sonar);
robot.runAsync(true); //与线程有关
// Goto action at lower priority
ArActionGoto gotoPoseAction("goto");
robot.addAction(&gotoPoseAction, 50);
// turn on the motors, turn off amigobot sounds
robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);
const int duration = 100000; //msec
ArLog::log(ArLog::Normal, "Going to four goals in turn for %d seconds, then cancelling goal and exiting.", duration/1000);
bool first = true;
int goalNum = 0;
ArTime start;
start.setToNow();
while (Aria::getRunning())
{
robot.lock();
// Choose a new goal if this is the first loop iteration, or if we
// achieved the previous goal.
if (first || gotoPoseAction.haveAchievedGoal())
{
first = false;
goalNum++;
if (goalNum > 4)
goalNum = 1; // start again at goal #1
// set our positions for the different goals
if (goalNum == 1)
{
gotoPoseAction.setSpeed(100);
gotoPoseAction.setGoal(ArPose(2500, 0));}
else if (goalNum == 2)
{
gotoPoseAction.setSpeed(500);
gotoPoseAction.setGoal(ArPose(2500, 2500));}
else if (goalNum == 3)
{
gotoPoseAction.setSpeed(1000);
gotoPoseAction.setGoal(ArPose(0, 2500));}
else if (goalNum == 4)
gotoPoseAction.setGoal(ArPose(0, 0));
ArLog::log(ArLog::Normal, "Going to next goal at %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if(start.mSecSince() >= duration) { //时间大，所以走完还会有往前溜。
ArLog::log(ArLog::Normal, "%d seconds have elapsed. Cancelling current goal, waiting 3 seconds, and exiting.", duration/1000);
gotoPoseAction.cancelGoal();
robot.unlock();
ArUtil::sleep(3000);
break;
}
robot.unlock();
ArUtil::sleep(100);
}
// Robot disconnected or time elapsed, shut down
Aria::exit(0);
return 0;
}
小修改——到目标点之后，输出编码器的坐标。
#include "Aria.h"
#include <iostream>
int main(int argc, char **argv)
{
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobot robot;
ArAnalogGyro gyro(&robot);
ArSonarDevice sonar;
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, "gotoActionExample: 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, "gotoActionExample: Connected to robot.");
robot.addRangeDevice(&sonar);
robot.runAsync(true);
// Make a key handler, so that escape will shut down the program
// cleanly
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");
// Collision avoidance actions at higher priority
ArActionLimiterForwards limiterAction("speed limiter near", 300, 600, 250);
ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400);
ArActionLimiterTableSensor tableLimiterAction;
robot.addAction(&tableLimiterAction, 100);
robot.addAction(&limiterAction, 95);
robot.addAction(&limiterFarAction, 90);
// Goto action at lower priority
ArActionGoto gotoPoseAction("goto");
robot.addAction(&gotoPoseAction, 50);
// Stop action at lower priority, so the robot stops if it has no goal
ArActionStop stopAction("stop");
robot.addAction(&stopAction, 40);
// turn on the motors, turn off amigobot sounds
robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);
const int duration = 60000; //msec
ArLog::log(ArLog::Normal, "Going to four goals in turn for %d seconds, then cancelling goal and exiting.", duration/1000);
bool first = true;
int goalNum = 0;
ArTime start;
start.setToNow();
while (Aria::getRunning())
{
robot.lock();
// Choose a new goal if this is the first loop iteration, or if we
// achieved the previous goal.
if (first || gotoPoseAction.haveAchievedGoal())
{
first = false;
goalNum++;
if (goalNum > 4)
goalNum = 1; // start again at goal #1
// set our positions for the different goals
if (goalNum == 1)
gotoPoseAction.setGoal(ArPose(2500, 0));
else if (goalNum == 2)
gotoPoseAction.setGoal(ArPose(2500, 2500));
else if (goalNum == 3)
gotoPoseAction.setGoal(ArPose(0, 2500));
else if (goalNum == 4)
gotoPoseAction.setGoal(ArPose(0, 0));
ArLog::log(ArLog::Normal, "Going to next goal at %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
ArLog::log(ArLog::Normal,"____%f,____%f",robot.getEncoderPose().getX(),robot.getEncoderPose().getY());} //输出编码器的计算位置。
if(start.mSecSince() >= duration) {
ArLog::log(ArLog::Normal, "%d seconds have elapsed. Cancelling current goal, waiting 3 seconds, and exiting.", duration/1000);
gotoPoseAction.cancelGoal();
robot.unlock();
ArUtil::sleep(3000);
break;
}
robot.unlock();
ArUtil::sleep(100);
}
// Robot disconnected or time elapsed, shut down
Aria::exit(0);
return 0;
}移动到单个点
#include "Aria.h"
int main(int argc, char **argv)
{
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArSimpleConnector simpleConnector(&parser);
ArRobot robot;
ArPose myposition;
ArSonarDevice sonar;
ArAnalogGyro gyro(&robot);
robot.addRangeDevice(&sonar);
// Make a key handler, so that escape will shut down the program
// cleanly
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");
// Collision avoidance actions at higher priority
ArActionLimiterForwards limiterAction("speed limiter near", 300, 600, 250);
ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400);
ArActionLimiterTableSensor tableLimiterAction;
robot.addAction(&tableLimiterAction, 100);
robot.addAction(&limiterAction, 95);
robot.addAction(&limiterFarAction, 90);
// Goto action at lower priority
ArActionGoto gotoPoseAction("goto");
robot.addAction(&gotoPoseAction, 50);
// Parse all command line arguments
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
exit(1);
}
// Connect to the robot
if (!simpleConnector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::exit(1);
}
robot.runAsync(true);
// turn on the motors, turn off amigobot sounds
robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);
gotoPoseAction.setGoal(ArPose(5000, 5000));
for(;;)
{
if(robot.isRunning())
{
robot.lock();
printf("Position( %f, %f)\n",robot.getX(), robot.getY()); //不断输出机器人里程计位置
robot.unlock();
}
}
// Robot disconnected, shut down
Aria::shutdown();
return 0;
}移动到4个点并计算距离数据等，这个例子有点奇怪，没有使用setGoal，而在循环判断时，定义了位置的x / y 这是属于什么原因？
#include "Aria.h"
/** @example gotoActionExample.cpp Uses ArActionGoto to drive the robot in a square
This program will make the robot drive in a 5x5 meter square by
setting each corner in turn as the goal for an ArActionGoto action.
It also uses speed limiting actions to avoid collisions.
Press escape to shut down Aria and exit.
*/
int main(int argc, char **argv)
{
double myX, myY, myTh;
double myDist=3000;
double disterr=409;
int count=0;
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArSimpleConnector simpleConnector(&parser);
ArRobot robot;
ArSonarDevice sonar;
ArAnalogGyro gyro(&robot);
robot.addRangeDevice(&sonar);
// Make a key handler, so that escape will shut down the program
// cleanly
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");
// Collision avoidance actions at higher priority
ArActionLimiterForwards limiterAction("speed limiter near", 300, 600, 250);
ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400);
ArActionLimiterTableSensor tableLimiterAction;
robot.addAction(&tableLimiterAction, 100);
robot.addAction(&limiterAction, 95);
robot.addAction(&limiterFarAction, 90);
// Goto action at lower priority
ArActionGoto gotoPoseAction("goto");
robot.addAction(&gotoPoseAction, 50);
// Parse all command line arguments
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
exit(1);
}
// Connect to the robot
if (!simpleConnector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::exit(1);
}
robot.runAsync(true);
// turn on the motors, turn off amigobot sounds
robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);
bool first = true;
int goalNum = 0;
double mygoalx,mygoaly;
double mylastx,mylasty;
mylastx=0;
mylasty=0;
mygoalx=3000;
mygoaly=0;
double lineA,lineB,lineC,theta,dd,dth,w;
double k1,k2;
//k1=-650;
//k2=5;
k1=2.0;
k2=2.0;
double myVRef,myWRef;
myVRef=245;
robot.lock();
robot.setVel(245);
robot.unlock();
while (Aria::getRunning())
{
// Choose a new goal if this is the first loop iteration, or if we
// achieved the previous goal.
//}
// robot.lock();
//robot.setVel(245);
// robot.unlock();
myX=robot.getX();
myY=robot.getY();
myTh=robot.getTh();
myDist=sqrt((myX-mygoalx)*(myX-mygoalx)+(myY-mygoaly)*(myY-mygoaly));
if (first||myDist<=disterr)
{
first=false;
if(goalNum<=4)goalNum++;
if (goalNum > 4)
{
disterr=disterr/2;count++;
if(count==4)
{//Aria::shutdown();
//return 0;
robot.lock();
robot.setRotVel(0);
robot.setVel(0);
robot.unlock();
break;}
//goalNum = 1; // start again at goal #1
//robot.setVel(0);
//Aria::shutdown();
// return 0;
}
// set our positions for the different goals
if (goalNum == 1)
{//gotoPoseAction.setGoal(ArPose(3000, 0));
mylastx=0;
mylasty=0;
mygoalx=3000;
mygoaly=0;}
else if (goalNum == 2)
{//gotoPoseAction.setGoal(ArPose(3000, 3000));
mylastx=3000;
mylasty=0;
mygoalx=3000;
mygoaly=3000;}
else if (goalNum == 3)
{//gotoPoseAction.setGoal(ArPose(0, 3000));
mylastx=3000;
mylasty=3000;
mygoalx=0;
mygoaly=3000;}
else if (goalNum == 4)
{//gotoPoseAction.setGoal(ArPose(0, 0));
mylastx=0;
mylasty=3000;
mygoalx=0;
mygoaly=0;}
//robot.setVel(0);
}
lineA=(mylasty-mygoaly);
lineB=(mygoalx-mylastx);
lineC=(mygoaly*mylastx-mygoalx*mylasty);
theta=atan2(mygoaly-mylasty, mygoalx-mylastx)*180/3.14;
dd=(lineA*myX+lineB*myY+lineC)/sqrt(lineA*lineA+lineB*lineB);
dd=dd/1000;
if(dd>0.500)
dd=0.500;
else if(dd<-0.500)
dd=-0.500;
dth = theta - myTh;
if(dth>180)
dth= dth-360;
else if(dth<-180)
dth=dth+360;
w=k1*dd+k2*dth;
if(w>40)
w=40;
else if(w<-40)
w=-40;
robot.lock();
robot.setRotVel(w);
//robot.setVel(myVRef);
//printf("x=%.0f y=%.0f theta=%.0f vel=%.0f rotVel=%.0f w=%.0f mylastx=%.0f \r",
// robot.getX(), robot.getY(), robot.getTh(),
// robot.getVel(), robot.getRotVel(),w,mylastx);
printf("x=%.0f y=%.0f t=%.0f v=%.0f w=%.0f mt=%.0f dth=%0.f No=%d d=%.2f \r",
robot.getX(), robot.getY(), robot.getTh(),
robot.getVel(), robot.getRotVel(),theta,dth,goalNum,dd);
robot.unlock();
}
// Robot disconnected, shut down
printf("\ndistance=%.0f",sqrt(myX*myX+myY*myY));
getchar();
Aria::shutdown();
return 0;
}
移动到某一个位置点
ArAction是Aria中定义的多线程优先级执行的机器人行为类。
所定义的行为均要从ArAction中继承，其中需要定义的行为通过 virtual ArActionDesired *fire(ArActionDesired currentDesired)定义实现方法，该方法返回一个mydisered 对象
#include "Aria.h"
#define PI 3.1415926
using namespace std;
int sgn(double x)
{
if (x>0) return(1);
if (x<0) return(-1);
if (x==0)return(0);
}
class LosV
{
public :double dist;
public :double angle;
//构造函数，角度和距离数据
public :LosV(double d,double a)
{
dist=d;
angle=a;
}
//拷贝构造函数
public :LosV(LosV &vect)
{
dist=vect.dist;
angle=vect.angle;
}
//运算符重载
public:LosV operator+(LosV vect)
{ double x,y;
x=dist*cos(angle)+vect.dist*cos(vect.angle);
y=dist*sin(angle)+vect.dist*sin(vect.angle);
dist=abs(sqrt(x*x+y*y));
if (x==0)
angle=3.1415/2*sgn(y);
else
angle=atan2(y,x);
return LosV(dist,angle);
}
public:LosV operator-(LosV vect)
{ double x,y;
x=dist*cos(angle)-vect.dist*cos(vect.angle);
y=dist*sin(angle)-vect.dist*sin(vect.angle);
dist=abs(sqrt(x*x+y*y));
if (x==0)
angle=3.1415/2*sgn(y);
else
angle=atan2(y,x);
return LosV(dist,angle);
}
};
//ArGoal 类定义，设置位置点
class ArGoal : public ArAction
{
public:
// constructor, sets myMaxSpeed and myStopDistance
ArGoal(ArPose Pose);
// destructor, its just empty, we don't need to do anything
virtual ~ArGoal (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
virtual void setRobot(ArRobot *robot);
protected:
// this is to hold the sonar device form the robot
ArRangeDevice *mySonar;
// what the action wants to do. fire() modifies this and returns a pointer
// to it.
ArActionDesired myDesired;
ArPose GoalPos;
};
ArGoal::ArGoal(ArPose Pose) : ArAction("ArGoal")
{
mySonar = NULL;
GoalPos=Pose;
//设置第二个参数类型，来自基类ArAction成员函数，括号类构造一个位置参数
setNextArgument(ArArg("Goal Pose ", &Pose, "Goal for Robot."));
}
/*
Sets the myRobot pointer (all setRobot overloaded functions must do this),重定义ArAction：：setRobot()
finds the sonar device from the robot, and if the sonar isn't there,
then it deactivates itself.
*/
void ArGoal::setRobot(ArRobot *robot)
{
myRobot = robot;
mySonar = myRobot->findRangeDevice("sonar");//获得声呐资源
if (mySonar == NULL)
{
printf("I found no sonar");
deactivate();
}
}
ArActionDesired *ArGoal::fire(ArActionDesired currentDesired)
{
double x,y,th; //指代当前的暂时位置信息
double dx,dy,dth; //计算距离角度
int k;
//此处V代表vector向量
LosV Vg(0,0); //goal vector，0只代表初始化
x=myRobot->getX();
y=myRobot->getY();
th=myRobot->getTh();
// goal is arrived at ，误差是100 mm以内
if((x-GoalPos.getX())*(x-GoalPos.getX())+(y-GoalPos.getY())*(y-GoalPos.getY())<=10000)
{
myRobot->stop();
exit(1);
}
// Goal vector,比较当前位置和目标位置
Vg.dist=myRobot->findDistanceTo(GoalPos);
Vg.angle=ArMath::degToRad(myRobot->findAngleTo(GoalPos));
myDesired.setHeading(ArMath::radToDeg(Vg.angle));
myDesired.setVel(200);
printf("__x__%f __y__%f\n",myRobot->getX(),myRobot->getY());
return &myDesired;
}
int main(int argc, char **argv)
{
Aria::init();
ArRobot robot;
// add KeyHandler to drive the robot to any position from orign
ArKeyHandler keyHandler;
// let the global aria stuff know about it
Aria::setKeyHandler(&keyHandler);
// toss it on the robot
// sonar, must be added to the robot
ArSonarDevice sonar;
ArActionConstantVelocity constantVelocity("Constant Velocity", 400);
ArPose Goal;
Goal.setPose(-3000,-3000,0);
ArGoal goGoal(Goal);
// mandatory init
ArSimpleConnector connector(&argc,argv);
if (!connector.parseArgs() || argc > 1)
{
connector.logOptions();
exit(1);
}
robot.attachKeyHandler(&keyHandler);
printf("Press escape to exit\n");
// add the sonar to the robot
robot.addRangeDevice(&sonar);
// try to connect, if we fail exit
if (!connector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// turn on the motors, turn off amigobot sounds
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
//robot running...
robot.addAction(&goGoal,50);
robot.addAction(&constantVelocity,25);
robot.run(true);
// now exit
Aria::shutdown();
return 0;
}
3976
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
