源程序如下:
/*
Template to access ARIA (ActivMedia Robotics Interface for Applications)
using Microsoft Visual C++, console Projects.
*/
# include "Aria.h"
# include <cstdlib>
# include <iostream>
# include <iomanip>
# include <fstream>
# include <ctime>
# include <math.h>
# include <cmath>
using namespace std;
// the robot
ArRobot *robot;
ArSonarDevice sonar; // sonar, must be added to the robot
ArSick sick; // the laser
int step=0;
double d0,d1,d3,d2,d4,d15;
double th,r,PI=3.1415926;
double minimum=0;
//sonar sensor offset positions with respect to robot local coordinate frame
double robotSonarX[8] = {69,114,148,166,166,148,114,69};
double robotSonarY[8] = {136,119,78,27,-27,-78,-119,-136};
double robotSonarTh[8] = {90.0, 50.0, 30.0, 10.0, -10.0, -30.0, -50.0, -90.0};
double laserData[2][180];
double sonarData[5][16]; // last 5 sets of sonar readings
double posData[5]; // odometry readings
long frameCount = -1; //how many iterations of the sensor grabbing functions have been
run
/*********************************************************************************
Files for logging position and sensor data
*********************************************************************************/
ofstream positionData("positionvals.txt");
ofstream sonarVals("sonarvals.txt");