Player Stage - Programmazione
E.Mumolo, DEEI
[email protected]
1
Programmazione Player
Librerie

C (libplayerc), C++ (libplayerclient), Tcl (tclPlayer), etc.


Ciclo di programmazione in Playerstage
1.
2.
3.
4.
5.
Connessione con il proxy
Sottoscrivere i device.
Leggi i sensori.
Elabora i dati.
Invia i comandi agli attuatori.
2
File di configurazione per Player



File *.cfg  formato dalla descrizione dei driver
Driver: controllo sensori/attuatori
Definizione:
 Nome
 Plugin: nome della libreria (opzionale)
 Provides: indirizzo dei driver
 Alwayson: il driver viene caricato subito (1) o quando
Player riceve una richiesta da un client (0)
Esempio (simple.cfg)
# load the Stage plugin simulation driver
driver
(
name "stage"
provides ["simulation:0" ]
plugin "libstageplugin"
# load the named file into the simulator
worldfile "simple.world"
)
# Create a Stage driver and attach position2d and laser interfaces to "robot1"
driver
(
name "stage"
provides ["position2d:0" "laser:0" ]
model "robot1"
)
Descrizione del mondo per Stage

Esempio: simple.world

Tutte le informazioni sull’ambiente



Specifica i sensori e attuatori
La mappa dell’ambiente
Parametri della simulazione



Risoluzione (dimensione di un pixel)
Intervallo di tempo di simulatione
…
Esempio: simple.world
# defines Pioneer-like robots
include "pioneer.inc"
# defines 'map' object used for floorplans
include "map.inc"
# defines sick laser
include "sick.inc"
# size of the world in meters
size [16 16]
# resolution of the underlying ...
# ...raytrace model in meters
resolution 0.02
# update the screen every 10ms
gui_interval 20
# configure the GUI window
window
(
size [ 591.000 638.000 ]
center [-0.010 -0.040]
scale 0.028
)
Y
(1)
sinistra
lp[179]
laser
davanti
lp[90]
lp[0]
X
destra
Esempio: simple.world
# load an environment bitmap
map
(
bitmap "bitmaps/cave.png"
size [16 16]
name "cave"
)
# create a robot
pioneer2dx
(
name "robot1"
color "red"
pose [-6.5 -6.5 45]
sick_laser( samples 361 laser_sample_skip 4 )
)
(2)
Robot Pioneer (pioneer.inc)
# The Pioneer2DX sonar array
define p2dx_sonar ranger
(
scount 16
# define the pose of each transducer [xpos ypos heading]
spose[0] [ 0.075 0.130 90 ]
spose[1] [ 0.115 0.115 50 ]
spose[2] [ 0.150 0.080 30 ]
spose[3] [ 0.170 0.025 10 ]
spose[4] [ 0.170 -0.025 -10 ]
spose[5] [ 0.150 -0.080 -30 ]
spose[6] [ 0.115 -0.115 -50 ]
0 1
spose[7] [ 0.075 -0.130 -90 ]
2
spose[8] [ -0.155 -0.130 -90 ]
3
spose[9] [ -0.195 -0.115 -130 ]Y
spose[10] [ -0.230 -0.080 -150 ]
4
spose[11] [ -0.250 -0.025 -170 ]
spose[12] [ -0.250 0.025 170 ]
5
76
spose[13] [ -0.230 0.080 150 ]
spose[14] [ -0.195 0.115 130 ]
spose[15] [ -0.155 0.130 90 ]
# define the field of view of each transducer [range_min range_max view_angle]
sview [0 5.0 15]
# define the size of each transducer [xsize ysize] in meters
ssize [0.01 0.05]
)
(1)
Y
sonar
X
Robot Pioneer (pioneer.inc)
# a Pioneer 2 or 3 in standard configuration
define pioneer2dx position
(
# actual size
size [0.44 0.33]
# the pioneer's center of rotation is offset from its center of area
origin [-0.04 0.0 0]
# draw a nose on the robot so we can see which way it points
gui_nose 1
# estimated mass in KG
mass 15.0
# this polygon approximates the shape of a pioneer
polygons 1
polygon[0].points 8
polygon[0].point[0] [ 0.23 0.05 ]
polygon[0].point[1] [ 0.15 0.15 ]
polygon[0].point[2] [ -0.15 0.15 ]
polygon[0].point[3] [ -0.23 0.05 ]
polygon[0].point[4] [ -0.23 -0.05 ]
polygon[0].point[5] [ -0.15 -0.15 ]
polygon[0].point[6] [ 0.15 -0.15 ]
polygon[0].point[7] [ 0.23 -0.05 ]
}
...
(2)
Y
X
Descrizione dell’ambiente

