1 Player Stage - Programmazione E.Mumolo, DEEI

Presentazione sul tema: "1 Player Stage - Programmazione E.Mumolo, DEEI"— Transcript della presentazione:

1 1 Player Stage - Programmazione E.Mumolo, DEEI mumolo@units.it

2 2 Programmazione Player Librerie C (libplayerc), C++ (libplayerclient), Tcl (tclPlayer), etc. Ciclo di programmazione in Playerstage 1. Connessione con il proxy 2. Sottoscrivere i device. 3. Leggi i sensori. 4. Elabora i dati. 5. Invia i comandi agli attuatori.

3 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)

4 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" )

5 Descrizione del mondo per Stage Esempio: simple.world Tutte le informazioni sullambiente Specifica i sensori e attuatori La mappa dellambiente Parametri della simulazione Risoluzione (dimensione di un pixel) Intervallo di tempo di simulatione …

6 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

7 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 ) )

8 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 2 3 4 5 6 7 sonar

9 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

10 Descrizione dellambiente Esempi di ambienti (files png in stage-2.0.1) Creazione di un ambiente cave.pngautolab.pngsimple_room.png

11 Compilazione/esecuzione Librerie utente Variabile dambiente: 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

12 Principali programmi di utilità Dgps_server Playercam Playerjoy joystick Playernav Playerprint Playerv visualizza quello che il robot vede

13 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(), …

14 Sick LMS-200 e Sonar

15 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 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

16 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 (x i,y i, i ) e raggiunge una configurazione finale (x f,y f, f ) Inseguimento della traiettoria (trajectory tracking ): il robot deve raggiungere e seguire un cammino geometrico con unassegnata legge temporale. Inseguimento del cammino (path following): il robot deve seguire un particolare cammino geometrico senza specifiche temporali. Inseguimento obiettivo (target tracking)

17 Sonar Obstacle Avoidance (primitivo) #include 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

18 Laser Obstacle Avoidance (primitivo) #include #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 lp[j]) minR = lp[j]; } for (uint j = count/2; j lp[j]) minL = lp[j]; } jog=5*(minL - minR); std::cout << "Jog" << jog << std::endl; pp.SetSpeed(0.5, jog); }

19 Random walk (1) #include 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) GetScan(7)+sp->GetScan(8)) newturnrate = -turnrate; else newturnrate = turnrate; } else

20 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); }

21 Wall following Esercizio: Dist (distanza desiderata) Jog=K*(Dist – minR) Legge di controllo: minR

22 Moto punto-punto (1) #include 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); }

23 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

24 Moto punto-punto (3) #include 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

25 Modello di un robot Controllo Basso/alto livello Feedforward/feedback (pianificazione/reattivo) Sensori Propriocettivi Esterocettivi Attuatori (motori, riduttori…)Effettori (ruote, manipolatori …)