Robot Pioneer - LibAria

html Minimal example to control the Pioneer Robot.
In this example you can see how to inicialize the robot, how to read the sonars, to read the odometry and to set the speed of the motors.

Install

sudo apt-get install libaria-dev

Compile

g++ aria.cpp -o aria -lAria

Execute

./aria -robotPort /dev/ttyUSB0

---

aria.cpp

#include <stdio.h>
#include <iostream>
#include <Aria/Aria.h>

using namespace std;




void readSonars(ArRobot& robot, int numSonar){
	char value[64];
	ArSensorReading* sonarReading;
	string res;
	for (int i=0; i < numSonar; i++){
		sonarReading = robot.getSonarReading(i);
		sprintf(value,"v%d=%05d;", i, sonarReading->getRange());
		res += value;
	}
	res += "\n";
	printf("%s",res.c_str());
}


void readPosition(ArRobot& robot){
	ArPose pose = robot.getPose();
	fprintf(stdout, "x=%0.6f;y=%0.6f;th=%0.6f;\n", pose.getX(), pose.getY(), pose.getTh());
}




void setMotors(ArRobot& robot){
	robot.lock();
	robot.setVel( 0 );
	robot.setRotVel( 10 );
	robot.unlock();
}




int main(int argc, char **argv){
	Aria::init();
	ArRobot robot;
	ArArgumentParser parser(&argc, argv);
	ArSimpleConnector connector(& parser);

	parser.loadDefaultArguments();
	Aria::logOptions();
	if (!connector.parseArgs()){
		cout << "Unknown settings\n";
		Aria::exit(0);
		exit(1);
	}

	if (!connector.connectRobot(&robot)){
		cout << "Unable to connect\n";
		Aria::exit(0);
		exit(1);
	}

	robot.runAsync(true);
	robot.lock();
	robot.comInt(ArCommands::ENABLE, 1);
	robot.unlock();


	ArSonarDevice sonar;
	robot.addRangeDevice(&sonar);

	int numSonar = robot.getNumSonar();
	while(1){
		readPosition(robot);
		readSonars(robot, 8);
		setMotors(robot);
		usleep(20000);
	}

	Aria::exit(0);
}