Esempi di ambienti (files png in stage-2.0.1)
cave.png

autolab.png
Creazione di un ambiente
simple_room.png
Compilazione/esecuzione

Librerie utente

Variabile d’ambiente:
export PKG_CONFIG_PATH=/robodeb/local/lib/pkgconfig

Compilatori: gcc o g++ o java

Makefile:
target : *.cc
g++ -o out ‘pkg-config –flags playerc++’ *.cc ‘pkg-config –libs playerc++’

Esecuzione (in bash):
./target
Principali programmi di utilità






Dgps_server
Playercam
Playerjoy  joystick
Playernav
Playerprint
Playerv  visualizza quello che il robot vede
Robot e Sensori
Oggetto PlayerClient: controlla ogni connessione al Player server.
PlayerClient
robot("localhost");
Alcuni metodi inclusi: robot.Read(), root.Write(), robot.SetFrequency() …

Oggetto Odometria
Position2dProxy pp(&robot,0);

Alcuni metodi inclusi: pp.SetSpeed(), pp.ResetOdometry(), GetXPose(),
GetYPose(), GetYaw(), …
Oggetto Sonar
SonarProxy

sp(&robot,0);
Alcuni metodi: sp.GetScan() (o sp[]), sp.GetPose(), sp.GetPoseCount(),..
 Oggetto Laser
LaserProxy
lp(&robot,0);
Metodi: lp.GetPoint(i) (o lp[i]), lp.GetRange(i), lp.GetCount(), …
Sick LMS-200 e Sonar
Schema di programma di base
int main(int argc, char *argv[])
{
using namespace PlayerCc;
double ss,phi;
double tx=5., ty=5.; //coordinate del punto target
PlayerClient
SonarProxy
LaserProxy
Position2dProxy
robot("localhost");
sp(&robot,0);
lp(&robot,0);
pp(&robot,0);
for(;;) {
robot.Read(); //aspetta i dati dei sensori
elabora();
//qualche elaborazione
...
pp.SetSpeed(ss,phi);
}
}
• Esercizio: lettura sensori
Alcune manovre elementari di moto
(navigazione semplice)





Evitamento degli ostacoli (obstacle avoidance)
Inseguimento del muro (wall following)
Moto punto-punto (point stabilization) :
Manovra di parcheggio (parking) :il robot parte da una
configurazione iniziale (xi,yi,fi) e raggiunge una configurazione finale
(xf,yf, ff)
Inseguimento della traiettoria (trajectory tracking): il robot
deve raggiungere e seguire un cammino geometrico con
un’assegnata legge temporale.

Inseguimento del cammino (path following): il robot deve
seguire un particolare cammino geometrico senza specifiche
temporali.

