25
2
Programmazione Player
Librerie C (libplayerc), C++ (libplayerclient), Tcl (tclPlayer), etc.
Ciclo di programmazione in Playerstage1. Connessione con il proxy
2. Sottoscrivere i device.
3. Leggi i sensori.
4. Elabora i dati.
5. Invia i comandi agli attuatori.
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 (1)
# 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
X
lp[179]
lp[0]
laser
lp[90]
davanti
sinistra
destra
Esempio: simple.world (2)
# 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 )
)
Robot Pioneer (pioneer.inc) (1)# 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 ]
spose[7] [ 0.075 -0.130 -90 ]
spose[8] [ -0.155 -0.130 -90 ]
spose[9] [ -0.195 -0.115 -130 ]Y
spose[10] [ -0.230 -0.080 -150 ]
spose[11] [ -0.250 -0.025 -170 ]
spose[12] [ -0.250 0.025 170 ]
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]
)
Y
X
0 1 23
4
567
sonar
Robot Pioneer (pioneer.inc) (2)# 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 ]
}
...
Y
X
Descrizione dell’ambiente
Esempi di ambienti (files png in stage-2.0.1)
Creazione di un ambiente
cave.png autolab.png 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 baseint main(int argc, char *argv[])
{
using namespace PlayerCc;
double ss,phi;
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(); //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,i) e raggiunge una configurazione finale (xf,yf,f)
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:
Dist (distanza desiderata)
Jog=K*(Dist – minR)
Legge di controllo:
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 <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; double dx,dy,dist;
PlayerClient robot("localhost"); SonarProxy sp(&robot,0); LaserProxy lp(&robot,0); Position2dProxy 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; }
for(;;) {
robot.Read();
dx=tx-pp.GetXPos(); dy=ty-pp.GetYPos();
dist=sqrt(dx*dx+dy*dy);
pp.SetSpeed(2.0,0);
std::cout << "target x,y = " << tx <<" " << ty << " dist" << dist << std::endl;
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 …)
