1 / 25

Player Stage - Programmazione

Player Stage - Programmazione. E.Mumolo, DEEI mumolo@units.it. Programmazione Player. Librerie C (libplayerc), C++ (libplayerclient), Tcl (tclPlayer), etc. Ciclo di programmazione in Playerstage Connessione con il proxy Sottoscrivere i device. Leggi i sensori. Elabora i dati.

sorena
Download Presentation

Player Stage - Programmazione

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

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

  2. Programmazione Player • Librerie • C (libplayerc), C++ (libplayerclient), Tcl (tclPlayer), etc. • Ciclo di programmazione in Playerstage • Connessione con il proxy • Sottoscrivere i device. • Leggi i sensori. • Elabora i dati. • 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 sull’ambiente • Specifica i sensori e attuatori • La mappa dell’ambiente • 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 sinistra laser lp[179] davanti lp[90] X lp[0] 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 sonar 0 1 2 3 4 X 5 6 7

  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 dell’ambiente • Esempi di ambienti (files png in stage-2.0.1) • Creazione di un ambiente cave.png autolab.png simple_room.png

  11. Compilazione/esecuzione • Librerie utente • Variabile d’ambiente: exportPKG_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 (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)

  17. 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

  18. 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); } }

  19. 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

  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) minR Legge di controllo: Jog=K*(Dist – minR)

  22. 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); } }

  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) 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; } #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; } • 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 …)

More Related