Inseguimento obiettivo (target tracking)
Sonar Obstacle Avoidance (primitivo)
#include <iostream>
#include <libplayerc++/playerc++.h>
Int main(int argc, char *argv[])
{
using namespace PlayerCc;
PlayerClient
robot("localhost");
SonarProxy
sp(&robot,0);
Position2dProxy pp(&robot,0);
for(;;)
{
double turnrate, speed;
robot.Read();
if((sp[0] + sp[1] + sp[2] + sp[3]) < (sp[4] + sp[5]+sp[6] + sp[7]))
turnrate = dtor(-50); // 20 gradi/s
else
turnrate = dtor(50);
pp.SetSpeed(1, turnrate);
}
}
• Esercizio: descrivere il log del cammino
Laser Obstacle Avoidance (primitivo)
#include <libplayerc++/playerc++.h>
#include <iostream>
#include "args.h"
int main(int argc, char **argv)
{
using namespace PlayerCc;
double jog;
PlayerClient robot(gHostname, gPort);
Position2dProxy pp(&robot, gIndex);
LaserProxy lp(&robot, gIndex);
pp.SetMotorEnable (true);
for(;;)
{
double minR = 1e9;
double minL = 1e9;
robot.Read();
uint count = lp.GetCount(); //trova la distanza minima a destra e sinistra
for (uint j=0; j < count/2; ++j)
{ if (minR > lp[j]) minR = lp[j]; }
for (uint j = count/2; j < count; ++j) { if (minL > lp[j]) minL = lp[j]; }
jog=5*(minL - minR);
std::cout << "Jog" << jog << std::endl;
pp.SetSpeed(0.5, jog);
}
}
Random walk (1)
#include <libplayerc++/playerc++.h>
#include <iostream>
using namespace PlayerCc;
#include "args.h“
double minfrontdistance = 1; double speed = 1; double avoidspeed = -1; double turnrate = DTOR(40);
int main(int argc, char** argv)
{
int randint;
int randcount = 0;
int avoidcount = 0;
bool obs = false;
parse_args(argc,argv);
LaserProxy *lp = NULL;
SonarProxy *sp = NULL;
PlayerClient robot(gHostname, gPort); Position2dProxy pp(&robot, gIndex); sp = new SonarProxy (&robot, gIndex);
pp.SetMotorEnable (true);
double newturnrate=0.0f, newspeed=0.0f;
for(;;)
{
robot.Read();
obs = ((sp->GetScan (2) < minfrontdistance) ||
(sp->GetScan (3) < minfrontdistance) ||
(sp->GetScan (4) < minfrontdistance) ||
(sp->GetScan (5) < minfrontdistance) );
if(obs )
{
newspeed=avoidspeed;
if(sp->GetScan(1)+sp->GetScan(15)<sp->GetScan(7)+sp->GetScan(8))
newturnrate = -turnrate; else newturnrate = turnrate;
}
else
Randow Walk (2)
{
avoidcount = 0;
newspeed = speed;
//ruota a random per 2 secondi
if(!randcount)
{
/* genera un numero random tra -20 e 20 */
randint = rand() % 41 - 20;
newturnrate = dtor(randint);
randcount = 20;
}
randcount--;
}
pp.SetSpeed(newspeed, newturnrate);
}
}
Wall following

Esercizio:
minR
Dist (distanza desiderata)
Legge di controllo:
Jog=K*(Dist – minR)
Moto punto-punto (1)
#include <iostream>
#include <libplayerc++/playerc++.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
Int main(int argc, char *argv[])
{
using namespace PlayerCc;
double ss,delta=0,phi=0,ang=0,ang1=0, dx,dy;;
double tx=5., ty=5.; //coordinate del punto target
PlayerClient
robot("localhost"); SonarProxy
sp(&robot,0);
LaserProxy
lp(&robot,0); Position2dProxy pp(&robot,0);
for(;;) {
robot.Read();
dx=tx-pp.GetXPos();
dy=ty-pp.GetYPos();
ang = atan2(dy,dx);//angolo target
ang1=pp.GetYaw(); //angolo del robot in rad
delta=ang-ang1; //errore
pp.SetSpeed(1.5,phi);
}
}
Moto punto-punto (2)

Esercizi:





Criterio di fermata
Percorso di una traiettoria preassegnata
(quadrato?)
Visualizzazione del cammino
Integrazione del programma di moto con un
algoritmo per evitare ostacoli
Evidenziare i problemi
Moto punto-punto (3)
#include
#include
#include
#include
#include
<iostream>
<libplayerc++/playerc++.h>
<stdio.h>
<stdlib.h>
<string.h>
for(;;) {
robot.Read();
dx=tx-pp.GetXPos();
dy=ty-pp.GetYPos();
int main(int argc, char *argv[])
{
using namespace PlayerCc;
double ss,delta=0,phi=0,ang=0,ang1=0;
double dx,dy,dist;
PlayerClient
SonarProxy
LaserProxy
Position2dProxy
pp.SetSpeed(2.0,0);
std::cout << "target x,y = " << tx
<<" " << ty << "
dist"
<< dist << std::endl;
robot("localhost");
sp(&robot,0);
lp(&robot,0);
pp(&robot,0);
double tx=5., ty=5.; //coordinate target
for(;;) {
robot.Read();
dx=tx-pp.GetXPos(); dy=ty-pp.GetYPos();
ang = atan2(dy,dx);//angolo target in rad
ang1=pp.GetYaw(); //angolo del robot
delta=ang-ang1;//errore
pp.SetSpeed(0.0,delta);
if (fabs(delta)<0.05)
break;
}
dist=sqrt(dx*dx+dy*dy);
if (dist<0.8)
break;
}
• Esercizi:
• generazione di un cammino
• tracciamento traiettoria
Modello di un robot

Controllo



Basso/alto livello
Feedforward/feedback (pianificazione/reattivo)
Sensori


Propriocettivi
Esterocettivi
Attuatori (motori, riduttori…)
Effettori (ruote, manipolatori …)