#include "Aria.h"
#include <math.h>
#include <iostream>
int main(int argc, char **argv)
{
Aria::init();
ArRobot robot;
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArLog::log(ArLog::Terse, "WARNING: this program does no sensing or avoiding of obstacles, the robot WILL collide with any objects in the way! Make sure the robot has approximately 3 meters of free space on all sides.");
// ArRobotConnector connects to the robot, get some initial data from it such as type and name,
// and then loads parameter files for this robot.
ArRobotConnector robotConnector(&parser, &robot);
if(!robotConnector.connectRobot())
{
ArLog::log(ArLog::Terse, "simpleMotionCommands: Could not connect to the robot.");
if(parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
return 1;
}
}
if (!Aria::parseArgs())
{
Aria::logOptions();
Aria::exit(1);
return 1;
}
ArLog::log(ArLog::Normal, "simpleMotionCommands: Connected.");
// 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);
double v=0,w=0;
double k1 = 0.6;
double k2 = 50;
double k3 = -50;
int xg = 2000;
int yg = -1000;
int tg = -3.14/2 ;
for(;;)
{
ArUtil::sleep(100);
double x0 = robot.getX();
double y0 = robot.getY();
double t0 = robot.getTh()*3.14/180 + tg;
std::cout <<x0<<"\t"<<y0<<"\t"<<t0<<std::endl;
double distance0 = sqrt((xg-x0)*(xg-x0) +(yg-y0)*(yg-y0));
double alpha = -t0 + atan2((yg-y0),(xg-x0));
double beta = -t0 - alpha;
v = k1 * distance0;
if(v>150)
v = 150;
w = k2 * alpha +k3 * beta;
if(w>45)
w = 45;
if(w < -45)
w= -45;
robot.lock();
robot.setVel(v);
robot.setRotVel(w);
robot.unlock();
if(abs(robot.getX()-xg)<2 && abs(robot.getY()-yg)<2 && abs(robot.getTh() - tg*180/3.14)<2)
{
break;
}
}
robot.stopRunning();
// wait for the thread to stop
robot.waitForRunExit();
// exit
ArLog::log(ArLog::Normal, "simpleMotionCommands: Exiting.");
Aria::exit(0);
return 0;
}
其实根本性实现就是定义了x 轴速度和z 轴旋转;