Rusu Dissertation Thesis

UNIVERSITATEA TEHNIC DIN CLUJ-NAPOCA
FACULTATEA DE AUTOMATIC I CALCULATOARE

D
D
I
I
S
S
E
E
R
R
T
T
A
A
I
I
E
E

Absolvent: ing. Radu Bogdan Rusu

ndrumtor: prof. dr. ing. Gheorghe Lazea

2005

Arhitecturi moderne pentru
roboi mobili:
J
J
a
a
v
v
a
a
c
c
l
l
i
i
e
e
n
n
t
t i
Z
Z
e
e
e
e
R
R
O
O

- Arhitecturi moderne pentru roboi mobili: J Ja av va ac cl li ie en nt t i Z Ze ee eR RO O -
Cuprins
1. Introducere .......................................................................................................................... 1
2. Roboi mobili ....................................................................................................................... 3
2.1. Considerente generale ............................................................................................. 4
2.2. Sisteme de locomoie ............................................................................................... 5
2.2.1. Sisteme de locomoie difereniale ................................................................ 6
2.2.2. Sisteme de locomoie sincrone ..................................................................... 8
2.2.3. Sisteme de locomoie tip triciclet............................................................... 8
2.2.4. Sisteme de locomoie Ackerman................................................................ 10
2.3. Sisteme senzoriale ................................................................................................... 11
2.3.1. Senzori de contact.......................................................................................... 13
2.3.2. Senzori IR......................................................................................................... 13
2.3.3. Senzori ultrasonici .......................................................................................... 16
2.3.4. Senzori laser .................................................................................................... 19
2.3.5. Senzori GPS ..................................................................................................... 19
2.3.6. Senzori piroelectrici ....................................................................................... 20
2.3.7. Senzori video................................................................................................... 20
2.3.8. Senzori de stare intern................................................................................ 21
2.3.9. Fuziunea datelor de la senzori ..................................................................... 22
2.4. Uniti centrale de procesare ............................................................................... 23
2.5. Sisteme de comunicare........................................................................................... 24
2.6. Inteligen, autonomie i control ......................................................................... 25
3. Player/Stage ........................................................................................................................ 31
3.1. Considerente generale ........................................................................................... 32
3.2. Arhitectura Player ................................................................................................... 32
3.3. Simulatorul 2D Stage............................................................................................... 34
3.4. Simulatorul 3D Gazebo.......................................................................................... 38
3.5. Adugarea noilor dispozitive................................................................................. 43
3.6. Metode de interfaare cu clienii .......................................................................... 46
4. Javaclient ............................................................................................................................. 48
4.1. Considerente generale ........................................................................................... 49
4.2. Arhitectura Javaclient.............................................................................................. 49
4.3. Suport pentru dispozitive noi ............................................................................... 55
4.4. Exemple de utilizare................................................................................................ 56
5. ZeeRO................................................................................................................................. 59
5.1. Arhitectura sistemului ............................................................................................ 60
Cuprins
5.2. Sistemul de locomoie ............................................................................................ 61
5.3. Sistemul senzorial .................................................................................................... 63
5.4. Unitatea central de procesare ............................................................................ 66
5.5. Programarea robotului ZeeRO............................................................................ 71
6. Concluzii ............................................................................................................................. 73
Bibliografie................................................................................................................................... 75
Anexe ...............................................................................................................................................I
A. Diagrama UML a claselor proiectului Javaclient ........................................................I
B. Codul surs al proiectului Javaclient 1.6.4................................................................ II
j ja av va ac cl li ie en nt t/ /A Ab bs st tr ra ac ct tP Po os si it ti io on nD De ev vi ic ce e. .j ja av va a............................................................... II
j ja av va ac cl li ie en nt t/ /A AI IO OI In nt te er rf fa ac ce e. .j ja av va a..................................................................................... II
j ja av va ac cl li ie en nt t/ /A Au ud di io oD DS SP PI In nt te er rf fa ac ce e. .j ja av va a....................................................................... III
j ja av va ac cl li ie en nt t/ /A Au ud di io oI In nt te er rf fa ac ce e. .j ja av va a ................................................................................ VI
j ja av va ac cl li ie en nt t/ /A Au ud di io oM Mi ix xe er rI In nt te er rf fa ac ce e. .j ja av va a...................................................................VII
j ja av va ac cl li ie en nt t/ /B Bl li in nk ke en nl li ig gh ht tI In nt te er rf fa ac ce e. .j ja av va a................................................................... IX
j ja av va ac cl li ie en nt t/ /B Bl lo ob bf fi in nd de er rI In nt te er rf fa ac ce e. .j ja av va a...................................................................... XI
j ja av va ac cl li ie en nt t/ /B Bu um mp pe er rI In nt te er rf fa ac ce e. .j ja av va a......................................................................... XIII
j ja av va ac cl li ie en nt t/ /C Ca am me er ra aI In nt te er rf fa ac ce e. .j ja av va a.........................................................................XVI
j ja av va ac cl li ie en nt t/ /D DI IO OI In nt te er rf fa ac ce e. .j ja av va a.............................................................................. XVIII
j ja av va ac cl li ie en nt t/ /E En ne er rg gy yI In nt te er rf fa ac ce e. .j ja av va a .......................................................................... XIX
j ja av va ac cl li ie en nt t/ /F Fi id du uc ci ia al lI In nt te er rf fa ac ce e. .j ja av va a......................................................................... XXI
j ja av va ac cl li ie en nt t/ /G GP PS SI In nt te er rf fa ac ce e. .j ja av va a............................................................................. XXVI
j ja av va ac cl li ie en nt t/ /G Gr ri ip pp pe er rI In nt te er rf fa ac ce e. .j ja av va a ................................................................... XXVIII
j ja av va ac cl li ie en nt t/ /I IR RI In nt te er rf fa ac ce e. .j ja av va a.................................................................................. XXIX
j ja av va ac cl li ie en nt t/ /J Jo oy ys st ti ic ck kI In nt te er rf fa ac ce e. .j ja av va a.....................................................................XXXII
j ja av va ac cl li ie en nt t/ /L La as se er rI In nt te er rf fa ac ce e. .j ja av va a.........................................................................XXXIII
j ja av va ac cl li ie en nt t/ /L Lo oc ca al li iz ze eI In nt te er rf fa ac ce e. .j ja av va a................................................................. XXXVII
j ja av va ac cl li ie en nt t/ /L Lo og gI In nt te er rf fa ac ce e. .j ja av va a ................................................................................... XL
j ja av va ac cl li ie en nt t/ /M Ma ap pI In nt te er rf fa ac ce e. .j ja av va a................................................................................ XLII
j ja av va ac cl li ie en nt t/ /M MC Co om mm mI In nt te er rf fa ac ce e. .j ja av va a ......................................................................XLV
j ja av va ac cl li ie en nt t/ /M Mo ot to or rI In nt te er rf fa ac ce e. .j ja av va a.......................................................................... XLIX
j ja av va ac cl li ie en nt t/ /N No om ma ad dI In nt te er rf fa ac ce e. .j ja av va a ........................................................................... LIII
j ja av va ac cl li ie en nt t/ /P Pl la an nn ne er rI In nt te er rf fa ac ce e. .j ja av va a............................................................................LV
j ja av va ac cl li ie en nt t/ /P Pl la ay ye er rC Cl li ie en nt t. .j ja av va a................................................................................. LVIII
j ja av va ac cl li ie en nt t/ /P Pl la ay ye er rD De ev vi ic ce e. .j ja av va a ........................................................................... LXXVI
Cuprins
j ja av va ac cl li ie en nt t/ /P Po os si it ti io on n2 2D DI In nt te er rf fa ac ce e. .j ja av va a........................................................... LXXVIII
j ja av va ac cl li ie en nt t/ /P Po os si it ti io on n3 3D DI In nt te er rf fa ac ce e. .j ja av va a.......................................................... LXXXIV
j ja av va ac cl li ie en nt t/ /P Po os si it ti io on nI In nt te er rf fa ac ce e. .j ja av va a........................................................................ XCI
j ja av va ac cl li ie en nt t/ /P Po ow we er rI In nt te er rf fa ac ce e. .j ja av va a ...................................................................... XCVIII
j ja av va ac cl li ie en nt t/ /P Pt tz zI In nt te er rf fa ac ce e. .j ja av va a............................................................................... XCIX
j ja av va ac cl li ie en nt t/ /S Si im mu ul la at ti io on nI In nt te er rf fa ac ce e. .j ja av va a.................................................................... CII
j ja av va ac cl li ie en nt t/ /S So on na ar rI In nt te er rf fa ac ce e. .j ja av va a..............................................................................CIV
j ja av va ac cl li ie en nt t/ /S So ou un nd dI In nt te er rf fa ac ce e. .j ja av va a........................................................................... CVII
j ja av va ac cl li ie en nt t/ /S Sp pe ee ec ch hI In nt te er rf fa ac ce e. .j ja av va a........................................................................ CVIII
j ja av va ac cl li ie en nt t/ /S Sp pe ee ec ch hR Re ec co og gn ni it ti io on nI In nt te er rf fa ac ce e. .j ja av va a................................................. CIX
j ja av va ac cl li ie en nt t/ /T Tr ru ut th hI In nt te er rf fa ac ce e. .j ja av va a.............................................................................. CX
j ja av va ac cl li ie en nt t/ /W Wa av ve ef fo or rm mI In nt te er rf fa ac ce e. .j ja av va a................................................................. CXII
j ja av va ac cl li ie en nt t/ /W Wi iF Fi iI In nt te er rf fa ac ce e. .j ja av va a............................................................................ CXIV
j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /B Bl lo ob b. .j ja av va a...........................................................................CXV
j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /H Hy yp po ot th he es si is s. .j ja av va a.......................................................... CXVII
j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rB Bu um mp pe er rD De ef fi in ne eT T. .j ja av va a ................................ CXVIII
j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rB Bu um mp pe er rG Ge eo om mT T. .j ja av va a.................................... CXX
j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rD De ev vi ic ce eD De ev vl li is st tT T. .j ja av va a .................................... CXXI
j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rD De ev vi ic ce eD Dr ri iv ve er rI In nf fo o. .j ja av va a ...............................CXXII
j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rD De ev vi ic ce eI Id dT T. .j ja av va a.............................................CXXIII
j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rF Fi id du uc ci ia al lI It te em m. .j ja av va a....................................... CXXIV
j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rI IR RP Po os se eR Re eq qT T. .j ja av va a......................................... CXXV
j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rL La as se er rC Co on nf fi ig gT T. .j ja av va a..................................... CXXVI
j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rL La as se er rG Ge eo om mT T. .j ja av va a.................................... CXXVIII
j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rL Lo oc ca al li iz ze eC Co on nf fi ig gT T. .j ja av va a...............................CXXIX
j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rP Pl la an nn ne er rW Wa ay yp po oi in nt tT T. .j ja av va a.........................CXXIX
j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rP Po os si it ti io on n2 2D DG Ge eo om mT T. .j ja av va a........................... CXXX
j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rP Po os si it ti io on n3 3D DG Ge eo om mT T. .j ja av va a..........................CXXXI
j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rP Po os si it ti io on nG Ge eo om mT T. .j ja av va a...............................CXXXII
j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rS Si im mu ul la at ti io on nP Po os se e2 2D D. .j ja av va a........................ CXXXIII
j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rS So on na ar rG Ge eo om mT T. .j ja av va a.................................. CXXXIV
j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rW Wi iF Fi iL Li in nk kT T. .j ja av va a......................................... CXXXV
j ja av va ac cl li ie en nt t/ /e ex xt tr ra a/ /C Co on nt tr ro ol ll le er r. .j ja av va a ..............................................................CXXXVIII
j ja av va ac cl li ie en nt t/ /e ex xt tr ra a/ /H He ea ad di in ng gC Co on nt tr ro ol l. .j ja av va a ..................................................CXXXVIII
j ja av va ac cl li ie en nt t/ /e ex xt tr ra a/ /P PC Co on nt tr ro ol ll le er r. .j ja av va a .................................................................... CXLI
Cuprins
j ja av va ac cl li ie en nt t/ /e ex xt tr ra a/ /P PD DC Co on nt tr ro ol ll le er r. .j ja av va a................................................................CXLII
j ja av va ac cl li ie en nt t/ /e ex xt tr ra a/ /P PI IC Co on nt tr ro ol ll le er r. .j ja av va a.................................................................CXLIII
j ja av va ac cl li ie en nt t/ /e ex xt tr ra a/ /P PI ID DC Co on nt tr ro ol ll le er r. .j ja av va a............................................................. CXLV
j ja av va ac cl li ie en nt t/ /e ex xt tr ra a/ /P Po os si it ti io on nC Co on nt tr ro ol l. .j ja av va a......................................................... CXLVI
j ja av va ac cl li ie en nt t/ /e ex xt tr ra a/ /P Po os si it ti io on nG Ge eo om me et tr ry yT To oo ol ls s. .j ja av va a........................................CXLIX
C. Codul surs al driverului CMUcam2............................................................................I
c ca am me er ra a. .h h ..............................................................................................................................I
c ca am me er ra a. .c c............................................................................................................................ III
c cm mu uc ca am m2 2. .c c...................................................................................................................... IX
D. Codul surs al driverului ZeeRO..................................................................................I
b br ra ai in ns st te em m. .h h.........................................................................................................................I
b br ra ai in ns st te em m. .c c...................................................................................................................... III
z ze ee er ro o. .h h .............................................................................................................................. IX
z ze ee er ro o. .c cp pp p ...........................................................................................................................X
Lista de figuri
Figura 1. Clasificarea vehiculelor robot................................................................................... 4
Figura 2. Sistem de locomoie diferenial................................................................................ 6
Figura 3. Sistem de locomoie sincron..................................................................................... 8
Figura 4. Sistem de locomoie tip triciclet ............................................................................ 9
Figura 5. Sistem de locomoie Ackerman ............................................................................. 10
Figura 6. Activarea senzorilor bumper n momentul ciocnirii robotului cu un obiect 13
Figura 7. Principiul de funcionare al unui senzor IR.......................................................... 14
Figura 8. Obiecte din mediu care pot sau nu pot fi detectate de ctre senzorul IR... 14
Figura 9. Obiectele de culoare deschis reflect puternic fascicolele de lumin din
spectrul infrarou....................................................................................................................... 15
Figura 10. Obiectele de culoare nchis reflect foarte slab fascicolele de lumin din
spectrul infrarou....................................................................................................................... 15
Figura 11. Sensibilitatea senzorului Devantech SRF08....................................................... 17
Figura 12. Sensibilitatea senzorului Devantech SRF10....................................................... 17
Figura 13. Exemplu de configuraie a senzorilor sonar la un robot mobil .................... 18
Figura 14. Modul de funcionare al unui senzor piroelectric............................................ 20
Figura 15. Exemplu de reea neuronal folosit la procesul de fuziune al senzorilor. 23
Figura 16. Elementele coordinative ale unui robot mobil ................................................. 26
Figura 17. Funcionarea navigatorului .................................................................................... 26
Figura 18. Schema arhitectural avansat a navigatorului .................................................. 27
Figura 19. Schem simpl de reglare n bucl deschis....................................................... 27
Figura 20. Schem de reglare n bucl nchis ...................................................................... 28
Figura 21. Schem de reglare folosind un controler PID.................................................. 28
Figura 22. Arhitectura sense-think-act .................................................................................. 29
Figura 23. Arhitectura behavior-based .................................................................................. 29
Figura 24. Arhitectura global a serverului Player .............................................................. 33
Figura 25. Arhitectura Player/Stage/Gazebo......................................................................... 33
Figura 26. Exemplu de mediu virtual construit pentru simulatorul Stage...................... 38
Figura 27. Arhitectura sistemului Gazebo............................................................................. 39
Figura 28. Exemplu de mediu virtual construit pentru simulatorul Gazebo ................. 39
Figura 29. Interacionnd cu mediul ....................................................................................... 40
Figura 30. Robotul NASA de tip paianjen ......................................................................... 40
Figura 31. Robot de tip Segway RMP simulat....................................................................... 40
Figura 32. Model al lumii virtuale n simulatorul Gazebo .................................................. 42
Figura 33. Imagine preluat de la o camer virtual instalat pe robotul mobil ........... 43
Lista de figuri
Figura 34. Accesarea modulelor Player din programele client....................................... 43
Figura 35. Legturile dintre drivere i interfee n Player 1.6.4........................................ 46
Figura 36. Modelul client-server Player ................................................................................. 46
Figura 37. Schema de comunicare ntre Player i librria client....................................... 47
Figura 38. Arhitectura pachetelor Javaclient ........................................................................ 50
Figura 39. Diagrama UML a claselor proiectului (schem de principiu)......................... 54
Figura 40. Arhitectura general a robotului mobil ZeeRO............................................... 61
Figura 41. Componentele bazei robot Rogue Blue............................................................. 62
Figura 42. Servomotor folosit de robotul ZeeRO.............................................................. 62
Figura 43. Senzorul IR Sharp GP2D02................................................................................... 64
Figura 44. Senzorul Devantech SRF08................................................................................... 64
Figura 45. Senzorul Devantech SRF10................................................................................... 64
Figura 46. Pachetul senzor piroelectric ................................................................................. 65
Figura 47. Camera video CMUcam2...................................................................................... 65
Figura 48. CMUcam2 - vedere frontal................................................................................. 66
Figura 49. CMUcam2 - vedere spate...................................................................................... 66
Figura 50. Platforma Gumstix Waysmall ............................................................................... 67
Figura 51. Conexiunile sistemului central de procesare.................................................... 68
Figura 52. Microcontrolerul Acroname Brainstem GP 1.0............................................... 68
Figura 53. Microcontrolerul Acroname Moto 1.0 .............................................................. 68
Figura 54. Interfaarea senzorilor IR la microcontrolerul Brainstem GP 1.0................ 69
Figura 55. Interfaarea senzorilor SONAR la microcontrolerul Brainstem GP 1.0 .... 70
Figura 56. Interfaarea senzorului Eltec 442-3 la microcontrolerul Brainstem Moto. 70
Figura 57. Modurile de programare ale robotului mobil ZeeRO.................................... 71
Figura 58. Arhitectura software Player a robotului ZeeRO............................................. 72
Introducere
pagina 1
1
1
.
.
I
I
n
n
t
t
r
r
o
o
d
d
u
u
c
c
e
e
r
r
e
e

Introducere
pagina 2
Roboii mobili reprezint un subiect de cercetare n plin ascensiune. Folosii
iniial n proiecte aparinnd departamentelor de aprare din armat, i apoi n
industrie pe post de vehicule ghidate autonom, roboii mobili constituie mai nou
atracia principal a omenirii secolului XXI.
Proiecte desfurate cu succes de corporaii precum Sony (Aibo, Qrio) sau
Honda (Asimo P2, P3) au constatat o capacitate mare de adopie a utilizatorilor
pentru roboi n medii familiale. Muli specialiti n domeniu presupun deja c pn n
anul 2050, n fiecare cas de pe planet va exista cel puin un robot cu form
humanoid.
Studiile arat ns c pentru a putea implementa concepte din domenii
precum inteligena artificial, este necesar construirea unor arhitecturi modulare,
scalabile i eficiente, att pentru componentele hardware ale robotului ct i pentru
platformele software pe care acesta le utilizeaz. Aceste aspecte sunt nc la
nceputul dezvoltrii lor, momentan existnd destul de multe probleme i
incompatibiliti ntre diferitele standarde impuse de productori.
Lucrarea de fa i propune studiul unui nou tip de robot mobil, construit n
cadrul grupului de cercetare n robotic, parte component a Facultii de
Automatic i Calculatoare din Universitatea Tehnic din Cluj-Napoca. Unele
concepte i idei utilizate n construcia acestui robot mobil sunt inovatoare, iar
arhitectura propus ofer un grad sporit de modularitate pentru robot.
Robotul mobil ns nu ar fi pe departe la fel de eficient, dac nu ar fi susinut
de o platform software adecvat. Autorul acestei lucrri propune folosirea i
dezvoltarea platformei software robot Player/Stage ca i coloana vertebral a
sistemului software al robotului mobil. Modularitatea i scalabilitatea acestei
platforme, combinate cu concepte precum stabilitatea i portabilitatea limbajului de
programare Java au dus la dezvoltarea unui client nou pentru platforma Player/Stage,
numit Javaclient.
Efortul imens realizat de comunitatea open-source a dus la o dezvoltare
exploziv a platformelor software mai sus menionate. Un rezultat clar al cooperrii
diferitelor grupuri de cercetare din universiti din ntreaga lume este reprezentat de
integrarea eficient a unora dintre cele mai folosite programe, reuind astfel s ofere
utilizatorilor finali posibilitatea de a reutiliza codul deja scris i de a dezvolta software
pentru sisteme formate din roboi heterogeni.
Pe parcursul lucrrii se vor studia att conceptele eseniale care stau la baza
construciei unui robot mobil, ct i modul n care acestea s-au materializat prin
construcia robotului mobil ZeeRO. De asemenea, se va face o prezentare i
evaluare a platformei Player/Stage, urmat de o descriere amnunit a programului
Javaclient. Codurile surs utilizate la dezvoltarea celor dou proiecte sunt incluse n
anex.

Roboi mobili
pagina 3
2
2
.
.
R
R
o
o
b
b
o
o
i
i
m
m
o
o
b
b
i
i
l
l
i
i

Roboi mobili
pagina 4
2 2. .1 1. . C Co on ns si id de er re en nt te e g ge en ne er ra al le e
Pentru a putea defini un vehicul robot, trebuie n primul rnd fcut apel la una
din definiiile de baz ale unui robot: un dispozitiv mecanic care poate fi programat
pentru a ndeplini anumite sarcini ce-i sunt date prin control automat. [8]
Cuvintele cheie din cadrul definiiei de mai sus sunt: programat, respectiv
control automat. Prin prisma acestora, se enun definiia unui vehicul robot:
Un vehicul robot este un vehicul echipat cu un mecanism de
propulsie, cu capacitatea de a fi programat pentru locomoie sub
control automat, funcional pentru o anumit sarcin dat.
n practic exist o diversitate mare de vehicule robot. O clasificare sumar a
acestora este prezentat n figura 1. [8]

Figura 1. Clasificarea vehiculelor robot
O prim clasificare care se poate observa din figura de mai sus, rezult din
faptul c se face o difereniere ntre vehiculele ghidate, precum AGV-urile (en.
Automated Guided Vehicles), i cele neghidate. Un vehicul ghidat este de regul limitat
la un set de traiectorii predefinite n spaiul su de lucru. Aceste traiectorii pot fi
marcate prin ine, linii optice sau magnetice, sau pur i simplu pot fi alctuite dintr-o
serie de poziii programate aprioric n memorie. Vehiculele ghidate nu pot, sub nici o
form, s prseasc traiectoria prestabilit.
O a doua clasificare are la baz mediul n care opereaz vehicolul robot. n
timp ce vehiculele robot ghidate sunt terestre, cele neghidate pot aciona i sub ap
Roboi mobili
pagina 5
respectiv n spaiu. Cea mai mare categorie ns este cea a vehiculelor robot
neghidate terestre, care la rndul lor se pot clasifica n funcie de tipul de sistem de
locomoie. Dintre toate sistemele de locomoie, cele mai des ntlnite sunt cele cu
roi.
n literatura de specialitate, vehiculele robot neghidate terestre cu roi se mai
numesc i roboi mobili (en. Wheeled Mobile Robots) sau vehicule robot mobile
autonome.
Un robot mobil este alctuit dintr-o serie de componente, unele de natur
fizic (hardware), iar altele de natur logic, computaional (software). Din punct de
vedere al componentelor hardware, un robot mobil poate fi considerat [7] ca o
colecie de sisteme pentru:
Locomoie - modalitatea prin care robotul se mic n mediul su ambiant;
Sesizare (sistem senzorial): - modalitatea prin care robotul msoar
proprietile lui i ale mediului nconjurtor;
Procesare: - modalitatea prin care robotul proceseaz i ia decizii n
legtur cu informaiile primite de la sistemul senzorial;
Comunicare: - modalitatea prin care robotul comunic cu ali roboi sau cu
un operator din mediu.
2 2. .2 2. . S Si is st te em me e d de e l lo oc co om mo o i ie e
Locomoia este procesul care i permite robotului mobil s se deplaseze prin
mediu, prin acionarea anumitor fore asupra sa. Studiul aciunii acestor fore se
numete dinamic, iar studiul formulelor matematice asociate micrii, fr a
considera forele fizice, se numete cinematic. [7]
Una din tehnicile de estimare a poziiei unui robot mobil n mediul su se
numete odometrie. Prin odometrie se calculeaz distana parcurs de un robot mobil
n funcie de ct de mult s-au rotit roile acestuia. n cazul unei roi ideale, la fiecare
rotaie a acesteia se poate considera c robotul a parcurs o distan egal cu 2r,
unde r reprezint raza roii respective. n practic ns, datorit forelor de frecare i
a aluncrilor, estimrile sunt mult mai puin precise.
Un robot mobil aflat ntr-un plan are 3 grade de libertate: o poziie (x, y) i o
orientare fa de axa orizontal. Tripletul (x, y, ) se mai numete i poziia
(relativ sau absolut) a robotului mobil (en. robot pose) i constituie variabila
efectiv de control a sistemului de locomoie.
Datorit faptului c un robot mobil nu are control complet asupra celor trei
variabile, o serie de manevre mai mult sau mai puin complexe trebuie ndeplinite
pentru ca robotul mobil s ajung dintr-o anumit poziie n alta. n literatura de
specialitate, poziia de start se noteaz cu (x
s
, y
s
,
s
), iar poziia unde trebuie s
ajung robotul cu (x
g
, y
g
,
g
). (en. goal) ([13], [14])
n dotarea unor roboi mobili, pe lng roile motoare, se regsesc fie roi
adiionale fie alte puncte de contact care nu fac altceva dect s asigure suportul
Roboi mobili
pagina 6
robotului. Cele mai des ntlnite sunt roile de tip castor, roi care nu sunt de obicei
considerate n ecuaiile cinematice ale robotului mobil.
Sistemele de locomoie pot avea proprieti i componente diferite unul fa
de cellalt. n continuare se prezint cteva dintre cele mai populare sisteme de
locomoie ale roboilor mobili (vezi [7]).
2 2. .2 2. .1 1. . S Si is st te em me e d de e l lo oc co om mo o i ie e d di if fe er re en n i ia al le e
Sistemele difereniale (en. differential drive) sunt printre cele mai simple
sisteme de locomoie ntlnite la un robot mobil. Un sistem diferenial este alctuit
din dou roi montate pe un ax comun controlate prin dou motoare separate (vezi
figura 2).

Figura 2. Sistem de locomoie diferenial
Cinematica se ocup de relaiile ntre parametrii de control i
comportamentul sistemului n spaiul strilor. n cadrul unui sistem de locomoie
diferenial, robotul trebuie s se roteasc n jurul unui punct care se afl pe axa
comun a celor dou roi motoare. Prin modificarea vitezelor celor dou roi,
traiectoria de rotaie poate fi modificat. Vitezele celor dou roi trebuie s respecte
urmtoarea relaie:
=
= +
s
d
v
l
R
v
l
R
)
2
(
)
2
(

Unde v
s
reprezint viteza roii din partea stng, v
d
viteza roii din partea
dreapt, R reprezint distana dintre mijlocul axei celor dou roi i centrul de
curbur CC, viteza unghiular, iar l distana ntre cele dou roi. Unghiul de rotaie
este reprezentat de . Rezolvarea sistemului de ecuaii de mai sus duce la
urmtoarele soluii:
l
v v
v v
v v l
R
s d
s d
s d

=
+
= ,
) (
) (
2

Roboi mobili
pagina 7
Un caz special l constituie v
s
=v
d
. Distana R devine n acest caz infinit, deci
robotul se va mica n linie dreapt. Dac v
s
=-v
d
, distana R devine 0, iar robotul se va
mica pe loc n jurul mijlocului axei l. Pentru orice alte valoari ale lui v
s
i v
d
, robotul
se va roti pe un cerc de raz R fa de centrul de curbur.
O multitudine de puncte diferite de punctul de start se pot atinge prin
alegerea vitezelor v
s
i v
d
. Determinarea unui punct care poate fi atins de robot prin
manipularea parametrilor de control poarta numele de ecuaiile cinematice directe
ale robotului.
t
y
x
y y
x x
t t
t t
y
x
CC
CC
CC
CC

*
1 0 0
0 ) cos( ) sin(
0 ) sin( ) cos(
'
'
'

Ecuaia de mai sus descrie micarea de rotaie a unui robot la o distan R
dintre mijlocul axei celor dou roi i centrul de curbur CC cu viteza unghiular .
Prin integrarea ecuaiei de mai sus, pornind de la un set de condiii iniiale (x
0
,
y
0
,
0
), se poate determina poziia robotului la un timp t, pe baza parametrilor de
control v
1
(t) i v
2
(t).
[ ] [ ]
[ ] [ ]
[ ]
=
+ =
+ =
t
s d
t
s d
t
s d
dt t v t v
l
t
dt t t v t v t y
dt t t v t v t x
0
0
0
) ( ) (
1
) (
) ( sin ) ( ) (
2
1
) (
) ( cos ) ( ) (
2
1
) (

Determinarea parametrilor de control (vitezele v
s
i v
d
) pentru a ajunge ntr-
un anumit punct deja stabilit, poart numele de ecuaiile cinematice inverse ale
robotului. Datorit faptului c rezolvarea sistemului de ecuaii de mai sus este
anevoioas, se propun dou cazuri speciale de micare a robotului cu sistem de
locomoie diferenial.
n primul caz, dac se egaleaz v
s
cu v
d
= v, ecuaia de mai sus devine:
+
+
=
t v y
t v x
y
x
) sin(
) cos(
'
'
'

n al doilea caz, dac se alege v
s
= -v
d
= v, ecuaia devine:
+
=
l
t v
y
x
y
x
2
'
'
'

Astfel, pentru a deplasa un robot din (x
s
, y
s
, s) n (x
g
, y
g
,
g
), cu
g

s
, se
poate folosi legea a doua de control (v
s
= -v
d
) pn n momentul n care
g
=
s
,
urmnd apoi a deplasa robotul folosind prima lege de control (v
s
= v
d
).
Roboi mobili
pagina 8
2 2. .2 2. .2 2. . S Si is st te em me e d de e l lo oc co om mo o i ie e s si in nc cr ro on ne e
ntr-un sistem de locomoie sincron, fiecare roat poate fi controlat separat.
Configuraiile tipice de sisteme de locomoie sincrone presupun existena a trei roi
motoare aezate pe vrfurile unui triunghi echilateral (vezi figura 3).

Figura 3. Sistem de locomoie sincron
Un tip de aranjament mecanic pentru un robot cu sistem de locomoie
sincron este de a folosi dou motoare, unul pentru a roti toate roile ntr-o direcie,
iar cel de-al doilea pentru a le mica nainte sau napoi.
Datorit faptului c toate roile rmn paralele, roboii cu locomoie sincron
se rotesc n jurul centrului lor geometric.
Ecuaiile cinematice directe pentru un robot cu sistem de locomoie sincron,
care se rotete cu viteza unghiular i se deplaseaz cu viteza v, sunt:
[ ]
[ ]
=
=
=
t
t
t
dt t t
dt t t v t y
dt t t v t x
0
0
0
) ( ) (
) ( sin ) ( ) (
) ( cos ) ( ) (

2 2. .2 2. .3 3. . S Si is st te em me e d de e l lo oc co om mo o i ie e t ti ip p t tr ri ic ci ic cl le et t
Un sistem tip triciclet este alctuit din 3 roi, dintre care dou n spate,
pasive, i o roat n fa, motoare, cu ajutorul creia se poate stabili viteza de
deplasare precum i direcia vehicolului. Robotul tip triciclet este controlat prin
unghiul , respectiv prin viteza de deplasare v (vezi figura 4).
Roboi mobili
pagina 9

Figura 4. Sistem de locomoie tip triciclet
Dac roata din fa este poziionat la un unghi , robotul se va roti cu viteza
unghiular , la o distan R pe direcia unei linii perpendiculare i care trece prin
roile din spate. R i sunt date de:
( )
2
1
2 2
,
2
tan *
R d
v
d R
+
=

Ca i n cazul sistemului de locomoie diferenial, ecuaiile cinematice inverse
sunt destul de complicate. Din aceleai considerente se analizeaz dou cazuri
speciale.
Dac =0, robotul se deplaseaz nainte, poziia lui fiind dat de:
+
+
=
t v y
t v x
y
x
) sin(
) cos(
'
'
'

n cazul al doilea, dac robotul este capabil s manevreze roata motoare la
unghiuri de +/- 90 grade, atunci robotul se poate ntoarce pe loc, iar poziia va fi dat
de:
d
t v
y
x
y
x
'
'
'

De notat faptul c, dac roata din fa nu se poate roti la unghiuri de +/- 90
grade, este imposibil de a schimba orientarea robotului fr a-i schimba poziia.
Roboi mobili
pagina 10
2 2. .2 2. .4 4. . S Si is st te em me e d de e l lo oc co om mo o i ie e A Ac ck ke er rm ma an n
Sistemele de locomoie Ackerman se regsesc n majoritatea automobilelor.
n modelul Ackerman, roile din fa se pot roti individual, pentru a putea modifica
distana fa de centrul de curbur aflat pe o linie perpendicular care trece prin
centrul roilor din spate (vezi figura 5).

Figura 5. Sistem de locomoie Ackerman
n sistemul de locomoie bazat pe modelul Ackerman, robotul se rotete n
jurul unui punct aflat pe dreapta perpendicular care trece prin mijlocul roilor de pe
spate, la o distan R, dat de relaia:
+ = +
1
2
tan *
2

d
l
R
Pentru ca robotul s fie n micare de rotaie, cea de-a doua roat motoare
trebuie rotit cu un unghi
2
, unde:
+ =
2
2
tan *
2

d
l
R
Mai multe detalii despre sistemul de locomoie Ackerman ct i despre alte
diverse sisteme pot fi gsite n [12].

Roboi mobili
pagina 11
2 2. .3 3. . S Si is st te em me e s se en nz zo or ri ia al le e
Senzorii sunt dispozitive care pot msura diferite proprieti ale mediului
precum: temperatura, distana, rezistena fizic, greutatea, mrimea, etc. [5].
Informaia primit de la acetia poate fi de cele mai multe ori contradictorie i
imprecis.
n cel mai general caz, senzorii pot fi mprii n dou categorii[7], i anume:
Senzori de stare intern senzori care ofer informaii despre starea
intern a robotului mobil, spre exemplu nivelul bateriei, poziia roilor, etc;
Senzori de stare extern senzori care ofer informaii despre mediul
ambiant n care robotul funcioneaz. Senzorii de stare extern se mai pot
mpri la rndul lor n dou categorii: senzori cu contact, mai precis acei
senzori care culeg informaia din mediu prin atingere (exemplu: senzor
bumper), respectiv senzori fr contact, care preiau informaia din mediu de
la distan (exemplu: camer video, senzor ultrasonic, etc).
Un senzor poate fi activ sau pasiv. Senzorii activi sunt acei senzori care emit
energie n mediu pentru a putea observa anumite caracteristici ale acestuia, spre
deosebire de senzorii pasivi care primesc energie din mediu pentru a putea prelua
informaia.
La modul general, despre toate categoriile de senzori se pot enuna
urmtoarele ipoteze:
Orice senzor este afectat de zgomot;
Orice senzor ofer o informaie incomplet a mediului n care efectueaz
msurtorile;
Nici un senzor nu poate fi modelat complet.

De asemenea, toate tipurile de senzori sunt caracterizate printr-o serie de
proprieti, cele mai importante fiind[5]:
Sensibilitatea: raportul dintre semnalul de ieire i semnalul de intrare;
Liniaritatea: exprim dac raportul dintre intrare i ieire este constant;
Intervalul de msurare: diferena ntre distana minim i maxim msurabil;
Timpul de rspuns: timpul necesar pentru ca informaia de la intrare s fie
observabil la ieire;
Acurateea: diferena ntre semnalul msurat i semnalul real;
Repetabilitatea: diferenele ntre msurtori succesive ale aceleiai entiti;
Rezoluia: exprim cea mai mic unitate de incrementare a semnalului
msurat;
Preul senzorului;
Puterea de calcul necesar pentru a interpreta rezultatele;
Roboi mobili
pagina 12
Tipul de semnal la ieire;
Greutatea, mrimea i cantitatea de energie consumat pentru a face o
msurtoare.

Relaia ntre proprietile fizice de interes e ale mediului i informaia primit
de la un senzor r ar putea fi modelat prin ecuaia:
r e f = ) (
n principiu, orice model al unui senzor ar trebui s includ i un model intern
al zgomotului care poate afecta senzorul n momentul citirii informaiei. Problema de
a recupera informaia din mediu din datele primite de la senzor poate fi destul de
complex.
Un senzor este considerat instabil dac pentru variaii mici ale intrrii, ieirea
se schimb radical. n caz general, pentru un senzor cu ieirea f(e), instabilitatea se
refer la:

+
h
h e f e f
h
) ( ) (
lim
0

n principiu orice tip de senzor poate fi afectat de mai multe tipuri de erori.
Dintre acestea, cele mai importante sunt erorile incidentale, erorile sistematice i
erorile stohastice. Erorile incidentale apar ocazional i pot avea un efect neprevzut
asupra informaiei, ele provenind n cea mai mare parte de la msurtori efectuate
greit. Erorile sistematice au o influen predictibil asupra acurateii informaiei,
acestea provenind de la o interpretare greit a parametrilor n algoritmii de
estimare, sau din cauza unor neconcordane (incertitudini) n modelare. n fine,
erorile stohastice, au un caracter aleator, ele diferind de fiecare dat cnd robotul
execut aceeai operaie.
n lumea roboilor mobili se ntlnesc o mare varietate de tipuri de senzori. O
clasificare de baz a acestora ar putea fi[7]:
senzori de distan acei senzori care ofer informaii despre distana
ntre senzor i obiectul de msurat din mediu;
senzori de poziie acei senzori care ofer informaii despre poziia
robotului n termeni absolui;
senzori de mediu acei senzori care ofer informaii despre diverse
proprieti i caracteristici ale mediului (exemplu: temperatur, culoare);
senzori ineriali acei senzori care msoar proprieti difereniale ale
poziiei robotului (exemplu: acceleraia).
O alt clasificare a senzorilor se poate face n funcie de tipul de semnal
primit, precum i de rolul senzorului n sistemul robotului mobil, mbinnd deci cele
dou clasificri de mai sus. n continuare, pentru descrierea diferitelor tipuri de
senzori, se va folosi aceast abordare.

Roboi mobili
pagina 13
2 2. .3 3. .1 1. . S Se en nz zo or ri i d de e c co on nt ta ac ct t
Cel mai des ntlnit tip de senzori de contact n lumea roboilor mobili este
reprezentat de senzorii bumper (en. to bump = a (se) lovii). Aceste dispozitive poti fi
simple ntreruptoare care raporteaz o valoare binar, 0 sau 1, valoare
corespondent strii de fapt: deschis sau nchis. Astfel, n momentul n care robotul
echipat cu senzori de acest tip se lovete de un alt obiect, ntreruptorul se va
nchide semnalnd acest lucru. Un exemplu de caz n care o anumit parte a unui
robot mobil se atinge de un obiect din mediu este prezentat n figura 6.

Figura 6. Activarea senzorilor bumper n momentul ciocnirii robotului cu un obiect
Robotul din figura 6 are un sistem senzorial format din 8 senzori bumper,
situai pe robot la circumferina acestuia. n momentul n care robotul se lovete de
obiectul din mediu, senzorii aflai pe partea respectiv a robotului (n acest caz
senzorul 4 i 5) vor raporta acest lucru.
Cu toate c senzorii bumper sunt folosii pentru a evita lovirea robotului de
obiecte din mediul ambiant n caz de coleziune cu acestea, dac fora de impact este
mare, detectarea coleziunii nu mai este suficient pentru a evita distrugerea anumitor
pri a robotului. Majoritatea roboilor mobili folosii n laboratoare nu sunt echipai
cu sisteme de frnare active, i chiar dac ar fi, pentru o anumit vitez, unui robot
mobil tot i-ar lua o distan considerabil pentru a se opri brusc, datorit ineriei
motoarelor. Unele autovehicule au instalate totui senzori bumper, nu att pentru
detectarea coleziunilor, ct pentru a absorbi din ocul produs la impactul cu un
obiect extern.[7]
2 2. .3 3. .2 2. . S Se en nz zo or ri i I IR R
Senzorii n infrarou (IR) constituie cu siguran categoria cea mai simpl de
senzori de distan folosit la un robot mobil. Metoda lor de funcionare este foarte
simpl, fiecare senzor fiind echipat cu un emitor i un detector. Emitorul
transmite un fascicol de lumin n spectrul infrarou (de regul sunt folosite lungimi
de und n intevalul 880-990nm [9]), fascicol care se propag n mediu, iar apoi se
reflect de obiectele aflate n acesta. Fascicolul reflectat este captat de componenta
detector, urmnd apoi ca printr-un calcul matematic simplu, s se estimeze o distan
Roboi mobili
pagina 14
ntre senzorul infrarou i obiectul detectat din mediu. Principiul de funcionare al
unui senzor IR este prezentat n figura 7.

Figura 7. Principiul de funcionare al unui senzor IR
Att emitorul ct i detectorul au o raz de aciune (en. field of view). n
funcie de poziia obiectului n mediu, acesta poate fi detectat sau nu de ctre senzor.
Dac obiectul este prezent la intersecia razelor de aciune a emitorului i a
detectorului, radiaia reflectat de obiect va fi observat de detector, n caz contrar,
obiectul nefiind detectat. Un exemplu de situaie n care un anumit obiect din mediu
nu poate fi detectat, este prezentat n figura 8.

Figura 8. Obiecte din mediu care pot sau nu pot fi detectate de ctre senzorul IR
Roboi mobili
pagina 15
Plecnd de la ipoteza c toate obiectele din mediul n care robotul mobil
opereaz au aceeai culoare, senzorul IR poate fi calibrat astfel nct s msoare
distana ntre el i orice obiect din mediu aflat n cmpul su de aciune la un moment
dat. Astfel, intensitatea luminii reflectate de ctre obiect, este invers proporional
cu ptratul distanei pn la obiectul respectiv.
2
1
D
I =
n realitate ns, obiectele au culori diferite, rezultatele fiind n aceste cazuri
diferite, unele obiecte reflectnd o mai mare sau mai mic cantitate de lumin napoi.
Spre exemplu, suprafeele de culoare nchis (negru de exemplu) sunt aproape
invizibile pentru majoritatea senzorilor IR (vezi figura 9 i figura 10).
De asemenea, informaiile primite de la senzorii IR sunt susceptibile la erori
dac n ncperea respectiv exist o cantitate mai mare dect o valoare prag de
lumin natural (provenit de la soare).
n principiu, dac un fascicol transmis este reflectat i detectat, se poate face
asumpia c un obiect este prezent n mediu. Cazurile n care un fascicol de lumin n
infrarou este detectat el nefiind trimis de emitorul senzorului sunt foarte rare.
Pe de alt parte, absena unui semnal transmis napoi la detector nu semnific
neaprat faptul c nu exist nici un obiect n mediu. Datorit faptului c unele obiecte
de culoare nchis nu reflect lumina n spectrul infrarou, se poate presupune c, n
momentul n care un fascicol a fost trimis dar nu a fost receptat napoi, cel puin un
obiect de culoare nchis exist n cmpul de aciune al senzorului.

Figura 9. Obiectele de culoare deschis
reflect puternic fascicolele de lumin din
spectrul infrarou
Figura 10. Obiectele de culoare nchis
reflect foarte slab fascicolele de lumin
din spectrul infrarou
Roboi mobili
pagina 16
Datorit motivelor enumerate mai sus, senzorii IR nu pot fi folosii cu succes
n detectarea i evitarea de obstacole. De asemenea, datorit faptului c obiectele din
mediu au suprafee de culori diferite, n expresia real a funciei intensitii de mai
sus va mai apare o component de reflectivitate a luminii de ctre suprafaa
respectiv. Aadar, n cele mai multe cazuri, senzorul IR nu poate detecta cu precizie
distana la care se afl obiectul respectiv.
Senzorii IR sunt de regul senzori cu raz de aciune relativ scurt (maxim
50-100cm) datorit faptului c intensitatea semnalului IR este invers proporional cu
ptratul distanei. [5]
Cu toate aceste probleme pe care senzorii IR le au, ei sunt destul de des
folosii la construcia roboilor mobili, n primul rnd datorit preului extrem de mic
pe care productorii lor l practic.
Senzorii IR cei mai des ntlnii n practic sunt fabricai de firma Sharp.
Familia de senzori IR Sharp include att senzori digitali (exemplu: GP2D02, GP2D05,
GP2D15 sau GP2Y0D02YK) ct i analogici (exemplu: GP2D12, GP2D120 sau
GP2Y0A02YK).
2 2. .3 3. .3 3. . S Se en nz zo or ri i u ul lt tr ra as so on ni ic ci i
Categoria de senzori cea mai des ntlnit la un robot mobil o constituie, fr
nici o ndoial, categoria senzorilor ultrasonici. ntlnii n literatura de specialitate i
sub denumirea de sonar (en. SOund Navigation and Ranging), senzorii ultrasonici
folosesc un principiu oarecum asemntor cu senzorii IR, dar n loc de a transmite
fascicole luminoase, ei folosesc semnale acustice (sunete). Un emitor sonar
transmite un semnal acustic n mediu, urmnd apoi ca reflecia acestuia s fie
recepionat de componenta detector a senzorului. Timpul n care semnalul este
receptat napoi de senzor precum i atenuarea semnalului reprezint aspecte
exploatate de diferitele tipuri de senzori sonar.
Suntele transmise de senzorii sonar sunt de regul n spectrul de sunete
ultrasonice, avnd o frecven foarte nalt pentru a nu putea fi detectate de urechea
uman.
Principiul de funcionare al senzorilor sonar a fost mprumutat din lumea
animal, att liliecii ct i delfinii folosind aceleai tehnici pentru a localiza i identifica
diferitele caracteristici ale unui anumit obiect.
Sensibilitatea unui senzor sonar nu este uniform, ci consist dintr-un lob
principal i cteva loburi laterale mai mici. De asemenea, aceast sensibilitate difer
de la un senzor sonar la altul. Diferenele de sensibilitate ale unora dintre cei mai
populari senzori sonar, i anume modelele Devantech SRF08 i SRF10 pot fi
observate n figurile 11 i 12.
Roboi mobili
pagina 17

Figura 11. Sensibilitatea senzorului
Devantech SRF08
Figura 12. Sensibilitatea senzorului
Devantech SRF10
Dac semnalul acustic trimis de emitor se propag direct ctre un obiect, se
reflect de acesta i revine napoi la detector, distana pn la obiectul respectiv
poate fi calculat prin:
vt d
2
1
=
n formula de mai sus, t reprezint intervalul de timp n care semnalul acustic
ajunge de la emitor napoi la detector, iar v reprezint viteza sunetului emis n
mediu, calculat cu formula:
] / [ 6 . 0
0
s m T v v + =
V
0
reprezint viteza sunetului n aer la temperatura de 0 grade Celsius, iar T
temperatura n grade Celsius. Astfel, se poate calcula viteza semnalului acustic n aer
la o anumit temperatur. Spre exemplu, v=344m/s la T=20
o
C, respectiv v=346m/s la
T=24
o
C.
Formula de mai sus este doar o aproximare. Viteza sunetului n practic mai
este afectat i de densitatea aerului (altitudine) precum i de ali factori precum
umiditatea aerului sau concentraia de praf n aer [7].
Majoritatea roboilor mobili folosesc o configuraie de senzori ultrasonici
aranjai similar cu cei din figura 13.
Distana minim care poate fi msurat de un senzor sonar, este dat de
formula[5]:
b
vt d
2
1
=
Unde t
b
reprezint durata pulsului n secunde. Spre exemplu, pentru
t
b
=1.2ms, distana minim msurabil este de aproximativ 21cm.
Roboi mobili
pagina 18

Figura 13. Exemplu de configuraie a senzorilor sonar la un robot mobil
Similar, distana maxim este dat de formula:
w
vt d
2
1
=
Unde t
w
reprezint durata n secunde ntre pulsuri. n fine, rezoluia
senzorului sonar d
res
este calculat n funcie de numrul de pai de cuantificare
disponibili pentru encoder q i de distana maxim msurabil de senzor.
q
d
d
res
max
=
Din nefericire, nici senzorii ultrasonici nu sunt lipsii de probleme. n primul
rnd, poziia exact a unui obiect detectat este necunoscut, datorit funciei de
sensibilitate a sonarului. Un obiect detectat la o distan d de sonar poate fi oriunde
n conul acestuia.
n al doilea rnd, anumite suprafee pot da natere unor citiri eronate,
semnalul acustic putnd fi reflectat n alt direcie. Mai multe metode despre cum se
pot elimina aceste erori poti fi gsite n [5].
n fine, o alt problem poate s apar n momentul n care se folosesc
configuraii cu mai muli senzori ultrasonici. Dac semnalele de la aceti senzori nu
sunt codificate, i se dorete folosirea mai multor senzori simultan, este posibil ca
semnalele senzorilor s interfereze unul cu cellalt, dnd natere la citiri complet
eronate. O modalitate de evitare a acestei probleme o constituie folosirea senzorilor
n mod secvenial.
Cu toate aceste probleme, senzorii ultrasonici sunt cu siguran cea mai des
ntlnit categorie de senzori de distan folosit la construcia unui robot mobil.
Preul lor se ncadreaz ntre preul foarte mic al senzorilor IR i preul mult mai
mare al altor categorii de senzori de distan mai precise.
Roboi mobili
pagina 19
2 2. .3 3. .4 4. . S Se en nz zo or ri i l la as se er r
Una din cele mai precise categorii de senzori de distan o constituie
categoria senzorilor laser. Cu ajutorul acestor senzori se pot msura distana, viteza
ct i acceleraia anumitor obiecte din mediu fa de robotul mobil. Principiul cu care
opereaz un senzor laser este similar cu cel al unui senzor sonar, ns n loc de a
trimite un semnal acustic, un puls scurt de lumin este trimis. Timpul ntre transmisia
i recepia pulsului luminos este apoi folosit pentru a determina distana pn la
obiectul respectiv.
Distana maxim detectabil a unui senzor laser poate fi de la cteva sute de
metri la kilometrii sau chiar sute de kilometrii. Aceeai tehnologie a fost folosit
pentru a msura distana de la Pmnt la Lun cu acuratee foarte mare.
Acurateea distanei este undeva n domeniul milimetrilor, deci cu precizie de
cateva zeci (chiar sute n unele cazuri) de ori mai mare dect cea a senzorilor
ultrasonici.
Datorit capacitilor sale, senzorul laser este ns foarte scump, majoritatea
aplicaiilor nejustificnd nc achiziionarea unui astfel de sistem.
Unul dintre cele mai cunoscute modele de senzori laser folosii n robotic
este modelul LMS 200 produs de firma SICK.
2 2. .3 3. .5 5. . S Se en nz zo or ri i G GP PS S
Sistemul de poziionare global prin satelit (en. Global Positioning System) a
fost iniial dezvoltat de ctre Departamentul de Aprare al Statelor Unite ale
Americii ncepnd cu anul 1973.
Sistemul este alctuit din aproximativ 21 de satelii [7], i permite oricrui
receptor autorizat s-i calculeze poziia i viteza cu care se deplaseaz. Sistemul GPS
nu poate fi folosit n interiorul cldirilor, deoarece el necesit ca ntre receptor i
satelit s existe vizibilitate direct.
Momentan exist mai multe implementri ale serviciului, printre care SPS (en.
Standard Positioning System) disponibil pentru civili, respectiv PPS (en. Precise
Positioning System), disponibil pentru armat. SPS are o acuratee de aproximativ
100m pe orizontal, fapt pentru care nu este folosit prea mult n lumea roboilor
mobili.
Prin folosirea unei combinaii de senzori GPS care comunic unul cu cellalt,
este posibil obinerea unei estimri mult mai precise de poziie. Sistemul astfel
construit poart denumirea de DPGS (en. Differential Global Positioning System),
acurateea lui fiind undeva ntre 1 i 10m pentru civili, deci substanial mai mare
dect cea a sistemului SPS clasic, dar i dect cea a sistemului PPS. O implementare i
mai precis a unui sistem diferenial GPS este DCGPS (en. Differential Carrier Global
Positioning System), cu ajutorul cruia se poate obine o acuratee de aproximativ
1mm.
Din punct de vedere al roboilor mobili, este recomandat folosirea unor
sisteme hibride de poziionare, prin folosirea informaiilor de la un sistem de senzori
GPS i unul clasic. Mai multe informaii despre GPS se pot regsii n [7] i [9].
Roboi mobili
pagina 20
2 2. .3 3. .6 6. . S Se en nz zo or ri i p pi ir ro oe el le ec ct tr ri ic ci i
Senzorii piroelectrici sunt traductoare care convertesc cldura ntr-un semnal
electric[9]. Aceti senzori sunt de regul sensibili la lungimi de und mari din spectrul
infrarou, zon n care animalele i omul emit radiaii. Schema de baz precum i
modul de funcionare al unui senzor piroelectric sunt prezentate n figura 14.

Figura 14. Modul de funcionare al unui senzor piroelectric
(imagine preluat cu amabilitatea Glolab Corporation)
Dup cum se poate observa i din figura 14, un senzor piroelectric are n
general 2 detectori conectai n paralel, n faa crora se poziioneaz o lentil
Fresnel pentru focalizarea semnalului.
Senzorii piroelectrici sunt folosii pentru a detecta persoane umane sau surse
de cldur n mediul ambiant. Fiind senzori de micare ns, este necesar ca fie
senzorul sau robotul s se mite pentru a detecta subiectul, fie ca subiectul s se
mite n faa senzorului. Semnalul de ieire al senzorului piroelectric este dat de
diferena semnalelor celor doi detectori paraleli.
Trebuie inut cont c senzorii piroelectrici rspund la cea mai puternic surs
de cldur din cmpul lor vizual. Este deci posibil situaia n care, dac un om este
poziionat lng o surs de cldur mai mare, senzorul s simt doar acea surs, nu i
pe om.
Datorit preului lor relativ sczut, senzorii piroelectrici constituie un arsenal
bun pentru un robot mobil.
2 2. .3 3. .7 7. . S Se en nz zo or ri i v vi id de eo o
Senzorii vizuali sunt reprezentai de regul n lumea roboilor mobili de
camere video. n momentul de fa senzorii de imagine folosesc dou tehnologii:
CCD i CMOS. Din punct de vedere al roboticistului ns, nu exist diferene
eseniale ntre cele dou tehnologii, astfel c, orice camer video poate fi folosit cu
succes.
Camerele video obin o cantitate mare de informaii din cmpul lor vizual.
Procesarea acestor date poate necesita o putere de calcul ridicat, mai ales dac se
dorete obinerea unor performane optime din informaiile culese.
Roboi mobili
pagina 21
Una din aplicaiile cele mai des folosite ale camerelor video este urmrirea de
culoare (en. blob tracking). Pentru a face uz de acest algoritm, este necesar alegerea
unei anumite culori (reprezentat prin R, G, B) din mediu. Spre deosebire de ochiul
uman ns, camera este destul de sensibil la variaii ale luminii, i va eua n
detectarea culorii alese. De asemenea, dac culoarea aleas nu este unic n spaiul
de lucru, algoritmii de blobtracking se vor ncurca, i vor oferi informaii cnd despre
obiectul de urmrit, cnd despre alte zone ale mediului n care se regsete culoarea
respectiv. Toate aceste neajunsuri pot fi ns eliminate prin folosirea unor algoritmi
mult mai compleci, care fac parte ns din alt domeniu de studiu.
Pentru mai multe detalii privind modul n care funcioneaz camerele video,
precum i algoritmii de procesare necesari pentru obinerea unor diferite informaii
a se consulta [5] respectiv [7].
2 2. .3 3. .8 8. . S Se en nz zo or ri i d de e s st ta ar re e i in nt te er rn n
Dup cum am afirmat deja, senzorii de stare intern sunt acei senzori care
ofer informaii despre starea intern a robotului mobil. Senzorii din aceast
categorie ofer fie informaii legate de poziia robotului (odometrie), fie informaii
referitoare la vitezele sau acceleraiile liniare respectiv unghiulare ale robotului
(accelerometru, giroscop).
Senzorii care ofer informaii despre poziie, se regsesc de regul sub forma
encoderelor la motoarele robotului. Prin msurarea rotaiei roilor robotului, i apoi
interpretarea acestei informaii se poate estima poziia robotului. Semnalul provenit
de la encodere poate oferi informaii despre direcia n care se deplaseaz robotul,
viteza cu care se deplaseaz robotul precum i distana aproximativ care a parcurs-
o robotul de la ultima evaluare.
Teoretic, poziia exact a robotului ar putea fi dedus doar din informaiile de
odometrie provenite de la encodere, n sensul c distana parcurs ar trebui s fie
egal cu 2r, unde r reprezint raza roii respective. n practic ns, n cel mai bun
caz, se poate tii doar c informaia de la encodere conine o anumit eroare, i c
aceast eroare de regul nu scade niciodat. n cea mai rea situaie, cnd roile
robotului alunec, nu mai exist nici o relaie ntre micarea roilor i micarea
robotului. n acest caz, cantitatea de eroare este necunoscut, iar estimarea poziiei
robotului devine aproape imposibil. Mai multe informaii despre odometrie, precum
i despre estimarea i corectarea erorilor se gsesc n [5] i [8].
Accelerometrul i giroscopul sunt considerai senzori ineriali, deoarece
acetia msoar derivata a doua a poziiei (acceleraia liniar, respectiv acceleraia
unghiular) a robotului.
Accelerometrul face uz n esen de legea lui Newton, ma F = , precum i de
relaia
2
kx F = . Combinnd cele dou formule, se poate obine[5]:
m
kx
a
2
=
Unde a este acceleraia, m masa, iar k este constanta elastic. n practic,
fiecare accelerometru msoar acceleraia pe o singur direcie. Astfel, montnd 3
accelerometre, poate fi obinut un sistem senzorial omnidirecional pentru
acceleraie.
Roboi mobili
pagina 22
Integrnd semnalul obinut de la senzorii inerial n funcie de timp, teoretic
se poate obine poziia robotului. n practic ns, ieirea unui sistem inerial ieftin
difer foarte mult de realitate.
Dou alte tipuri de senzori care intr n categoria senzorilor de stare intern
sunt: compasul i inclinometrul. Inclinometrul este un dispozitiv simplu care msoar
orientarea fa de vectorul gravitate, spre deosebire de compas care msoar
orientarea n raport cu cmpul magnetic al Pmntului.
Senzorii care utilizeaz cmpul magnetic pentru a detecta direcia Nord sunt
susceptibili la variaii locale n cmpurile magnetice ambiente[5]. Diferite structuri de
metal, chiar i robotul n sine, pot afecta ntr-un mod negativ aceast categorie de
senzori, rezultatele fiind imprevizibile. Aceast categorie de senzori nu poate deci fi
folosit n interiorul cldirilor, i chiar i n exterior, sunt necesare diverse calibrri
ale acestora n raport cu robotul sau cu mediul nconjurtor.
Cu toate acestea, compasul poate fi folosit ca i o referin local n acele
situaii n care influenele externe sunt constante n timp.
2 2. .3 3. .9 9. . F Fu uz zi iu un ne ea a d da at te el lo or r d de e l la a s se en nz zo or ri i
Pentru majoritatea sarcinilor atribuite unui robot mobil, folosirea unui singur
tip de senzori nu poate da rezultate satisfctoare. Spre exemplu, pentru navigare,
anumite obiecte din mediu pot fi detectate doar de senzori IR, iar altele doar de
senzori sonar, i doar unele de ambele tipuri de senzori. Se pune deci problema
gsirii unei metode care s combine ntr-un mod eficient informaiile de la o
multitudine de senzori de categorii i caracteristici diferite. Termenul cel mai des
ntlnit n literatura de specialitate este fuziune de senzori (en. data sensor fusion).
n contextul roboilor mobili, fuziunea trebuie s fie efectuat pe urmtoarele
trei nivele[7]:
Trebuie s combine msurtori de la senzori de categorii diferite;
Trebuie s combine msurtori din poziii diferite;
Trebuie s combine msurtori efectuate la diferite intervale de timp.
Procesul de combinare al msurtorilor din diferite poziii, intervale de timp
sau senzori este oarecum similar procesului de interpolare, extrapolare i gsirea
unei funcii matematice pentru un anumit set de date. n cea mai simpl form static,
combinaia msurtorilor ar putea fi exprimat printr-o formul de genul[15]:
=
=
n
j
j j
Pondere Senzor val
0
*
O metod mai complex pentru fuziunea datelor de la senzori const n
folosirea reelelor neuronale pentru a asocia ieirea sistemului cu un set de intrri
date. Un exemplu simplu este oferit n figura 15.
Roboi mobili
pagina 23

Figura 15. Exemplu de reea neuronal folosit la procesul de fuziune al senzorilor
Exemplul din figura 15, combin datele provenite de la o serie de senzori
ultrasonici (S1..16) cu datele provenite de la o serie de senzori interni ai robotului,
date de odometrie. Aceste informaii sunt introduse ntr-o reea neuronal feed-
forward, la ieire fiind obinute comenzile pentru motoare sub forma vitezei liniare v
i a vitezei unghiulare . Mai multe informaii despre procesul de fuziune al datelor
de la senzori folosind reele neuronale pot fi gsite n [5], respectiv [15].
2 2. .4 4. . U Un ni it t i i c ce en nt tr ra al le e d de e p pr ro oc ce es sa ar re e
Sarcinile pe care un robot mobil trebuie s le ndeplineasc pot s fie de la
foarte simple la extrem de complexe. Totul depinde de scopul final al robotului
mobil construit. n funcie de tipul de sarcini atribuite unui robot mobil, procesarea i
descompunerea sarcinilor n aciuni simple pe care robotul le poate executa necesit
prezena unei uniti centrale de procesare.
Unitatea central de procesare se poate afla fie pe robot (en. onboard), fie la
distan conectat de acesta prin diverse metode (en. offboard). n mod normal, se
dorete ca robotul mobil s fie autonom, deci s poat duce la bun sfrit oarecum
independent i sarcini mai complexe, fr intervenia unui program de control din
partea utilizatorului, aflat la distan. n multe cazuri ns, prezena unei uniti de
procesare pe robot duce la o autonomie sczut a bateriei acestuia. O alt problem
ar putea fi creterea n greutate a robotului, n funcie de tipul de unitate central de
procesare aleas. De asemenea, n cazul n care necesitile computaionale sunt
mari, s-ar putea ca o unitate central de procesare s nu existe astfel nct s se
ncadreze n limitele fizice i geometrice ale robotului, caz n care aceasta nu poate fi
instalat.
Exist totui o serie de avantaje de a avea o unitate de procesare onboard:
Roboi mobili
pagina 24
Dispar ntrzierile asociate comunicaiei ntre o unitate de procesare offboard
i componentele electronice ale robotului;
n cazul unor probleme de comunicare, robotul poate aciona n continuare
autonom;
Unitile de procesare onboard au acces direct la hardware-ul robotului, n
timp ce unitile offboard trebuie s apeleze la un anumit protocol care s
interfaeze ntre componente i unitate.
Evident, soluia cea mai bun este ca robotul s poate funciona att autonom
prin intermediul unei uniti de procesare onboard, ct i n regim de comunicare cu
alte uniti de procesare mai performante, care pot asigura necesitile
computaionale pentru sarcini mai complexe.
n practic unitile de comunicare onboard se aleg astfel nct s aib
suficient putere de calcul pentru a ndeplini majoritatea sarcinilor simple, cu
specificaia s nu consume prea mult energie. Unitile offboard sunt de regul
reprezentate de calculatoare personale.
2 2. .5 5. . S Si is st te em me e d de e c co om mu un ni ic ca ar re e
Roboii mobili trebuie s posede capaciti de comunicare fie cu ali roboi
din mediu fie cu un operator uman, pentru a raporta dac o anumit sarcin dat a
fost ndeplinit cu succes, sau nu. Sistemele de comunicare aferente roboilor mobili
pot fi de 2 categorii: cu fir respectiv fr fir.
Comunicaia cu fir este modalitatea cea mai simpl de transfer de date ntre
robot i operator. O serie de probleme asociate transmisiei fr fir, dispar n cazul
comunicaiei de date cu fir, cu preul ngrdirii mobilitii robotului. Avantajul
principal al sistemelor de comunicare cu fir este acela c, pe lng datele transmise
ntre robot i operatorul uman, se mai poate transmite i curent electric, eliminnd
astfel necesitatea unei baterii onboard pe robot i mrind autonomia robotului.
Un dezavantaj direct al folosirii unui sistem de comunicare cu fir, ar fi faptul
c spaiul de lucru al robotului va fi limitat de lungimea maxim admisibil a cablului.
De asemenea, cablul ar putea interfera cu sistemul de locomoie sau chiar cu
senzorii, acetia putnd raporta valori eronate. Din aceste considerente, nu se
recomand folosirea sistemelor de comunicare cu fir dect n cazurile n care
folosirea tehnologiilor fr fir ar fi o problem (spre exemplu pentru vehicule robot
din mediul acvatic).
Sistemele de comunicare fr fir se folosesc de o serie de tehnologii de
transmitere a datelor prin aer (en. wireless), dintre care amintim: transmisia prin unde
n spectrul infrarou, bluetooth, radio modem-uri, WiFi, i altele.
Mecanismele de transmitere a datelor prin unde infrarou sunt cele mai puin
robuste, ele neputnd fi folosite dect la distane relativ mici. De asemenea, ele
funcioneaz doar n linie dreapt n cmp deschis, orice obstacol aflat ntre
dispozitivele de transmisie-recepie obturnd semnalul. Un alt duman al
tehnologiilor de transmitere n infrarou este lumina solar.
Roboi mobili
pagina 25
Tehnologiile WiFi sunt deja rspndite n domeniul calculatoarelor personale,
i ofer un suport complet de integrare al roboilor n reele de calculatoare. O
problem a acestor tehnologii ar fi faptul c sunt consumatoare de energie, lucru
care nu este prielnic robotului mobil.
Eliminnd problema energiei consumate, tehnologiile Bluetooth ofer servicii
similare cu dispozitivele WiFi, ns distana maxim admisibil ntre emitor-
receptor este mult mai mic dect n cazurile celorlalte tehnologii. n momentul de
fa, tehnologiile Bluetooth se folosesc n interiorul cldirilor, n laborator, unde
distana maxim fr repetor este de maxim 20m.
n fine, tehnologiile radio modem ofer o soluie wireless pentru acele
dispozitive care sunt deja conectate prin cablu. Consumul de energie al modemurilor
radio este de regul redus, ele fiind optimizate pentru aplicaii unde nu se dorete o
vitez foarte mare de transmisie a datelor. Un mic experiment realizat n laborator a
demonstrat folosirea cu succes a unei perechi de radio modemuri MaxStream n
locul cablului RS232 deja existent pe un robot ActivMedia Pioneer 2.
2 2. .6 6. . I In nt te el li ig ge en n , , a au ut to on no om mi ie e i i c co on nt tr ro ol l
Deoarece n lumea roboilor mobili conceptele inteligen i autonomie
sunt foarte strns legate unul de cellalt, se vorbete de regul doar despre roboi
autonomi[8].
Inteligena este legat de capacitile robotului de a observa, nelege i nva
diferite aspecte ale sarcinilor sale. Cunotinele dobndite pe parcursul procesului de
nvare trebuie s duc la performane mai bune pentru executarea sarcinilor pe
viitor. De asemenea, prin procesul de nelegere al anumitor fenomene, informaiile
obinute pot duce la soluii pentru rezolvarea anumitor sarcini noi, sarcini pentru
care robotul nu a fost proiectat iniial. n literatura de specialitate se vorbete despre
aplicarea tehnicilor de inteligen artificial (en. artificial intelligence) pentru
controlarea robotul mobil.
Inteligena unui robot este demonstrat prin modul n care robotul poate
naviga eficient n mediu, cu alte cuvinte, prin modalitatea prin care procesul de
navigare difer de cazul n care aciunile robotului ar fi controlate direct de un
operator uman.
Autonomia unui robot mobil se refer la capacitatea acestuia de a executa
independent sarcinile ce-i sunt date aprioric, fr intervenia unui operator uman.
Dac n executarea sarcinilor sale, robotul ntlnete situaii pentru care nu a fost
programat, este imperativ ca el s continue sa funcioneze corect, fr intervenie
exterioar, mbinnd astfel conceptele de inteligen i autonomie.
Se spune despre un robot mobil c este automat dar nu autonom, n
momentul n care o situaie neateptat care apare, duce la oprirea execuiei sarcinii
pe care robotul mobil o desfura n momentul respectiv. Dac robotul se poate
descurca cu noua situaie ntlnit, n sensul c o ignor sau face mici ajustri i
continu sarcina, robotul are un grad mic de autonomie. Dac ns robotul se
adapteaz perfect la situaia nou prin observarea caracteristicilor mediului, atunci
gradul de autonomie al robotului mobil este mare. Similar, dac sarcina robotului
Roboi mobili
pagina 26
este exprimat n termeni vagi, i robotul trebuie s se descurce la faa locului prin
evaluarea situaiei, robotul este complet autonom.
Elementele coordinative ale unui robot mobil sunt prezentate n figura 16.

Figura 16. Elementele coordinative ale unui robot mobil
Procesul de navigare determin micarea robotului mobil n mediul su de
lucru. Procesul de percepie nregistreaz i prelucreaz datele de la senzori,
transformndu-le n ecuaii de micare pentru navigator.
Descompunerea n blocuri funcionale a navigatorului este prezentat n figura 17.

Figura 17. Funcionarea navigatorului
Dup cum se poate observa din figura de mai sus, prima etap const n
prelucrarea datelor de la senzori, n funcie de modelul senzorial aferent existent.
Rezultatul prelucrrii se mbin mpreun cu sarcinile atribuite robotului mobil n
procesul de planificare. n aceast etap, un rol important l joac modelul mediului
existent. n fine, etapa final este cea de control, n care rezultatele procesului de
planificare se mbin mpreun cu modelul robot, trimind comenzile necesare ctre
motoarele robotului.
Pe msura execuiei algoritmilor de control, navigatorul trebuie s aib n
vedere i actualizarea celor 3 modele din baza de date. Erorile existente ntre
modelele vechi i realitate trebuie minimizate, i noile modele calculate trebuie
stocate n locul celor vechi. Cele trei modele nu vor fi perfecte niciodat datorit
incertitudinilor existente n realitate. Pentru reducerea incertitudinilor se pot folosi
diveri algoritmi de adaptare (exemplu - filtre Kalman).
Problema fundamental a metodei de funcionare a navigatorului descris
anterior este c necesit un timp de execuie mult prea mare[8]. La fiecare pas,
comanda ctre motoare trebuie s fie calculat din datele de la senzori prin
planificare. Datorit acestui fapt, robotul nu va putea niciodat naviga cu viteze mari.
Roboi mobili
pagina 27
O serie de calcule complexe vor uza de puterea de calcul a unitii centrale de
procesare.
Soluia problemei de mai sus se obine prin mprirea funciilor navigatorului
n dou categorii: de nivel jos (nivel de ghidare), respectiv de nivel nalt (nivel
planificare). Astfel se obine noua schem arhitectural a navigatorului, prezentat n
figura 18.
Funciile de nivel jos sunt reprezentate printr-un controler de traiectorii i un
estimator de poziie (nivelul de ghidare). Acest nivel acioneaz ca o bucl nchis
rapid. Funciile de nivel nalt, opereaz n paralel cu funciile de nivel jos, oferind
informaiile necesare controlerului pentru a opera, dar la un interval de timp mai
mare dect rata cu care opereaz nivelul ghidare.

Figura 18. Schema arhitectural avansat a navigatorului
n cea mai simpl form, un robot mobil poate funciona n bucl deschis[9].
Operatorul sau algoritmul de control alege o vitez dorit pentru navigare iar viteza
este transformat de un bloc convertor (bloc care conine diferite formule necesare
conversie) n comenzi ctre motoare. Pentru a observa eroarea ntre viteza dorit i
viteza real, se pot folosi anumii senzori. Schema unui sistem simplu n bucl
deschis ar putea arta ca cea din figura 19.

Figura 19. Schem simpl de reglare n bucl deschis
Dezavantajele buclei deschise sunt evidente: n momentul n care condiiile
din mediu se modific, eroarea dintre viteza msurat i viteza dorit va crete. n
acest caz, algoritmul de control nu va putea face o compensare, deoarece el nu
primete nici o informaie din partea sistemului senzorial. Din acest motiv, bucla de
reglare trebuie nchis prin introducerea informaiilor de la senzori napoi n
algoritmul de control, astfel nct eroarea s poat fi minimizat. Schema aferent
acestui tip de reglare este prezentat n figura 20.
Roboi mobili
pagina 28

Figura 20. Schem de reglare n bucl nchis
Una din problemele asociate schemei de reglare n bucl nchis, n forma ei
simplificat din figura 20, este aceea c n momentul n care spre exemplu se dorete
deplasarea robotului ctre o anumit locaie din mediu, din momentul n care se
introduce viteza dorit i pn n momentul n care aceasta se modific apare o
anumit ntrziere (laten). Aceast ntrziere se materializeaz prin faptul c
robotul nu se va opri n locaia dorit ci va continua s se deplaseze n continuare
pentru puin timp. Din aceast cauz pot s apar oscilaii n jurul locaiei dorite,
manifestate prin deplasarea robotului nainte i napoi fa de punctul de referin
dorit. Soluia acestei probleme const n folosirea unui controler de tipul PID,
precum cel din figura 21.

Figura 21. Schem de reglare folosind un controler PID
Pe lng controlerele clasice PID, unii cercettori studiaz posibilitatea
folosirii unor controlere mai complexe, cum ar fi controlerele optimale sau
controlerele robuste. Un exemplu de astfel de controlere este dat n [13] i [14].
Din punct de vedere funcional, schema de control clasic a unui robot mobil
const dintr-o serie de pai repetitivi:
Robotul primete informaii asupra mediului nconjurtor de la senzorii si;
Robotul planific ce s fac n continuare, construind un model al lumii
nconjurtoare, bineneles lund n calcul att obiectivele pe termen scurt
(spre exemplu detectarea i ocolirea unui obstacol) ct i cele pe termen
lung (exemplu deplasarea dintr-o locaie n alta n mediu);
Robotul i pune n aplicare planul prin intermediul efectorilor.
Arhitectura funcional prezentat este ntlnit n literatura de specialitate
sub denumirea de ciclul simte-gndete-acioneaz (en. sense-think-act cycle)[5]. n
figura 22, sistemul de control a fost descompus n 5 blocuri funcionale.
Roboi mobili
pagina 29

Figura 22. Arhitectura sense-think-act
Blocul de percepie se ocup de procesarea datelor primite de la senzorii
robotului, date care se folosesc n continuare pentru construirea sau actualizarea
unor anumite modele ale mediului n care robotul lucreaz (n cadrul blocului de
modelare).
Blocul de planificare, folosete modelul mediului pentru a decide un plan de
aciune pentru robot, n funcie de sarcinile ce i sunt atribuite acestuia. Blocurile de
execuie respectiv controlul motoarelor vor executa apoi setul de aciuni generate
de ctre planificator.
Paii de mai sus vor fi repetai pn n momentul n care robotul i duce la
bun sfrit sarcina ce i-a fost atribuit. O problem care trebuie luat n calcul este
rigiditatea modelului, care l face foarte lent. La fiecare iteraie a acestui algoritm,
trebuie realizate toate operaiile din nou. Lent semnific defapt c, la viteza cu care
mediul se schimb, obiectivele planificate de la nceput vor fi greu de atins. n final,
algoritmul implementat ajunge s funcioneze ca o simpl bucl, ceea ce-l face
oarecum inadecvat ntr-un mediu dinamic n continu schimbare.
O arhitectur de control complet diferit a fost propus de Rodney Brooks
n [16] i apoi mai trziu de Ronald Arkin n [1]. Aceast arhitectur (en. behavior-
based) propune folosirea unui set de nivele (en. layers), fiecare nivel coninnd un
program de control capabil s lucreze la viteza cu care pot apare schimbri n mediul
nconjurtor (vezi figura 23). Aceast arhitectur asigur o cale pentru a putea
construi si testa incremental un sistem de control pentru roboi. Prin calculul paralel,
se mrete viteza de execuie a unei iteraii, fiecare modul devenind autonom. Astfel
se evit controlul centralizat, inteligena algoritmului fiind atribuit n totalitate
planificatorului.

Figura 23. Arhitectura behavior-based
Roboi mobili
pagina 30
Nici modelul lui Brooks nu este lipsit de probleme, cea mai mare fiind aceea
c, datorit faptului c robotul rspunde direct stimulilor din mediul n care acesta
acioneaz, executarea unui set de sarcini aprioric stabilite devine foarte greoaie,
dac nu imposibil.
Mai multe informaii despre alte modaliti de control a roboilor mobili pot
fi gsite n [5].

Player/Stage
pagina 31
3
3
.
.
P
P
l
l
a
a
y
y
e
e
r
r
/
/
S
S
t
t
a
a
g
g
e
e

Player/Stage
pagina 32
Proiectul Player/Stage a nceput n anul 1998 la University of Southern
California din Statele Unite ale Americii. El este rezultatul muncii a o serie de
cercettori, printre care Brian Gerkey, Richard Vaughan, Andrew Howard i Nate
Koenig. Cu toate c proiectul a nceput la USC, el a fost mutat n anul 2001 pe
Sourceforge.net. Din acel moment dezvoltarea lui a nceput o pant ascendent, n
primul rnd datorit bazei crescnde de utilizatori. Proiectul se distribuie sub licen
GPL (GNU Public License).
Proiectul Player/Stage este format din dou componente principale:
Componenta server, numit Player, care ofer o interfa flexibil pentru o
varietate de senzori i actuatori (componente ale roboilor mobili);
Componenta simulator, iniial Stage, care este un simulator 2D, i mai trziu i
Gazebo, care este un simulator 3D.
Proiectul a ajuns la versiunea 1.6.4, i poate rula pe o serie de platforme
hardware i sisteme de operare printre care: Linux x86, SunOS Sparc, Linux Sparc64,
Linux ARM, Linux Alpha, FreeBSD x86 i OS PowerPC.
Cercetarea anumitor aspecte ale sistemelor de roboi mobili folosind un
simulator precum Stage sau Gazebo poate salva timp preios. Experimentele fcute n
simulator pot fi repetate cu o mare acuratee, aceste simulatoare fiind de regul
foarte apropiate de mediul real. Un alt avantaj al folosiri unui simulator este faptul c
acesta permite lucrul cu anumite componente care poate nu exist fizic n dotarea
laboratorului respectiv[17].
3 3. .2 2. . A Ar rh hi it te ec ct tu ur ra a P Pl la ay ye er r
Dup cum s-a afirmat deja, Player este un server pentru dispozitive cum ar fi
senzorii sau actuatorii prezeni pe diferite tipuri de roboi mobili. Adevrata putere a
acestui server const n faptul c sunt suportate o multitudine de platforme
hardware i software, spre deosebire de alte produse similare. Astfel, teoretic este
posibil ca o aplicaie scris pentru un anumit gen de roboi s funcioneze fr
modificri majore pe alt gen de roboi, fcui de alt firm, dar cu caracteristici
tehnice asemntoare[19].
Player folosete un model de server client bazat pe protocolul de reea
TCP sau UDP. Datorit acestui fapt, aplicaiile client pot fi scrise practic n orice
limbaj de programare, ele putnd fi executate de pe orice calculator conectat la o
reea de calculatoare care conine server-ul Player activ conectat la robot.
Dezvoltatorii proiectului Player au avut grij s fac o distincie clar ntre interfaa
programator i structura de control.
Serverul Player este implementat n limbajul C++ i folosete funciile
standard POSIX pthread pentru a utiliza mai multe fire de execuie. Structura
arhitectural general a sistemului este prezentat n figura 24. Poriunea din centru
Player/Stage
pagina 33
descrie chiar sistemul Player, n partea stng se gsesc dispozitivele fizice, iar n
partea dreapt clienii care doresc s le utilizeze. Fiecare client folosete un socket
de tip TCP/UDP pentru a se conecta la serverul Player. Dac clientul se afl pe
acelai calculator ca i serverul Player, atunci conexiunea va fi de tip loopback (ro.
bucl nchis), n caz contrar realizndu-se o conexiune TCP/UDP prin reea ntre
cele dou componente. La cellalt capt, Player se conecteaz la dispozitivele
respective prin diverse standarde, cel mai popular fiind standardul serial RS232.
nuntrul Player, fiecare fir de execuie comunic cu celelalte fire printr-un spaiu de
adresare comun.

Figura 24. Arhitectura global a serverului Player
Legtura din punct de vedere arhitectural a serverului Player cu simulatoarele
Stage i Gazebo este ilustrat n figura 25[17].

Figura 25. Arhitectura Player/Stage/Gazebo

Dispozitiv fizic
D0
Dispozitiv fizic
DN
Dispozitiv
D0
Dispozitiv
DN
date
comenzi
Buffer
date D0
Buffer comenzi
DN
Buffer comenzi
D0
Buffer
date DN
Serviciul pentru
clieni multifir
Client
C0
Client
CN
comenzi
comenzi
date
date
date
SHM
TCP/UDP
TCP/UDP
date
comenzi
Client Player
Stage
Gazebo
Robot
fizic
comenzi
Player/Stage
pagina 34
3 3. .3 3. . S Si im mu ul la at to or ru ul l 2 2D D S St ta ag ge e
A dou component principal a proiectului Player/Stage este simulatorul 2D
numit Stage. Acest simulator folosete roboi virtuali care interacioneaz cu device-
uri simulate. Stage suport mai multe modele de senzori, incluznd sonare, senzori
laser, camere video i altele. Tot mediul construit n Stage poate fi accesat prin
intermediul unui client care va comunica cu un server Player[19].
Lumea construit pentru simulatorul Stage se ncarc dintr-un fiier cu
extensia .world. Toate obiectele i dispozitivele care vor fi ncrcate n mediul virtual
trebuie definite n fiierul care descrie lumea. Exist dou tipuri de dispozitive care
pot fi definite: pasive i active. Dispozitivele pasive sunt de fapt obiecte, toate fiind
derivate dintr-un model de baz numit entitate (en. entity).
Lista obiectelor care se pot defini este:
Bitmap - o colecie de obiecte fixe care se ncarc dintr-un fiier de tip
PNM (en. Portable Anymap Format). Din aceste fiiere se pot ncrca
perei, planul unei camere, etc;
Box - obstacol de tip fix (poate fi mutat de utilizator dar nu de robot);
Puck obstacol de tip mobil (poate fi mutat i de utilizator i de robot).
Toate obiectele de mai sus sunt derivate din tipul de baz entity. n timp ce
tipul entity nu poate fi instaniat direct, toate celelalte entiti motenesc proprietile
tipului de baz, aceste fiind:
Name de tip string, descrie un nume pentru entitatea respectiv,
folosit de GUI (en. Graphical User Interface, ro. Interfaa Grafic
Utilizator);
Pose de tip [x y a], stabilete poziia iniial a obiectului respectiv prin
X i Y, respectiv orientarea n grade fa de axa orizontal;
Shape forma entitii respective, valorile admisibile fiind rect pentru
dreptunghi respectiv circle pentru cerc;
Size mrimea entitii respective, de forma [sizeX sizeY];
Mass de tip float, stabilete masa obiectului respectiv (folosit la
obiectele de tip puck);
Color de tip string, stabilete culoarea obiectului respectiv, pentru o
list complet de culori trebuie consultat fiierul rgb.txt din distribuia
de X-Windows);
Fiducial_id de tip ntreg, stabilete un ID care va fi folosit de dispozitivul
fiducialfinder pentru a recunoate obiectul respectiv. Valorile admisibile
sunt ntre 0 i 255;
Obstacle_return specific prin visible sau invisible dac entitatea
respectiv va fi tratat ca un obstacol sau nu n calea robotului;
Sonar_return specific prin visible sau invisible dac entitatea
respectiv va fi detectat de sonarele unui robot sau nu;
Player/Stage
pagina 35
Vision_return specific prin visible sau invisible dac entitatea
respectiv va fi observat de o camer video sau nu;
Laser_return specific prin visible sau invisible dac entitatea
respectiv va fi observat de un senzor laser sau nu. O alt valoare
posibil este bright care specific faptul c entitatea va fi observat ca
o lumin strlucitoare de laser;
Idar_return specific ce tip de comportament va manifesta obiectul
cnd va interaciona cu o raz IDAR (en. Infrared Data and Ranging),
valorile posibile fiind IDARTransparent, IDARReflect,
IDARReceive;
Interval specific intervalul folosit pentru actualizarea modelului n
interfaa grafic n secunde. Un interval mai mic poate s mreasc
fidelitatea simulrii, dar procesorul va fi folosit ntr-un procent mai mare.
Valorile implicite pentru proprietile de mai sus sunt:

Fiecare obiect din cele definite mai sus motentete aceste proprieti, ns
schimb valorile implicite ale acestora.
Box
Este cel mai simplu tip de obiect care poate fi descris. Tipul obstacolului
poate fi dreptunghiular sau sub form de cerc. Valorile implicite ale proprietilor
entitate pentru acest tip de obiect sunt:

Bitmap
Acest tip de obiect este folosit pentru a ncrca o lume virtual dintr-un fiier
de tip PNM. Valorile implicite ale proprietilor entitate pentru acest tip de obiect
sunt:
Shape rect
Size [1.0 1.0]
Color yellow
Obstacle_return visible
Sonar_return visible
Vision_return visible
Laser_return visible
Idar_return IDARReflect

Name
Shape none
Size [0 0]
Pose [0 0 0]
Color black
Obstacle_return invisible
Sonar_return invisible
Vision_return invisible
Laser_return invisible
Idar_return IDARTransparent
Id -1
Mass 1000.0
Interval 0.1
Player/Stage
pagina 36

Se menioneaz c pe lng valorile motenite de la tipul entity, acest tip de
obiect mai adaug nc dou proprieti, i anume file, care va descrie numele
fiierului PNM care se va ncrca de pe disc, respectiv scale, care specific scara
imaginii n metri/pixeli (sau uniti/pixeli n cazul n care se vor specifica alte uniti de
msur diferite de metru).
Puck
Obiectul de tip puck descrie o entitate care poate fi mutat att de robot ct
i de utilizator. Pe lng proprietile de baz, mai adaug i o proprietate numit
friction, care descrie coeficientul de frecare al obiectului cu mediul respectiv. Valorile
implicite ale proprietilor entitate pentru acest tip de obiect sunt:

Tipurile de dispozitive active care funcioneaz corect cu simulatorul Stage
versiunea 1.6.3 sunt:
Blobfinder detector de pete de culoare;
Laser senzor de tip laser;
Gripper dispozitiv de tip gripper cu dou poziii (nchis deschis);
Fiducial identific anumite obiecte din mediu, se bazeaz pe senzorul de
tip laser;
Ranger senzor de tip sonar;
Simulation dispozitiv de localizare folosit doar n simulator;
Position robot cu motor diferenial.
O serie de dispozitive funcionale n versiunile trecute ale simulatorului sunt
n curs de reconstrucie. Dintre ele, se amintesc: broadcast (permite clienilor s
comunice unul cu cellalt prin platforma Player/Stage, mesajele trimise de un client
fiind primite de toi ceilali), gps (dispozitiv de tip GPS), omniposition (robot de tip
omnidirecional), ptz (camer video cu funcii de zoom i rotaie/translaie), truth
(nlocuit de dispozitivul simulation), idar (dispozitiv de tip senzori cu infrarou)
Shape circle
Size [0.08 0.08]
Color green
Friction 0.05
Mass 0.2
Interval 0.01
Color black
Laser_return visible
Idar_return IDARReflect
File
Scale 0

Player/Stage
pagina 37
respectiv idarturret (list de senzori de tip IDAR accesai mpreun pentru o
eficien mai mare).
Toate aceste tipuri folosesc un alt tip de baz numit device, care are dou
tipuri de proprieti i anume: port, care descrie numrul portului unde dispozitivul
respectiv este ataat, respectiv index care descrie numrul dispozitivului respectiv.
Proprietatea index se folosete atunci cnd se folosesc mai multe instane ale
aceluiai tip de dispozitiv.
n continuare se prezint continuare un exemplu de mediu pentru robot,
creat dintr-un fiier de tip world. De notat c toate liniile care ncep cu caracterul
# sunt considerate comentarii i ignorate de ctre componenta programului Stage
care se ocup cu citirea obiectelor din fiier.

Dup ce acest fiier va fi citit de ctre Stage, se va ncrca imaginea din fiierul
cave.pnm.gz i se va aduga un dispozitiv robot echipat cu senzori laser i care se va
conecta la un server Player pe portul 6665.
Pentru a accesa robotul respectiv, trebuie scris un client care s se conecteze
pe portul 6665 folosind protocolul TCP/UDP i s comunice cu serverul Player.
Robotul se va deplasa n simulatorul Stage exact ca i cum ar fi n lumea real. Astfel
se pot testa anumii algoritmi fr a folosi un robot fizic.
Exemplul prezentat n figura 26 folosete mai multe tipuri de roboi virtuali
respectiv de obiecte n mediu. Astfel, cele trei dispozitive de culoare roie din partea
dreapt a ecranului sunt trei roboi AmigoBot fabricai de firma ActivMedia Robotics.
Dispozitivele de culoare albastra (jos n ecran), roie (stnga jos) respectiv verde
(stnga mijloc) sunt roboi de tip Pioneer fabricai tot de firma ActivMedia Robotics.
Celelalte obiecte colorate sunt de tip puck. Pereii sunt descrii prin liniile de culoare
neagr.
De notat este faptul c Stage este construit pentru a suporta populaii de
dimensiuni mari de roboi mobili, astfel fiind posibil construirea de lumi relativ
complexe.
Figura 26 prezint o imagine captur a ecranului utilizator Stage.
# fiier simple.world

resolution 0.02
gui ( size [502.0 506.0]
origin [5.018 4.950 0]
scale 0.021
)
bitmap ( file cave.pnm.gz
resolution 0.02
)

# ncarc anumite tipuri de dispozitive
#din alt fiier cu numele pioneer.inc

include pioneer.inc
position ( port 6665
pose [1.0 1.0 20]
laser ()
)

Player/Stage
pagina 38

Figura 26. Exemplu de mediu virtual construit pentru simulatorul Stage
Ultima versiune a simulatorului Stage ruleaz n mod plugin pentru serverul
Player. Astfel, n fiierul de configurare Player, va trebui adugat o linie de genul:
driver (name "stage" provides ["simulation:0"] plugin "libstageplugin" worldfile "harta.world")
3 3. .4 4. . S Si im mu ul la at to or ru ul l 3 3D D G Ga az ze eb bo o
Pe lng simulatorul 2D Stage, mai exist nc un simulator anexat proiectului
Player/Stage, i anume simulatorul 3D Gazebo, ajuns la versiunea stabil 0.5.3.
Gazebo este un simulator pentru roboi, i este folosit ndeosebi pentru
medii deschise, de mari dimensiuni. Ca i Stage, Gazebo este capabil s simuleze
populaii de roboi, senzori i obiecte, dar face acest lucru ntr-o lume virtual n 3D.
Diferena esenial dintre cele dou simulatoare (pe lng faptul evident c Stage este
2D, iar Gazebo 3D) este c, n timp ce Stage este construit pentru a simula populaii
de mari dimensiuni de roboi cu o fidelitate moderat, Gazebo este construit pentru
a simula comportamentul unor grupuri mai mici de roboi (mai puin dect 10) ns
Player/Stage
pagina 39
cu o fidelitate mult mai mare. Aadar, cele dou simulatoare sunt complementare,
utilizatorii putnd schimba ntre ele n funcie de necesitile lor[19].
Arhitectura simulatorului Gazebo este prezentat n figura 27[17].

Figura 27. Arhitectura sistemului Gazebo
(imagine preluat i prelucrat din [17])
Exemple de medii posibile construite n simulatorul Gazebo sunt ilustrate n
figurile 28, 29, 30 i 31.

Figura 28. Exemplu de mediu virtual construit pentru simulatorul Gazebo
Player/Stage
pagina 40

Exemplul din figura alturat arat doi
roboi de tip Pioneer 2 AT fabricai de firma
ActivMedia Robotics ntr-un mediu virtual
3D dinamic.
Cei doi roboi sunt echipai cu senzori laser
respectivi GPS.
Figura 29. Interacionnd cu mediul
Figura 30 prezint un tip de robot paianjen
(hexapod) cu 18 grade de libertate. Robotul
a fost construit de ctre Dyan A. Shell de la
University of Southern California, SUA i
imit modelul robotului hexapod JPL (en.
abr. Jet Propulsion Laboratory) al NASA.

Figura 30. Robotul NASA de tip paianjen

Gazebo ofer o simulare realist a fizicii
obiectelor rigide: roboii pot mpinge
obiecte, le pot ridica (folosind grippere sau
orice alte dispozitive de prindere) i n
general pot interaciona cu mediul
nconjurtor.
Figura 31. Robot de tip Segway RMP simulat
Toate dispozitivele suportate de simulatorul Gazebo pot fi accesate prin
interfeele standard ale serverului Player. Ca i toate celelalte componente ale
proiectului Player/Stage, simulatorul Gazebo este dezvoltat sub licen GPL.
Motorul grafic al simulatorului folosete funcii OpenGL. Pentru ca mediul s
funcioneze corect, cel puin o fereastr i o camer video trebuie s fie definite n
fiierul care definete lumea virtual. Parametrii pentru fereastr definesc mrimea
acesteia, titlul i modul n care aceasta va fi manipulat. Parametrii pentru camera
video definesc modelele poligonale, rata de remprosptare a ecranului, etc.
Player/Stage
pagina 41
Lumea virtual este definit ntr-un fiier n care se definesc declaraiile
modelelor care se vor folosi. Un model poate fi un robot (exemplu Pioneer 2 AT sau
Segway RPM), un senzor (exemplu SICK LMS200) sau o component static a
mediului (exemplu MapExtruder). Un exemplu de declaraie a unui robot
Pioneer2AT ar fi:

Fiecrui model i este asociat un set de atribute ca de exemplu poziia
modelului <pos> i orientarea <rot>. Definiiile modelelor pot de asemenea s fie
mbricate. Exemplu:

n exemplul menionat, robotul mpreun cu senzorul laser formeaz un corp
compact.
Gazebo suport o varietate de modele de dispozitive hardware. n continuare
se prezint o list cu modelele existente n versiunea 0.5.3.
Atrv: robotul mobil ActivMedia Atrv cu 16 sonare;
Avatar helicopter: elicopterul USC AVATAR;
B21r: robotul mobil B21r cu 48 sonare;
Blimp: robotul Upenn Blimp;
Canon VCC4: camera video Canon VCC4 cu capaciti de pan-tilt-zoom;
Car chassis: model care simuleaz un vehicol main cu 4 roi;
ClodBuster: robot cu 4 roi;
Factory: o unealt pentru crearea de modele dinamice;
Garmin GPS: model care simuleaz un senzor GPS;
Ground plane: plan infinit neted;
Light source: model care simuleaz o surs de lumin;
Mono camera: model pentru o camer monocular simpl;
Observer camera: model pentru observarea simulrii din diferite unghiuri;
Peoplebot: robotul mobil ActivMedia People Bot cu 16 sonare;
Pioneer2AT: robot ActivMedia Pioneer2AT (en. All Terrain) cu 4 roi;
<model:Pioneer2AT>
<id>robot1</id>
<pos>0 0 0.40</pos>
<rot>0 0 45</rot>
<model:SickLMS200>
<id>laser1</id>
<pos>0.15 0 0.20</pos>
<rot>0 0 0</rot>
</model:SickLMS200>
</model:Pioneer2AT>

<model: Pioneer2AT>
<id>robot1</id>
<pos>0 0 0.40 </pos>
<rot>0 0 45</rot>
</model: Pioneer2AT>
Player/Stage
pagina 42
Pioneer2DX: robot ActivMedia Pioneer2DX cu 2 roi motoare plus o
roat castor;
Pioneer2Gripper: model care simuleaz un gripper pentru un robot
Pioneer2;
Road: model care simuleaz un drum folosind funcii Catmull-Rom spline;
Shrimp: robot BlueBotics Shrimp;
SickLMS200: senzorul laser SICK;
SimpleSolid: obiecte solide de tip cutie sau sfer;
Sky dome: model care simuleaz un cer;
SonyVID30: camer video color cu control ajustabil al poziiei i cu
facilitate de zoom optic;
Stereo Head: model care simuleaz un camer video stereo;
Terrain: model care creaz suprafee precum ziduri, dealuri sau vi;
TruthWidget: model pentru localizare folosit de simulator;
Wheelchair: model care simuleaz un scaun cu rotile robotic.
Figura 32 prezint o imagine captur a ecranului utilizator Gazebo.

Figura 32. Model al lumii virtuale n simulatorul Gazebo
Robotul Pioneer 2 DX din figur are o camer video instalat (vezi figura 33).
Player/Stage
pagina 43

Figura 33. Imagine preluat de la o camer virtual instalat pe robotul mobil
3 3. .5 5. . A Ad d u ug ga ar re ea a n no oi il lo or r d di is sp po oz zi it ti iv ve e
Unul dintre cele mai importante atuuri al serverului Player este faptul c noi
dispozitive se pot aduga dinamic ca i componente ale sistemului, fie ca module (en.
drivere) direct n sursele Player, fie ca librrii dinamice pe care Player le poate ncrca
la pornirea aplicaiei client.
O schem arhitectural a modului n care programele client funcioneaz cu
interfeele i driverele oferite de Player este prezentat n figura 34.

Figura 34. Accesarea modulelor Player din programele client
Dup cum se poate observa din figura de mai sus, modulele Player primesc
informaii de la robotul fizic (sau simulat) de regul prin interfee seriale. Aceste
informaii sunt procesate i clusterizate n interfeele Player i oferite apoi
programului client prin intermediul acestora. Programul client primete informaiile
de la interfeele Player i printr-un algoritm de control, calculeaz valorile
comenzilor pentru motoarele robotului, urmnd a le trimite serverului Player tot
prin intermediul acelorai interfee. n momentul n care Player primete informaiile
de la client, le va trimite la robot.
Player/Stage
pagina 44
Lista de drivere suportate de Player 1.6.4 este impresionant. Legtura dintre
drivere i interfeele aferente este prezentat n figura 35.
Toate driverele din serverul Player au o structur intern asemntoare.
Declaraiile care trebuie s existe n codul fiecrui driver sunt:
Definiia clasei driverului respectiv, mpreun cu metodele/funciile
private respectiv publice oferite de driverul respectiv;

Metoda Init apelat n momentul ncrcrii driverului;

Metoda Register apelat n momentul nregistrrii driverului n tabela de
drivere disponibile pentru client;

Constructorul efectiv al driverului;

Metoda Setup apelat n momentul n care clientul solicit accesul la
modul;
ModulNou :: ModulNou (ConfigFile* cf, int section) : Driver (cf, section)
{
if (cf->ReadDeviceId (&(this->modulnou_id), section, provides,
PLAYER_MODULNOU_CODE, -1, NULL) == 0)
if (this->AddInterface (this->modulnou_id, PLAYER_ALL_MODE,
sizeof (player_modulnou_data_t), 0, 5, 5) != 0)
{
this->SetError (-1);
return;
}
//
}
void ModulNou_Register (DriverTable* table)
{
table->AddDriver (modulnou, ModulNou_Init);
}
Driver* ModulNou_Init (ConfigFile* cf, int section)
{
return ((Driver*)(new ModulNou (cf, section));
}
class ModulNou :: public Driver
{
private:
player_device_id_t modulnou_id;
// ...
public:
ModulNou (ConfigFile* cf, int section);
virtual void Main ();
int Setup ();
int Shutdown ();
};

Player/Stage
pagina 45

Metoda Shutdown apelat n momentul n care clientul renun la accesul
la modul;

Metoda Main apelat n momentul n care clientul lucreaz cu modulul
respectiv.

Pe baza scheletului de cod prezentat mai sus se poate aduga un nou driver.
PLAYER_MODULNOU_CODE trebuie s fie un numr distinct i el trebuie s
existe ca i declaraie n fiierul player.h.
Serverul Player conine deja o serie de drivere pentru diverse componente
senzoriale i locomotorii ale roboilor mobili folosii n cercetare n diversele
laboratoare din lume. Dac ns se dorete adugarea unei componente noi, aceasta
poate fi foarte uor integrat prin construirea unui driver bazat pe scheletul
prezentat anterior.
ncepnd cu versiunea 1.6.4, driverele se pot ncrca dinamic ca i librrii
(folosind libtool). n cazul n care nu se dorete crearea unei interfee noi,
construirea unui driver pentru un anumit modul devine o simpl formalitate. Pentru
ca driverul s funcioneze trebuie doar scris codul care va controla componenta
hardware respectiv, i alese interfeele Player cu care driverul va lucra.
void ModulNou :: Main ()
{
//
for (;;)
{
// ...
}
pthread_exit (NULL);
}
int ModulNou :: Shutdown ()
{
//
StopThread ();
return (0);
}
int ModulNou :: Setup ()
{
// ...
StartThread ();
return (0);
}
Player/Stage
pagina 46

Figura 35. Legturile dintre drivere i interfee n Player 1.6.4
3 3. .6 6. . M Me et to od de e d de e i in nt te er rf fa a a ar re e c cu u c cl li ie en n i ii i
Datorit faptului c interfeele din serverul Player au fost standardizate,
driverele disponibile n acesta pot fi folosite n orice limbaj de programare, cu
precizarea ca s existe un client scris pentru acel limbaj. Pentru versiunea 1.6.4 a
serverului Player exist clieni scrii pentru limbajele: C#, C/C++, Guille, Java, Lisp,
Octave, Python, Ruby i Tcl.

Figura 36. Modelul client-server Player
Serverul Player

Driver Interfa

Program
utilizator
SICK
LMS 200
Pioneer
SICK
PLS
Segway
Khepera
sicklms200
sickpls
p2os
segwayrmp
khepera
laser
laser
position
position
position
Date
Configuraie
Comenzi
TCP/IP
IPC
Librriile
Client
Player

C/C++
C#
Guille
Java
Lisp
Octave
Python
Ruby
Tcl
Player/Stage
pagina 47
Modelul de interfaare al unui client cu serverul Player este prezentat n figura
36.
Pe baza modelului de interfaare prezentat, n figura 37 se detaliaz procesul de
comunicare ntre un client scris n orice limbaj de programare i serverul Player.

Figura 37. Schema de comunicare ntre Player i librria client
Player
Client
Library
Programe
utilizator
Player
Server
Componente
robot
Stabilete conexiunea
Cere acces la dispozitive

Citete date de la senzori

Trimite comenzi
Javaclient
pagina 48
4
4
.
.
J
J
a
a
v
v
a
a
c
c
l
l
i
i
e
e
n
n
t
t

Javaclient
pagina 49
Javaclient permite dezvoltarea aplicaiilor client pentru platforma Player/Stage
folosind limbajul de programare Java. Proiectul a fost iniial dezvoltat la University of
Southern California din Statele Unite ale Americii de ctre Esben stergard i Maxim
Batalin. Proiectul a fost ntrerupt n anul 2004, i apoi reluat n iarna anului 2005 de
ctre autorul acestei lucrri.
Ultima versiune a proiectului nainte de perioada de ntrerupere a fost 1.5a,
versiune funcional pentru platforma Player/Stage la momentul respectiv. Platforma
Player/Stage ns a continuat o dezvoltare ascendent, n timp ce Javaclient nu.
Datorit acestui motiv, n iarna anului 2005, nu exista nici un client Java funcional
pentru Player/Stage. De asemenea, numrul de interfee i drivere Player s-a triplat
fa de anul trecut, iar Javaclient nu suporta nici unul dintre acestea.
n momentul de fa, Javaclient suport absolut toate interfeele i driverele
Player, ultima versiune att a serverului Player ct i a proiectului Javaclient fiind
1.6.4. Pe lng corectarea tututor bugurilor (ro. greeli de programare, neologism) din
versiunea 1.5a, i aducerea la zi a interfeelor vechi, au fost aduse o serie de
mbuntiri, printre care:
posibilitatea de a folosi toate modulele Player existente;
adugarea de noi funcionaliti pentru o serie de module Player;
modularizarea proiectului prin folosirea mai multor clase i subpachete
Java;
documentarea tuturor metodelor i atributelor din fiecare clas a
proiectului;
adugarea de controlere soft (P, PD, PI, PID) pentru diverse operaii
asupra robotului (spre exemplu pentru poziionarea la de un anumit
unghi fa de ax sau micarea robotului dintr-un punct n altul pe hart).
Odat cu preluarea proiectului, s-a ncercat mrirea popularitii acestuia prin
gzduirea lui n cadrul comunitii Sourceforge. Ca i statistic, la aceast or,
Javaclient este folosit de o gam larg de universiti din ntreaga lume, respectiv
centre de cercetare precum German Research Center for Artificial Intelligence sau US
Naval Research Laboratory.
4 4. .2 2. . A Ar rh hi it te ec ct tu ur ra a J Ja av va ac cl li ie en nt t
Proiectul este mprit ntr-o serie de pachete i clase, pentru o mai mare
modularitate. Arhitectura de baz a pachetelor Javaclient este prezentat n figura 38.
Javaclient
pagina 50

Figura 38. Arhitectura pachetelor Javaclient
Pachetul javaclient.* conine clasele principale ale proiectului, respectiv
interfeele necesare pentru a comunica i accesa modulele Player. n total, pachetul
conine 39 clase, i aproximativ 9375
*
linii de cod [* versiunea 1.6.4 a proiectului].
Clasele componente sunt:
AbstractPositionDevice clas abstract pentru toate interfeele de
poziie (PositionInterface, Position2DInterface, Position3DInterface) din
Javaclient. Folosit de asemenea de PositionControl i HeadingControl
pentru poziionarea la de un anumit unghi fa de ax sau micarea
robotului dintr-un punct n altul pe hart;
AIOInterface clas care se ocup de interfaarea modulului Player AIO
cu Javaclient. AIO permite accesul la un dispozitiv I/O analogic;
AudioDSPInterface clas care se ocup de interfaarea modulului
Player AudioDSP cu Javaclient. AudioDSP permite accesul la diverse
echipamente hardware audio;
AudioInterface clas care se ocup de interfaarea modulului Player
Audio cu Javaclient. Audio permite accesul la diverse echipamente
hardware audio;
AudioMixerInterface clas care se ocup de interfaarea modulului
Player AudioMixer cu Javaclient. AudioMixer controleaz nivelul
semnalului audio al interfeelor AudioInterface i AudioDSPInterface;
BlinkenlightInterface clas care se ocup de interfaarea modulului
Player Blinkenlight cu Javaclient. Blinkenlight este folosit pentru a aprinde
sau a stinge un indicator luminos cu o anumit perioad;
BlobfinderInterface clas care se ocup de interfaarea modulului
Player Blobfinder cu Javaclient. Blobfinder este folosit pentru a detecta
pete de culoare n imagini provenite de la camere video;
BumperInterface clas care se ocup de interfaarea modulului Player
Bumper cu Javaclient. Bumper permite accesul la senzori de tip bumper
instalai pe robotul mobil;
CameraInterface clas care se ocup de interfaarea modulului Player
Camera cu Javaclient. Camera este folosit pentru a transfera imagini de
la un dispozitiv fizic, precum o camer video, la un client Player;
Javaclient
pagina 51
DIOInterface clas care se ocup de interfaarea modulului Player DIO
cu Javaclient. DIO permite accesul la un dispozitiv I/O numeric;
EnergyInterface clas care se ocup de interfaarea modulului Player
Energy cu Javaclient. Energy ofer informaii despre nivelul bateriei unui
robot mobil;
FiducialInterface clas care se ocup de interfaarea modulului Player
Fiducial cu Javaclient. Fiducial este folosit pentru a detecta diverse
marcaje n mediul n care robotul opereaz;
GPSInterface clas care se ocup de interfaarea modulului Player GPS
cu Javaclient. GPS permite accesul la dispozitive de localizare i
poziionare global GPS;
GripperInterface clas care se ocup de interfaarea modulului Player
Gripper cu Javaclient. Gripper permite accesul la un bra fizic instalat pe
robotul mobil (dac exist);
IRInterface clas care se ocup de interfaarea modulului Player IR cu
Javaclient. IR permite accesul la senzori care funcioneaz n spectrul
infrarou (IR) instalai pe robotul mobil;
JoystickInterface clas care se ocup de interfaarea modulului Player
Joystick cu Javaclient. Joystick se ocup de interfaarea cu un dispozitiv
de tip joystick fizic;
LaserInterface clas care se ocup de interfaarea modulului Player
Laser cu Javaclient. Laser permite accesul la senzori de tipul SICK
LMS200 instalai pe robotul mobil;
LocalizeInterface clas care se ocup de interfaarea modulului Player
Localize cu Javaclient. Localize este o interfa folosit pentru localizarea
robotului n spaiul de lucru folosind diveri algoritmi (Adaptive Monte
Carlo Localization spre exemplu);
LogInterface clas care se ocup de interfaarea modulului Player Log
cu Javaclient. Log ofer o interfa flexibil pentru lucrul cu fiiere de
date jurnal;
MapInterface clas care se ocup de interfaarea modulului Player Map
cu Javaclient. Map ofer acces la o hart mprit n regiuni;
MComInterface clas care se ocup de interfaarea modulului Player
Mcom cu Javaclient. MCom este o interfa folosit pentru schimbul de
date ntre clieni, folosind tipuri de mesaje i canale de transmisie;
MotorInterface clas care se ocup de interfaarea modulului Player
Motor cu Javaclient. Motor este folosit pentru a comanda un singur
motor;
NomadInterface clas care se ocup de interfaarea modulului Player
Nomad cu Javaclient. Nomad este o interfa folosit pentru a accesa
componentele roboilor mobili fabricai de firma Nomadics;
PlannerInterface clas care se ocup de interfaarea modulului Player
Planner cu Javaclient. Planner controleaz un planificator de micare 2D;
Javaclient
pagina 52
PlayerClient clasa principal a proiectului. Rutinele de iniializare,
comunicare cu serverul Player, precum i interfaare a claselor
prezentate anterior sunt coninute de aceast clas;
PlayerDevice clas abstract pentru toate interfeele existente n
Javaclient;
Position2Dinterface clas care se ocup de interfaarea modulului
Player Position2D cu Javaclient. Position2D controleaz o baz robot,
fiind similar cu Position, cu diferena c unitile sunt n mrad n loc de
grade;
Position3Dinterface clas care se ocup de interfaarea modulului
Player Position3D cu Javaclient. Position3D controleaz o baz robot n
spaiul 3D;
PositionInterface clas care se ocup de interfaarea modulului Player
Position cu Javaclient. Position controleaz o baz robot n spaiul 2D,
similar cu Position2D;
PowerInterface clas care se ocup de interfaarea modulului Player
Power cu Javaclient. Power ofer acces la sistemul de putere al
robotului;
PtzInterface clas care se ocup de interfaarea modulului Player Ptz
cu Javaclient. Ptz ofer faciliti de pan-tilt-zoom pentru camere video
sau alte dispozitive similare;
SimulationInterface clas care se ocup de interfaarea modulului
Player Simulation cu Javaclient. Simulation permite accesul clientului la
un simulator precum Stage sau Gazebo;
SonarInterface clas care se ocup de interfaarea modulului Player
Sonar cu Javaclient. Sonar permite accesul la senzori de tip sonar
instalai pe robotul mobil;
SoundInterface clas care se ocup de interfaarea modulului Player
Sound cu Javaclient. Sound permite redarea unor sunete nregistrate
aprioric (de exemplu robotul Amigobot poate reda sunete aflate n
memoria lui);
SpeechInterface clas care se ocup de interfaarea modulului Player
Speech cu Javaclient. Speech ofer acces la un sistem de sintetizare
vocal;
SpeechRecognitionInterface clas care se ocup de interfaarea
modulului Player SpeechRecognition cu Javaclient. SpeechRecognition
ofer acces la un sistem de recunoatere a vocii;
TruthInterface clas care se ocup de interfaarea modulului Player
Truth cu Javaclient. Truth permite aflarea poziiei absolute a robotului n
simulator;
WaveformInterface clas care se ocup de interfaarea modulului
Player Waveform cu Javaclient. Waveform este folosit pentru a
recepiona sample-uri arbitrare digitale de la diferite dispozitive (spre
exemplu dispozitive audio);
Javaclient
pagina 53
WiFiInterface clas care se ocup de interfaarea modulului Player
WiFi cu Javaclient. WiFi ofer acces la o interfa de reea wireless.
Pachetul javaclient.structures.* conine acele clase ale proiectului, n care sunt
definite diversele structuri folosite de unele interfee Player. n total, pachetul
conine 19 clase, i aproximativ 1840
*
linii de cod [* versiunea 1.6.4 a proiectului].
Clasele componente sunt:
Blob clas n care este definit structura Player
player_blobfinder_blob, folosit de BlobInterface;
Hypothesis clas n care este definit structura Player player_localize_
hypoth, folosit de LocalizeInterface;
PlayerBumperDefineT clas n care este definit structura Player
player_bumper_define, folosit de BumperInterface;
PlayerBumperGeomT clas n care este definit structura Player
player_bumper_geom, folosit de BumperInterface;
PlayerDeviceDevlistT clas n care este definit structura Player
player_device_devlist, folosit de PlayerClient;
PlayerDeviceDriverInfo clas n care este definit structura Player
player_device_driverinfo, folosit de PlayerClient;
PlayerDeviceIdT clas n care este definit structura Player
player_device_ id, folosit de PlayerDeviceDriverInfo;
PlayerFiducialItem clas n care este definit structura Player
player_fiducial_item, folosit de FiducialInterface;
PlayerIRPoseReqT clas n care este definit structura Player
player_ir_ pose_req, folosit de IRInterface;
PlayerLaserConfigT clas n care este definit structura Player player_
laser_config, folosit de LaserInterface;
PlayerLaserGeomT clas n care este definit structura Player player_
laser_geom, folosit de LaserInterface;
PlayerLocalizeConfigT clas n care este definit structura Player
player_ localize_config, folosit de LocalizeInterface;
PlayerPlannerWaypointT clas n care este definit structura Player
player_planner_waypoint, folosit de PlannerInterface;
PlayerPosition2DGeomT clas n care este definit structura Player
player_position2d_geom, folosit de Position2DInterface;
PlayerPosition3DGeomT clas n care este definit structura Player
player_position3d_geom, folosit de Position3DInterface;
PlayerPositionGeomT clas n care este definit structura Player
player_position_geom, folosit de PositionInterface;
PlayerSimulationPose2D clas n care este definit structura Player
player_simulation_pose2d_req, folosit de SimulationInterface;
Javaclient
pagina 54
PlayerSonarGeomT clas n care este definit structura Player
player_sonar_geom, folosit de SonarInterface;
PlayerWiFiLinkT clas n care este definit structura Player
player_wifi_link, folosit de WiFiInterface;
Pachetul javaclient.extra.* conine clase auxiliare proiectului, clase care
asigur o funcionalitate extins n unele cazuri. n total, pachetul conine 8 clase, i
aproximativ 1003
*
linii de cod [* versiunea 1.6.4 a proiectului]. Clasele componente
sunt:
Controller clas abstract motenit apoi de alte clase pentru
dezvoltarea controlerelor de baz {P,I,D};
HeadingControl clas folosit n tandem cu Position{{2,3},D}Interface
pentru a roti robotul diferenial sau absolut cu un anumit unghi dat pe
loc. Clasa este fiu al PIDController;
PController clas n care este definit un controler de tipul P
(proporional);
PDController clas n care este definit un controler de tipul PD
(proporional-derivativ);
PIController clas n care este definit un controler de tipul PI
(proporional-integral);
PIDController clas n care este definit un controler de tipul PID
(proporional-integral-derivativ);
PositionControl clas folosit n tandem cu Position{{2,3},D}Interface
pentru a mica robotul cu o anumit distan. Clasa este fiu al
PIDController;
PositionGeometryTools clas care conine diferite metode pentru
calculul geometric al poziiei. Folosit de PositionControl.
n total, proiectul Javaclient versiunea 1.6.4 nsumeaz aproximat 12.218 linii
de cod surs. n figura 39 se prezint diagrama UML a claselor proiectului.
Complexitatea proiectului reiese nc din figura 39, fiind aproape imposibil de a
recunoate numele claselor din figura de mai jos. n anexa A s-a inclus o imagine mai
mare, vizibil, reprezentnd diagrama UML a claselor proiectului Javaclient.

Figura 39. Diagrama UML a claselor proiectului (schem de principiu)
Javaclient
pagina 55
n partea central de sus a schemei de principiu prezentat n figura 39 se afl
clasa abstract PlayerDevice, motenit de toate interfeele Player disponibile n
Javaclient. n partea dreapt, blocul mare reprezint clasa PlayerClient care
instaniaz toate interfeele n funcie de apelul primit de la programul utilizator.
4 4. .3 3. . S Su up po or rt t p pe en nt tr ru u d di is sp po oz zi it ti iv ve e n no oi i
Pachetul javaclient a fost n aa mod rescris, nct s permit adugarea de
interfee i dispozitive suportate noi, ntr-un mod ct mai uor. n continuare se va
prezenta modul n care trebuie modificate sursele proiectului pentru a aduga suport
pentru o nou interfa, ipotetic a proiectului Player.
Se presupune c noua interfa va citii un senzor pe un canal I/O numeric.
Pentru a nu scrie i o interfa Player (vezi anexa D), se va presupune c interfaa
Player este similar interfeei DIO. Astfel, paii pentru a aduga noua interfa la
pachetul Javaclient sunt:
1. Se creaz o clas nou, n formatul <NumeInterfa>Interface. Se presupune
c numele noii interfee este Test, deci clasa creat va fi TestInterface.java
*
(*
structura clasei va fi explicat n detaliu n seciunea urmtoare). Clasa
TestInterface va extinde PlayerDevice;
2. Se modific PlayerClient.java, i se adaug urmtoarele seciuni de cod (se
presupune c codul Player al noii interfee este 100):
a. n antetul clasei PlayerClient, se adaug atributul:

b. n corpul clasei PlayerClient, se adaug metoda:

c. n metoda privat requestSatisfy () din PlayerClient, se adaug:
/**
* Request a Test device.
* @param index the device index
* @param r access mode
* @return a Test device if successful, null otherwise
*/
public TestInterface requestInterfaceTest (int index, char r) {
return (TestInterface)requestDeviceAccess (PLAYER_TEST_CODE,
(short)deviceCnt[100], r);
}
/* the current assigned interface codes for Player 1.6.4 */

protected static final short PLAYER_TEST_CODE = 100;
...
Javaclient
pagina 56

3. Se scrie codul clasei TestInterface, un schelet pentru acesta fiind prezentat n
continuare:

Dup finalizarea pailor 1-3, noua interfa TestInterface poate fi folosit de
ctre programul client. Pentru aceasta, programul client va trebui s instanieze un
obiect de tipul PlayerClient, iar apoi un alt obiect de tipul TestInterface. Valoarea
citit de la interfaa DIO Player se va putea chestiona prin metoda getValue () a clasei
TestInterface.
4 4. .4 4. . E Ex xe em mp pl le e d de e u ut ti il li iz za ar re e
Javaclient impune un nou stil de programare pentru roboi mobili. Utiliznd
modularitatea i flexibilitatea limbajului de programare Java, i mbinnd elementele
arhitecturale ale serverului Player, utilizatorul beneficiaz de o suit de librrii cu o
funcionalitate puternic. Beneficiul utilizrii Javaclient i n special al programrii
roboilor mobili n limbajul Java este acela c utilizatorul poate folosi i alte
librrii/pachete precum Joone (Java Object Oriented Neural Engine) sau ADK (Agents
Development Kit), mbinnd astfel tehnicile de programare clasice cu diferite aspecte
ale inteligenei artificiale (vezi [15] i [19]).
package javaclient;
public class TestInterface extends PlayerDevice {
private final short PLAYER_TEST_CODE = PlayerClient.PLAYER_TEST_CODE; /* Test */
private int value = 0; /* read value */
public TestInterface (PlayerClient pc, short indexOfDevice) {
super(pc);
device = PLAYER_TEST_CODE;
index = indexOfDevice;
}
public synchronized void readData () {
readHeader ();
try {
value = is.readInt (); /* read value */
} catch (Exception e) {
System.err.println ("[Test] : Error when reading payload: " + e.toString ());
}
}
public synchronized int getValue () { return value; }
}
switch (device2) {

case PLAYER_TEST_CODE: { /* Test I/O */
newpd = new TestInterface (this, (short)deviceCnt[device2]);
break;
}

}
Javaclient
pagina 57
n continuare se prezint scheletul unui program client folosind Javaclient,
pentru un robot echipat cu senzori ultrasonici i IR. Programul va afia pe ecranul
calculatorului datele primite de la senzori, i va ncerca s mite robotul nspre acea
zon a mediului cel mai puin ocupat. Algoritmii de acest gen poart denumirea de
cuttori de spaiu (en. SpaceWanderer). Odat cu prezentare algoritmului se vor
explica i paii necesari pentru utilizarea Javaclient n programarea roboilor mobili.
Primul lucru pe care un programator trebuie s-l fac este s importe cele
trei pachete Javaclient.

Pentru afiarea informaiilor primite de la senzori ntr-un mod mai eficient, se
vor folosi i dou clase din pachetul java.text, i anume: NumberFormat i FieldPosition.
Dup definiia clasei utilizator i a metodei main, se va trece la conectarea
programului cu serverul Player. Pentru acest lucru, se va instania un obiect de tipul
PlayerClient, cruia i se vor transmite ca i parametrii att host-ul sau adresa IP a
serverului ce ruleaz Player ct i portul pe care acesta ruleaz. n continuare, se va
trece la iniializarea i cererea accesului pentru interfeele Player dorite. n cazul de
fa se vor instania interfeele: SonarInterface, IRInterface i PositionInterface.

n exemplul de cod de mai sus, s-a presupus c serverul Player ruleaz pe
acelai calculator ca i programul client, respectiv c portul pe care serverul Player
ruleaz este cel implicit, i anume portul 6665.
Dup crearea interfeelor se trece la bucla efectiv de control. Pentru
simplitate, s-a ales o bucl infinit, n care se vor citi periodic informaiile de la
senzori, se vor afia pe ecranul calculatorului i se va ncerca micarea robotului n
acele zone libere menionate anterior. nainte de aceste operaii, codul principal al
clasei PlayerClient va fi pornit prin apelarea metodei runThreaded ().

Algoritmul (de principiu) de mai sus este prezentat n form minimal. Se
citesc senzorii sonar, se alege valoarea cea mai mare, se calculeaz i se transmite
robot.runThreaded (-1, -1);
while (true) {
int max = 0;
int[] sonars = si.getRanges ();
int[] irs = ii.getRanges ();
System.out.println (decodeSonars (sonars));
System.out.println (decodeIRs (irs));
for (int i = 0; i < si.getSamplesCount (); i++)
if (sonars[i] > max)
max = sonars[i];
pi.setSpeed (10, (max/20)*360/(2*Math.PI));
try { Thread.sleep (1000); } catch (Exception e) { }
}
PlayerClient robot = new PlayerClient (localhost, 6665);
SonarInterface si = robot.requestInterfaceSonar (0, a);
IRInterface ii = robot.requestInterfaceIR (0, a);
PositionInterface pi = robot.requestInterfacePosition (0, a);
import javaclient.*;
import javaclient.structures.*;
import javaclient.extra.*;
Javaclient
pagina 58
viteza unghiular ctre robot, pstrnd viteza liniar la valoarea de 10cm/s, cu
precizarea c baza robotului este de 20cm.
Metodele decodeSonars () i decodeIRs () sunt similare. Se prezint n
continuare codul pentru una dintre ele.

Align () este o metod folosit pentru alinierea valorilor afiate pe ecran.
Metoda mai face uz i de obiectul fmt, care trebuie definit ca instan a clasei
NumberFormat: static NumberFormat fmt = NumberFormat.getInstance ();

Exemplul prezenat nu este funcional, scopul lui fiind doar de a demonstra
uurina cu care se pot utiliza pachetele Javaclient. Pentru mai multe exemple a se
consulta documentaia proiectului Javaclient.
static String align (NumberFormat fmt, int n, int sp) {
StringBuffer buf = new StringBuffer ();
FieldPosition fpos = new FieldPosition (NumberFormat.INTEGER_FIELD);
fmt.format (n, buf, fpos);
for (int i = 0; i < sp - fpos.getEndIndex (); ++i)
buf.insert (0, ' ');
return buf.toString ();
}
public static String decodeSonars() {
String out = "\nSonar vars: \n";
for (int i = 0; i < spd.getSamplesCount(); i++) {
out += " [" + align(fmt, i+1, 2) + "] = " + align(fmt, spd.getRanges()[i], 5);
if (((i+1) % 8) == 0)
out += "\n";
}
return out;
}
ZeeRO
pagina 59
5
5
.
.
Z
Z
e
e
e
e
R
R
O
O

ZeeRO
pagina 60
5 5. .1 1. . A Ar rh hi it te ec ct tu ur ra a s si is st te em mu ul lu ui i
Robotul mobil ZeeRO a fost construit de o serie de membrii ai grupului de
cercetare n robotic (Robotics Research Group) din cadrul Facultii de Automatic i
Calculatoare a Universitii Tehnice din Cluj-Napoca.
n ncercarea de a construi un robot mobil cu o arhitectur modular,
eficient dar i cu un cost redus, a fost necesar un studiu ndelungat att al diferitelor
componente existente n comer, ct i al platformelor software cu care robotul va
trebui integrat pentru a obine performane maxime.
Numele robotului vine de la cuvntul robotul, care n limba englez se traduce
prin the robot (pronunat zee robot). Scopul construirii acestui robot a fost de a
pune n practic o serie de cunotine acumulate i dobndite pe parcursul multor ani
de studiu n domeniu. De asemenea, datorit preului foarte mare practicat de
firmele care comercializeaz roboi mobili, s-a dorit crearea unui robot mobil mai
ieftin, dar cel puin la fel de eficient, cu costuri maximale impuse din start de
aproximativ 1000 de euro.
nainte de achiziionarea efectiv a componentelor robotului, s-au studiat o
gam larg de componente, care ar putea alctui cele patru sisteme vitale ale
robotului: sistemul de locomoie, sistemul senzorial, unitatea central de procesare
precum i sistemul de comunicaie. Cteva dintre aceste componente vor fi
prezentate n seciunile urmtoare.
Din punct de vedere software, cerina principal a fost ca noul robot s poat
fi integrat cu ceilali roboi existeni n dotarea laboratorului, n principal cu robotul
Pioneer2 produs de firma ActivMedia. Urmnd calea deschis de [19], s-a decis ca
platforma peste care se vor integra funciile i apoi algoritmii software ai robotului s
fie Player/Stage. Astfel, robotul devine programabil ntr-o multitudine de limbaje de
programare, printre care i Java, datorit proiectului Javaclient dezvoltat de autorul
acestei lucrri. Pentru a putea fi controlat ns, a fost nevoie de scrierea unor drivere
software pentru Player, precum i de corectarea unor buguri n driverele Player
existente.
O alt cerin care s-a ncercat respectat a fost modularizarea pe ct posibil
a componentelor hardware. A trebuit studiat posibilitatea folosirii unor interfee de
conectare inteligente pentru sistemul senzorial, pe ct posibil Plug&Play. Din pcate
ns, la ora actual, nc nu exist un standard unificat pentru senzorii folosii pe
roboii mobili n aceast privin, majoritatea fiind nc conectai prin metodele
clasice (intrri-ieiri analogice sau numerice).
n fine, s-a dorit ca unitatea central de procesare s fie pe ct posibil format
din mai multe subuniti, pentru ca operaiile de nivel jos respectiv de nivel nalt s se
poat executa distribuit. O alt cerin important a fost ca robotul s aib
posibilitatea de a comunica fr fir cu ali roboi din mediu, sau cu un calculator
personal.
Arhitectura general a robotului ZeeRO este prezentat n figura 40.
ZeeRO
pagina 61

Figura 40. Arhitectura general a robotului mobil ZeeRO
Componentele prezentate n figura 40 vor fi pe rnd analizate n seciunile
urmtoare.
5 5. .2 2. . S Si is st te em mu ul l d de e l lo oc co om mo o i ie e
Dup o analiz ndelungat a sistemelor de locomoie existente n domeniu, s-
a decis implementarea unui sistem de locomoie diferenial. Datorit faptului c
robotul ZeeRO a fost prima ncercare de a construi un robot mobil a grupului de
cercetare, idea achiziionrii unei baze robot a fost binevenit. Baza robot urma s
conin motoarele folosite pentru deplasarea robotului mobil precum i un schelet
minimal, pe care s existe posibilitatea de a monta diversele componente ale
celorlalte sisteme care intr n componena robotului mobil.
Preul maxim admisibil pentru baza robot nu trebuia s depeasc pe cel al
celorlalte componente. Din acest motiv, opiunile pentru gsirea unei baze robot
optime au fost mult reduse. Dup discuii cu membrii grupului, s-a decis
achiziionarea bazei robot Rogue Blue Base, fabricat i comercializat de firma
Rogue Robotics din Canada. Componentele bazei robot sunt prezentate n figura 41.
Problema cea mai mare a bazei robot achiziionate este reprezentat de
calitatea slab a servomotoarelor. Schimbarea acestora cu motoare de curent
continuu de calitate echipate cu encodere constituie un lucru imperativ pentru
continuarea cercetrilor.
ZeeRO
pagina 62

O prim observaie asupra achiziiei
fcute este faptul c baza robot trebuia
s conin 3 nivele. Datorit faptului
ns c firma Rogue Robotics a
modificat componena produselor
comercializate, baza achiziionat
conine doar 2 nivele.
Astfel, a fost nevoie de construirea
unui nivel superior, pe care s se poat
monta componentele senzoriale ale
robotului. Nivelele inferioare au un
diametru de 18cm, n timp ce nivelul
superior are un diametru de 20cm.
Servomotoarele bazei robot arat
similar cu cele din figura 42. Ele pot fi
comandate direct de o component a
unitii centrale de procesare (vezi
seciunea 5.4). Singurul aspect negativ
este faptul c servomotoarele nu
conin encodere, drept urmare,
robotul nu va beneficia de informaii de
odometrie.
Figura 41. Componentele bazei robot
Rogue Blue

Construirea scheletului robot nu a constituit o problem major. Controlarea
servomotoarelor ns da. Aceste aspecte in ns mai mult de natura fizic a
servomotoarelor i, este n opinia autorului acestei lucrri c ele nu pot fi remediate
dect prin schimbarea n totalitate a servomotoarelor cu alte motoare (preferabil de
curent continuu).

Figura 42. Servomotor folosit de robotul ZeeRO
ZeeRO
pagina 63
5 5. .3 3. . S Si is st te em mu ul l s se en nz zo or ri ia al l
Dac opiunile nu au fost suficiente pentru a achiziiona o baz robot mai
performant, lucrurile au stat diferit n momentul alegerii componentelor sistemului
senzorial. Din fericire, exist o multitudine de tipuri de senzori precum i de
companii care i comercializeaz n lume.
Arhitectura sistemului senzorial dezvoltat iniial propunea folosirea unei
game ct mai variate de senzori, printre care:
Senzori ultrasonici (sonar) utilizai pentru detectarea i evitarea
obstacolelor din mediu;
Senzori n spectrul infrarou (IR) utilizai de asemenea pentru
detectarea i evitarea obstacolelor din mediu;
Senzori video, reprezentai de camere video sau webcam utilizai
pentru achiziionarea unui spectru larg de informaii, care apoi prin
prelucrare s ofere posibiliti precum detectarea i urmrirea unei pete
de culoare, a unui obiect sau persoane umane, detecia apariiei unor
anumite evenimente n mediu, etc;
Senzori piroelectrici utilizai pentru detectarea persoanelor umane,
sau a unor obiecte calde (precum o lumnare);
Senzori de tip compas utilizai pentru detectarea cmpurilor
magnetice, i deci oferind informaii de orientare pentru robotul mobil;
Senzori de contact, reprezentai prin senzori de tipul bumper utilizai
pentru detectarea i absorbia ocurilor la contactul cu obiecte din
mediu.
Datorit bugetului redus, ct i a scopului final al robotului mobil, senzori de
tipul laser sau GPS nu au fcut parte din arhitectura sistemului senzorial.
La o analiz mai aprofundat a componentelor senzoriale enumerate mai sus,
s-a decis renunarea la senzorii de contact i la compas, i achiziionarea n schimb a
mai multor uniti de tipul sonar.
Furnizorul principal al componentelor senzoriale a fost firma Acroname, din
Statele Unite ale Americii. n continuare se vor prezenta senzorii achiziionai,
precum i caracteristicile acestora. Pentru mai multe informaii a se consulta [20].
Modulele IR sunt fabricate de firma Sharp, modelul ales fiind GP2D02 (vezi
figura 43). S-a optat pentru modelul GP2D02 att datorit faptului c baza robot are
dimensiuni mici, senzorul oferind o distan msurabil ntre 10 i 80cm, ct i
datorit faptului c semnalul primit este numeric.
Tot n gama GP, firma Sharp mai ofer i ali senzori att digitali ct i
analogici, printre care: GP2D05 (digital, detecteaz obiecte doar la 24cm distan),
GP2D12 (analog, detecteaz obiecte ntre 10 i 80cm), GP2D15 (similar GP2D05),
GP2D120 (analog, detecteaz obiecte ntre 4 i 30cm), GP2Y0A02YK (analog,
detecteaz obiecte ntre 20 i 150cm) respectiv GP2Y0D02YK (digital, detecteaz
obiecte doar la 80cm distan).
ZeeRO
pagina 64

Figura 43. Senzorul IR Sharp GP2D02
Interfaarea senzorilor GP2D02 cu unitatea central este prezentat n
seciunea 5.4.
Cele dou categorii de senzori ultrasonici alese, SRF08 i SRF10, sunt
fabricate de firma Devantech (vezi figurile 44 i 45). Senzorii ofer o distan
msurabil ntre 3cm i 6m, cu o eroare de +/-3cm la distana maxim. Unul dintre
cele mai importante aspecte ale alegerii acestor modele este ns faptul c pot fi
interfaate pe IIC, simplificnd astfel enorm rutinele de programare.

Figura 44. Senzorul Devantech SRF08 Figura 45. Senzorul Devantech SRF10

Diferena ntre SRF08 i SRF10 const n faptul c modelul SRF08 mai conine
un senzor de lumin montat pe plac. Ambele modele se alimenteaz la 5V i au un
consum redus de energie. Pentru mai multe informaii cu privire la caracteristicile lor
tehnice a se consulta [20].
Senzorul piroelectric Eltec 442-3 detecteaz cldura unui obiect folosind
principiile deja explicate n seciunea 2.3.6. Pachetul provenit de la firma Acroname
conine att detectorul Eltec 442-3 ct i o lentil Fresnel mpreun cu kitul aferent
pentru a forma un con (vezi figura 46).
ZeeRO
pagina 65

Figura 46. Pachetul senzor piroelectric
Senzorul Eltec 442-3 conine doi detectori care detecteaz cldura n spectrul
infrarou, montai paraleli. Semnalul provenit de la senzor este analogic.
Locul senzorului video este preluat de camera CMUcam2 (figura 47, 48 i 49)
produs de Carnegie-Mellon University, din Statele Unite ale Americii.

Figura 47. Camera video CMUcam2
CMUcam2 este format din microcontrolerul SX52 interfaat cu un senzor
CCD OV6620. Legtura ntre camer i alte componente se realizeaz prin
intermediul unei interfee seriale RS232. Camera are o serie de faciliti, printre care
posibilitatea de a urmrii pete de culoare la 50fps sau de a detecta micarea
ZeeRO
pagina 66
corpurilor din spaiul ei vizual prin diferenierea ntre cadre, toate acestea fiind
realizate la nivel hardware, graie microcontrolerului ncorporat.

Figura 48. CMUcam2 - vedere frontal Figura 49. CMUcam2 - vedere spate
O caracteristic important a microcontrolerului SX52 este posibilitatea de a
comanda pn la 5 servomotoare direct de pe placa CMUcam2. Primele dou
servomotoare conectate pot fi folosite pentru funcii de rotire pe vertical i
orizontal a camerei, n cazul n care camera este montat pe un dispozitiv special de
acest gen. Pentru mai multe informaii despre modul n care s-a realizat acest lucru, a
se consulta [20].
Toate componentele sistemului senzorial descrise anterior sunt alimentate
direct din una dintre componentele unitii centrale de procesare, cu excepia
camerei CMUcam2 care este alimentat separat de 5 acumulatori GP AA (1.2V).
Pentru utilizarea software a camerei CMUcam2 au trebuit rescrise o serie de
funcii ale driver-ului Player (vezi seciunea 5.5).
5 5. .4 4. . U Un ni it ta at te ea a c ce en nt tr ra al l d de e p pr ro oc ce es sa ar re e
Aspectul cel mai important care s-a dorit realizat, a fost distribuirea funciilor
unitii centrale de procesare, astfel nct s existe posibilitatea de a executa att
funcii de nivel nalt ct i funcii de nivel jos concomitent. Pentru a obine acest
deziderat, s-au ales urmtoarele componente hardware:
microcalculatorul Gumstix Waysmall, produs de firma Gumstix din
Statele Unite ale Americii rol de a procesa i executa funciile de nivel
nalt;
microcontrolerele Acroname BrainStem GP (en. General Purpose) 1.0 i
BrainStem Moto 1.0 rol de a procesa i executa funciile de nivel jos.
Gumstix Waysmall (vezi figura 50) este un microcalculator construit n jurul
microprocesorului Intel PXA255 XScale. Numele gumstix provine de la faptul c
dimensiunile efective ale plcii sunt aproximativ egale cu cele ale unei gume de
mestecat. n configuraia de baz, platforma Gumstix vine fie cu procesor care
ruleaz la frecvena de 200Mhz, fie cu procesor la 400Mhz. Iniial platforma coninea
ZeeRO
pagina 67
4MB de memorie flash, dar ultimele versiuni comercializate de firma productoare
conin 16MB. n versiunea de baz, Gumstix conine 64MB RAM.
Toate versiunile de Gumstix conin un conector Hirose pentru interfaarea la
plci de tipul expansion boards secundare. Pe lng acest conector, platforma
Gumstix mai ofer posibilitatea folosirii unor memorii flash de tipul SD/MMC.
Posibilitile de extensie ale platformei Gumstix sunt numeroase. Se amintesc
cteva din modelele de plci de extensie disponibile n acest moment:
etherstix plac de extensie care ofer un conector standard RJ45;
cfstix plac de extensie care permite folosirea memoriilor de tipul
CompactFlash;
audiostix plac de extensie care ofer un conector USB client precum
i dou porturi audio (IN i OUT);
thumbstix plac de extensie care transform platforma Gumstix ntr-
un USB stick;
waysmall plac de extensie care ofer 2 conectori RS232 (n format
minidin8) precum i un conector USB client.

Figura 50. Platforma Gumstix Waysmall
Cel mai important aspect al platformei de dezvoltate Gumstix ns, este faptul
c vine cu un minisistem de operare Linux deja instalat n memoria Flash. Astfel,
utilizatorul beneficiaz de o gam foarte larg de produse software att open-source
ct i comerciale, disponibile pe reeaua Internet.
Conform arhitecturii prezentate deja n figura 40, microcalculatorul Gumstix
joac rolul creierului central al sistemului robotului mobil ZeeRO. Camera video
CMUcam2 precum i unul din cele dou microcontrolere BrainStem sunt conectate
la Gumstix prin intermediul celor dou interfee seriale ale acestuia (vezi figura 51).
O alt facilitate oferit de platforma Gumstix este posibilitatea de a fi accesat
prin unde radio, folosind standardul de transmisie wireless Bluetooth. Astfel, robotul
mobil devine nu numai autonom, ci i interfaabil ca entitate de sine stttoare.
ZeeRO
pagina 68

Figura 51. Conexiunile sistemului central de procesare
Cele dou microcontrolere comercializate de firma Acroname sunt
prezentate n figurile 52 i 53. Ambele sunt echipate cu procesoare RISC la 40Mhz, i
ofer o gam larg de porturi I/O, interfee seriale, precum i diferite alte posibiliti
de extensie sau conectare cu alte dispozitive.

Figura 52. Microcontrolerul Acroname
Brainstem GP 1.0
Figura 53. Microcontrolerul Acroname
Moto 1.0
Caracteristicile microcontrolerului Acroname Brainstem GP 1.0:
5 canale analogice A/D pe 10 bii;
5 linii digitale de I/O;
port interfa IIC (maxim 1Mbps);
4 ieiri pentru servomotoare;
port pentru senzor IR GP2D02;
port serial RS232;
ZeeRO
pagina 69
conectori alimentare board + conectori alimentare separat
servomotoare.
Caracteristicile microcontrolerului Acroname Moto 1.0:
2 canale pentru control PWM;
1 canal analogic A/D pe 10 bii;
1 canal digital I/O;
port interfa IIC (maxim 1Mbps);
port serial RS232;
conector H-Bridge.
Cele dou microcontrolere au fost conectate ntre ele prin interfaa IIC.
Legtura ntre Gumstix i Brainstem GP 1.0 s-a realizat prin interfaa RS232.
Pentru interfaarea senzorilor Sharp GP2D02, s-au folosit interfee digitale ale
microcontrolerului Brainstem GP 1.0. Modul de conectare al acestora este prezentat
n figura 54.

Figura 54. Interfaarea senzorilor IR la microcontrolerul Brainstem GP 1.0
Cele 4 fire ale senzorului IR GP2D02 (alimentare, mas, linie de intrare i linie
de ieire) sunt conectate la Brainstem GP, folosind 2 intrri digitale configurate ca o
intrare plus o ieire.
Interfaarea senzorilor ultrasonici SRF08 i SRF10 s-a realizat ntr-un mod
mult mai uor, folosind interfaa IIC a microcontrolerului Brainstem (vezi figura 55).
Pe interfaa IIC se pot conecta virtual pn la 126 de dispozitive, viteza maxim de
transfer fiind de 1Mbps. Unele dispozitive ns nu pot comunica la viteza de 1Mbps,
motiv pentru care Brainstem ofer posibilitatea utilizrii a dou viteze de comunicare
mai mici, de 400Kbps respectiv 100Kbps.
Pentru ca modulele sonar s poat fi accesate simultan, a fost nevoie de
modificarea adreselor IIC a modulelor sonar. Astfel, s-au ales adresele E0h i E2h
pentru senzorii SRF10 respectiv E4h i E6h pentru senzorii SRF08.
ZeeRO
pagina 70

Figura 55. Interfaarea senzorilor SONAR la microcontrolerul Brainstem GP 1.0
(imagine preluat cu amabilitatea Acroname)
Microcontrolerul Brainstem Moto 1.0 a fost folosit doar pentru interfaarea
senzorului piroelectric Eltec 442-3 (vezi figura 56), ns utilizarea pe viitor a unui
modul H-Bridge mpreun cu dou motoare de curent continuu va muta centrul
sistemului de locomoie de pe Brainstem GP pe Brainstem Moto.
Pentru interfaarea senzorului piroelectric s-a folosit intrarea analogic a
Brainstem Moto.

Figura 56. Interfaarea senzorului Eltec 442-3 la microcontrolerul Brainstem Moto
De notat c alimentarea componentelor unitii centrale de procesare a fost
realizat separat, folosind un set de 4 acumulatori AA GP (1.2V) pentru
ZeeRO
pagina 71
microcalculatorul Gumstix, respectiv un set de 5 acumulatori AA GP (1.2V) pentru
microcontrolerele Brainstem respectiv servomotoarele robotului ZeeRO.
Pentru mai multe informaii referitoare la caracteristicile tehnice ale
microcontrolerelor Acroname Brainstem a se consulta [20].
5 5. .5 5. . P Pr ro og gr ra am ma ar re ea a r ro ob bo ot tu ul lu ui i Z Ze ee eR RO O
Cu scopul de a construi un robot ct mai modular din punct de vedere
arhitectural, robotul ZeeRO a fost conceput n aa fel nct s poat fi programat pe
mai multe nivele. n continuare se prezint cele trei moduri de funcionare (vezi
figura 57), din punct de vedere software, ale robotului ZeeRO.

Figura 57. Modurile de programare ale robotului mobil ZeeRO
Modul TEA
n cel mai simplu mod de programare posibil, dar i la nivelul cel mai jos,
robotul poate fi programat folosind direct instruciunile microcontrolerelor
Brainstem. Conform [20], microcontrolerele Brainstem pot fi programate ntr-un
limbaj numit TEA (en. Tiny Embedded Application). Acest limbaj este defapt un subset
al limbajului C, i este utilizabil datorit unei maini virtuale existente pentru o serie
de platforme.
Programele scrise n limbajul TEA se compileaz n fiiere obiect i ruleaz
nativ pe Brainstem. n corpul acestora, se pot folosi i instruciuni specializate scrise
n limbaj de asamblare, oferind deci vitez maxim pentru aplicaiile utilizatorului.
Datorit faptului c programele scrise n limbajul TEA ruleaz doar pe
Brainstem, microcalculatorul Gumstix Waysmall nu este utilizat. De aici rezult o
serie de dezavantaje, precum dispariia modalitii de comunicare a robotului fr fir.
Firma Acroname pune la dispoziia utilizatorilor o serie de librrii TEA pe
care utilizatorul le poate integra n aplicaiile sale. Limbajul TEA este ns destul de
rudimentar n sine, motiv pentru care este foarte greu (dac nu imposibil) pentru
utilizator s creeze aplicaii complexe pentru robotul mobil.
Modul C-Acroname
Cteva din dezavantajele enumerate mai sus pot fi nlturate prin folosirea
librriilor C programate de firma Acroname. Aceste librrii se pot utiliza att sub
sistemele de operare Unix, ct i sub Windows sau MacOS. n acest mod,
ZeeRO
pagina 72
microcontrolerele Brainstem vor fi interfaate de minicalculatorul Gumstix.
Programele utilizator se vor compila sub sistemul de operare Linux folosind un
compilator precum GCC, i se vor linkedita mpreun cu librriile Acroname pentru
a putea accesa interfeele i canalele analogice/digitale ale microcontrolerelor
Brainstem.
Programnd direct pe Gumstix, complexitatea codului utilizator poate crete
semnificativ, datorit resurselor mult mai consistente ale platformei Waysmall. n
acest mod de programare se poate folosi i interfaa Bluetooth a platformei. Astfel,
robotul deja autonom poate comunica cu ali roboi din mediul de lucru, sau cu un
operator uman.
Modul Player
Problema cea mai mare pe care modurile de mai sus nu o pot rezolva este
dependena de limbajul C i librriile de cod Acroname. Pentru o gam larg de
aplicaii aceast dependen nu este benefic, innd cont c unii utilizatori au deja
librrii proprii de cod scrise n limbajele lor prefereniale.
nlturarea dependenei de limbajul C, precum i obinerea unei modulariti
i flexibiliti avansate, sunt cteva din atuurile folosirii arhitecturii Player. Prin
scrierea unor drivere pentru Player n limbajul C, i integrarea acestora cu restul
modulelor deja existente, utilizatorul beneficiaz de posibilitatea programrii i
utilizrii robotului ZeeRO n toate limbajele de programare suportate de Player.
Principalul motiv pentru folosirea Player i a Javaclient, este de a integra
robotul mobil ZeeRO n sistemul multiagent Robotux [19]. Dezvoltarea de aplicaii
pentru sisteme de roboi heterogeni autonomi devine astfel o real posibilitate
pentru membrii grupului de cercetare.
Arhitectura software Player a robotului ZeeRO a fost mprit n:
Driverul robot ZeeRO folosit pentru interfaarea senzorilor ultrasonici, IR,
piroelectric precum i a bazei robot;
Driverul camerei video CMUcam2 folosit pentru interfaarea camerei
cmucam2.

Figura 58. Arhitectura software Player a robotului ZeeRO
Codul surs al celor dou drivere Player este prezentat n anexa C respectiv
D. Interfaa aio este folosit pentru a comunica cu senzorul piroelectric.
Concluzii
pagina 73
6
6
.
.
C
C
o
o
n
n
c
c
l
l
u
u
z
z
i
i
i
i

Concluzii
pagina 74
n cele 4 luni de dezvoltare continu a proiectului Javaclient, s-a constatat
existena unei baze largi de utilizatori, din diferite coluri ale lumii, fapt ce a dus la
recunoaterea internaional a grupului nostru de cercetare din cadrul Universitii
Tehnice din Cluj-Napoca.
Javaclient va continua aceeai dezvoltare n plin ascensiune, urmnd a
contribui n cele din urm la mbuntirea platformei Player/Stage, i la integrarea
altor proiecte deja existente pe Internet, n lumea roboilor mobili.
n ceea ce privete robotul mobil ZeeRO, trebuie fcute cteva precizri.
Fiind prima ncercare de amploare de a construi un robot mobil, pe parcursul
realizrii lui au aprut o serie de probleme. Dintre acestea, sistemul de locomoie
reprezint probabil problema numrul unu, problem care trebuie neaprat rezolvat
n cazul n care se dorete continuarea cercetrilor. Este imperativ, n opinia
autorului acestei lucrri, ca servomotoarele existente s fie nlocuite cu motoare
performante, de curent continuu, prevzute cu encodere, comandate probabil printr-
un modul H-Bridge de ctre microcontrolerul Brainstem Moto.
Ca i dezvoltare ulterioar a sistemului senzorial, s-ar putea renuna uor la
senzorii IR, deoarece acetia au demonstrat un grad mare de ineficien practic.
nlocuirea lor cu senzori ultrasonici auxiliari ar putea reprezenta o alternativ. Un alt
aspect care s-ar putea mbunti este componenta senzorial piroelectric. Apariia
unor sisteme piroelectrice noi, digitale, interfaabile prin IIC, vor duce n mod sigur la
nlocuirea detectoarelor Eltec. Un exemplu n aceast privin este senzorul TPA81
Termophile Array. Celelalte componente ale robotului mobil ZeeRO s-au
comportant excelent, cu precdere unitile centrale de procesare i camera
CMUcam2.
Realizarea fizic a robotului mobil ZeeRO reprezint un prim pas ctre
sisteme robotice modulare, eficiente i moderne.

Bibliografie
pagina 75
B
B
i
i
b
b
l
l
i
i
o
o
g
g
r
r
a
a
f
f
i
i
e
e

[1]. Ronald C. Arkin, (1998), Behavior-Based Robotics, MIT Press 1998
[2]. Richard T. Vaughan, Brian P. Gerkey, Andrew Howard. On device abstractions
for portable, reusable robot code. In Proc. of the IEEE/RSJ Intl. Conf. on Intelligent
Robots and Systems (IROS), pages 21212427, Las Vegas, Nevada, October 2003
[3]. Brian P. Gerkey, Richard T. Vaughan, Andrew Howard. The Player/Stage
Project: Tools for Multi-Robot and Distributed Sensor Systems. In Proc. of the Intl. Conf.
on Advanced Robotics (ICAR), pages 317323, Coimbra, Portugal, July 2003
[4]. Brian P. Gerkey, Richard T. Vaughan, Kasper Sty, Andrew Howard, Maja J
Mataric, Gaurav S Sukhatme. Most Valuable Player: A Robot Device Server for
Distributed Control. In Proc. of the IEEE/RSJ Intl. Conf. on Intelligent Robots and
Systems (IROS), pages 12261231, Wailea, Hawaii, October 2001
[5]. Ulrich Nehmzow, Mobile Robotics: A practical introduction, second edition,
Springer-Verlag London, 2003
[6]. Kikuo Fujimura, Motion Planning in Dynamic Environments, Springer-Verlag
Tokyo, 1991
[7]. Gregory Dudek, Michael Jenkin, Computational Principles of Mobile Robotics,
Cambridge University Press, 2000
[8]. Peter van Turennout, Autonomous Motion on Wheels, Universiteitsdrukkerij
T.U. Delft, 1994
[9]. Joseph Jones, Robot Programming: A practical guide to Behavior-Based Robotics,
McGraw-Hill, 2004
[10]. Thomas Hsiu, Steve Richards, Ajinkya Bhave, Andres Perez-Bergquist, Illah
Nourbakhsh, Designing a Low-cost, Expressive Educational Robot, The Robotics Institute,
Carnegie Mellon University
[11]. Gareth Branwyan, Absolute Beginners Guide to Building Robots, Que, 2003
[12]. Pete Miles, Tom Carroll, Build your own combat robot, McGraw-Hill, 2002
[13]. Christfried Webers, Uwe R. Zimmer, Robust tracking under uncertainties,
Oceans 2000, Providence, USA
[14]. Michele Aicardi, Giuseppe Casalino, Antonio Bicchi, Aldo Balestrino, Closed
Loop Steering of Unicycle-like Vehicles via Lyapunov Techniques, IEEE Robotics &
Automation magazine, March 1995
[15]. Radu Bogdan Rusu, On data fusion methods using Neural Networks, from a
practical implementation POV*, unpublished paper, May 2005
[16]. Rodney A. Brooks, A robust layered control system for a mobile robot, A.I. Memo
864, Artificial Intelligence, Massachusetts Institute of Technology, September 1985
Bibliografie
pagina 76
[17]. Nate Koenig, Stage and Gazebo The Instant Experts Guide, USC Robotics
Research Lab, 15 Sept 2004
[18]. Boyoon Jung, Player Tutorial, USC Robotics Research Lab
[19]. Radu Bogdan Rusu, Sistem multiagent pentru roboi mobili: Robotux, proiect de
diplom, Universitatea Tehnic din Cluj-Napoca, Iunie 2004
[20]. Remus Sime, Sistem senzorial pentru robotul mobil ZeeRO, proiect de diplom,
Universitatea Tehnic din Cluj-Napoca, Iunie 2005

Anexe
pagina I
A
A
n
n
e
e
x
x
e
e

A A. . D Di ia ag gr ra am ma a U UM ML L a a c cl la as se el lo or r p pr ro oi ie ec ct tu ul lu ui i
J Ja av va ac cl li ie en nt t
Anexe
pagina II
B B. . C Co od du ul l s su ur rs s a al l p pr ro oi ie ec ct tu ul lu ui i J Ja av va ac cl li ie en nt t
1 1. .6 6. .4 4
j ja av va ac cl li ie en nt t/ /A Ab bs st tr ra ac ct tP Po os si it ti io on nD De ev vi ic ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Abst r act Posi t i onDevi ce. j ava
* Copyr i ght ( C) 2005 Radu Bogdan Rusu
*
*
* Thi s pr ogr ami s f r ee sof t war e; you can r edi st r i but e i t and/ or modi f y
* i t under t he t er ms of t he GNU Gener al Publ i c Li cense as publ i shed by
* t he Fr ee Sof t war e Foundat i on; ei t her ver si on 2 of t he Li cense, or
* ( at your opt i on) any l at er ver si on.
*
* Thi s pr ogr ami s di st r i but ed i n t he hope t hat i t wi l l be usef ul ,
* but WI THOUT ANY WARRANTY; wi t hout even t he i mpl i ed war r ant y of
* MERCHANTABI LI TY or FI TNESS FOR A PARTI CULAR PURPOSE. See t he
* GNU Gener al Publ i c Li cense f or mor e det ai l s.
*
* You shoul d have r ecei ved a copy of t he GNU Gener al Publ i c Li cense
* al ong wi t h t hi s pr ogr am; i f not , wr i t e t o t he Fr ee Sof t war e
* Foundat i on, I nc. , 59 Templ e Pl ace, Sui t e 330, Bost on, MA 02111- 1307 USA
*
* $I d: Abst r act Posi t i onDevi ce. j ava, v 1. 1 2005/ 06/ 14 09: 44: 32 veedee Exp $
*
*/
package j av acl i ent ;

/ **
* Abst r act cl ass f or al l Pl ayer Posi t i on* i nt er f aces. Used f or
* Posi t i onCont r ol and Headi ngCont r ol .
* @aut hor Radu Bogdan Rusu
*/
publ i c abs t r act cl as s Abs t r act Pos i t i onDev i ce ex t ends Pl ay er Dev i ce {

/ **
* Abst r act const r uct or f or each Abst r act Posi t i onDevi ce.
* @par ampl c a r ef er ence t o t he Pl ayer Cl i ent obj ect
*/
publ i c Abs t r act Pos i t i onDev i ce ( Pl ay er Cl i ent pl c) {
s uper ( pl c) ;
}

publ i c abs t r act i nt get X ( ) ;
publ i c abs t r act i nt get Y ( ) ;
publ i c abs t r act i nt get Yaw ( ) ;
publ i c abs t r act v oi d s et Speed ( i nt s peed, i nt t ur nr at e) ;
}

j ja av va ac cl li ie en nt t/ /A AI IO OI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - AI OI nt er f ace. j ava
* Copyr i ght ( C) 2003- 2005 Maxi mA. Bat al i n & Radu Bogdan Rusu
*
*
*
*
*
* $I d: AI OI nt er f ace. j ava, v 1. 2 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/
Anexe
pagina III

/ **
* The ai o i nt er f ace pr ovi des access t o an anal og I / O devi ce.
* @aut hor Maxi mA. Bat al i n & Radu Bogdan Rusu
* @ver si on
* <ul >
* <l i >v1. 6. 3 - Pl ayer 1. 6. 3 ( al l i nt er f aces) suppor t ed
* <l i >v1. 6. 2 - Pl ayer 1. 6. 2 suppor t ed, J avadoc document at i on, sever al bugf i xes
* <l i >v1. 5a - Pl ayer 1. 5 suppor t ed ( most popul ar devi ces)
* </ ul >
*/
publ i c cl as s AI OI nt er f ace ex t ends Pl ay er Dev i ce {

pr i v at e f i nal s hor t PLAYER_AI O_CODE = Pl ay er Cl i ent . PLAYER_AI O_CODE; / * anal og I / O */

/ ** maxi mumnumber of anal og I / O sampl es */
publ i c s t at i c f i nal s hor t PLAYER_AI O_MAX_SAMPLES = 8;

pr i v at e i nt count = 0; / * number of val i d sampl es */
pr i v at e i nt [ ] ani n = new i nt [ PLAYER_AI O_MAX_SAMPLES] ; / * t he sampl es */
/ **
* Const r uct or f or AI OI nt er f ace.
* @par ampc a r ef er ence t o t he Pl ayer Cl i ent obj ect
* @par ami ndexOf Devi ce t he i ndex of t he devi ce
*/
publ i c AI OI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_AI O_CODE;
i ndex = i ndex Of Dev i ce;
}

/ **
* Read t he sampl es val ues.
*/
publ i c s y nchr oni z ed v oi d r eadDat a ( ) {
r eadHeader ( ) ;
t r y {
count = i s . r eadUns i gnedBy t e ( ) ; / * number of val i d sampl es */
f or ( i nt i = 0; i < count ; i ++)
ani n[ i ] = i s . r eadI nt ( ) ; / * t he sampl es */
} cat ch ( Ex cept i on e) {
Sy s t em. er r . pr i nt l n ( "[ AI O] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Ret ur ns t he number of val i d sampl es
* @r et ur n t he number of val i d sampl es as a byt e
*/
publ i c s y nchr oni z ed by t e get Count ( ) { r et ur n ( by t e) count ; }

/ **
* Ret ur ns t he sampl es val ues up t o PLAYER_AI O_MAX_SAMPLES.
* @r et ur n an ar r ay f i l l ed wi t h t he sampl es val ues
*/
publ i c s y nchr oni z ed i nt [ ] get Ani n ( ) { r et ur n ani n; }
}

j ja av va ac cl li ie en nt t/ /A Au ud di io oD DS SP PI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Audi oDSPI nt er f ace. j ava
*
*
*
*
*
* $I d: Audi oDSPI nt er f ace. j ava, v 1. 2 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/
Anexe
pagina IV

/ **
* The audi odsp i nt er f ace i s used t o cont r ol sound har dwar e, i f equi pped.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Audi oDSPI nt er f ace ex t ends Pl ay er Dev i ce {

pr i v at e s t at i c f i nal bool ean i s Debuggi ng = Pl ay er Cl i ent . i s Debuggi ng;

/ * audi o dsp I / O */
pr i v at e f i nal s hor t PLAYER_AUDI ODSP_CODE = Pl ay er Cl i ent . PLAYER_AUDI ODSP_CODE;

/ * t he pl ayer message t ypes ( see pl ayer . h) */
pr i v at e s t at i c f i nal s hor t PLAYER_MSGTYPE_CMD = Pl ay er Cl i ent . PLAYER_MSGTYPE_CMD;
pr i v at e s t at i c f i nal s hor t PLAYER_MSGTYPE_REQ = Pl ay er Cl i ent . PLAYER_MSGTYPE_REQ;

/ * st r i ng t o mat ch t he cur r ent l y assi gned devi ces */
pr i v at e s t at i c f i nal s hor t PLAYER_MAX_DEVI CE_STRI NG_LEN =
Pl ay er Cl i ent . PLAYER_MAX_DEVI CE_STRI NG_LEN;

/ * conf i gur at i on subt ypes */
publ i c s t at i c f i nal s hor t PLAYER_AUDI ODSP_SET_CONFI G = 1;
publ i c s t at i c f i nal s hor t PLAYER_AUDI ODSP_GET_CONFI G = 2;
publ i c s t at i c f i nal s hor t PLAYER_AUDI ODSP_PLAY_TONE = 3;
publ i c s t at i c f i nal s hor t PLAYER_AUDI ODSP_PLAY_CHI RP = 4;
publ i c s t at i c f i nal s hor t PLAYER_AUDI ODSP_REPLAY = 5;

pr i v at e i nt f r equency [ ] = new i nt [ 5] ;
pr i v at e i nt ampl i t ude[ ] = new i nt [ 5] ;

pr i v at e i nt s ampl eFor mat ; / * f or mat wi t h whi ch t o sampl e */
pr i v at e i nt s ampl eRat e; / * sampl e r at e i n Her t z */
pr i v at e by t e channel s ; / * number of channel s t o use. 1=mono, 2=st er eo */

/ **
* Const r uct or f or Audi oDSPI nt er f ace.
*/
publ i c Audi oDSPI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_AUDI ODSP_CODE;
}

/ **
* The audi odsp i nt er f ace r eads t he audi o st r eamf r om/ dev/ dsp ( whi ch i s assumed
* t o be associ at ed wi t h a sound car d connect ed t o a mi cr ophone) and per f or ms some
* anal ysi s on i t . Fi ve f r equency/ ampl i t ude pai r s ar e t hen r et ur ned as dat a.
*/
r eadHeader ( ) ;
t r y {
f or ( i nt i = 0; i < 5; i ++)
f r equency [ i ] = i s . r eadUns i gnedShor t ( ) ; / * Hz */
f or ( i nt i = 0; i < 5; i ++)
ampl i t ude[ i ] = i s . r eadUns i gnedShor t ( ) ; / * dB */
Sy s t em. er r . pr i nt l n ( "[ Audi oDSP] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Ret ur ns t he f i ve hi ghest f r equenci es.
* @r et ur n an ar r ay f i l l ed wi t h t he hi ghest f i ve f r equenci es
*/
publ i c s y nchr oni z ed i nt [ ] get Fr equenci es ( ) { r et ur n f r equency ; }

/ **
* Ret ur ns t he f i ve hi ghest ampl i t udes.
* @r et ur n an ar r ay f i l l ed wi t h t he hi ghest f i ve ampl i t udes
*/
publ i c s y nchr oni z ed i nt [ ] get Ampl i t udes ( ) { r et ur n ampl i t ude; }

/ **
* The audi odsp i nt er f ace accept s commands t o pr oduce f i xed- f r equency t ones
* or bi nar y phase shi f t keyed( BPSK) chi r ps t hr ough / dev/ dsp ( whi ch i s assumed
* t o be associ at ed wi t h a sound car d t o whi ch a speaker i s at t ached) .
* @par amsubt ype The packet subt ype. Set t o PLAYER_AUDI ODSP_PLAY_TONE t o pl ay
Anexe
pagina V
* a si ngl e f r equency; bi t St r i ng and bi t St r i ngLen do not need t o be set . Set t o
* PLAYER_AUDI ODSP_PLAY_CHI RP t o pl ay a BPSKeyed chi r p; bi t St r i ng shoul d cont ai n
* t he bi nar y st r i ng t o encode, and bi t St r i ngLen set t o t he l engt h of t he
* bi t St r i ng. Set t o PLAYER_AUDI ODSP_REPLAY t o r epl ay t he l ast sound.
* @par amf r eq Fr equency t o pl ay ( Hz)
* @par amamp Ampl i t ude t o pl ay ( dB?)
* @par amdur at i on Dur at i on t o pl ay ( msec)
* @par ambi t St r i ng Bi t St r i ng t o encode i n si ne wave
* @par ambi t St r i ngLen Lengt h of t he bi t st r i ng
*/
publ i c v oi d command ( by t e s ubt y pe, i nt f r eq, i nt amp,
i nt dur at i on, char [ ] bi t St r i ng, i nt bi t St r i ngLen) {
t r y {
i nt s i z e = 20 + PLAYER_MAX_DEVI CE_STRI NG_LEN;
s endHeader ( PLAYER_MSGTYPE_CMD, s i z e) ; / * payl oad */
os . wr i t eI nt ( s ubt y pe) ; / * t he packet subt ype */
os . wr i t eI nt ( f r eq) ; / * f r equency t o pl ay */
os . wr i t eI nt ( amp) ; / * ampl i t ude t o pl ay */
os . wr i t eI nt ( dur at i on) ; / * dur at i on t o pl ay */
f or ( i nt i = 0; i < PLAYER_MAX_DEVI CE_STRI NG_LEN; i ++)
os . wr i t eChar ( bi t St r i ng[ i ] ) ; / * si ne wave t o pl ay */
os . wr i t eI nt ( bi t St r i ngLen) ; / * l engt h of t he bi t st r i ng */
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Audi oDSP] : Coul dn' t s end new command: " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Get audi o pr oper t i es.
* 
* The audi odsp conf i gur at i on can be quer i ed usi ng t he PLAYER_AUDI ODSP_GET_CONFI G
* r equest and modi f i ed usi ng t he PLAYER_AUDI ODSP_SET_CONFI G r equest . 
* The sampl e f or mat i s def i ned i n sys/ soundcar d. h, and def i nes t he byt e si ze and
* endi an f or mat f or each sampl e. 
* The sampl e r at e def i nes t he Her t z at whi ch t o sampl e. 
* Mono or st er eo sampl i ng i s def i ned i n t he channel s par amet er wher e 1==mono and
* 2==st er eo. 
* See t he pl ayer _audi odsp_conf i g st r uct ur e f r ompl ayer . h
*/
publ i c v oi d get Audi oPr oper t i es ( ) {
t r y {
s endHeader ( PLAYER_MSGTYPE_REQ, 1) ; / * 1 byt e payl oad */
os . wr i t eBy t e ( PLAYER_AUDI ODSP_GET_CONFI G) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Audi oDSP] : Coul dn' t s end PLAYER_AUDI ODSP_GET_CONFI G " +
"command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Set audi o pr oper t i es.
* 
* The audi odsp conf i gur at i on can be quer i ed usi ng t he PLAYER_AUDI ODSP_GET_CONFI G
* r equest and modi f i ed usi ng t he PLAYER_AUDI ODSP_SET_CONFI G r equest . 
* The sampl e f or mat i s def i ned i n sys/ soundcar d. h, and def i nes t he byt e si ze and
* endi an f or mat f or each sampl e. 
* The sampl e r at e def i nes t he Her t z at whi ch t o sampl e. 
* Mono or st er eo sampl i ng i s def i ned i n t he channel s par amet er wher e 1==mono and
* 2==st er eo. 
* See t he pl ayer _audi odsp_conf i g st r uct ur e f r ompl ayer . h
* @par amsampl eFor mat For mat wi t h whi ch t o sampl e
* @par amsampl eRat e Sampl e r at e i n Her t z
* @par amchannel s Number of channel s t o use. 1=mono, 2=st er eo
*/
publ i c v oi d s et Audi oPr oper t i es ( i nt s ampl eFor mat , i nt s ampl eRat e, by t e channel s ) {
t r y {
os . wr i t eBy t e ( PLAYER_AUDI ODSP_SET_CONFI G) ;
os . wr i t eI nt ( s ampl eFor mat ) ; / * f or mat wi t h whi ch t o sampl e */
os . wr i t eShor t ( s ampl eRat e) ; / * Sampl e r at e i n Her t z */
os . wr i t eBy t e ( channel s ) ; / * nr of channel s t o use, 1=mono, 2=st er eo */
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Audi oDSP] : Coul dn' t s end PLAYER_AUDI ODSP_SET_CONFI G " +
}
}

/ **
* Handl e acknowl edgement r esponse messages ( t hr eaded mode) .
* @par amsi ze si ze of t he payl oad
*/
Anexe
pagina VI

publ i c v oi d handl eRes pons e ( i nt s i z e) {
i f ( s i z e == 0) {
i f ( i s Debuggi ng)
Sy s t em. er r . pr i nt l n ( "[ Audi oDSP] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
/ * each r epl y begi ns wi t h a ui nt 8_t subt ype f i el d */
by t e s ubt y pe = i s . r eadBy t e ( ) ;
s wi t ch ( s ubt y pe) {
cas e PLAYER_AUDI ODSP_GET_CONFI G: {
s ampl eFor mat = i s . r eadI nt ( ) ; / * f or mat wi t h whi ch t o sampl e */
s ampl eRat e = i s . r eadUns i gnedShor t ( ) ; / * sampl e r at e i n Her t z */
channel s = ( by t e) i s . r eadUns i gnedBy t e ( ) ; / * number of channel s t o use */
br eak;
}
cas e PLAYER_AUDI ODSP_SET_CONFI G: {
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ Audi oDSP] : Unex pect ed r es pons e " + s ubt y pe +
" of s i z e = " + s i z e) ;
br eak;
}
}
Sy s t em. er r . pr i nt l n ( "[ Audi oDSP] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Ret ur ns t he f or mat wi t h whi ch t o sampl e.
* @r et ur n t he f or mat wi t h whi ch t o sampl e as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Sampl eFor mat ( ) { r et ur n s ampl eFor mat ; }

/ **
* Ret ur ns t he sampl e r at e i n Her t z.
* @r et ur n t he sampl e r at e i n Her t z as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Sampl eRat e ( ) { r et ur n s ampl eRat e; }

/ **
* Ret ur ns t he number of channel s t o use ( 1=mono, 2=st er eo) .
* @r et ur n t he number of channel s t o use as a byt e
*/
publ i c s y nchr oni z ed by t e get Channel s ( ) { r et ur n channel s ; }
}

j ja av va ac cl li ie en nt t/ /A Au ud di io oI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Audi oI nt er f ace. j ava
* Copyr i ght ( C) 2002- 2005 Maxi mA. Bat al i n, Esben H. Ost er gaar d & Radu Bogdan Rusu
*
*
*
*
*
* $I d: Audi oI nt er f ace. j ava, v 1. 2 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/

/ **
* The audi o i nt er f ace i s used t o cont r ol sound har dwar e, i f equi pped.
* @aut hor Maxi mA. Bat al i n, Esben H. Ost er gaar d & Radu Bogdan Rusu
* @ver si on
* <ul >
Anexe
pagina VII
* </ ul >
*/
publ i c cl as s Audi oI nt er f ace ex t ends Pl ay er Dev i ce {

pr i v at e f i nal s hor t PLAYER_AUDI O_CODE = Pl ay er Cl i ent . PLAYER_AUDI O_CODE; / * audi o I / O */

pr i v at e f i nal s hor t PLAYER_MSGTYPE_CMD = Pl ay er Cl i ent . PLAYER_MSGTYPE_CMD;

pr i v at e i nt f r equency [ ] = new i nt [ 5] ;
pr i v at e i nt ampl i t ude[ ] = new i nt [ 5] ;
/ **
* Const r uct or f or Audi oI nt er f ace.
*/
publ i c Audi oI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_AUDI O_CODE;

f or ( i nt i = 0; i < 5; i ++) {
f r equency [ i ] = 0;
ampl i t ude[ i ] = 0;
}
}

/ **
* The audi o i nt er f ace r eads t he audi o st r eamf r om/ dev/ audi o ( whi ch i s assumed t o be
* associ at ed wi t h a sound car d connect ed t o a mi cr ophone) and per f or ms some anal ysi s on
* i t . Fi ve f r equency/ ampl i t ude pai r s ar e t hen r et ur ned as dat a.
*/
r eadHeader ( ) ;
t r y {
f or ( i nt i = 0; i < 5; i ++) {
f r equency [ i ] = i s . r eadUns i gnedShor t ( ) ;
ampl i t ude[ i ] = i s . r eadUns i gnedShor t ( ) ;
}
Sy s t em. er r . pr i nt l n ( "[ Audi o] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Ret ur ns t he f i ve hi ghest f r equenci es.
* @r et ur n an ar r ay f i l l ed wi t h t he hi ghest f i ve f r equenci es
*/
publ i c s y nchr oni z ed i nt [ ] get Fi v eHi ghes t Fr equenci es ( ) { r et ur n f r equency ; }

/ **
* Ret ur ns t he f i ve hi ghest ampl i t udes.
* @r et ur n an ar r ay f i l l ed wi t h t he hi ghest f i ve ampl i t udes
*/
publ i c s y nchr oni z ed i nt [ ] get Fi v eHi ghes t Ampl i t udes ( ) { r et ur n ampl i t ude; }

/ **
* The audi o i nt er f ace accept s commands t o pr oduce f i xed- f r equency t ones t hr ough
* / dev/ dsp ( whi ch i s assumed t o be associ at ed wi t h a sound car d t o whi ch a speaker i s
* at t ached) .
* @par amf r eq f r equency t o pl ay ( Hz?)
* @par amamp ampl i t ude t o pl ay ( dB?)
* @par amdur at i on dur at i on t o pl ay ( sec?)
*/
publ i c v oi d pr oduceSound ( s hor t f r eq, s hor t amp, s hor t dur at i on) {
t r y {
s endHeader ( PLAYER_MSGTYPE_CMD, 6) ; / * 6 byt es payl oad */
os . wr i t eShor t ( f r eq) ; / * f r eq f r equency t o pl ay ( Hz?) */
os . wr i t eShor t ( amp) ; / * amp ampl i t ude t o pl ay ( dB?) */
os . wr i t eShor t ( dur at i on) ; / * dur at i on dur at i on t o pl ay ( sec?) */
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Audi o] : Coul dn' t s end pr oduce s ound command r eques t : " +
e. t oSt r i ng ( ) ) ;
}
}
}

j ja av va ac cl li ie en nt t/ /A Au ud di io oM Mi ix xe er rI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Audi oMi xer I nt er f ace. j ava
Anexe
pagina VIII
*
*
*
*
*
* $I d: Audi oMi xer I nt er f ace. j ava, v 1. 2 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/

/ **
* The audi omi xer i nt er f ace i s used t o cont r ol sound l evel s.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Audi oMi x er I nt er f ace ex t ends Pl ay er Dev i ce {


/ * audi o I / O */
pr i v at e f i nal s hor t PLAYER_AUDI OMI XER_CODE = Pl ay er Cl i ent . PLAYER_AUDI OMI XER_CODE;


pr ot ect ed s t at i c f i nal s hor t PLAYER_AUDI OMI XER_SET_MASTER = 1;
pr ot ect ed s t at i c f i nal s hor t PLAYER_AUDI OMI XER_SET_PCM = 2;
pr ot ect ed s t at i c f i nal s hor t PLAYER_AUDI OMI XER_SET_LI NE = 3;
pr ot ect ed s t at i c f i nal s hor t PLAYER_AUDI OMI XER_SET_MI C = 4;
pr ot ect ed s t at i c f i nal s hor t PLAYER_AUDI OMI XER_SET_I GAI N = 5;
pr ot ect ed s t at i c f i nal s hor t PLAYER_AUDI OMI XER_SET_OGAI N = 6;

pr i v at e i nt mas t er Lef t , mas t er Ri ght , pcmLef t , pcmRi ght , l i neLef t , l i neRi ght ;
pr i v at e i nt mi cLef t , mi cRi ght , i Gai n, oGai n;

/ **
* Const r uct or f or Audi oMi xer I nt er f ace.
*/
publ i c Audi oMi x er I nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_AUDI OMI XER_CODE;
}

/ **
* The audi omi xer i nt er f ace accept s commands t o set t he l ef t and r i ght vol ume
* l evel s of var i ous channel s. The channel may be PLAYER_AUDI OMI XER_MASTER f or
* t he mast er vol ume, PLAYER_AUDI OMI XER_PCM f or t he PCM vol ume,
* PLAYER_AUDI OMI XER_LI NE f or t he l i ne i n vol ume, PLAYER_AUDI OMI XER_MI C f or t he
* mi cr ophone vol ume, PLAYER_AUDI OMI XER_I GAI N f or t he i nput gai n, and
* PLAYER_AUDI OMI XER_OGAI N f or t he out put gai n. 
* See t he pl ayer _audi omi xer _cmd st r uct ur e f r ompl ayer . h
* @par amsubt ype one of t he t ypes above
* @par aml ef t val ue f or t he l ef t channel
* @par amr i ght val ue f or t he r i ght channel
*/
publ i c v oi d s et Vol ume ( by t e s ubt y pe, s hor t l ef t , s hor t r i ght ) {
t r y {
s endHeader ( PLAYER_MSGTYPE_CMD, 5) ; / * 5 byt e payl oad */
os . wr i t eBy t e ( s ubt y pe) ; / * t he packet subt ype */
os . wr i t eShor t ( l ef t ) ; / * l ef t channel */
os . wr i t eShor t ( r i ght ) ; / * r i ght channel */
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Audi oMi x er ] : Coul dn' t s end new command: " +
Anexe
pagina IX
e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Get l evel s.
* 
* The audi omi xer i nt er f ace pr ovi des accept s a conf i gur at i on r equest whi ch
* r et ur ns t he cur r ent st at e of t he mi xer l evel s 
* See t he pl ayer _audi omi xer _conf i g st r uct ur e f r ompl ayer . h
*/
publ i c v oi d get Lev el s ( by t e s ubt y pe) {
t r y {
/ / Not yet i mpl ement ed i n pl ayer !
os . wr i t eBy t e ( s ubt y pe) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Audi oMi x er ] : Coul dn' t s end command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
*/
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ Audi oMi x er ] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
def aul t : {
mas t er Lef t = i s . r eadUns i gnedShor t ( ) ;
mas t er Ri ght = i s . r eadUns i gnedShor t ( ) ;
pcmLef t = i s . r eadUns i gnedShor t ( ) ;
pcmRi ght = i s . r eadUns i gnedShor t ( ) ;
l i neLef t = i s . r eadUns i gnedShor t ( ) ;
l i neRi ght = i s . r eadUns i gnedShor t ( ) ;
mi cLef t = i s . r eadUns i gnedShor t ( ) ;
mi cRi ght = i s . r eadUns i gnedShor t ( ) ;
i Gai n = i s . r eadUns i gnedShor t ( ) ;
oGai n = i s . r eadUns i gnedShor t ( ) ;
br eak;
}
/ * def aul t : {
Syst em. er r . pr i nt l n ( " [ Audi oMi xer ] : Unexpect ed r esponse " + subt ype +
" of si ze = " + si ze) ;
br eak;
}*/
}
Sy s t em. er r . pr i nt l n ( "[ Audi oMi x er ] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
}
}

publ i c s y nchr oni z ed i nt get Mas t er Lef t ( ) { r et ur n mas t er Lef t ; }
publ i c s y nchr oni z ed i nt get Mas t er Ri ght ( ) { r et ur n mas t er Ri ght ; }
publ i c s y nchr oni z ed i nt get PCMLef t ( ) { r et ur n pcmLef t ; }
publ i c s y nchr oni z ed i nt get PCMRi ght ( ) { r et ur n pcmLef t ; }
publ i c s y nchr oni z ed i nt get Li neLef t ( ) { r et ur n l i neLef t ; }
publ i c s y nchr oni z ed i nt get Li neRi ght ( ) { r et ur n l i neLef t ; }
publ i c s y nchr oni z ed i nt get Mi cLef t ( ) { r et ur n mi cLef t ; }
publ i c s y nchr oni z ed i nt get Mi cRi ght ( ) { r et ur n mi cLef t ; }
publ i c s y nchr oni z ed i nt get I Gai n ( ) { r et ur n i Gai n; }
publ i c s y nchr oni z ed i nt get OGai n ( ) { r et ur n oGai n; }
}

j ja av va ac cl li ie en nt t/ /B Bl li in nk ke en nl li ig gh ht tI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Bl i nkenl i ght I nt er f ace. j ava
*
*
Anexe
pagina X
*
*
*
* $I d: Bl i nkenl i ght I nt er f ace. j ava, v 1. 2 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/

/ **
* The bl i nkenl i ght i nt er f ace i s used t o swi t ch on and of f a f l ashi ng i ndi cat or
* l i ght , and t o set i t ' s f l ash per i od. 
* Thi s i nt er f ace accept s no conf i gur at i on r equest s.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Bl i nkenl i ght I nt er f ace ex t ends Pl ay er Dev i ce {

/ * bl i nki ng l i ght s */
pr i v at e f i nal s hor t PLAYER_BLI NKENLI GHT_CODE = Pl ay er Cl i ent . PLAYER_BLI NKENLI GHT_CODE;


pr i v at e by t e enabl e; / * zer o: di sabl ed, non- zer o: enabl ed */
pr i v at e i nt per i od_ms ; / * f l ash per i od ( one whol e on- of f cycl e) i n mi l l i seconds */

/ **
* Const r uct or f or Bl i nkenl i ght I nt er f ace.
*/
publ i c Bl i nkenl i ght I nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_BLI NKENLI GHT_CODE;
}

/ **
* The bl i nkenl i ght dat a pr ovi des t he cur r ent st at e of t he i ndi cat or l i ght .
*/
r eadHeader ( ) ;
t r y {
enabl e = ( by t e) i s . r eadUns i gnedBy t e ( ) ; / * enabl e/ di sabl e */
per i od_ms = i s . r eadUns i gnedShor t ( ) ; / * f l ash per i od i n ms */
Sy s t em. er r . pr i nt l n ( "[ Bl i nkenl i ght ] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Ret ur ns t he cur r ent bl i nki ng l i ght s st at e ( zer o: di sabl ed, non- zer o: enabl ed) .
* @r et ur n t he cur r ent bl i nki ng l i ght s st at e as a byt e
*/
publ i c s y nchr oni z ed by t e get Enabl e ( ) { r et ur n t hi s . enabl e; }

/ **
* Ret ur ns t he f l ash per i od ( one whol e on- of f cycl e) i n mi l l i seconds.
* @r et ur n t he f l ash per i od as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Per i od ( ) { r et ur n t hi s . per i od_ms ; }

/ **
* Set t he bl i nkenl i ght st at e and per i od.
* @par amena zer o: di sabl ed, non- zer o: enabl ed
* @par amper i od f l ash per i od ( one whol e on- of f cycl e) i n mi l l i seconds
*/
publ i c v oi d s et ( by t e ena, i nt per i od) {
t r y {
os . wr i t eBy t e ( ena) ; / * enabl e/ di sabl e */
os . wr i t eShor t ( per i od) ; / * f l ash per i od i n ms */
os . f l us h ( ) ;
Anexe
pagina XI
Sy s t em. er r . pr i nt l n ( "[ Bl i nkenl i ght ] : Coul dn' t s end s et command r eques t : " +
e. t oSt r i ng ( ) ) ;
}
}

}

j ja av va ac cl li ie en nt t/ /B Bl lo ob bf fi in nd de er rI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Bl obf i nder I nt er f ace. j ava
*
*
*
*
*
* $I d: Bl obf i nder I nt er f ace. j ava, v 1. 5 2005/ 06/ 14 09: 41: 05 veedee Exp $
*
*/

i mpor t j av acl i ent . s t r uct ur es . Bl ob;

/ **
* The bl obf i nder i nt er f ace pr ovi des access t o devi ces t hat det ect bl obs i n i mages.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Bl obf i nder I nt er f ace ex t ends Pl ay er Dev i ce {


/ * vi sual bl obf i nder */
pr i v at e f i nal s hor t PLAYER_BLOBFI NDER_CODE = Pl ay er Cl i ent . PLAYER_BLOBFI NDER_CODE;

/ ** t he maxi mumnumber of bl obs i n t ot al */
publ i c f i nal s hor t PLAYER_BLOBFI NDER_MAX_BLOBS = 256;


/ * conf i g r equest codes */
pr i v at e f i nal s hor t PLAYER_BLOBFI NDER_SET_COLOR_REQ = 1;
pr i v at e f i nal s hor t PLAYER_BLOBFI NDER_SET_I MAGER_PARAMS_REQ = 2;

/ * t he i mage di mensi ons */
pr i v at e s hor t i mageWi dt h;
pr i v at e s hor t i mageHei ght ;

pr i v at e s hor t bl obCount = 0; / * number of bl obs */
pr i v at e Bl ob[ ] bl obLi s t = new Bl ob[ PLAYER_BLOBFI NDER_MAX_BLOBS] ; / * l i st of bl obs */

/ **
* Const r uct or f or Bl obf i nder I nt er f ace.
*/
publ i c Bl obf i nder I nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_BLOBFI NDER_CODE;
}

/ **
* Read t he l i st of det ect ed bl obs.
*/
Anexe
pagina XII

r eadHeader ( ) ;
t r y {
i mageWi dt h = i s . r eadShor t ( ) ;
i mageHei ght = i s . r eadShor t ( ) ;
bl obCount = i s . r eadShor t ( ) ;
f or ( i nt i = 0; i < bl obCount ; i ++) {
bl obLi s t [ i ] = new Bl ob ( ) ;
bl obLi s t [ i ] . s et I D ( i s . r eadShor t ( ) ) ; / * bl ob I D */
bl obLi s t [ i ] . s et Col or ( i s . r eadI nt ( ) ) ; / * 32- bi t RGB col or bl ob */
bl obLi s t [ i ] . s et Ar ea ( i s . r eadI nt ( ) ) ; / * t he bl ob ar ea ( pi xel s) */
bl obLi s t [ i ] . s et X ( i s . r eadShor t ( ) ) ; / * t he bl ob cent r oi d */
bl obLi s t [ i ] . s et Y ( i s . r eadShor t ( ) ) ;
bl obLi s t [ i ] . s et Lef t ( i s . r eadShor t ( ) ) ; / * boundi ng box f or t he bl ob */
bl obLi s t [ i ] . s et Ri ght ( i s . r eadShor t ( ) ) ;
bl obLi s t [ i ] . s et Top ( i s . r eadShor t ( ) ) ;
bl obLi s t [ i ] . s et Bot t om ( i s . r eadShor t ( ) ) ;
bl obLi s t [ i ] . s et Range ( i s . r eadShor t ( ) ) ; / * r ange t o t he bl ob cent er */
}
Sy s t em. er r . pr i nt l n ( "[ Bl obf i nder ] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Set t r acki ng col or .
* 
* For some sensor s ( i e CMUcam) , si mpl e bl ob t r acki ng t r acks onl y one col or . To set t he
* t r acki ng col or , send a r equest wi t h t he f or mat bel ow, i ncl udi ng t he RGB col or r anges
* ( max and mi n) . Val ues of - 1 wi l l cause t he t r ack col or t o be aut omat i cal l y set t o t he
* cur r ent wi ndow col or . Thi s i s usef ul f or set t i ng t he t r ack col or by hol di ng t he t r acki ng
* obj ect i n f r ont of t he l ens.
* @par amr mi n Red mi ni mumval ue ( 0- 255)
* @par amr max Red maxi mumval ue ( 0- 255)
* @par amgmi n Gr een mi ni mumval ue ( 0- 255)
* @par amgmax Gr een maxi mumval ue ( 0- 255)
* @par ambmi n Bl ue mi ni mumval ue ( 0- 255)
* @par ambmax Bl ue maxi mumval ue ( 0- 255)
*/
publ i c v oi d s et Tr acki ngCol or ( i nt r mi n, i nt r max ,
i nt gmi n, i nt gmax ,
i nt bmi n, i nt bmax ) {
t r y {
s endHeader ( PLAYER_MSGTYPE_REQ, 13) ; / * 13 byt es payl oad */
os . wr i t eBy t e ( PLAYER_BLOBFI NDER_SET_COLOR_REQ) ;
os . wr i t eShor t ( ( s hor t ) r mi n) ; / * RGB mi ni mumand max val ues ( 0- 255) */
os . wr i t eShor t ( ( s hor t ) r max ) ;
os . wr i t eShor t ( ( s hor t ) gmi n) ;
os . wr i t eShor t ( ( s hor t ) gmax ) ;
os . wr i t eShor t ( ( s hor t ) bmi n) ;
os . wr i t eShor t ( ( s hor t ) bmax ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Bl obf i nder ] : Coul dn' t s end PLAYER_BLOBFI NDER_SET_COLOR_REQ " +
}
}

/ **
* Conf i gur at i on r equest : Set i mager par ams.
* 
* I magi ng sensor s t hat do bl ob t r acki ng gener al l y have some sor t s of i mage qual i t y
* par amet er s t hat you can t weak. The f ol l owi ng ones ar e i mpl ement ed her e:
* <ul >
* <l i >br i ght ness ( 0- 255)
* <l i >cont r ast ( 0- 255)
* <l i >aut o gai n ( 0=of f , 1=on)
* <l i >col or mode ( 0=RGB/ Aut oWhi t eBal ance Of f , 1=RGB/ Aut oWhi t eBal ance On,
* 2=YCr CB/ AWB Of f , 3=YCr Cb/ AWB On) To set t he par ams, send a r equest wi t h t he
* f or mat bel ow. Any val ues set t o - 1 wi l l be l ef t unchanged.
* </ ul >
* @par ambr i ght ness br i ght ness val ue ( 0- 255)
* @par amcont r ast cont r ast val ue ( 0- 255)
* @par amcol or mode col or mode ( 0=RGB/ AWB of f , 1=RGB/ AWB on, 2=YCr CB/ AWB of f , 3=YCr Cb/ AWB on)
* @par amaut ogai n aut o gai n ( 0=of f , 1=on)
*/
publ i c v oi d s et I mager Par ams ( i nt br i ght nes s , i nt cont r as t ,
i nt col or mode, i nt aut ogai n) {
t r y {
os . wr i t eBy t e ( PLAYER_BLOBFI NDER_SET_I MAGER_PARAMS_REQ) ;
os . wr i t eShor t ( ( s hor t ) br i ght nes s ) ; / * cont r ast : ( 0- 255) - 1=no change */
os . wr i t eShor t ( ( s hor t ) cont r as t ) ; / * br i ght ness: ( 0- 255) - 1=no change */
os . wr i t eBy t e ( ( by t e) col or mode) ; / * col or mode */
Anexe
pagina XIII
os . wr i t eBy t e ( ( by t e) aut ogai n) ; / * aut oGai n: 0=of f , 1=on. - 1=no change */
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Bl obf i nder ] : Coul dn' t " +
" s end PLAYER_BLOBFI NDER_SET_I MAGER_PARAMS_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Get i mage wi dt h i n pi xel s.
* @r et ur n i mage wi dt h i n pi xel s as a shor t
*/
publ i c s y nchr oni z ed s hor t get I mageWi dt h ( ) { r et ur n i mageWi dt h; }

/ **
* Get i mage hei ght i n pi xel s.
* @r et ur n i mage hei ght i n pi xel s as a shor t
*/
publ i c s y nchr oni z ed s hor t get I mageHei ght ( ) { r et ur n i mageHei ght ; }

/ **
* Get t he number of val i d bl obs.
* @r et ur n number of val i d bl obs as a shor t
*/
publ i c s y nchr oni z ed s hor t get Bl obCount ( ) { r et ur n bl obCount ; }

/ **
* Get al l t he bl obs.
* @r et ur n an ar r ay of Bl ob obj ect s f i l l ed wi t h dat a
*/
publ i c s y nchr oni z ed Bl ob[ ] get Bl obs ( ) { r et ur n bl obLi s t ; }

/ **
* Get a speci f i ed bl ob.
* @par ami t he number of bl ob f r omt he bl ob ar r ay
* @r et ur n t he speci f i ed Bl ob obj ect
*/
publ i c s y nchr oni z ed Bl ob get Bl ob ( i nt i ) { r et ur n bl obLi s t [ i ] ; }

/ **
*/
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ Bl obf i nder ] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
cas e PLAYER_BLOBFI NDER_SET_COLOR_REQ: {
br eak;
}
cas e PLAYER_BLOBFI NDER_SET_I MAGER_PARAMS_REQ: {
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ Bl obf i nder ] : Unex pect ed r es pons e " + s ubt y pe +
" of s i z e = " + s i z e) ;
br eak;
}
}
Sy s t em. er r . pr i nt l n ( "[ Bl obf i nder ] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
}
}

}

j ja av va ac cl li ie en nt t/ /B Bu um mp pe er rI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Bumper I nt er f ace. j ava
*
*
Anexe
pagina XIV
*
*
*
* $I d: Bumper I nt er f ace. j ava, v 1. 3 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/

i mpor t j av acl i ent . s t r uct ur es . Pl ay er Bumper GeomT;
i mpor t j av acl i ent . s t r uct ur es . Pl ay er Bumper Def i neT;
/ **
* The bumper i nt er f ace r et ur ns dat a f r oma bumper ar r ay.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Bumper I nt er f ace ex t ends Pl ay er Dev i ce {


pr i v at e f i nal s hor t PLAYER_BUMPER_CODE = Pl ay er Cl i ent . PLAYER_BUMPER_CODE; / * bumper ar r ay */

pr i v at e f i nal s hor t PLAYER_MSGTYPE_REQ = Pl ay er Cl i ent . PLAYER_MSGTYPE_REQ;

/ ** maxi mumnumber of bumper sampl es */
publ i c s t at i c f i nal i nt PLAYER_BUMPER_MAX_SAMPLES = 32;

/ * r equest t ypes */
pr ot ect ed f i nal s hor t PLAYER_BUMPER_GET_GEOM_REQ = 1;

/ * t he number of val i d bumper r eadi ngs */
pr i v at e by t e bumper Count = PLAYER_BUMPER_MAX_SAMPLES;
/ * ar r ay of bumper val ues */
pr i v at e by t e[ ] bumper s = new by t e[ PLAYER_BUMPER_MAX_SAMPLES] ;

/ * obj ect cont ai ni ng pl ayer _bumper _geomi n case of a t hr eaded cal l */
pr i v at e Pl ay er Bumper GeomT pbgt ;
pr i v at e bool ean r eady PBGT = f al s e;

/ **
* Const r uct or f or Bumper I nt er f ace.
*/
publ i c Bumper I nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_BUMPER_CODE;
}

/ **
* Read t he bumper val ues.
*/
t r y {
r eadHeader ( ) ;
bumper Count = i s . r eadBy t e ( ) ; / * t he number of val i d bumper r eadi ngs */
f or ( i nt i = 0; i < bumper Count ; i ++) {
bumper s [ i ] = i s . r eadBy t e ( ) ; / * ar r ay of bumper val ues */
}
Sy s t em. er r . pr i nt l n ( "[ Bumper ] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Quer y geomet r y.
* 
* See t he pl ayer _bumper _geomst r uct ur e f r ompl ayer . h
*/
publ i c v oi d quer y Geomet r y ( ) {
t r y {
Anexe
pagina XV
os . wr i t eBy t e ( PLAYER_BUMPER_GET_GEOM_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Bumper ] : Coul dn' t s end PLAYER_BUMPER_GET_GEOM_REQ command: " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
* Ret ur ns t he bumper s ar r ay dat a val ues up t o PLAYER_BUMPER_MAX_SAMPLES.
* @r et ur n an ar r ay f i l l ed wi t h t he bumper val ues
*/
publ i c s y nchr oni z ed by t e[ ] get Bumper s ( ) { r et ur n bumper s ; }
/ **
* Ret ur ns t he number of bumper s speci f i ed i n t he Pl ayer wor l d f i l e
* ( t he number of val i d bumper r eadi ngs) .
* @r et ur n t he number of bumper s speci f i ed i n t he Pl ayer wor l d f i l e as a byt e
*/
publ i c s y nchr oni z ed by t e get Bumper Count ( ) { r et ur n bumper Count ; }

/ **
* Get t he geomet r y dat a.
* @r et ur n an obj ect of t ype Pl ayer Bumper GeomT cont ai ni ng t he r equi r ed geomet r y dat a
*/
publ i c Pl ay er Bumper GeomT get Pl ay er Bumper Geom ( ) { r et ur n pbgt ; }

/ **
* Check i f geomet r y dat a i s avai l abl e.
* @r et ur n t r ue i f r eady, f al se i f not r eady
*/
publ i c bool ean i s GeomReady ( ) {
i f ( r eady PBGT) {
r eady PBGT = f al s e;
r et ur n t r ue;
}
r et ur n f al s e;
}

/ **
*/
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ Bumper ] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
s hor t s ubt y pe = i s . r eadBy t e ( ) ;
cas e PLAYER_BUMPER_GET_GEOM_REQ: {
pbgt = new Pl ay er Bumper GeomT ( ) ;
r eady PBGT = t r ue;
pbgt . s et Bumper Count ( i s . r eadShor t ( ) ) ;

Pl ay er Bumper Def i neT[ ] pbgt Dat a=new
Pl ay er Bumper Def i neT[ PLAYER_BUMPER_MAX_SAMPLES] ;
f or ( i nt i = 0; i < PLAYER_BUMPER_MAX_SAMPLES; i ++) {
pbgt Dat a[ i ] . s et XOf f s et ( i s . r eadShor t ( ) ) ;
pbgt Dat a[ i ] . s et YOf f s et ( i s . r eadShor t ( ) ) ;
pbgt Dat a[ i ] . s et ThOf f s et ( i s . r eadShor t ( ) ) ;
pbgt Dat a[ i ] . s et Lengt h ( i s . r eadShor t ( ) ) ;
pbgt Dat a[ i ] . s et Radi us ( i s . r eadShor t ( ) ) ;
}
pbgt . s et Dat a ( pbgt Dat a) ;
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ Bumper ] : Unex pect ed r es pons e " + s ubt y pe +
" of s i z e = " + s i z e) ;
br eak;
}
}
Sy s t em. er r . pr i nt l n ( "[ Bumper ] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
}
}
}

Anexe
pagina XVI

j ja av va ac cl li ie en nt t/ /C Ca am me er ra aI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Camer aI nt er f ace. j ava
*
*
*
*
*
* $I d: Camer aI nt er f ace. j ava, v 1. 3 2005/ 06/ 01 19: 05: 47 veedee Exp $
*
*/

/ **
* The camer a i nt er f ace i s used t o see what t he camer a sees. I t i s i nt ended
* pr i mar i l y f or ser ver - si de ( i . e. , dr i ver - t o- dr i ver ) dat a t r ansf er s, r at her
* t han ser ver - t o- cl i ent t r ansf er s. I mage dat a can be i n may f or mat s ( see bel ow) ,
* but i s al ways packed ( i . e. , pi xel r ows ar e byt e- al i gned) . 
* Thi s i nt er f ace has no commands or conf i gur at i on r equest s.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Camer aI nt er f ace ex t ends Pl ay er Dev i ce {


/ * camer a devi ce ( gazebo) */
pr i v at e f i nal s hor t PLAYER_CAMERA_CODE = Pl ay er Cl i ent . PLAYER_CAMERA_CODE;

/ ** I mage di mensi ons */
publ i c s t at i c f i nal s hor t PLAYER_CAMERA_I MAGE_WI DTH = 640;
publ i c s t at i c f i nal s hor t PLAYER_CAMERA_I MAGE_HEI GHT = 480;
publ i c s t at i c f i nal i nt PLAYER_CAMERA_I MAGE_SI ZE = ( 640 * 480 * 4) ;

/ ** I mage f or mat : 8- bi t monochr ome */
publ i c s t at i c f i nal s hor t PLAYER_CAMERA_FORMAT_MONO8 = 1;
/ ** I mage f or mat : 16- bi t monochr ome ( net wor k byt e or der ) */
publ i c s t at i c f i nal s hor t PLAYER_CAMERA_FORMAT_MONO16 = 2;
/ ** I mage f or mat : 16- bi t col or ( 5 bi t s R, 6 bi t s G, 5 bi t s B) */
publ i c s t at i c f i nal s hor t PLAYER_CAMERA_FORMAT_RGB565 = 4;
/ ** I mage f or mat : 24- bi t col or ( 8 bi t s R, 8 bi t s G, 8 bi t s B) */
publ i c s t at i c f i nal s hor t PLAYER_CAMERA_FORMAT_RGB888 = 5;

publ i c s t at i c f i nal s hor t PLAYER_CAMERA_COMPRESS_RAW = 0;
publ i c s t at i c f i nal s hor t PLAYER_CAMERA_COMPRESS_JPEG = 1;

pr i v at e s hor t wi dt h, hei ght ; / * I mage di mensi ons ( pi xel s) */
pr i v at e by t e bpp; / * I mage bi t s- per - pi xel ( 8, 16, 24, 32) */
pr i v at e by t e f or mat ; / * I mage f or mat ( must be compat i bl e wi t h dept h) */
pr i v at e s hor t f di v ; / * Some i mages ( such as di spar i t y maps) use scal ed
pi xel val ues; f or t hese i mages, f di v speci f i es
t he scal e di vi sor ( i . e. , di vi de t he i nt eger pi xel
val ue by f di v t o r ecover t he r eal pi xel val ue) . */
pr i v at e by t e compr es s i on; / * I mage compr essi on; PLAYER_CAMERA_COMPRESS_RAW
i ndi cat es no compr essi on. */
pr i v at e i nt i mage_s i z e; / * Si ze of i mage dat a as st or ed i n i mage buf f er ( byt es) */
/ * Compr essed i mage dat a ( byt e- al i gned, r ow maj or or der ) . Mul t i - byt e i mage f or mat s
*( such as MONO16) must be conver t ed t o net wor k byt e or der i ng.
*/
pr i v at e by t e[ ] i mage = new by t e[ PLAYER_CAMERA_I MAGE_SI ZE] ;

/ **
* Const r uct or f or Camer aI nt er f ace.
Anexe
pagina XVII
*/
publ i c Camer aI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_CAMERA_CODE;
}

/ **
* Read t he camer a dat a. 
* See t he pl ayer _camer a_dat a st r uct ur e f r ompl ayer . h
*/
r eadHeader ( ) ;
t r y {
wi dt h = ( s hor t ) i s . r eadUns i gnedShor t ( ) ; / * i mage di mensi ons ( pi xel s) */
hei ght = ( s hor t ) i s . r eadUns i gnedShor t ( ) ;
bpp = ( by t e) i s . r eadUns i gnedBy t e ( ) ; / * bi t s- per - pi xel */
f or mat = ( by t e) i s . r eadUns i gnedBy t e ( ) ; / * i mage f or mat */
f di v = ( s hor t ) i s . r eadUns i gnedShor t ( ) ; / * scal e di vi sor */
compr es s i on = ( by t e) i s . r eadUns i gnedBy t e ( ) ; / * i mage compr essi on st at us */
i mage_s i z e = i s . r eadI nt ( ) ; / * si ze of i mage dat a */
f or ( i nt i = 0; i < PLAYER_CAMERA_I MAGE_SI ZE; i ++)
i mage[ i ] = ( by t e) i s . r eadUns i gnedBy t e ( ) ; / * compr essed i mage dat a */
Sy s t em. er r . pr i nt l n ( "[ Camer a] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Ret ur ns t he i mage wi dt h di mensi on.
* @r et ur n t he i mage wi dt h di mensi on as a shor t
*/
publ i c s y nchr oni z ed s hor t get Wi dt h ( ) { r et ur n wi dt h; }

/ **
* Ret ur ns t he i mage hei ght di mensi on.
* @r et ur n t he i mage hei ght di mensi on as a shor t
*/
publ i c s y nchr oni z ed s hor t get Hei ght ( ) { r et ur n hei ght ; }

/ **
* Ret ur ns t he i mage bi t s- per - pi xel ( 8, 16, 24, 32) val ue.
* @r et ur n t he i mage bpp as a byt e
*/
publ i c s y nchr oni z ed by t e get BPP ( ) { r et ur n bpp; }

/ **
* Ret ur ns t he i mage f or mat ( must be compat i bl e wi t h dept h) .
* @r et ur n t he i mage f or mat as a byt e
*/
publ i c s y nchr oni z ed by t e get For mat ( ) { r et ur n f or mat ; }

/ **
* Some i mages ( such as di spar i t y maps) use scal ed pi xel val ues; f or t hese
* i mages, f di v speci f i es t he scal e di vi sor ( i . e. , di vi de t he i nt eger pi xel
* val ue by f di v t o r ecover t he r eal pi xel val ue) .
* @r et ur n t he scal e di vi sor as a shor t
*/
publ i c s y nchr oni z ed s hor t get FDi v ( ) { r et ur n f di v ; }

/ **
* Ret ur ns t he i mage compr essi on st at us ( PLAYER_CAMERA_COMPRESS_RAWi ndi cat es
* no compr essi on. ) .
* @r et ur n t he i mage compr essi on st at us as a byt e
*/
publ i c s y nchr oni z ed by t e get Compr es s i on ( ) { r et ur n compr es s i on; }

/ **
* Ret ur ns t he si ze of i mage dat a as st or ed i n i mage buf f er ( byt es) .
* @r et ur n t he i mage si ze as an i nt eger
*/
publ i c s y nchr oni z ed i nt get I mageSi z e ( ) { r et ur n i mage_s i z e; }

/ **
* Ret ur ns t he compr essed i mage dat a ( byt e- al i gned, r ow maj or or der ) .
* Mul t i - byt e i mage f or mat s ( such as MONO16) must be conver t ed t o
* net wor k byt e or der i ng.
* @r et ur n t he i mage dat a as an ar r ay of byt es
*/
publ i c s y nchr oni z ed by t e[ ] get I mage ( ) { r et ur n i mage; }

/ **
*/
Anexe
pagina XVIII
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ Camer a] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ Camer a] : Unex pect ed r es pons e " + s ubt y pe +
" of s i z e = " + s i z e) ;
br eak;
}
}
Sy s t em. er r . pr i nt l n ( "[ Camer a] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
}
}
}

j ja av va ac cl li ie en nt t/ /D DI IO OI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - DI OI nt er f ace. j ava
*
*
*
*
*
* $I d: DI OI nt er f ace. j ava, v 1. 2 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/

/ **
* The di o i nt er f ace pr ovi des access t o a di gi t al I / O devi ce.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s DI OI nt er f ace ex t ends Pl ay er Dev i ce {

pr i v at e f i nal s hor t PLAYER_DI O_CODE = Pl ay er Cl i ent . PLAYER_DI O_CODE; / * di gi t al I / O */


pr i v at e i nt count = 0; / * number of val i d sampl es */
pr i v at e i nt bi t Fi el d = 0; / * bi t f i el d of sampl es */
/ **
* Const r uct or f or DI OI nt er f ace.
*/
publ i c DI OI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_DI O_CODE;
}

/ **
* Read t he sampl es val ues.
*/
r eadHeader ( ) ;
Anexe
pagina XIX
t r y {
count = i s . r eadUns i gnedBy t e ( ) ; / * number of val i d sampl es */
bi t Fi el d = i s . r eadI nt ( ) ; / * bi t f i el d of sampl es */
Sy s t em. er r . pr i nt l n ( "[ DI O] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Ret ur ns t he number of val i d sampl es
* @r et ur n t he number of val i d sampl es as a byt e
*/
publ i c s y nchr oni z ed by t e get Count ( ) { r et ur n ( by t e) count ; }

/ **
* Ret ur ns t he bi t f i el d of sampl es.
* @r et ur n t he bi t f i el d of sampl es as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Bi t Fi el d ( ) { r et ur n bi t Fi el d; }

/ **
* The di o i nt er f ace accept s 4- byt e commands whi ch consi st of t he ouput bi t f i el d.
* @par amcount t he command
* @par amdi gout t he out put bi t f i el d
*/
publ i c v oi d s et Out put Bi t f i el d ( by t e count , i nt di gout ) {
t r y {
os . wr i t eBy t e ( count ) ;
os . wr i t eI nt ( di gout ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ DI O] : Coul dn' t s end out put bi t f i el d command r eques t : " +
e. t oSt r i ng ( ) ) ;
}
}

}

j ja av va ac cl li ie en nt t/ /E En ne er rg gy yI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Ener gyI nt er f ace. j ava
*
*
*
*
*
* $I d: Ener gyI nt er f ace. j ava, v 1. 2 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/

/ **
* The ener gy i nt er f ace pr ovi des dat a about ener gy st or age, consumpt i on and char gi ng.
* Thi s i nt er f ace accept s no commands.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Ener gy I nt er f ace ex t ends Pl ay er Dev i ce {

/ * ener gy char gi ng */
pr i v at e f i nal s hor t PLAYER_ENERGY_CODE = Pl ay er Cl i ent . PLAYER_ENERGY_CODE;


Anexe
pagina XX
pr i v at e i nt mj oul es ; / * ener gy st or ed, i n mi l l i J oul es */
pr i v at e i nt mwat t s ; / * est i mat ed cur r ent ener gy consumpt i on ( negat i ve val ues)
or aqui si t i on ( posi t i ve val ues) , i n mi l l i Wat t s
( mi l l i J oul es/ sec) . */
pr i v at e by t e char gi ng; / * char ge exchange st at us: i f 1, t he devi ce i s cur r ent l y
r ecei vi ng char ge f r omanot her ener gy devi ce. I f - 1 t he
devi ce i s cur r ent l y pr ovi di ng char ge t o anot her ener gy devi ce.
I f 0, t he devi ce i s not exchangi ng char ge wi t h an anot her
devi ce. */

/ **
* Const r uct or f or Ener gyI nt er f ace.
*/
publ i c Ener gy I nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_ENERGY_CODE;
}

/ **
* The ener gy i nt er f ace r epor t s t he amount of ener gy st or ed, cur r ent r at e of
* ener gy consumpt i on or aqui si t i on, and whet her or not t he devi ce i s connect ed
* t o a char ger .
*/
r eadHeader ( ) ;
t r y {
mj oul es = i s . r eadI nt ( ) ; / * ener gy st or ed */
mwat t s = i s . r eadI nt ( ) ; / * ener gy consumpt i on/ aqui si t i on */
char gi ng = i s . r eadBy t e ( ) ; / * char ge exchange st at us */
Sy s t em. er r . pr i nt l n ( "[ Ener gy ] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Ret ur ns t he ener gy st or ed, i n mi l l i J oul es.
* @r et ur n t he ener gy st or ed i n mi l l i J oul es as an i nt eger
*/
publ i c s y nchr oni z ed i nt get MJoul es ( ) { r et ur n t hi s . mj oul es ; }

/ **
* Ret ur ns t he est i mat ed cur r ent ener gy consumpt i on ( negat i ve val ues) or aqui si t i on
* ( posi t i ve val ues) , i n mi l l i Wat t s ( mi l l i J oul es/ sec) .
* @r et ur n t he est i mat ed cur r ent ener gy consumpt i on/ aqui si t i on as an i nt eger
*/
publ i c s y nchr oni z ed i nt get MWat t s ( ) { r et ur n t hi s . mwat t s ; }

/ **
* Ret ur ns t he char ge exchange st at us: i f 1, t he devi ce i s cur r ent l y r ecei vi ng
* char ge f r omanot her ener gy devi ce. I f - 1 t he devi ce i s cur r ent l y pr ovi di ng
* char ge t o anot her ener gy devi ce. I f 0, t he devi ce i s not exchangi ng char ge
* wi t h an anot her devi ce.
* @r et ur n t he char ge exchange st at us as a byt e
*/
publ i c s y nchr oni z ed by t e get Char gi ng ( ) { r et ur n t hi s . char gi ng; }

/ **
* Conf i gur at i on r equest : Cont r ol l r echar gi ng.
* 
* See t he pl ayer _ener gy_command st r uct ur e f r ompl ayer . h
* @par amenabl e_i nput bool ean cont r ol l i ng r echar gi ng. I f FALSE, r echar gi ng i s
* di sabl ed. Def aul t s t o TRUE
* @par amenabl e_out put bool ean cont r ol l i ng whet her ot her s can r echar ge f r omt hi s
* devi ce. I f FALSE, char gi ng ot her s i s di sabl ed. Def aul t s t o TRUE.
*/
publ i c v oi d get MapDat a ( by t e enabl e_i nput , by t e enabl e_out put ) {
t r y {
os . wr i t eBy t e ( enabl e_i nput ) ;
os . wr i t eBy t e ( enabl e_out put ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Ener gy ] : Coul dn' t s end command: " + e. t oSt r i ng ( ) ) ;
}
}
}

Anexe
pagina XXI

j ja av va ac cl li ie en nt t/ /F Fi id du uc ci ia al lI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Fi duci al I nt er f ace. j ava
*
*
*
*
*
* $I d: Fi duci al I nt er f ace. j ava, v 1. 4 2005/ 05/ 25 21: 55: 48 veedee Exp $
*
*/

i mpor t j av acl i ent . s t r uct ur es . Pl ay er Fi duci al I t em;

/ **
* The f i duci al i nt er f ace pr ovi des access t o devi ces t hat det ect coded f i duci al s
* ( mar ker s) pl aced i n t he envi r onment . I t can al so be used f or devi ces t he
* det ect nat ur al l andmar ks.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Fi duci al I nt er f ace ex t ends Pl ay er Dev i ce {


/ * f i duci al det ect or */
pr i v at e f i nal s hor t PLAYER_FI DUCI AL_CODE = Pl ay er Cl i ent . PLAYER_FI DUCI AL_CODE;


/ ** The maxi mumnumber of f i duci al s t hat can be det ect ed at one t i me */
publ i c s t at i c f i nal s hor t PLAYER_FI DUCI AL_MAX_SAMPLES = 32;
/ ** The maxi mumsi ze of a dat a packet exchanged wi t h a f i duci al at one t i me */
publ i c s t at i c f i nal s hor t PLAYER_FI DUCI AL_MAX_MSG_LEN = 32;

/ * r equest packet subt ypes */
pr ot ect ed f i nal s hor t PLAYER_FI DUCI AL_GET_GEOM = 1;
pr ot ect ed f i nal s hor t PLAYER_FI DUCI AL_GET_FOV = 2;
pr ot ect ed f i nal s hor t PLAYER_FI DUCI AL_SET_FOV = 3;
pr ot ect ed f i nal s hor t PLAYER_FI DUCI AL_SEND_MSG = 4;
pr ot ect ed f i nal s hor t PLAYER_FI DUCI AL_RECV_MSG = 5;
pr ot ect ed f i nal s hor t PLAYER_FI DUCI AL_EXCHANGE_MSG = 6;
pr ot ect ed f i nal s hor t PLAYER_FI DUCI AL_GET_I D = 7;
pr ot ect ed f i nal s hor t PLAYER_FI DUCI AL_SET_I D = 8;

/ * t he number of det ect ed f i duci al s */
pr i v at e i nt count ;
/ * l i st of det ect ed f i duci al s */
pr i v at e Pl ay er Fi duci al I t em[ ] f i duci al s = new Pl ay er Fi duci al I t em[ PLAYER_FI DUCI AL_MAX_SAMPLES] ;

/ * pose of t he det ect or i n t he r obot cs ( x, y, or i ent ) i n uni t s i f ( mm, mm, degr ees) */
pr i v at e i nt [ ] det ect Pos e = new i nt [ 3] ;
/ * si ze of t he det ect or i n uni t s of ( mm, mm) */
pr i v at e i nt [ ] det ect Si z e = new i nt [ 2] ;
/ * di mensi ons of t he f i duci al s i n uni t s of ( mm, mm) */
pr i v at e i nt [ ] f i duci al _s i z e = new i nt [ 2] ;

pr i v at e bool ean geomet r y Ready , f ov Ready , i dReady ;

/ * t he mi ni mumand maxi mumr anges of t he sensor i n mm*/
pr i v at e i nt mi nRange, max Range;
/ * t he r ecept i ve angl e of t he sensor i n degr ees */
Anexe
pagina XXII
pr i v at e i nt v i ewAngl e;

pr i v at e i nt i d; / * t he val ue di spl ayed */

/ **
* Const r uct or f or Fi duci al I nt er f ace.
*/
publ i c Fi duci al I nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_FI DUCI AL_CODE;
}

/ **
* Read t he f i duci al dat a packet ( al l f i duci al s) .
*/
r eadHeader ( ) ;
t r y {
count = i s . r eadUns i gnedShor t ( ) ; / * t he number of det ect ed f i duci al s */
f or ( i nt i = 0; i < count ; i ++) {
/ * t he f i duci al i d */
f i duci al s [ i ] . s et I D ( i s . r eadShor t ( ) ) ;

i nt [ ] pos = new i nt [ 3] ;
f or ( i nt j = 0; j < 3; j ++)
pos [ i ] = i s . r eadI nt ( ) ; / * f i duci al posi t i on */
f i duci al s [ i ] . s et Pos ( pos ) ;

i nt [ ] r ot = new i nt [ 3] ;
f or ( i nt j = 0; j < 3; j ++)
r ot [ i ] = i s . r eadI nt ( ) ; / * f i duci al or i ent at i on */
f i duci al s [ i ] . s et Pos ( r ot ) ;

i nt [ ] upos = new i nt [ 3] ;
f or ( i nt j = 0; j < 3; j ++)
upos [ i ] = i s . r eadI nt ( ) ; / * uncer t ai nt y i n t he measur ed pose */
f i duci al s [ i ] . s et Pos ( upos ) ;

i nt [ ] ur ot = new i nt [ 3] ;
f or ( i nt j = 0; j < 3; j ++)
ur ot [ i ] = i s . r eadI nt ( ) ; / * uncer t ai nt y i n f i duci al or i ent at i on */
f i duci al s [ i ] . s et Pos ( ur ot ) ;
}
Sy s t em. er r . pr i nt l n ( "[ Fi duci al ] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Get geomet r y.
* 
* See t he pl ayer _f i duci al _geomst r uct ur e f r ompl ayer . h
*/
publ i c v oi d get Geomet r y ( ) {
t r y {
os . wr i t eBy t e ( PLAYER_FI DUCI AL_GET_GEOM) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Fi duci al ] : Coul dn' t s end PLAYER_FI DUCI AL_GET_GEOM " +
}
}

/ **
*/
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ Fi duci al ] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
cas e PLAYER_FI DUCI AL_GET_GEOM: {
/ * pose of t he det ect or i n t he r obot cs ( x, y, or i ent )
* i n uni t s i f ( mm, mm, degr ees) */
f or ( i nt i = 0; i < 3; i ++)
Anexe
pagina XXIII
det ect Pos e[ i ] = i s . r eadShor t ( ) ;

/ * si ze of t he det ect or i n uni t s of ( mm, mm) */
f or ( i nt i = 0; i < 2; i ++)
det ect Si z e[ i ] = i s . r eadUns i gnedShor t ( ) ;

/ * di mensi ons of t he f i duci al s i n uni t s of ( mm, mm) */
f or ( i nt i = 0; i < 2; i ++)
f i duci al _s i z e[ i ] = i s . r eadUns i gnedShor t ( ) ;

geomet r y Ready = t r ue;
br eak;
}
cas e PLAYER_FI DUCI AL_GET_FOV: {
/ * t he mi ni mumr ange of t he sensor i n mm*/
mi nRange = i s . r eadUns i gnedShor t ( ) ;

/ * t he maxi mumr ange of t he sensor i n mm*/
max Range = i s . r eadUns i gnedShor t ( ) ;

v i ewAngl e = i s . r eadUns i gnedShor t ( ) ;

f ov Ready = t r ue;
br eak;
}
cas e PLAYER_FI DUCI AL_SET_FOV: {
br eak;
}
cas e PLAYER_FI DUCI AL_GET_I D: {
i d = i s . r eadI nt ( ) ; / * t he val ue di spl ayed */

i dReady = t r ue;
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ Fi duci al ] : Unex pect ed r es pons e " + s ubt y pe +
" of s i z e = " + s i z e) ;
br eak;
}
}
Sy s t em. er r . pr i nt l n ( "[ Fi duci al ] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
}
}

/ **
*/
i f ( geomet r y Ready ) {
geomet r y Ready = f al s e;
r et ur n t r ue;
}
r et ur n f al s e;
}

/ **
* Get t he pose of t he det ect or i n t he r obot cs ( x, y, or i ent )
* i n uni t s i f ( mm, mm, degr ees) .
* @r et ur n an ar r ay of i nt eger s f i l l ed wi t h t he pose of t he det ect or
*/
publ i c s y nchr oni z ed i nt [ ] get Det ect Pos e ( ) { r et ur n det ect Pos e; }

/ **
* Get t he si ze of t he det ect or i n uni t s of ( mm, mm) .
* @r et ur n t he si ze of t he det ect or i n uni t s of ( mm, mm) as an i nt eger ar r ay.
*/
publ i c s y nchr oni z ed i nt [ ] get Det ect Si z e ( ) { r et ur n det ect Si z e; }

/ **
* Get t he di mensi ons of t he f i duci al s i n uni t s of ( mm, mm) .
* @r et ur n t he di mensi ons of t he f i duci al s i n uni t s of ( mm, mm) as an i nt eger
* ar r ay.
*/
publ i c s y nchr oni z ed i nt [ ] get Fi duci al Si z e ( ) { r et ur n f i duci al _s i z e; }

/ **
* Conf i gur at i on r equest : Get sensor f i el d of vi ew.
* 
* The f i el d of vi ew of t he f i duci al devi ce can be set usi ng t he
* PLAYER_FI DUCI AL_SET_FOV r equest , and quer i ed usi ng t he
* PLAYER_FI DUCI AL_GET_FOV r equest . The devi ce r epl i es t o a SET r equest
* wi t h t he act ual FOV achi eved. I n bot h cases t he r equest and r epl y
Anexe
pagina XXIV
* packet s have t he same f or mat .
* 
* See t he pl ayer _f i duci al _f ov st r uct ur e f r ompl ayer . h
*/
publ i c v oi d get FOV ( ) {
t r y {
os . wr i t eBy t e ( PLAYER_FI DUCI AL_GET_FOV) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Fi duci al ] : Coul dn' t s end PLAYER_FI DUCI AL_GET_FOV " +
}
}

/ **
* Check i f FOV dat a i s avai l abl e.
*/
publ i c bool ean i s FOVReady ( ) {
i f ( f ov Ready ) {
f ov Ready = f al s e;
r et ur n t r ue;
}
r et ur n f al s e;
}

/ **
* Get t he mi ni mumr ange of t he sensor i n mm.
* @r et ur n t he mi ni mumr ange of t he sensor i n mmas an i nt eger
*/
publ i c s y nchr oni z ed i nt get Mi nRange ( ) { r et ur n mi nRange; }

/ **
* Get t he maxi mumr ange of t he sensor i n mm.
* @r et ur n t he maxi mumr ange of t he sensor i n mmas an i nt eger
*/
publ i c s y nchr oni z ed i nt get Max Range ( ) { r et ur n max Range; }

/ **
* Get t he r ecept i ve angl e of t he sensor i n degr ees.
* @r et ur n t he r ecept i ve angl e of t he sensor i n degr ees as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Vi ewAngl e ( ) { r et ur n v i ewAngl e; }

/ **
* Conf i gur at i on r equest : Set sensor f i el d of vi ew.
* 
* The f i el d of vi ew of t he f i duci al devi ce can be set usi ng t he
* PLAYER_FI DUCI AL_SET_FOV r equest , and quer i ed usi ng t he
* PLAYER_FI DUCI AL_GET_FOV r equest . The devi ce r epl i es t o a SET r equest
* wi t h t he act ual FOV achi eved. I n bot h cases t he r equest and r epl y
* packet s have t he same f or mat .
* 
* See t he pl ayer _f i duci al _f ov st r uct ur e f r ompl ayer . h
*/
publ i c v oi d s et FOV ( i nt newMi nRange, i nt newMax Range, i nt newVi ewAngl e) {
t r y {
os . wr i t eBy t e ( PLAYER_FI DUCI AL_SET_FOV) ;
/ * t he mi ni mumr ange of t he sensor i n mm*/
os . wr i t eShor t ( newMi nRange) ;
/ * t he maxi mumr ange of t he sensor i n mm*/
os . wr i t eShor t ( newMax Range) ;
os . wr i t eShor t ( newVi ewAngl e) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Fi duci al ] : Coul dn' t s end PLAYER_FI DUCI AL_SET_FOV " +
}
}

/ **
* Conf i gur at i on r equest : Get f i duci al val ue.
* 
* Some f i duci al f i nder devi ces di spl ay t hei r own f i duci al . They can use t he
* PLAYER_FI DUCI AL_GET_I D conf i g t o r epor t t he i dent i f i er di spl ayed by t he
* f i duci al . Make t he r equest usi ng t he pl ayer _f i duci al _i d_t st r uct ur e. The
* devi ce r epl i es wi t h t he same st r uct ur e wi t h t he i d f i el d set .
* 
* Some devi ces can dynami cal l y change t he i dent i f i er t hey di spl ay. They can
* use t he PLAYER_FI DUCI AL_SET_I D conf i g t o al l ow a cl i ent t o set t he cur r ent l y
* di spl ayed val ue. Make t he r equest wi t h t he pl ayer _f i duci al _i d_t st r uct ur e.
* The devi ce r epl i es wi t h t he same st r uct ur e wi t h t he i d f i el d set t o t he val ue
Anexe
pagina XXV
* i t act ual l y used. You shoul d check t hi s val ue, as t he devi ce may not be abl e
* t o di spl ay t he val ue you r equest ed.
* 
* Cur r ent l y suppor t ed by t he st g_f i duci al dr i ver .
* 
* See t he pl ayer _f i duci al _i d st r uct ur e f r ompl ayer . h
*/
publ i c v oi d get Fi duci al Val ( ) {
t r y {
os . wr i t eBy t e ( PLAYER_FI DUCI AL_GET_I D) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Fi duci al ] : Coul dn' t s end PLAYER_FI DUCI AL_GET_I D " +
}
}

/ **
* Check i f f i duci al val ue dat a i s avai l abl e.
*/
publ i c bool ean i s I DReady ( ) {
i f ( i dReady ) {
i dReady = f al s e;
r et ur n t r ue;
}
r et ur n f al s e;
}

/ **
* Get t he f i duci al dat a val ue di spl ayed.
* @r et ur n t he f i duci al dat a val ue as an i nt eger
*/
publ i c s y nchr oni z ed i nt get I D ( ) { r et ur n i d; }

/ **
* Conf i gur at i on r equest : Get f i duci al val ue.
* 
* Some f i duci al f i nder devi ces di spl ay t hei r own f i duci al . They can use t he
* PLAYER_FI DUCI AL_GET_I D conf i g t o r epor t t he i dent i f i er di spl ayed by t he
* f i duci al . Make t he r equest usi ng t he pl ayer _f i duci al _i d_t st r uct ur e. The
* devi ce r epl i es wi t h t he same st r uct ur e wi t h t he i d f i el d set .
* 
* Some devi ces can dynami cal l y change t he i dent i f i er t hey di spl ay. They can
* use t he PLAYER_FI DUCI AL_SET_I D conf i g t o al l ow a cl i ent t o set t he cur r ent l y
* di spl ayed val ue. Make t he r equest wi t h t he pl ayer _f i duci al _i d_t st r uct ur e.
* The devi ce r epl i es wi t h t he same st r uct ur e wi t h t he i d f i el d set t o t he val ue
* i t act ual l y used. You shoul d check t hi s val ue, as t he devi ce may not be abl e
* t o di spl ay t he val ue you r equest ed.
* 
* Cur r ent l y suppor t ed by t he st g_f i duci al dr i ver .
* 
* See t he pl ayer _f i duci al _i d st r uct ur e f r ompl ayer . h
*/
publ i c v oi d s et Fi duci al Val ( i nt newI D) {
t r y {
os . wr i t eBy t e ( PLAYER_FI DUCI AL_SET_I D) ;
os . wr i t eI nt ( newI D) ; / * t he val ue di spl ayed */
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Fi duci al ] : Coul dn' t s end PLAYER_FI DUCI AL_SET_I D " +
}
}

/ **
* Get t he number of f i duci al s.
* @r et ur n t he number of f i duci al s as an i nt eger .
*/
publ i c i nt get Fi duci al Count ( ) {
r et ur n t hi s . count ;
}

/ **
* Get t he f i duci al s.
* @r et ur n an ar r ay of Pl ayer Fi duci al I t em.
*/
publ i c Pl ay er Fi duci al I t em[ ] get Fi duci al s ( ) {
r et ur n t hi s . f i duci al s ;
}

/ / TO DO: i mpl ement f i duci al messagi ng once Pl ayer suppor t s i t
}
Anexe
pagina XXVI

j ja av va ac cl li ie en nt t/ /G GP PS SI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - GPSI nt er f ace. j ava
*
*
*
*
*
* $I d: GPSI nt er f ace. j ava, v 1. 2 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/

/ **
* The gps i nt er f ace pr ovi des access t o an absol ut e posi t i on syst em, such as GPS.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s GPSI nt er f ace ex t ends Pl ay er Dev i ce {

pr i v at e s hor t PLAYER_GPS_CODE = Pl ay er Cl i ent . PLAYER_GPS_CODE; / * GPS uni t */

pr i v at e i nt t i meSec = 0; / * GPS ( UTC) t i me i n seconds si nce t he epoch */
pr i v at e i nt t i meuSec = 0; / * GPS ( UTC) t i me i n mi cr oseconds si nce t he epoch */

/ * Lat i t ude i n degr ees / 1e7 ( uni t s ar e scal ed such t hat t he ef f ect i ve r esol ut i on i s
* r oughl y 1cm) . Posi t i ve i s nor t h of equat or , negat i ve i s sout h of equat or .
*/
pr i v at e i nt l at i t ude = 0;

/ * Longi t ude i n degr ees / 1e7 ( uni t s ar e scal ed such t hat t he ef f ect i ve r esol ut i on i s
* r oughl y 1cm) . Posi t i ve i s east of pr i me mer i di an, negat i ve i s west of pr i me mer i di an.
*/
pr i v at e i nt l ongi t ude = 0;

/ * Al t i t ude, i n mi l l i met er s. Posi t i ve i s above r ef er ence ( e. g. sea- l evel ) , and negat i ve
* i s bel ow.
*/
pr i v at e i nt al t i t ude = 0;

pr i v at e i nt ut mE = 0; / * UTM WGS84 coor di nat es, east i ng ( cm) */
pr i v at e i nt ut mN = 0; / * UTM WGS84 coor di nat es, nor t hi ng ( cm) */
pr i v at e by t e qual i t y = 0; / * qual i t y of f i x 0 = i nval i d, 1 = GPS f i x, 2 = DGPS f i x */
pr i v at e by t e numSat s = 0; / * number of sat el l i t es i n vi ew */
pr i v at e s hor t hDop = 0; / * hor i zont al di l ut i on of posi t i on ( HDOP) , t i mes 10 */
pr i v at e s hor t v Dop = 0; / * ver t i cal di l ut i on of posi t i on ( VDOP) , t i mes 10 */
pr i v at e i nt er r Hor z = 0; / * hor i zont al er r or ( mm) */
pr i v at e i nt er r Ver t = 0; / * ver t i cal er r or ( mm) */

/ **
* Const r uct or f or GPSI nt er f ace.
*/
publ i c GPSI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_GPS_CODE;
}

/ **
* Read t he cur r ent gl obal posi t i on and headi ng i nf or mat i on.
*/
Anexe
pagina XXVII
t r y {
r eadHeader ( ) ;
t i meSec = i s . r eadI nt ( ) ; / * GPS ( UTC) t i me, seconds */
t i meuSec = i s . r eadI nt ( ) ; / * GPS ( UTC) t i me, mi cr oseconds */
l at i t ude = i s . r eadI nt ( ) ; / * l at i t ude */
l ongi t ude = i s . r eadI nt ( ) ; / * l ongi t ude */
al t i t ude = i s . r eadI nt ( ) ; / * al t i t ude */
ut mE = i s . r eadI nt ( ) ; / * UTM WGS84 coor di nat es, East */
ut mN = i s . r eadI nt ( ) ; / * UTM WGS84 coor di nat es, Nor t h */
qual i t y = ( by t e) i s . r eadUns i gnedBy t e ( ) ; / * qual i t y */
numSat s = ( by t e) i s . r eadUns i gnedBy t e ( ) ; / * number of sat el l i t es i n vi ew */
hDop = ( s hor t ) i s . r eadUns i gnedShor t ( ) ; / * hor i zont al di l ut i on of posi t i on */
v Dop = ( s hor t ) i s . r eadUns i gnedShor t ( ) ; / * ver t i cal di l ut i on of posi t i on */
er r Hor z = i s . r eadI nt ( ) ; / * hor i zont al er r or */
er r Ver t = i s . r eadI nt ( ) ; / * ver t i cal er r or */
Sy s t em. er r . pr i nt l n ( "[ GPS] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng( ) ) ;
}
}

/ **
* Get t he GPS ( UTC) t i me i n seconds si nce t he epoch.
* @r et ur n t he GPS ( UTC) t i me i n seconds si nce t he epoch as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Ti meSec ( ) { r et ur n t i meSec; }

/ **
* Get t he GPS ( UTC) t i me i n mi cr oseconds si nce t he epoch.
* @r et ur n t he GPS ( UTC) t i me i n mi cr oseconds si nce t he epoch as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Ti meuSec ( ) { r et ur n t i meuSec; }

/ **
* Get t he l at i t ude i n degr ees / 1e7.
* @r et ur n t he l at i t ude i n degr ees / 1e7 as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Lat i t ude ( ) { r et ur n l at i t ude; }

/ **
* Get t he l ongi t ude i n degr ees / 1e7.
* @r et ur n t he l ongi t ude i n degr ees / 1e7 as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Longi t ude ( ) { r et ur n l ongi t ude; }

/ **
* Get t he al t i t ude, i n mi l l i met er s.
* @r et ur n t he al t i t ude, i n mi l l i met er s as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Al t i t ude ( ) { r et ur n al t i t ude; }

/ **
* Get t he UTM WGS84 coor di nat es, east i ng i n cm.
* @r et ur n t he UTM WGS84 coor di nat es, east i ng i n cmas an i nt eger .
*/
publ i c s y nchr oni z ed i nt get Ut mE ( ) { r et ur n ut mE; }

/ **
* Get t he UTM WGS84 coor di nat es, nor t hi ng i n cm.
* @r et ur n t he UTM WGS84 coor di nat es, nor t hi ng i n cmas an i nt eger .
*/
publ i c s y nchr oni z ed i nt get Ut mN ( ) { r et ur n ut mN; }

/ **
* Get t he qual i t y of f i x.
* @r et ur n 0 = i nval i d, 1 = GPS f i x, 2 = DGPS f i x
*/
publ i c s y nchr oni z ed by t e get Qual i t y ( ) { r et ur n qual i t y ; }

/ **
* Get t he number of sat el l i t es i n vi ew.
* @r et ur n t he number of sat el l i t es i n vi ew as a byt e.
*/
publ i c s y nchr oni z ed by t e get NumSat s ( ) { r et ur n numSat s ; }

/ **
* Get t he hor i zont al di l ut i on of posi t i on ( HDOP) , t i mes 10.
* @r et ur n t he hor i zont al di l ut i on of posi t i on ( HDOP) , t i mes 10 as a shor t
*/
publ i c s y nchr oni z ed s hor t get HDop ( ) { r et ur n hDop; }

/ **
* Get t he ver t i cal di l ut i on of posi t i on ( VDOP) , t i mes 10.
* @r et ur n t he ver t i cal di l ut i on of posi t i on ( VDOP) , t i mes 10 as a shor t
*/
Anexe
pagina XXVIII
publ i c s y nchr oni z ed s hor t get VDop ( ) { r et ur n v Dop; }

/ **
* Get t he hor i zont al er r or i n mm.
* @r et ur n t he hor i zont al er r or i n mmas an i nt eger
*/
publ i c s y nchr oni z ed i nt get Er r Hor z ( ) { r et ur n er r Hor z ; }

/ **
* Get t he ver t i cal er r or i n mm.
* @r et ur n t he ver t i cal er r or i n mmas an i nt eger
*/
publ i c s y nchr oni z ed i nt get Er r Ver t ( ) { r et ur n er r Ver t ; }
}

j ja av va ac cl li ie en nt t/ /G Gr ri ip pp pe er rI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Gr i pper I nt er f ace. j ava
* Copyr i ght ( C) 2003- 2005 Maxi mBat al i n & Radu Bogdan Rusu
*
*
*
*
*
* $I d: Gr i pper I nt er f ace. j ava, v 1. 3 2005/ 05/ 17 08: 59: 35 veedee Exp $
*
*/

/ **
* The gr i pper i nt er f ace pr ovi des access t o a r obot i c gr i pper . Thi s i nt er f ace i s VERY
* Pi oneer - speci f i c, and shoul d r eal l y be gener al i zed.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Gr i pper I nt er f ace ex t ends Pl ay er Dev i ce {

pr i v at e f i nal s hor t PLAYER_GRI PPER_CODE = Pl ay er Cl i ent . PLAYER_GRI PPER_CODE; / * gr i pper */


/ * t he cur r ent gr i pper l i f t and br eakbeamst at e */
pr i v at e by t e s t at e;

/ * t he cur r ent gr i pper l i f t and br eakbeamst at e */
pr i v at e by t e beams ;

/ **
* Const r uct or f or Gr i pper I nt er f ace.
*/
publ i c Gr i pper I nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_GRI PPER_CODE;
}

/ **
* Read t he cur r ent st at e of t he gr i pper ; t he f or mat i s gi ven bel ow. Not e t hat t he exact
* i nt er pr et at i on of t hi s dat a may var y dependi ng on t he det ai l s of your gr i pper and how i t i s
* connect ed t o your r obot ( e. g. , Gener al I / O vs. User I / O f or t he Pi oneer gr i pper ) .
* 
* The f ol l owi ng l i st def i nes how t he dat a can be i nt er pr et ed f or some Pi oneer r obot s & St age:
* 
Anexe
pagina XXIX
* <ul >
* <l i >st at e ( unsi gned byt e)
* <ul >
* <l i >bi t 0: Paddl es open
* <l i >bi t 1: Paddl es cl osed
* <l i >bi t 2: Paddl es movi ng
* <l i >bi t 3: Paddl es er r or
* <l i >bi t 4: Li f t i s up
* <l i >bi t 5: Li f t i s down
* <l i >bi t 6: Li f t i s movi ng
* <l i >bi t 7: Li f t er r or
* </ ul >
* <l i >beams ( unsi gned byt e)
* <ul >
* <l i >bi t 0: Gr i pper l i mi t r eached
* <l i >bi t 1: Li f t l i mi t r eached
* <l i >bi t 2: Out er beamobst r uct ed
* <l i >bi t 3: I nner beamobst r uct ed
* <l i >bi t 4: Lef t paddl e open
* <l i >bi t 5: Ri ght paddl e open
* </ ul >
*/
t r y {
r eadHeader ( ) ;
s t at e = i s . r eadBy t e ( ) ; / * t he cur r ent gr i pper l i f t and br eakbeamst at e */
beams = i s . r eadBy t e ( ) ; / * t he cur r ent gr i pper l i f t and br eakbeamst at e */
Sy s t em. er r . pr i nt l n ( "[ Gr i pper ] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* The gr i pper i nt er f ace accept s 2- byt e commands, t he f or mat of whi ch i s gi ven bel ow. These t wo
* byt es ar e sent di r ect l y t o t he gr i pper ; r ef er t o Tabl e 3- 3 page 10 i n t he Pi oneer 2 Gr i pper
* Manual f or a l i st of commands. The f i r st byt e i s t he command. The second i s t he ar gument f or
* t he LI FTcar r y and GRI Ppr ess commands, but f or al l ot her s i t i s i gnor ed.
* @par amcmd t he command
* @par amar g t he ar gument f or t he LI FTcar r y and GRI Ppr ess commands
*/
publ i c v oi d s et Gr i pper ( i nt cmd, i nt ar g) {
t r y {
os . wr i t eBy t e ( ( by t e) cmd) ;
os . wr i t eBy t e ( ( by t e) ar g) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Gr i pper ] : Coul dn' t s end gr i pper command r eques t : " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
* Get t he gr i pper ' s st at e.
* @r et ur n t he gr i pper ' s st at e as a byt e
*/
publ i c s y nchr oni z ed by t e get St at e ( ) { r et ur n s t at e; }
/ **
* Get t he gr i pper ' s beams.
* @r et ur n t he gr i pper ' s beams as a byt e
*/
publ i c s y nchr oni z ed by t e get Beams ( ) { r et ur n beams ; }

}

j ja av va ac cl li ie en nt t/ /I IR RI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - I RI nt er f ace. j ava
*
*
*
*
Anexe
pagina XXX
*
* $I d: I RI nt er f ace. j ava, v 1. 4 2005/ 06/ 01 19: 05: 48 veedee Exp $
*
*/

i mpor t j av acl i ent . s t r uct ur es . Pl ay er I RPos eReqT;

/ **
* The i r i nt er f ace pr ovi des access t o an ar r ay of i nf r ar ed ( I R) r ange sensor s. Thi s i nt er f ace
* accept s no commands.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s I RI nt er f ace ex t ends Pl ay er Dev i ce {


pr i v at e f i nal s hor t PLAYER_I R_CODE = Pl ay er Cl i ent . PLAYER_I R_CODE; / * I R ar r ay */

/ ** maxi mumnumber of I R sampl es */
publ i c s t at i c f i nal s hor t PLAYER_I R_MAX_SAMPLES = 32;

pr ot ect ed f i nal s hor t PLAYER_I R_POSE_REQ = 1;
pr ot ect ed f i nal s hor t PLAYER_I R_POWER_REQ = 2;


pr i v at e i nt r angeCount ; / * number of sampl es */
pr i v at e i nt [ ] v ol t ages = new i nt [ PLAYER_I R_MAX_SAMPLES] ; / * vol t ages ( uni t s?) */
pr i v at e i nt [ ] r anges = new i nt [ PLAYER_I R_MAX_SAMPLES] ; / * r anges ( mm) */

/ * obj ect cont ai ni ng pl ayer _i r _pose_r eq i n case of a t hr eaded cal l */
pr i v at e Pl ay er I RPos eReqT pi pr t ;
pr i v at e bool ean r eady PI PRT = f al s e;

/ **
* Const r uct or f or I RI nt er f ace.
*/
publ i c I RI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_I R_CODE;
}

/ **
* Read t he I R val ues.
*/
r eadHeader ( ) ;
t r y {
r angeCount = i s . r eadUns i gnedShor t ( ) ; / * t he number of val i d I R r eadi ngs */
f or ( i nt i = 0; i < PLAYER_I R_MAX_SAMPLES; i ++)
v ol t ages [ i ] = i s . r eadUns i gnedShor t ( ) ; / * vol t ages ( uni t s?) */
f or ( i nt i = 0; i < PLAYER_I R_MAX_SAMPLES; i ++)
r anges [ i ] = i s . r eadUns i gnedShor t ( ) ; / * r anges ( mm) */
Sy s t em. er r . pr i nt l n ( "[ I R] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Ret ur ns t he I R ar r ay dat a vol t ages up t o PLAYER_I R_MAX_SAMPLES.
* @r et ur n an ar r ay f i l l ed wi t h t he I R vol t ages
*/
publ i c s y nchr oni z ed i nt [ ] get Vol t ages ( ) { r et ur n v ol t ages ; }

/ **
* Ret ur ns t he I R ar r ay dat a r anges up t o PLAYER_I R_MAX_SAMPLES.
* @r et ur n an ar r ay f i l l ed wi t h t he I R r anges
*/
publ i c s y nchr oni z ed i nt [ ] get Ranges ( ) { r et ur n r anges ; }

/ **
Anexe
pagina XXXI
* Ret ur ns t he number of I R sensor s speci f i ed i n t he Pl ayer wor l d f i l e
* ( t he number of val i d r ange r eadi ngs) .
* @r et ur n t he number of I R sensor s speci f i ed i n t he Pl ayer wor l d f i l e as an i nt eger
*/
publ i c s y nchr oni z ed i nt get RangeCount ( ) { r et ur n r angeCount ; }

/ **
* Conf i gur at i on r equest : Quer y pose.
* 
* See t he pl ayer _i r _pose st r uct ur e f r ompl ayer . h
*/
publ i c v oi d quer y Pos e ( ) {
t r y {
os . wr i t eBy t e ( PLAYER_I R_POSE_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ I R] : Coul dn' t s end PLAYER_I R_POSE_REQ command: " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : I R power .
* @par amval ue 0 f or power of f , 1 f or power on
*/
publ i c v oi d s et I RPower ( i nt v al ue) {
t r y {
os . wr i t eBy t e ( PLAYER_I R_POWER_REQ) ;
os . wr i t eBy t e ( ( by t e) v al ue) ; / * 0 f or power of f , 1 f or power on */
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ I R] : Coul dn' t s end PLAYER_I R_POWER_REQ command: " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
*/
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ I R] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
cas e PLAYER_I R_POSE_REQ: {
pi pr t = new Pl ay er I RPos eReqT ( ) ;

pi pr t . s et Pos eCount ( i s . r eadShor t ( ) ) ;
s hor t I RPos es [ ] [ ] = new s hor t [ PLAYER_I R_MAX_SAMPLES] [ 3] ;
f or ( i nt i = 0; i < PLAYER_I R_MAX_SAMPLES; i ++) {
I RPos es [ i ] [ 0] = i s . r eadShor t ( ) ; / * X pos i n mm*/
I RPos es [ i ] [ 1] = i s . r eadShor t ( ) ; / * Y pos i n mm*/
I RPos es [ i ] [ 2] = i s . r eadShor t ( ) ; / * Yaw i n degr ees */
}
pi pr t . s et Pos es ( I RPos es ) ;

r eady PI PRT = t r ue;
br eak;
}
cas e PLAYER_I R_POWER_REQ: {
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ I R] : Unex pect ed r es pons e " + s ubt y pe +
" of s i z e = " + s i z e) ;
br eak;
}
}
Sy s t em. er r . pr i nt l n ( "[ I R] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
}
}

/ **
*/
Anexe
pagina XXXII
publ i c bool ean i s PI PRTReady ( ) {
i f ( r eady PI PRT) {
r eady PI PRT = f al s e;
r et ur n t r ue;
}
r et ur n f al s e;
}

/ **
* @r et ur n an obj ect of t ype Pl ayer I RPoseReqT cont ai ni ng t he r equi r ed geomet r y dat a
*/
publ i c Pl ay er I RPos eReqT get PI PRT ( ) { r et ur n pi pr t ; }

}

j ja av va ac cl li ie en nt t/ /J Jo oy ys st ti ic ck kI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - J oyst i ckI nt er f ace. j ava
*
*
*
*
*
* $I d: J oyst i ckI nt er f ace. j ava, v 1. 2 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/

/ **
* The j oyst i ck i nt er f ace pr ovi des access t o t he st at e of a j oyst i ck. I t al l ows
* anot her dr i ver or a ( possi bl y of f - boar d) cl i ent t o r ead and use t he st at e of a j oyst i ck. 
* Thi s i nt er f ace accept s no commands or conf i gur at i on r equest s.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Joy s t i ckI nt er f ace ex t ends Pl ay er Dev i ce {

pr i v at e f i nal s hor t PLAYER_JOYSTI CK_CODE = Pl ay er Cl i ent . PLAYER_JOYSTI CK_CODE; / * J oyst i ck */

pr i v at e s hor t x pos , y pos ; / * cur r ent j oyst i ck posi t i on ( unscal ed) */
pr i v at e s hor t x s cal e, y s cal e; / * scal i ng f act or s */
pr i v at e i nt but t ons ; / * but t on st at es ( bi t mask) */

/ **
* Const r uct or f or J oyst i ckI nt er f ace.
*/
publ i c Joy s t i ckI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_JOYSTI CK_CODE;
}

/ **
* The j oyst i ck dat a packet , whi ch cont ai ns t he cur r ent st at e of t he j oyst i ck.
*/
r eadHeader ( ) ;
t r y {
x pos = i s . r eadShor t ( ) ; / * cur r ent j oyst i ck posi t i on ( unscal ed) */
y pos = i s . r eadShor t ( ) ;
x s cal e = i s . r eadShor t ( ) ; / * scal i ng f act or s */
y s cal e = i s . r eadShor t ( ) ;
but t ons = i s . r eadUns i gnedShor t ( ) ; / * but t on st at es ( bi t mask) */
Anexe
pagina XXXIII
Sy s t em. er r . pr i nt l n ( "[ Joy s t i ck] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Ret ur ns t he cur r ent X j oyst i ck posi t i on ( unscal ed) .
* @r et ur n t he cur r ent X j oyst i ck posi t i on as a shor t
*/
publ i c s y nchr oni z ed s hor t get XPos ( ) { r et ur n t hi s . x pos ; }

/ **
* Ret ur ns t he cur r ent Y j oyst i ck posi t i on ( unscal ed) .
* @r et ur n t he cur r ent Y j oyst i ck posi t i on as a shor t
*/
publ i c s y nchr oni z ed s hor t get YPos ( ) { r et ur n t hi s . y pos ; }

/ **
* Ret ur ns t he cur r ent X scal i ng f act or .
* @r et ur n t he cur r ent X scal i ng f act or as a shor t
*/
publ i c s y nchr oni z ed s hor t get XScal e ( ) { r et ur n t hi s . x s cal e; }

/ **
* Ret ur ns t he cur r ent Y scal i ng f act or .
* @r et ur n t he cur r ent Y scal i ng f act or as a shor t
*/
publ i c s y nchr oni z ed s hor t get YScal e ( ) { r et ur n t hi s . y s cal e; }

/ **
* Ret ur ns t he cur r ent but t on st at es ( bi t mask) .
* @r et ur n t he cur r ent but t on st at es as an i nt eger
*/
publ i c s y nchr oni z ed i nt get But t ons ( ) { r et ur n t hi s . but t ons ; }
}

j ja av va ac cl li ie en nt t/ /L La as se er rI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Laser I nt er f ace. j ava
*
*
*
*
*
* $I d: Laser I nt er f ace. j ava, v 1. 4 2005/ 05/ 14 21: 16: 23 veedee Exp $
*
*/

i mpor t j av acl i ent . s t r uct ur es . Pl ay er Las er Conf i gT;
i mpor t j av acl i ent . s t r uct ur es . Pl ay er Las er GeomT;

/ **
* The l aser i nt er f ace pr ovi des access t o a si ngl e- or i gi n scanni ng r ange sensor , such as a
* SI CK l aser r ange- f i nder ( e. g. , si ckl ms200) .
* 
* Devi ces suppor t i ng t he l aser i nt er f ace can be conf i gur ed t o scan at di f f er ent angl es and
* r esol ut i ons. As such, t he dat a r et ur ned by t he l aser i nt er f ace can t ake di f f er ent f or ms. To
* make i nt er pr et at i on of t he dat a si mpl e, t he l aser dat a packet cont ai ns some ext r a f i el ds
* bef or e t he act ual r ange dat a. These f i el ds t el l t he cl i ent t he st ar t i ng and endi ng angl es of
* t he scan, t he angul ar r esol ut i on of t he scan, and t he number of r ange r eadi ngs i ncl uded. Scans
* pr oceed count er cl ockwi se about t he l aser ( 0 degr ees i s f or war d) . The l aser can r et ur n a maxi mum
* of 401 r eadi ngs; t hi s l i mi t s t he val i d combi nat i ons of scan wi dt h and angul ar r esol ut i on.
* 
* @ver si on
* <ul >
Anexe
pagina XXXIV
* </ ul >
*/
publ i c cl as s Las er I nt er f ace ex t ends Pl ay er Dev i ce {


/ * scanni ng r ange- f i nder */
pr i v at e s hor t PLAYER_LASER_CODE = Pl ay er Cl i ent . PLAYER_LASER_CODE;


pr ot ect ed f i nal s hor t PLAYER_LASER_GET_GEOM = 1;
pr ot ect ed f i nal s hor t PLAYER_LASER_SET_CONFI G = 2;
pr ot ect ed f i nal s hor t PLAYER_LASER_GET_CONFI G = 3;
pr ot ect ed f i nal s hor t PLAYER_LASER_POWER_CONFI G = 4;

/ ** maxi mumnumber of l aser r ange val ues */
publ i c f i nal i nt PLAYER_LASER_MAX_SAMPLES = 401;

pr i v at e s hor t mi nAngl e = 0; / * st ar t angl e f or t he l aser scan ( i n uni t s of 0. 01 degr ees) */
pr i v at e s hor t max Angl e = 0; / * end angl e f or t he l aser scan ( i n uni t s of 0. 01 degr ees) . */
pr i v at e i nt r es ol ut i on = 0; / * angul ar r esol ut i on ( i n uni t s of 0. 01 degr ees) */
pr i v at e i nt r angeRes = 0; / * r ange r esol ut i on. r anges shoul d be mul t i pl ed by t hi s */
pr i v at e i nt s ampl es Count = 0; / * number of r ange/ i nt ensi t y r eadi ngs */
pr i v at e i nt r ange[ ] = new i nt [ PLAYER_LASER_MAX_SAMPLES] ; / * r ange r eadi ngs ( mm) */
pr i v at e i nt i nt ens i t y [ ] = new i nt [ PLAYER_LASER_MAX_SAMPLES] ; / * i nt ensi t y r eadi ngs */

pr i v at e bool ean newI nf o = f al s e;

/ * obj ect cont ai ni ng pl ayer _l aser _geomi n case of a t hr eaded cal l */
pr i v at e Pl ay er Las er GeomT pl gt ;
pr i v at e bool ean r eady PLGT;

/ * obj ect cont ai ni ng pl ayer _l aser _conf i g i n case of a t hr eaded cal l */
pr i v at e Pl ay er Las er Conf i gT pl ct ;
pr i v at e bool ean r eady PLCT;
/ **
* Const r uct or f or Laser I nt er f ace.
*/
publ i c Las er I nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_LASER_CODE;

f or ( i nt i = 0; i < r ange. l engt h; i ++) {
r ange[ i ] = 0;
i nt ens i t y [ i ] = 0;
}
}

/ **
* Read t he l aser dat a packet .
*/
r eadHeader ( ) ;
t r y {
mi nAngl e = i s . r eadShor t ( ) ; / * st ar t angl e f or t he l aser scan */
max Angl e = i s . r eadShor t ( ) ; / * end angl e f or t he l aser scan */
r es ol ut i on = i s . r eadUns i gnedShor t ( ) ; / * angul ar r esol ut i on */
r angeRes = i s . r eadUns i gnedShor t ( ) ; / * r ange r esol ut i on */
s ampl es Count = i s . r eadUns i gnedShor t ( ) ; / * number of r ange/ i nt ensi t y r eadi ngs */

f or ( i nt i = 0; i < PLAYER_LASER_MAX_SAMPLES; i ++)
r ange[ i ] = i s . r eadUns i gnedShor t ( ) ; / * r ange r eadi ngs ( mm) */
f or ( i nt i = 0; i < PLAYER_LASER_MAX_SAMPLES; i ++)
i nt ens i t y [ i ] = i s . r eadUns i gnedBy t e ( ) ; / * i nt ensi t y r eadi ngs */
Sy s t em. er r . pr i nt l n ( "[ Las er ] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Ret ur n t he st ar t angl e f or t he l aser scan i n uni t s of 0. 01 degr ees.
* @r et ur n t he st ar t angl e f or t he l aser scan as a shor t
*/
publ i c s y nchr oni z ed s hor t get Mi nAngl e ( ) { r et ur n mi nAngl e; }

/ **
* Ret ur n t he end angl e f or t he l aser scan i n uni t s of 0. 01 degr ees.
* @r et ur n t he end angl e f or t he l aser scan as a shor t
Anexe
pagina XXXV
*/
publ i c s y nchr oni z ed s hor t get Max Angl e ( ) { r et ur n max Angl e; }

/ **
* Ret ur n t he angul ar r esol ut i on i n uni t s of 0. 01 degr ees.
* @r et ur n t he angul ar r esol ut i on as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Res ol ut i on ( ) { r et ur n r es ol ut i on; }

/ **
* Ret ur n t he r ange r esol ut i on.
* @r et ur n t he r ange r esol ut i on as an i nt eger
*/
publ i c s y nchr oni z ed i nt get RangeRes ( ) { r et ur n r angeRes ; }

/ **
* Ret ur n t he number of r ange/ i nt ensi t y r eadi ngs.
* @r et ur n t he number of r ange/ i nt ensi t y r eadi ngs as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Sampl es Count ( ) { r et ur n s ampl es Count ; }

/ **
* Ret ur n t he r ange r eadi ngs i n mm.
* @r et ur n t he r ange r eadi ngs as an ar r ay of i nt eger s
*/
publ i c s y nchr oni z ed i nt [ ] get Ranges ( ) { r et ur n r ange; }

/ **
* Ret ur n t he i nt ensi t y r eadi ngs.
* @r et ur n t he i nt ensi t y r eadi ngs as an ar r ay of i nt eger s
*/
publ i c s y nchr oni z ed i nt [ ] get I nt ens i t y ( ) { r et ur n i nt ens i t y ; }

/ **
* Conf i gur at i on r equest : Get geomet r y.
* 
* See t he pl ayer _l aser _geomst r uct ur e f r ompl ayer . h
*/
publ i c v oi d get Geomet r y ( ) {
t r y {
os . wr i t eBy t e ( PLAYER_LASER_GET_GEOM) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Las er ] : Coul dn' t s end PLAYER_LASER_GET_GEOM command: " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Set scan pr oper t i es.
* @par ammi nA st ar t angl e f or t he l aser scan
* @par ammaxA end angl e f or t he l aser scan
* @par amr es scan r esol ut i on
* @par amr ange r ange r esol ut i on
* @par ami nt ent enabl e r ef l ect i on i nt ensi t y dat a
*/
publ i c v oi d s et ScanPr oper t i es ( s hor t mi nA, s hor t max A, s hor t r es , s hor t r ange, by t e i nt ent ) {
t r y {
os . wr i t eBy t e ( PLAYER_LASER_SET_CONFI G) ;
os . wr i t eShor t ( mi nA) ; / * st ar t angl e f or t he l aser scan */
os . wr i t eShor t ( max A) ; / * end angl e f or t he l aser scan */
os . wr i t eShor t ( r es ) ; / * scan r esol ut i on */
os . wr i t eShor t ( r ange) ; / * r ange r esol ut i on */
os . wr i t eBy t e ( i nt ent ) ; / * enabl e r ef l ect i on i nt ensi t y dat a */
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Las er ] : Coul dn' t s end PLAYER_LASER_SET_CONFI G command: " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Get scan pr oper t i es.
*
*/
publ i c v oi d get ScanPr oper t i es ( ) {
t r y {
os . wr i t eBy t e ( PLAYER_LASER_GET_CONFI G) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Las er ] : Coul dn' t s end PLAYER_LASER_GET_CONFI G command: " +
e. t oSt r i ng ( ) ) ;
Anexe
pagina XXXVI
}
}

/ **
* Conf i gur at i on r equest : Tur n power on/ of f .
* @par amval ue 0 t o t ur n l aser of f , 1 t o t ur n l aser on
*/
publ i c v oi d s et Power ( by t e v al ue) {
t r y {
os . wr i t eBy t e ( PLAYER_LASER_POWER_CONFI G) ;
os . wr i t eBy t e ( v al ue) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Las er ] : Coul dn' t s end PLAYER_LASER_POWER_CONFI G command: " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
*/
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ Las er ] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
cas e PLAYER_LASER_GET_GEOM: {
pl gt = new Pl ay er Las er GeomT ( ) ;

s hor t l as er Pos e[ ] = new s hor t [ 2] ; / *l aser pose, i n r obot cs ( mm, mm, degr ees) */
s hor t l as er Si z e[ ] = new s hor t [ 3] ; / * l aser di mensi ons ( mm, mm) */

l as er Pos e[ 0] = i s . r eadShor t ( ) ; / * get t he l aser pose */
l as er Pos e[ 1] = i s . r eadShor t ( ) ;
l as er Pos e[ 2] = i s . r eadShor t ( ) ;
l as er Si z e[ 0] = i s . r eadShor t ( ) ; / * get t he l aser di mensi ons */
l as er Si z e[ 1] = i s . r eadShor t ( ) ;
pl gt . s et Pos e ( l as er Pos e) ;
pl gt . s et Si z e ( l as er Si z e) ;

r eady PLGT = t r ue;
br eak;
}
cas e PLAYER_LASER_SET_CONFI G: {
br eak;
}
cas e PLAYER_LASER_GET_CONFI G: {
pl ct = new Pl ay er Las er Conf i gT ( ) ;

pl ct . s et Mi nAngl e ( i s . r eadShor t ( ) ) ; / * st ar t angl e f or t he l aser scan */
pl ct . s et Max Angl e ( i s . r eadShor t ( ) ) ; / * end angl e f or t he l aser scan */
pl ct . s et Res ol ut i on ( i s . r eadUns i gnedShor t ( ) ) ; / * scan r esol ut i on */
pl ct . s et RangeRes ( i s . r eadUns i gnedShor t ( ) ) ; / * r ange r esol ut i on */
pl ct . s et I nt ens i t y ( i s . r eadBy t e ( ) ) ; / * enabl e r ef l ect i on i nt ensi t y dat a */

r eady PLCT = t r ue;
br eak;
}
cas e PLAYER_LASER_POWER_CONFI G: {
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ Las er ] : Unex pect ed r es pons e " + s ubt y pe +
" of s i z e = " + s i z e) ;
br eak;
}
}
Sy s t em. er r . pr i nt l n ( "[ Las er ] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Get t he l aser geomet r y af t er a PLAYER_LASER_GET_GEOM r equest .
* @r et ur n an obj ect of Pl ayer Laser GeomT t ype cont ai ni ng t he l aser geomet r y
* @see #i sReadyPLGT( )
*/
publ i c Pl ay er Las er GeomT get Pl ay er Las er Geom ( ) { r et ur n pl gt ; }
Anexe
pagina XXXVII

/ **
* Get t he l aser conf i gur at i on af t er a PLAYER_LASER_GET_CONFI G r equest .
* @r et ur n an obj ect of Pl ayer Laser Conf i gT t ype cont ai ni ng t he l aser conf i gur at i on
* @see #i sReadyPLCT( )
*/
publ i c Pl ay er Las er Conf i gT get Pl ay er Las er Conf i g ( ) { r et ur n pl ct ; }

/ **
* Check i f t he geomet r y dat a i s avai l abl e.
* @see #get Pl ayer Laser Geom( )
*/
publ i c bool ean i s Ready PLGT ( ) {
i f ( r eady PLGT) {
r eady PLGT = f al s e;
r et ur n t r ue;
}
r et ur n f al s e;
}

/ **
* Check i f t he conf i gur at i on dat a i s avai l abl e.
* @see #get Pl ayer Laser Conf i g( )
*/
publ i c bool ean i s Ready PLCT ( ) {
i f ( r eady PLCT) {
r eady PLCT = f al s e;
r et ur n t r ue;
}
r et ur n f al s e;
}
}

j ja av va ac cl li ie en nt t/ /L Lo oc ca al li iz ze eI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Local i zeI nt er f ace. j ava
*
*
*
*
*
* $I d: Local i zeI nt er f ace. j ava, v 1. 3 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/

i mpor t j av acl i ent . s t r uct ur es . Hy pot hes i s ;
i mpor t j av acl i ent . s t r uct ur es . Pl ay er Local i z eConf i gT;
/ **
* The l ocal i ze i nt er f ace pr ovi des pose i nf or mat i on f or t he r obot . Gener al l y speaki ng,
* l ocal i zat i on dr i ver s wi l l est i mat e t he pose of t he r obot by compar i ng obser ved sensor
* r eadi ngs agai nst a pr e- def i ned map of t he envi r onment . See, f or t he exampl e, t he amcl dr i ver ,
* whi ch i mpl ement s a pr obabi l i st i c Mont e- Car l o l ocal i zat i on al gor i t hm.
* 
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Local i z eI nt er f ace ex t ends Pl ay er Dev i ce {


pr i v at e f i nal s hor t PLAYER_LOCALI ZE_CODE = Pl ay er Cl i ent . PLAYER_LOCALI ZE_CODE; / *l ocal i zat i on */
Anexe
pagina XXXVIII


/ ** t he maxi mumnumber of pose hypot heses */
publ i c f i nal s hor t PLAYER_LOCALI ZE_MAX_HYPOTHS = 10;

/ * r equest / r epl y packet subt ypes */
pr ot ect ed f i nal s hor t PLAYER_LOCALI ZE_SET_POSE_REQ = 1;
pr ot ect ed f i nal s hor t PLAYER_LOCALI ZE_GET_CONFI G_REQ = 2;
pr ot ect ed f i nal s hor t PLAYER_LOCALI ZE_SET_CONFI G_REQ = 3;

pr i v at e s hor t pendi ngCount = 0; / * t he number of pendi ng ( unpr ocessed) obser vat i ons */
pr i v at e i nt pendi ngTi meSec = 0; / * t he t i me st amp of t he l ast obser vat i on pr ocessed */
pr i v at e i nt pendi ngTi meuSec = 0;
pr i v at e i nt hy pot hCount = 0; / * t he number of pose hypot heses */

/ * t he ar r ay of t he hypot heses */
pr i v at e Hy pot hes i s [ ] hy pot hs = new Hy pot hes i s [ PLAYER_LOCALI ZE_MAX_HYPOTHS] ;

pr i v at e Pl ay er Local i z eConf i gT pl ct = new Pl ay er Local i z eConf i gT ( ) ;
pr i v at e bool ean r eady PLCT = f al s e;

/ **
* Const r uct or f or Local i zeI nt er f ace.
*/
publ i c Local i z eI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_LOCALI ZE_CODE;
}

/ **
* Read an ar r ay of hypot heses.
*/
r eadHeader ( ) ;
t r y {
pendi ngCount = ( s hor t ) i s . r eadUns i gnedShor t ( ) ; / * number of pendi ng obser vat i ons */
pendi ngTi meSec = i s . r eadI nt ( ) ; / * t i me st amp of t he l ast obser vat i on */
pendi ngTi meuSec = i s . r eadI nt ( ) ;
hy pot hCount = i s . r eadI nt ( ) ; / * number of pose hypot heses */

f or ( i nt i = 0; i < hy pot hCount ; i ++) {
hy pot hs [ i ] = new Hy pot hes i s ( ) ;
i nt [ ] l ocal i z eMean = new i nt [ 3] ;
f or ( i nt j = 0; j < 3; j ++) / * mean val ue of t he pose est i mat e */
l ocal i z eMean[ j ] = i s . r eadI nt ( ) ;
hy pot hs [ i ] . s et Mean ( l ocal i z eMean) ;

l ong[ ] [ ] l ocal i z eCov = new l ong[ 3] [ 3] ;
f or ( i nt j = 0; j < 3; j ++) / * covar i ance mat r i x pose est i mat e */
f or ( i nt k = 0; k < 3; k++)
l ocal i z eCov [ j ] [ k] = i s . r eadLong ( ) ;
hy pot hs [ i ] . s et Cov ( l ocal i z eCov ) ;
hy pot hs [ i ] . s et Al pha ( i s . r eadI nt ( ) ) ; / * wei ght coef f i ci ent f or l i near combi nat i on */
}
Sy s t em. er r . pr i nt l n ( "[ Local i z e] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Set t he r obot pose est i mat e.
* @par ammean t he mean val ue of t he pose est i mat e ( mm, mm, ar c- seconds)
* @par amcov t he covar i ance mat r i x pose est i mat e ( mm$^2$, ar c- seconds$^2$)
*/
publ i c v oi d s et Pos e ( i nt [ ] mean, l ong[ ] [ ] cov ) {
t r y {
os . wr i t eBy t e ( PLAYER_LOCALI ZE_SET_POSE_REQ) ;
f or ( i nt i = 0; i < 3; i ++)
os . wr i t eI nt ( mean[ i ] ) ; / * mean val ue of t he pose est i mat e */
f or ( i nt i = 0; i < 3; i ++)
f or ( i nt j = 0; j < 3; j ++)
os . wr i t eLong ( cov [ i ] [ j ] ) ; / * covar i ance mat r i x pose est i mat e */
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Local i z e] : Coul dn' t s end PLAYER_LOCALI ZE_SET_POSE_REQ " +
}
}

Anexe
pagina XXXIX
/ **
* Conf i gur at i on r equest : Get conf i gur at i on.
*/
publ i c v oi d get Conf i gur at i on ( ) {
t r y {
os . wr i t eBy t e ( PLAYER_LOCALI ZE_GET_CONFI G_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Local i z e] : Coul dn' t s end PLAYER_LOCALI ZE_GET_CONFI G_REQ " +
}
}

/ **
* Conf i gur at i on r equest : Set conf i gur at i on.
* @par amnumPar t i cl es maxi mumnumber of par t i cl es ( f or dr i ver s usi ng par t i cl e f i l t er s)
*/
publ i c v oi d s et Conf i gur at i on ( i nt numPar t i cl es ) {
t r y {
os . wr i t eBy t e ( PLAYER_LOCALI ZE_SET_CONFI G_REQ) ;
os . wr i t eI nt ( numPar t i cl es ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Local i z e] : Coul dn' t s end PLAYER_LOCALI ZE_GET_CONFI G_REQ " +
}
}

/ **
*/
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ Local i z e] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
cas e PLAYER_LOCALI ZE_SET_POSE_REQ: {
br eak;
}
cas e PLAYER_LOCALI ZE_GET_CONFI G_REQ: {
pl ct = new Pl ay er Local i z eConf i gT ( ) ;
pl ct . s et NumPar t i cl es ( i s . r eadI nt ( ) ) ;
r eady PLCT = t r ue;
br eak;
}
cas e PLAYER_LOCALI ZE_SET_CONFI G_REQ: {
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ Local i z e] : Unex pect ed r es pons e " + s ubt y pe +
" of s i z e = " + s i z e) ;
br eak;
}
}
Sy s t em. er r . pr i nt l n ( "[ Local i z e] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Check i f conf i gur at i on dat a i s avai l abl e.
*/
publ i c bool ean i s PLCTReady ( ) {
i f ( r eady PLCT) {
r eady PLCT = f al s e;
r et ur n t r ue;
}
r et ur n f al s e;
}

/ **
* Get t he conf i gur at i on dat a.
* @r et ur n an obj ect of t ype Pl ayer Local i zeConf i gT cont ai ni ng t he r equi r ed conf i gur at i on dat a
*/
publ i c Pl ay er Local i z eConf i gT get PLCT ( ) { r et ur n pl ct ; }
}
Anexe
pagina XL

j ja av va ac cl li ie en nt t/ /L Lo og gI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - LogI nt er f ace. j ava
*
*
*
*
*
* $I d: LogI nt er f ace. j ava, v 1. 2 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/

/ **
* The l og i nt er f ace pr ovi des st ar t / st op cont r ol of dat a l oggi ng/ pl ayback. 
* The l og i nt er f ace pr oduces no dat a and accept s no commands.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s LogI nt er f ace ex t ends Pl ay er Dev i ce {


/ * l og R/ Wcont r ol */
pr i v at e f i nal s hor t PLAYER_LOG_CODE = Pl ay er Cl i ent . PLAYER_LOG_CODE;


/ * t he subt ypes f or conf i g r equest s */
pr ot ect ed s t at i c f i nal s hor t PLAYER_LOG_SET_WRI TE_STATE_REQ = 1;
pr ot ect ed s t at i c f i nal s hor t PLAYER_LOG_SET_READ_STATE_REQ = 2;
pr ot ect ed s t at i c f i nal s hor t PLAYER_LOG_GET_STATE_REQ = 3;
pr ot ect ed s t at i c f i nal s hor t PLAYER_LOG_SET_READ_REWI ND_REQ = 4;
pr ot ect ed s t at i c f i nal s hor t PLAYER_LOG_SET_FI LENAME = 5;

/ * t ypes of l og devi ces */
pr ot ect ed s t at i c f i nal s hor t PLAYER_LOG_TYPE_READ = 1;
pr ot ect ed s t at i c f i nal s hor t PLAYER_LOG_TYPE_WRI TE = 2;

/ * t he l oggi ng/ pl ayback st at e and t ype */
pr i v at e by t e s t at e;
pr i v at e by t e t y pe;
pr i v at e bool ean r eady St at e = f al s e;

/ **
* Const r uct or f or LogI nt er f ace.
*/
publ i c LogI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_LOG_CODE;
}

/ **
* Conf i gur at i on r equest : Set l oggi ng st at e.
* 
* St ar t / st op dat a l oggi ng. 
* See t he pl ayer _l og_set _wr i t e_st at e st r uct ur e f r ompl ayer . h
* @par amst at e 0=di sabl ed, 1=enabl ed
*/
publ i c v oi d s et Loggi ngSt at e ( i nt s t at e) {
t r y {
Anexe
pagina XLI
os . wr i t eBy t e ( PLAYER_LOG_SET_WRI TE_STATE_REQ) ;
os . wr i t eBy t e ( ( by t e) s t at e) ; / * 0=di sabl ed, 1=enabl ed */
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Log] : Coul dn' t s end PLAYER_LOG_SET_WRI TE_STATE_REQ " +
}
}

/ **
* Conf i gur at i on r equest : Set pl ayback st at e.
* 
* St ar t / st op dat a pl ayback. 
* See t he pl ayer _l og_set _r ead_st at e st r uct ur e f r ompl ayer . h
* @par amst at e 0=di sabl ed, 1=enabl ed
*/
publ i c v oi d s et Pl ay backSt at e ( i nt s t at e) {
t r y {
os . wr i t eBy t e ( PLAYER_LOG_SET_READ_STATE_REQ) ;
os . wr i t eBy t e ( ( by t e) s t at e) ; / * 0=di sabl ed, 1=enabl ed */
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Log] : Coul dn' t s end PLAYER_LOG_SET_READ_STATE_REQ " +
}
}

/ **
* Conf i gur at i on r equest : Rewi nd pl ayback.
* 
* Rewi nd l og pl ayback t o begi nni ng of l ogf i l e; does not af f ect pl ayback
* st at e ( i . e. , whet her i t i s st ar t ed or st opped) . 
* See t he pl ayer _l og_set _r ead_r ewi nd st r uct ur e f r ompl ayer . h
*/
publ i c v oi d r ewi ndPl ay back ( ) {
t r y {
os . wr i t eBy t e ( PLAYER_LOG_SET_READ_REWI ND_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Log] : Coul dn' t s end PLAYER_LOG_SET_READ_REWI ND_REQ " +
}
}

/ **
* Conf i gur at i on r equest : Get st at e.
* 
* Fi nd out whet her l oggi ng/ pl ayback i s enabl ed or di sabl ed. 
* See t he pl ayer _l og_get _st at e st r uct ur e f r ompl ayer . h
*/
publ i c v oi d r eadSt at e ( ) {
t r y {
os . wr i t eBy t e ( PLAYER_LOG_GET_STATE_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Log] : Coul dn' t s end PLAYER_LOG_GET_STATE_REQ " +
}
}

/ **
* Conf i gur at i on r equest : Set f i l ename.
* 
* Set t he name of t he f i l e t o wr i t e t o when l oggi ng. 
* See t he pl ayer _l og_set _f i l ename st r uct ur e f r ompl ayer . h
* @par amf i l eName t he name of t he f i l e ( max 255 char s + t er mi nat i ng NULL)
*/
publ i c v oi d s et Fi l eName ( St r i ng f i l eName) {
i f ( f i l eName. l engt h ( ) > 255)
f i l eName = f i l eName. s ubs t r i ng( 0, 255) ;
t r y {
i nt s i z e = 2 + f i l eName. l engt h ( ) ;
s endHeader ( PLAYER_MSGTYPE_REQ, s i z e) ; / * payl oad */
os . wr i t eBy t e ( PLAYER_LOG_SET_FI LENAME) ;
f or ( i nt i = 0; i < f i l eName. l engt h ( ) ; i ++)
os . wr i t eBy t e ( f i l eName. t oChar Ar r ay ( ) [ i ] ) ;
os . wr i t eBy t e ( 0) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Log] : Coul dn' t s end PLAYER_LOG_SET_FI LENAME " +
Anexe
pagina XLII
}
}

/ **
*/
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ Log] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
cas e PLAYER_LOG_SET_WRI TE_STATE_REQ: {
br eak;
}
cas e PLAYER_LOG_SET_READ_STATE_REQ: {
br eak;
}
cas e PLAYER_LOG_GET_STATE_REQ: {
t y pe = i s . r eadBy t e ( ) ; / * i gnor e t he t ype */
s t at e = i s . r eadBy t e ( ) ; / * l oggi ng/ pl ayback st at e:
0=di sabl ed, 1=enabl ed */
r eady St at e = t r ue;
br eak;
}
cas e PLAYER_LOG_SET_READ_REWI ND_REQ: {
br eak;
}
cas e PLAYER_LOG_SET_FI LENAME: {
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ Log] : Unex pect ed r es pons e " + s ubt y pe +
" of s i z e = " + s i z e) ;
br eak;
}
}
Sy s t em. er r . pr i nt l n ( "[ Log] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Get t he t ype of l og devi ce.
* @r et ur n t he t ype of l og devi ce, ei t her PLAYER_LOG_TYPE_READ or
* PLAYER_LOG_TYPE_WRI TE
*/
publ i c by t e get Ty pe ( ) { r et ur n t hi s . t y pe; }

/ **
* Get t he l oggi ng/ pl ayback st at e.
* @r et ur n t he l oggi ng/ pl ayback st at e, 0=di sabl ed, 1=enabl ed
*/
publ i c by t e get St at e ( ) { r et ur n t hi s . s t at e; }

/ **
* Check i f st at e dat a i s avai l abl e.
*/
publ i c bool ean i s St at eReady ( ) {
i f ( r eady St at e) {
r eady St at e = f al s e;
r et ur n t r ue;
}
r et ur n f al s e;
}

}

j ja av va ac cl li ie en nt t/ /M Ma ap pI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - MapI nt er f ace. j ava
*
*
Anexe
pagina XLIII
*
*
*
* $I d: MapI nt er f ace. j ava, v 1. 3 2005/ 05/ 26 09: 14: 47 veedee Exp $
*
*/

/ **
* The map i nt er f ace pr ovi des acces t o an occupancy gr i d map. Thi s i nt er f ace r et ur ns no dat a
* and accept s no commands. The map i s del i ver ed i n t i l es, vi a a sequence of conf i gur at i on
* r equest s.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s MapI nt er f ace ex t ends Pl ay er Dev i ce {


/ * get a map */
pr i v at e f i nal s hor t PLAYER_MAP_CODE = Pl ay er Cl i ent . PLAYER_MAP_CODE;


/ ** t he max number of cel l s we can send i n one t i l e */
publ i c s t at i c f i nal s hor t PLAYER_MAP_MAX_CELLS_PER_TI LE =
Pl ay er Cl i ent . PLAYER_MAX_REQREP_SI ZE - 17;

pr ot ect ed s t at i c f i nal s hor t PLAYER_MAP_GET_I NFO_REQ = 1;
pr ot ect ed s t at i c f i nal s hor t PLAYER_MAP_GET_DATA_REQ = 2;

pr i v at e i nt s cal e; / * t he scal e of t he map ( pi xel s per ki l omet er ) */
pr i v at e i nt wi dt h; / * t he wi dt h of t he map ( pi xel s) */
pr i v at e i nt hei ght ; / * t he hei ght of t he map ( pi xel s) */

pr i v at e i nt t i l eCol ; / * t he t i l e or i gi n ( X - pi xel s) */
pr i v at e i nt t i l eRow; / * t he t i l e or i gi n ( Y - pi xel s) */
pr i v at e i nt t i l eWi dt h; / * t he wi dt h of a t i l e i n t he map ( pi xel s) */
pr i v at e i nt t i l eHei ght ; / * t he hei ght of a t i l e t he map ( pi xel s) */

/ * cel l occupancy val ue ( empt y = - 1, unknown = 0, occupi ed = +1) */
pr i v at e i nt t i l eDat a[ ] = new i nt [ PLAYER_MAP_MAX_CELLS_PER_TI LE] ;

/ **
* Const r uct or f or MapI nt er f ace.
*/
publ i c MapI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_MAP_CODE;
}

/ **
* Conf i gur at i on r equest : Get map i nf or mat i on.
* 
* Ret r i eve t he si ze and scal e i nf or mat i on of a cur r ent map. Thi s r equest i s used t o
* get t he si ze i nf or mat i on bef or e you r equest t he act ual map dat a. Set t he subt ype t o
* PLAYER_MAP_GET_I NFO_REQ; t he ser ver wi l l r epl y wi t h t he si ze i nf or mat i on f i l l ed i n.
* 
* See t he pl ayer _map_i nf o st r uct ur e f r ompl ayer . h
*/
publ i c v oi d get MapI nf or mat i on ( ) {
t r y {
os . wr i t eBy t e ( PLAYER_MAP_GET_I NFO_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Map] : Coul dn' t s end PLAYER_MAP_GET_I NFO_REQ " +
Anexe
pagina XLIV
}
}

/ **
* Conf i gur at i on r equest : Get map dat a.
* 
* Ret r i eve t he map dat a. Beacause of t he l i mi t ed si ze of a r equest - r epl y messages, t he
* map dat a i s t r anf er ed i n t i l es. I n t he r equest packet , set t he col umn and r ow i ndex
* of a speci f i c t i l e; t he ser ver wi l l r epl y wi t h t he r equest ed map dat a f i l l ed i n.
* 
* See t he pl ayer _map_dat a st r uct ur e f r ompl ayer . h
* @par amcol t he t i l e or i gi n ( X - pi xel s)
* @par amr ow t he t i l e or i gi n ( Y - pi xel s)
* @par amwi dt h t he wi dt h of t he t i l e i n t he map ( pi xel s)
* @par amhei ght t he hei ght of t he t i l e i n t he map ( pi xel s)
*/
publ i c v oi d get MapDat a ( i nt col , i nt r ow, i nt wi dt h, i nt hei ght ) {
t r y {
os . wr i t eBy t e ( PLAYER_MAP_GET_DATA_REQ) ;
os . wr i t eI nt ( col ) ; / * t he t i l e or i gi n ( X - pi xel s) */
os . wr i t eI nt ( r ow) ; / * t he t i l e or i gi n ( Y - pi xel s) */
os . wr i t eI nt ( wi dt h) ; / * t he wi dt h of t he t i l e i n t he map ( pi xel s) */
os . wr i t eI nt ( hei ght ) ; / * t he hei ght of t he t i l e i n t he map ( pi xel s) */
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Map] : Coul dn' t s end PLAYER_MAP_GET_DATA_REQ " +
}
}

/ **
*/
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ Map] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
s hor t s ubt y pe = ( s hor t ) i s . r eadUns i gnedBy t e ( ) ;
/ / shoul d be byt e subt ype = i s. r eadByt e ( ) ;
cas e PLAYER_MAP_GET_I NFO_REQ: {
s cal e = i s . r eadI nt ( ) ; / * t he scal e of t he map ( pi xel s per ki l omet er ) */
wi dt h = i s . r eadI nt ( ) ; / * t he wi dt h of t he map ( pi xel s) */
hei ght = i s . r eadI nt ( ) ; / * t he hei ght of t he map ( pi xel s) */
br eak;
}
cas e PLAYER_MAP_GET_DATA_REQ: {
t i l eCol = i s . r eadI nt ( ) ; / * t he t i l e or i gi n ( X - pi xel s) */
t i l eRow = i s . r eadI nt ( ) ; / * t he t i l e or i gi n ( Y - pi xel s) */
t i l eWi dt h = i s . r eadI nt ( ) ; / * t he wi dt h of a t i l e i n t he map ( pi xel s) */
t i l eHei ght = i s . r eadI nt ( ) ; / * t he hei ght of a t i l e i n t he map ( pi xel s) */
f or ( i nt i = 0; i < PLAYER_MAP_MAX_CELLS_PER_TI LE; i ++) {
/ * cel l occupancy val ue ( empt y = - 1, unknown = 0, occupi ed = +1) */
t i l eDat a[ i ] = i s . r eadBy t e ( ) ;
}
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ Map] : Unex pect ed r es pons e " + s ubt y pe +
" of s i z e = " + s i z e) ;
br eak;
}
}
Sy s t em. er r . pr i nt l n ( "[ Map] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Get t he scal e of t he map ( pi xel s per ki l omet er )
* @r et ur n t he scal e of t he map as an i nt
*/
publ i c s y nchr oni z ed i nt get Scal e ( ) { r et ur n t hi s . s cal e; }

/ **
* Get t he wi dt h of t he map ( pi xel s)
* @r et ur n t he wi dt h of t he map as an i nt
*/
Anexe
pagina XLV
publ i c s y nchr oni z ed i nt get Wi dt h ( ) { r et ur n t hi s . wi dt h; }

/ **
* Get t he hei ght of t he map ( pi xel s)
* @r et ur n t he hei ght of t he map as an i nt
*/
publ i c s y nchr oni z ed i nt get Hei ght ( ) { r et ur n t hi s . hei ght ; }

/ **
* Get t he t i l e or i gi n ( X - pi xel s)
* @r et ur n t he t i l e or i gi n ( X - pi xel s) as an i nt
*/
publ i c s y nchr oni z ed i nt get Ti l eCol ( ) { r et ur n t hi s . t i l eCol ; }

/ **
* Get t he t i l e or i gi n ( Y - pi xel s)
* @r et ur n t he t i l e or i gi n ( Y - pi xel s) as an i nt
*/
publ i c s y nchr oni z ed i nt get Ti l eRow ( ) { r et ur n t hi s . t i l eRow; }

/ **
* Get t he wi dt h of a t i l e i n t he map ( pi xel s)
* @r et ur n t he wi dt h of a t i l e t he map as an i nt
*/
publ i c s y nchr oni z ed i nt get Ti l eWi dt h ( ) { r et ur n t hi s . t i l eWi dt h; }

/ **
* Get t he hei ght of a t i l e i n t he map ( pi xel s)
* @r et ur n t he hei ght of a t i l e i n t he map as an i nt
*/
publ i c s y nchr oni z ed i nt get Ti l eHei ght ( ) { r et ur n t hi s . t i l eHei ght ; }

/ **
* Get a t i l e dat a
* @r et ur n t he dat a of a speci f i ed t i l e as an ar r ay of i nt eger s
* @see #get MapDat a( i nt , i nt , i nt , i nt )
*/
publ i c s y nchr oni z ed i nt [ ] get Ti l eDat a ( ) { r et ur n t hi s . t i l eDat a; }

/ **
* Handl e negat i ve acknowl edgement r esponse messages.
*/
publ i c v oi d handl eNARMes s age ( ) {
t r y {
i nt s i z e = i s . r eadI nt ( ) ; / * r ead t he packet si ze */
Sy s t em. er r . pr i nt l n ( "[ Map] : Handl i ng NAR of s i z e = " + s i z e) ;
Sy s t em. er r . pr i nt l n ( "[ Map] : handl eRes pons ePos i t i on ERROR " + e. t oSt r i ng ( ) ) ;
}
}

}

j ja av va ac cl li ie en nt t/ /M MC Co om mm mI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - MCommI nt er f ace. j ava
*
*
*
*
*
* $I d: MComI nt er f ace. j ava, v 1. 4 2005/ 05/ 17 08: 59: 35 veedee Exp $
*
*/

/ **
* The mcomi nt er f ace i s desi gned f or exchangi ng i nf or mat i on bet ween cl i ent s. A cl i ent
* sends a message of a gi ven " t ype" and " channel " . Thi s devi ce st or es adds t he message
* t o t hat channel ' s st ack. A second cl i ent can t hen r equest dat a of a gi ven " t ype" and
Anexe
pagina XLVI
* " channel " . Push, Pop, Read, and Cl ear oper at i ons ar e def i ned, but t hei r semant i cs can
* var y, based on t he st ack di sci pl i ne of t he under l yi ng dr i ver . For exampl e, t he
* l i f omcomdr i ver enf or ces a l ast - i n- f i r st - out st ack. 
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s MComI nt er f ace ex t ends Pl ay er Dev i ce {


pr i v at e f i nal s hor t PLAYER_MCOM_CODE = Pl ay er Cl i ent . PLAYER_MCOM_CODE; / * mul t i coms */


/ ** si ze of t he dat a f i el d i n messages */
publ i c s t at i c f i nal s hor t MCOM_DATA_LEN = 128;

/ ** number of buf f er s t o keep per channel */
publ i c s t at i c f i nal s hor t MCOM_N_BUFS = 10;

/ ** si ze of channel name */
publ i c s t at i c f i nal s hor t MCOM_CHANNEL_LEN = 8;

/ ** r et ur ns t hi s i f empt y */
publ i c s t at i c f i nal St r i ng MCOM_EMPTY_STRI NG = "( EMPTY) ";

pr ot ect ed s t at i c f i nal s hor t MCOM_COMMAND_BUFFER_SI ZE = 111;
pr ot ect ed s t at i c f i nal s hor t MCOM_DATA_BUFFER_SI ZE = 0;

/ * r equest i ds */
publ i c s t at i c f i nal s hor t PLAYER_MCOM_PUSH_REQ = 0;
publ i c s t at i c f i nal s hor t PLAYER_MCOM_POP_REQ = 1;
publ i c s t at i c f i nal s hor t PLAYER_MCOM_READ_REQ = 2;
publ i c s t at i c f i nal s hor t PLAYER_MCOM_CLEAR_REQ = 3;
publ i c s t at i c f i nal s hor t PLAYER_MCOM_SET_CAPACI TY_REQ = 4;

pr i v at e char f ul l ; / * a f l ag */
pr i v at e char [ ] dat a = new char [ MCOM_DATA_LEN] ; / * t he dat a */

pr i v at e s hor t dat aTy pe; / * t he " t ype" of t he dat a */
pr i v at e char [ ] channel Name = new char [ MCOM_CHANNEL_LEN] ; / * t he name of t he channel */
pr i v at e char f ul l F;
pr i v at e char [ ] dat aF = new char [ MCOM_DATA_LEN] ;
pr i v at e bool ean r eady Dat a = f al s e;

/ **
* Const r uct or f or MComI nt er f ace.
*/
publ i c MComI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_MCOM_CODE;
}

/ **
* Read a pi ece of dat a.
*/
r eadHeader ( ) ;
t r y {
f ul l = i s . r eadChar ( ) ; / * a f l ag */
f or ( i nt i = 0; i < MCOM_DATA_LEN; i ++) {
dat a[ i ] = i s . r eadChar ( ) ; / * t he dat a */
}
Sy s t em. er r . pr i nt l n ( "[ MCom] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Ret ur ns t he f l ag.
* @r et ur n t he f l ag as a char
*/
publ i c s y nchr oni z ed char get Fl ag ( ) { r et ur n t hi s . f ul l ; }

/ **
* Ret ur ns t he dat a.
* @r et ur n t he dat a as a ar r ay of char s
Anexe
pagina XLVII
*/
publ i c s y nchr oni z ed char [ ] get Dat a ( ) { r et ur n t hi s . dat a; }

/ **
* Conf i gur at i on r equest : Conf i g r equest s sent t o ser ver .
* 
* See t he pl ayer _mcom_conf i g st r uct ur e f r ompl ayer . h
* @par amwhi chReq whi ch r equest ( shoul d be one of t he def i ned r equest i ds)
* @par amt ype t he " t ype" of t he dat a
*/
publ i c v oi d s endConf i gReq ( i nt whi chReq, i nt t y pe, St r i ng channel ,
bool ean f ul l T, char [ ] dat aT) {
t r y {
i nt t ot al = 4 + MCOM_CHANNEL_LEN + MCOM_DATA_LEN;
s endHeader ( PLAYER_MSGTYPE_REQ, t ot al ) ; / * payl oad */
os . wr i t eBy t e ( whi chReq) ;
os . wr i t eShor t ( ( s hor t ) t y pe) ;

i nt s i z e = channel . l engt h ( ) ;
i f ( s i z e > MCOM_CHANNEL_LEN)
s i z e = MCOM_CHANNEL_LEN;
f or ( i nt i = 0; i < s i z e; i ++)
os . wr i t eChar ( channel . t oChar Ar r ay ( ) [ i ] ) ;
i f ( s i z e < MCOM_CHANNEL_LEN)
f or ( i nt i = 0; i < ( MCOM_CHANNEL_LEN - s i z e) ; i ++)
os . wr i t eChar ( 0) ;

os . wr i t eBool ean ( f ul l T) ;
f or ( i nt i = 0; i < MCOM_DATA_LEN; i ++)
os . wr i t eChar ( dat aT[ i ] ) ;

os . f l us h ( ) ;
St r i ng s ubt y pe = "";
s wi t ch ( t y pe) {
cas e PLAYER_MCOM_PUSH_REQ: {
s ubt y pe = "PLAYER_MCOM_PUSH_REQ";
br eak;
}
cas e PLAYER_MCOM_POP_REQ: {
s ubt y pe = "PLAYER_MCOM_POP_REQ";
br eak;
}
cas e PLAYER_MCOM_READ_REQ: {
s ubt y pe = "PLAYER_MCOM_READ_REQ";
br eak;
}
cas e PLAYER_MCOM_CLEAR_REQ: {
s ubt y pe = "PLAYER_MCOM_CLEAR_REQ";
br eak;
}
cas e PLAYER_MCOM_SET_CAPACI TY_REQ: {
s ubt y pe = "PLAYER_MCOM_SET_CAPACI TY_REQ";
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ MCom] : Coul dn' t s end " + s ubt y pe +
" command: " + e. t oSt r i ng ( ) ) ;
}
}
}
}

/ **
* Conf i gur at i on r equest : Push ( PLAYER_MCOM_PUSH_REQ)
*/
publ i c v oi d Pus h ( i nt t y pe, St r i ng channel , char [ ] dat aT) {
s endConf i gReq ( PLAYER_MCOM_PUSH_REQ, t y pe, channel , t r ue, dat aT) ;
}

/ **
* Conf i gur at i on r equest : Pop ( PLAYER_MCOM_POP_REQ)
*/
publ i c v oi d Pop ( i nt t y pe, St r i ng channel ) {
char [ ] dat aT = new char [ MCOM_DATA_LEN] ;
s endConf i gReq ( PLAYER_MCOM_POP_REQ, t y pe, channel , f al s e, dat aT) ;
}

/ **
* Conf i gur at i on r equest : Read ( PLAYER_MCOM_READ_REQ)
*/
publ i c v oi d Read ( i nt t y pe, St r i ng channel ) {
s endConf i gReq ( PLAYER_MCOM_READ_REQ, t y pe, channel , f al s e, dat aT) ;
}
Anexe
pagina XLVIII

/ **
* Conf i gur at i on r equest : Cl ear ( PLAYER_MCOM_CLEAR_REQ)
*/
publ i c v oi d Cl ear ( i nt t y pe, St r i ng channel ) {
s endConf i gReq ( PLAYER_MCOM_CLEAR_REQ, t y pe, channel , f al s e, dat aT) ;
}

/ **
* Conf i gur at i on r equest : Set capaci t y ( PLAYER_MCOM_SET_CAPACI TY_REQ)
*/
publ i c v oi d s et Capaci t y ( i nt t y pe, St r i ng channel , char capaci t y ) {
dat aT[ 0] = capaci t y ;
s endConf i gReq ( PLAYER_MCOM_SET_CAPACI TY_REQ, t y pe, channel , f al s e, dat aT) ;
}

/ **
* Check i f dat a i s avai l abl e.
*/
publ i c bool ean i s Dat aReady ( ) {
i f ( r eady Dat a) {
r eady Dat a = f al s e;
r et ur n t r ue;
}
r et ur n f al s e;
}

/ **
*/
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ MCom] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
cas e PLAYER_MCOM_PUSH_REQ: {
br eak;
}
cas e PLAYER_MCOM_POP_REQ: {
/ * t he " t ype" of dat a */
dat aTy pe = ( s hor t ) i s . r eadUns i gnedShor t ( ) ;

f or ( i nt i = 0; i < MCOM_CHANNEL_LEN; i ++)
channel Name[ i ] = i s . r eadChar ( ) ; / * t he name of t he channel */

f ul l F = i s . r eadChar ( ) ; / * a f l ag ( bool ean 0/ 1) */
dat aF[ i ] = i s . r eadChar ( ) ; / * t he dat a */

r eady Dat a = t r ue;
br eak;
}
cas e PLAYER_MCOM_READ_REQ: {
/ * t he " t ype" of dat a */
dat aTy pe = ( s hor t ) i s . r eadUns i gnedShor t ( ) ;

f or ( i nt i = 0; i < MCOM_CHANNEL_LEN; i ++)
channel Name[ i ] = i s . r eadChar ( ) ; / * t he name of t he channel */

f ul l F = i s . r eadChar ( ) ; / * a f l ag ( bool ean 0/ 1) */
dat aF[ i ] = i s . r eadChar ( ) ; / * t he dat a */

r eady Dat a = t r ue;
br eak;
}
cas e PLAYER_MCOM_CLEAR_REQ: {
br eak;
}
cas e PLAYER_MCOM_SET_CAPACI TY_REQ: {
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ MCom] : Unex pect ed r es pons e " + s ubt y pe +
" of s i z e = " + s i z e) ;
br eak;
Anexe
pagina XLIX
}
}
Sy s t em. er r . pr i nt l n ( "[ MCom] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Ret ur ns t he f l ag af t er a sendConf i gReq ( ) cal l .
* @r et ur n t he f l ag as a char
*/
publ i c s y nchr oni z ed char get RFl ag ( ) { r et ur n t hi s . f ul l F; }

/ **
* Ret ur ns t he dat a af t er a sendConf i gReq ( ) cal l .
* @r et ur n t he dat a as a ar r ay of char s
*/
publ i c s y nchr oni z ed char [ ] get RDat a ( ) { r et ur n t hi s . dat aF; }

/ **
* Ret ur ns t he t ype of dat a af t er a sendConf i gReq ( ) cal l .
* @r et ur n t he t ype of dat a as a shor t
*/
publ i c s y nchr oni z ed s hor t get Dat aTy pe ( ) { r et ur n t hi s . dat aTy pe; }

/ **
* Ret ur ns t he name of t he channel af t er a sendConf i gReq ( ) cal l .
* @r et ur n t he name of t he channel as a ar r ay of char s
*/
publ i c s y nchr oni z ed char [ ] get Channel Name ( ) { r et ur n t hi s . channel Name; }
}

j ja av va ac cl li ie en nt t/ /M Mo ot to or rI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Mot or I nt er f ace. j ava
*
*
*
*
*
* $I d: Mot or I nt er f ace. j ava, v 1. 4 2005/ 05/ 26 09: 14: 47 veedee Exp $
*
*/

/ **
* The mot or i nt er f ace i s used t o cont r ol a si ngl e mot or .
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Mot or I nt er f ace ex t ends Pl ay er Dev i ce {

/ * mot or i nt er f ace */
pr i v at e f i nal s hor t PLAYER_MOTOR_CODE = Pl ay er Cl i ent . PLAYER_MOTOR_CODE;


/ * t he var i ous conf i gur at i on r equest t ypes */
pr ot ect ed f i nal s hor t PLAYER_MOTOR_MOTOR_POWER_REQ = 2;
pr ot ect ed f i nal s hor t PLAYER_MOTOR_VELOCI TY_MODE_REQ = 3;
pr ot ect ed f i nal s hor t PLAYER_MOTOR_RESET_ODOM_REQ = 4;
pr ot ect ed f i nal s hor t PLAYER_MOTOR_POSI TI ON_MODE_REQ = 5;
pr ot ect ed f i nal s hor t PLAYER_MOTOR_SPEED_PI D_REQ = 6;
pr ot ect ed f i nal s hor t PLAYER_MOTOR_POSI TI ON_PI D_REQ = 7;
Anexe
pagina L
pr ot ect ed f i nal s hor t PLAYER_MOTOR_SPEED_PROF_REQ = 8;
pr ot ect ed f i nal s hor t PLAYER_MOTOR_SET_ODOM_REQ = 9;
pr ot ect ed f i nal s hor t PLAYER_MOTOR_SET_GEAR_REDUCTI ON_REQ = 10;
pr ot ect ed f i nal s hor t PLAYER_MOTOR_SET_TI CS_REQ = 11;

pr i v at e i nt t het a; / * Thet a i n mr ad ( 1 mi l l i r adi an = ~. 06 degr ees) */
pr i v at e i nt t het aSpeed; / * Angul ar vel oci t y i n mr ad/ sec */
pr i v at e by t e s t al l ; / * Ar e t he mot or s st al l ed? */

/ **
* Const r uct or f or Mot or I nt er f ace.
*/
publ i c Mot or I nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_MOTOR_CODE;
}

/ **
* The mot or i nt er f ace r et ur ns dat a r egar di ng t he posi t i on and vel oci t y
* of t he mot or , as wel l as st al l i nf or mat i on.
*/
t r y {
r eadHeader ( ) ;
t het a = i s . r eadI nt ( ) ;
t het aSpeed = i s . r eadI nt ( ) ;
s t al l = i s . r eadBy t e ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Mot or ] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng( ) ) ;
}
}

/ **
* Get t het a i n mr ad ( 1 mi l l i r adi an = ~. 06 degr ees) .
* @r et ur n t het a i n mr ad as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Thet a ( ) { r et ur n t het a; }

/ **
* Get angul ar vel oci t y i n mr ad/ sec.
* @r et ur n angul ar vel oci t y i n mr ad/ sec as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Thet aSpeed ( ) { r et ur n t het aSpeed; }

/ **
* Get mot or s st at us.
* @r et ur n st al l ed or not ?
*/
publ i c s y nchr oni z ed i nt get St al l ( ) { r et ur n s t al l ; }

/ **
* The mot or i nt er f ace accept s new posi t i ons and/ or vel oci t i es f or t he mot or s
* ( dr i ver s may suppor t posi t i on cont r ol , speed cont r ol , or bot h) .
* 
* See t he pl ayer _mot or _cmd st r uct ur e f r ompl ayer . h
* @par amt het aP t het a i n mr ad
* @par amt het aS angul ar vel oci t i es, i n mr ad/ sec
* @par amst at e mot or st at e ( zer o i s ei t her of f or l ocked, dependi ng on t he dr i ver )
* @par amt ype command t ype; 0 = vel oci t y, 1 = posi t i on
*/
publ i c v oi d s et Mot or ( i nt t het aP, i nt t het aS,
by t e s t at e, by t e t y pe) {
t r y {
os . wr i t eI nt ( t het aP) ;
os . wr i t eI nt ( t het aS) ;
os . wr i t eBy t e ( s t at e) ;
os . wr i t eBy t e ( t y pe) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Mot or ] : Coul dn' t s end pos i t i on commands : " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Change posi t i on cont r ol .
* @par ammode 0 f or vel oci t y mode, 1 f or posi t i on mode
*/
publ i c v oi d changePos i t i onCont r ol ( by t e mode) {
t r y {
os . wr i t eBy t e ( PLAYER_MOTOR_POSI TI ON_MODE_REQ) ;
Anexe
pagina LI
os . wr i t eBy t e ( mode) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Mot or ] : Coul dn' t s end " +
"PLAYER_MOTOR_POSI TI ON_MODE_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Change vel oci t y cont r ol mode.
* 
* Some mot or s of f er di f f er ent vel oci t y cont r ol modes. I t can be changed by sendi ng
* a r equest wi t h t he f or mat gi ven bel ow, i ncl udi ng t he appr opr i at e mode. No mat t er
* whi ch mode i s used, t he ext er nal cl i ent i nt er f ace t o t he mot or devi ce r emai ns
* t he same.
* @par ammode dr i ver - speci f i c
*/
publ i c v oi d s et Vel oci t y Cont r ol ( by t e mode) {
t r y {
os . wr i t eBy t e ( PLAYER_MOTOR_VELOCI TY_MODE_REQ) ;
os . f l us h ( ) ;
"PLAYER_MOTOR_VELOCI TY_MODE_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Reset odomet r y.
* 
* Reset s t he mot or ' s odomet r y t o t het a = 0.
*/
publ i c v oi d r es et Odomet r y ( ) {
t r y {
os . wr i t eBy t e ( PLAYER_MOTOR_RESET_ODOM_REQ) ;
os . f l us h ( ) ;
"PLAYER_MOTOR_RESET_ODOM_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Set odomet r y.
* @par amt het a t het a i n mr ad
*/
publ i c v oi d s et Odomet r y ( i nt x T, i nt y T, i nt t het a) {
t r y {
os . wr i t eBy t e ( PLAYER_MOTOR_SET_ODOM_REQ) ;
os . wr i t eI nt ( t het a) ;
os . f l us h ( ) ;
"PLAYER_MOTOR_SET_ODOM_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Set vel oci t y PI D par amet er s.
* @par amkp P par amet er
* @par amki I par amet er
* @par amkd D par amet er
*/
publ i c v oi d s et Vel oci t y PI DPar ams ( i nt kp, i nt ki , i nt kd) {
t r y {
os . wr i t eBy t e ( PLAYER_MOTOR_SPEED_PI D_REQ) ;
os . wr i t eI nt ( kp) ;
os . wr i t eI nt ( ki ) ;
os . wr i t eI nt ( kd) ;
os . f l us h ( ) ;
"PLAYER_MOTOR_SPEED_PI D_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Set mot or PI D par amet er s.
Anexe
pagina LII
*/
publ i c v oi d s et Mot or PI DPar ams ( i nt kp, i nt ki , i nt kd) {
t r y {
os . wr i t eBy t e ( PLAYER_MOTOR_POSI TI ON_PI D_REQ) ;
os . f l us h ( ) ;
"PLAYER_MOTOR_POSI TI ON_PI D_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Set speed pr of i l e par amet er s. 
* Thi s i s usef ul l i n posi t i on cont r ol mode when you want t o r amp your
* accel er at i on and deaccel er at i on.
* @par amsp max speed
* @par amacc max accel er at i on
*/
publ i c v oi d s et SpeedPr of i l ePar ams ( i nt s p, i nt acc) {
t r y {
os . wr i t eBy t e ( PLAYER_MOTOR_SPEED_PROF_REQ) ;
os . wr i t eI nt ( s p) ;
os . wr i t eI nt ( acc) ;
os . f l us h ( ) ;
"PLAYER_MOTOR_SPEED_PROF_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Mot or power .
* 
* On some r obot s, t he mot or power can be t ur ned on and of f f r omsof t war e.
* 
* Be VERY car ef ul wi t h t hi s command! You ar e ver y l i kel y t o st ar t t he r obot
* r unni ng acr oss t he r oomat hi gh speed wi t h t he bat t er y char ger st i l l at t ached.
* @par amst at e 0 f or of f , 1 f or on
*/
publ i c v oi d s et Mot or Power ( i nt s t at e) {
t r y {
os . wr i t eBy t e ( PLAYER_MOTOR_MOTOR_POWER_REQ) ;
os . f l us h ( ) ;
"PLAYER_MOTOR_MOTOR_POWER_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Handl e Negat i ve Acknowl edgement Response messages.
*/
t r y {
Sy s t em. er r . pr i nt l n ( "[ Mot or ] : Handl i ng NAR of s i z e = " + s i z e) ;
Sy s t em. er r . pr i nt l n ( "[ Mot or ] : handl eRes pons ePos i t i on ERROR " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Handl e Er r or Acknowl edgement Response messages.
*/
publ i c v oi d handl eEARMes s age ( ) {
t r y {
Sy s t em. er r . pr i nt l n ( "[ Mot or ] : Handl i ng EAR of s i z e = " + s i z e) ;
Sy s t em. er r . pr i nt l n ( "[ Mot or ] : handl eRes pons ePos i t i on ERROR " + e. t oSt r i ng ( ) ) ;
}
}
}

Anexe
pagina LIII

j ja av va ac cl li ie en nt t/ /N No om ma ad dI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - NomadI nt er f ace. j ava
*
*
*
*
*
* $I d: NomadI nt er f ace. j ava, v 1. 2 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/

/ **
* The nomad i nt er f ace af f or ds cont r ol of Nomadi cs Nomad r obot s and r el at i ves. 
* War ni ng: some of t he command and dat a var i abl es ar e speci f i ed i n nat i ve Nomad uni t s; t hey
* don' t f ol l ow Pl ayer ' s convent i ons. Thi s wi l l be changed once someone f i gur es out exact l y
* what t he Nomad uni t s ar e.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s NomadI nt er f ace ex t ends Pl ay er Dev i ce {

/ * nomad r obot */
pr i v at e f i nal s hor t PLAYER_NOMAD_CODE = Pl ay er Cl i ent . PLAYER_NOMAD_CODE;


publ i c s t at i c f i nal s hor t PLAYER_NOMAD_SONAR_COUNT = 16;
publ i c s t at i c f i nal s hor t PLAYER_NOMAD_BUMPER_COUNT = 16;
publ i c s t at i c f i nal s hor t PLAYER_NOMAD_I R_COUNT = 16;

pr i v at e i nt x , y ; / * X and Y posi t i on, i n mm*/
pr i v at e i nt a; / * headi ng, i n degr ees */
pr i v at e i nt v el _t r ans ; / * t r ansl at i on vel oci t y ( i n nat i ve Nomad uni t s) */
pr i v at e i nt v el _s t eer ; / * st eer i ng vel oci t y ( i n nat i ve Nomad uni t s) */
pr i v at e i nt v el _t ur r et ; / * t ur r et r ot at i on vel oci t y ( i n nat i ve Nomad uni t s) */
/ * sonar r ange sensor s: r ange i n mm*/
pr i v at e i nt [ ] s onar = new i nt [ PLAYER_NOMAD_SONAR_COUNT] ;
/ * i nf r ar ed r ange sensor s: r ange i n mm*/
pr i v at e i nt [ ] i r = new i nt [ PLAYER_NOMAD_I R_COUNT] ;
/ * bump sensor s: zer o - no cont act , non- zer o - cont act */
pr i v at e i nt [ ] bumper = new i nt [ PLAYER_NOMAD_BUMPER_COUNT] ;

/ **
* Const r uct or f or NomadI nt er f ace.
*/
publ i c NomadI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_NOMAD_CODE;
}

/ **
* Read t he NOMAD dat a. 
* The nomad dat a packet pr ovi des a pose est i mat e, sonar and i nf r ar ed r ange
* r eadi ngs, and bumper cont act r eadi ngs.
*/
r eadHeader ( ) ;
t r y {
x = i s . r eadI nt ( ) ; / * X and Y posi t i on, i n mm*/
Anexe
pagina LIV
y = i s . r eadI nt ( ) ;
a = i s . r eadI nt ( ) ; / * headi ng, i n degr ees */
v el _t r ans = i s . r eadI nt ( ) ; / * t r ansl at i on vel oci t y ( i n nat i ve Nomad uni t s) */
v el _s t eer = i s . r eadI nt ( ) ; / * st eer i ng vel oci t y ( i n nat i ve Nomad uni t s) */
v el _t ur r et = i s . r eadI nt ( ) ; / * t ur r et r ot at i on vel oci t y ( i n nat i ve Nomad uni t s) */
f or ( i nt i = 0; i < PLAYER_NOMAD_SONAR_COUNT; i ++)
s onar [ i ] = i s . r eadUns i gnedShor t ( ) ; / * sonar r ange sensor s: r ange i n mm*/
f or ( i nt i = 0; i < PLAYER_NOMAD_I R_COUNT; i ++)
i r [ i ] = i s . r eadUns i gnedShor t ( ) ; / * i nf r ar ed r ange sensor s: r ange i n mm*/
f or ( i nt i = 0; i < PLAYER_NOMAD_BUMPER_COUNT; i ++)
bumper [ i ] = i s . r eadUns i gnedShor t ( ) ; / * bump sensor s: 0 - no cont act */
Sy s t em. er r . pr i nt l n ( "[ Nomad] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Ret ur n t he X posi t i on i n mm.
* @r et ur n t he X posi t i on i n mmas an i nt eger
*/
publ i c s y nchr oni z ed i nt get X ( ) { r et ur n t hi s . x ; }

/ **
* Ret ur n t he Y posi t i on i n mm.
* @r et ur n t he Y posi t i on i n mmas an i nt eger
*/
publ i c s y nchr oni z ed i nt get Y ( ) { r et ur n t hi s . y ; }

/ **
* Ret ur n t he headi ng i n degr ees.
* @r et ur n t he headi ng i n degr ees as an i nt eger
*/
publ i c s y nchr oni z ed i nt get A ( ) { r et ur n t hi s . a; }

/ **
* Ret ur n t he t r ansl at i on vel oci t y ( i n nat i ve Nomad uni t s) .
* @r et ur n t he t r ansl at i on vel oci t y ( i n nat i ve Nomad uni t s) as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Vel Tr ans ( ) { r et ur n t hi s . v el _t r ans ; }

/ **
* Ret ur n t he st eer i ng vel oci t y ( i n nat i ve Nomad uni t s) .
* @r et ur n t he st eer i ng vel oci t y ( i n nat i ve Nomad uni t s) as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Vel St eer ( ) { r et ur n t hi s . v el _s t eer ; }

/ **
* Ret ur n t he t ur r et r ot at i on vel oci t y ( i n nat i ve Nomad uni t s)
* @r et ur n t he t ur r et r ot at i on vel oci t y ( i n nat i ve Nomad uni t s) as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Vel Tur r et ( ) { r et ur n t hi s . v el _t ur r et ; }

/ **
* Ret ur n t he sonar r ange sensor s r ange i n mm.
* @r et ur n t he sonar r ange sensor s r ange i n mmas an ar r ay of i nt eger s
*/
publ i c s y nchr oni z ed i nt [ ] get Sonar s ( ) { r et ur n t hi s . s onar ; }

/ **
* Ret ur n t he i nf r ar ed r ange sensor s r ange i n mm.
* @r et ur n t he i nf r ar ed r ange sensor s r ange i n mmas an ar r ay of i nt eger s
*/
publ i c s y nchr oni z ed i nt [ ] get I Rs ( ) { r et ur n t hi s . i r ; }

/ **
* Ret ur n t he bump sensor s st at us.
* @r et ur n t he bump sensor s st at us ( zer o - no cont act , non- zer o - cont act ) as
* an ar r ay of i nt eger s
*/
publ i c s y nchr oni z ed i nt [ ] get Bumper s ( ) { r et ur n t hi s . bumper ; }

/ **
* The Nomad command packet l et s you set i ndependent vel oci t i es f or t r ansl at i on,
* st eer i ng and t ur r et r ot at i on. These ar e speci f i ed i n nat i ve Nomad uni t s. 
* Todo: 
* Thi s shoul d change i n f ut ur e t o mat ch nor mal Pl ayer st yl e ( mm/ sec) , once someone
* f i gur es out exact l y what t he Nomad uni t s ar e.
* @par amnewvel t r ans t r ansl at i on vel oci t y ( i n nat i ve Nomad uni t s)
* @par amnewvel st eer st eer i ng vel oci t y ( i n nat i ve Nomad uni t s)
* @par amnewvel t ur r et t ur r et r ot at i on vel oci t y ( i n nat i ve Nomad uni t s)
*/
publ i c v oi d s et Vel oci t i es ( i nt newv el t r ans , i nt newv el s t eer , i nt newv el t ur r et ) {
t r y {
os . wr i t eI nt ( newv el t r ans ) ; / * t r ansl at i on vel oci t y */
Anexe
pagina LV
os . wr i t eI nt ( newv el s t eer ) ; / * st eer i ng vel oci t y */
os . wr i t eI nt ( newv el t ur r et ) ; / * t ur r et r ot at i on vel oci t y */
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Nomad] : Coul dn' t s end s et new goal s command: " +
e. t oSt r i ng ( ) ) ;
}
}

}

j ja av va ac cl li ie en nt t/ /P Pl la an nn ne er rI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl anner I nt er f ace. j ava
*
*
*
*
*
* $I d: Pl anner I nt er f ace. j ava, v 1. 2 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/

i mpor t j av acl i ent . s t r uct ur es . Pl ay er Pl anner Way poi nt T;

/ **
* The pl anner i nt er f ace pr ovi des cont r ol of a 2- D mot i on pl anner .
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pl anner I nt er f ace ex t ends Pl ay er Dev i ce {


/ * 2D mot i on pl anner */
pr i v at e f i nal s hor t PLAYER_PLANNER_CODE = Pl ay er Cl i ent . PLAYER_PLANNER_CODE;


pr ot ect ed s t at i c f i nal s hor t PLAYER_PLANNER_GET_WAYPOI NTS_REQ = 10;
pr ot ect ed s t at i c f i nal s hor t PLAYER_PLANNER_ENABLE_REQ = 11;
/ ** t he maxi mumnumber of waypoi nt s i n a si ngl e pl an */
publ i c s t at i c f i nal s hor t PLAYER_PLANNER_MAX_WAYPOI NTS = 128;

/ * obj ect cont ai ni ng t he pl ayer _pl anner _waypoi nt s i n case of a t hr eaded cal l */
pr i v at e Pl ay er Pl anner Way poi nt T ppwt [ ] = new
Pl ay er Pl anner Way poi nt T[ PLAYER_PLANNER_MAX_WAYPOI NTS] ;
pr i v at e i nt way poi nt s _count = PLAYER_PLANNER_MAX_WAYPOI NTS;
pr i v at e bool ean r eady PPWT = f al s e;

pr i v at e s hor t v al i d; / * Di d t he pl anner f i nd a val i d pat h? */
pr i v at e s hor t done; / * Have we ar r i ved at t he goal ? */
pr i v at e i nt px , py , pa; / * Cur r ent l ocat i on ( mm, mm, deg) */
pr i v at e i nt gx , gy , ga; / * Goal l ocat i on ( mm, mm, deg) */
pr i v at e i nt wx , wy , wa; / * Cur r ent waypoi nt l ocat i on ( mm, mm, deg) */
pr i v at e s hor t cur r _way poi nt ; / * Cur r ent waypoi nt i ndex ( handy i f you al r eady have t he l i st
of waypoi nt s) . May be negat i ve i f t her e' s no pl an, or i f
t he pl an i s done */
pr i v at e i nt way poi nt Pl an_count ; / * Number of waypoi nt s i n t he pl an */

/ **
* Const r uct or f or Pl anner I nt er f ace.
Anexe
pagina LVI
*/
publ i c Pl anner I nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_PLANNER_CODE;
}

/ **
* Read t he pl anner dat a.
*/
r eadHeader ( ) ;
t r y {
v al i d = ( s hor t ) i s . r eadUns i gnedBy t e( ) ; / * Di d t he pl anner f i nd a val i d pat h? */
done = ( s hor t ) i s . r eadUns i gnedBy t e ( ) ; / * Have we ar r i ved at t he goal ? */
px = i s . r eadI nt ( ) ; / * Cur r ent l ocat i on ( mm, mm, deg) */
py = i s . r eadI nt ( ) ;
pa = i s . r eadI nt ( ) ;
gx = i s . r eadI nt ( ) ; / * Goal l ocat i on ( mm, mm, deg) */
gy = i s . r eadI nt ( ) ;
ga = i s . r eadI nt ( ) ;
wx = i s . r eadI nt ( ) ; / * Cur r ent waypoi nt l ocat i on ( mm, mm, deg) */
wy = i s . r eadI nt ( ) ;
wa = i s . r eadI nt ( ) ;
cur r _way poi nt = i s . r eadShor t ( ) ; / * Cur r ent waypoi nt i ndex */
way poi nt Pl an_count = ( i nt ) i s . r eadUns i gnedShor t ( ) ; / * Number of waypoi nt s i n t he pl an */
Sy s t em. er r . pr i nt l n ( "[ Pl anner ] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Di d t he pl anner f i nd a val i d pat h?
* @r et ur n 1 i f t he pl anner f ound a val i d pat h, 0 ot her wi se
*/
publ i c s y nchr oni z ed s hor t get Val i d ( ) { r et ur n t hi s . v al i d; }

/ **
* Have we ar r i ved at t he goal ?
* @r et ur n 1 i f t he goal was r eached, 0 ot her wi se
*/
publ i c s y nchr oni z ed s hor t get Done ( ) { r et ur n t hi s . done; }

/ **
* Get t he cur r ent l ocat i on ( X - mm)
* @r et ur n t he cur r ent X l ocat i on i n mm
*/
publ i c s y nchr oni z ed i nt get Px ( ) { r et ur n t hi s . px ; }

/ **
* Get t he cur r ent l ocat i on ( Y - mm)
* @r et ur n t he cur r ent Y l ocat i on i n mm
*/
publ i c s y nchr oni z ed i nt get Py ( ) { r et ur n t hi s . py ; }

/ **
* Get t he cur r ent l ocat i on ( A - deg)
* @r et ur n t he cur r ent A l ocat i on i n deg
*/
publ i c s y nchr oni z ed i nt get Pa ( ) { r et ur n t hi s . pa; }

/ **
* Get t he goal l ocat i on ( X - mm)
* @r et ur n t he goal X l ocat i on i n mm
*/
publ i c s y nchr oni z ed i nt get Gx ( ) { r et ur n t hi s . gx ; }

/ **
* Get t he goal l ocat i on ( Y - mm)
* @r et ur n t he goal Y l ocat i on i n mm
*/
publ i c s y nchr oni z ed i nt get Gy ( ) { r et ur n t hi s . gy ; }

/ **
* Get t he goal l ocat i on ( A - deg)
* @r et ur n t he goal A l ocat i on i n deg
*/
publ i c s y nchr oni z ed i nt get Ga ( ) { r et ur n t hi s . ga; }

/ **
* Get t he cur r ent waypoi nt l ocat i on ( X - mm)
* @r et ur n t he cur r ent waypoi nt X l ocat i on i n mm
*/
Anexe
pagina LVII
publ i c s y nchr oni z ed i nt get Wx ( ) { r et ur n t hi s . wx ; }

/ **
* Get t he waypoi nt cur r ent l ocat i on ( Y - mm)
* @r et ur n t he cur r ent waypoi nt Y l ocat i on i n mm
*/
publ i c s y nchr oni z ed i nt get Wy ( ) { r et ur n t hi s . wy ; }

/ **
* Get t he cur r ent waypoi nt l ocat i on ( A - deg)
* @r et ur n t he cur r ent waypoi nt A l ocat i on i n deg
*/
publ i c s y nchr oni z ed i nt get Wa ( ) { r et ur n t hi s . wa; }

/ **
* Get t he cur r ent waypoi nt i ndex
* @r et ur n t he cur r ent waypoi nt i ndex as a shor t
*/
publ i c s y nchr oni z ed s hor t get Cur Way poi nt ( ) { r et ur n t hi s . cur r _way poi nt ; }

/ **
* Get t he number of waypoi nt s i n t he pl an
* @r et ur n t he number of waypoi nt s i n t he pl an as an i nt
*/
publ i c s y nchr oni z ed i nt get Way poi nt Pl anCount ( ) { r et ur n t hi s . way poi nt Pl an_count ; }

/ **
* Sends a new goal t o t he pl anner i nt er f ace.
* @par amnewgx goal l ocat i on ( X - mm)
* @par amnewgy goal l ocat i on ( Y - mm)
* @par amnewga goal l ocat i on ( A - deg)
*/
publ i c v oi d s et Goal ( i nt newgx , i nt newgy , i nt newga) {
t r y {
os . wr i t eI nt ( newgx ) ; / * Goal l ocat i on ( mm, mm, deg) */
os . wr i t eI nt ( newgy ) ;
os . wr i t eI nt ( newga) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pl anner ] : Coul dn' t s end s et new goal s command: " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Get waypoi nt s.
* 
* See t he pl ayer _pl anner _waypoi nt s_r eq st r uct ur e f r ompl ayer . h
*/
publ i c v oi d get Way poi nt s ( ) {
t r y {
os . wr i t eBy t e ( PLAYER_PLANNER_GET_WAYPOI NTS_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pl anner ] : Coul dn' t s end PLAYER_PLANNER_GET_WAYPOI NTS_REQ " +
}
}

/ **
* Conf i gur at i on r equest : Enabl e/ di sabl e r obot mot i on.
* 
* See t he pl ayer _pl anner _enabl e_r eq st r uct ur e f r ompl ayer . h
* @par amst at e 1 t o enabl e, 0 t o di sabl e
*/
publ i c v oi d s et Robot Mot i on ( by t e s t at e) {
t r y {
os . wr i t eBy t e ( PLAYER_PLANNER_ENABLE_REQ) ;
os . wr i t eBy t e ( s t at e) ; / * 1 t o enabl e, 0 t o di sabl e */
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pl anner ] : Coul dn' t s end PLAYER_PLANNER_ENABLE_REQ command: " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
*/
i f ( s i z e == 0) {
Anexe
pagina LVIII
Sy s t em. er r . pr i nt l n ( "[ Pl anner ] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
cas e PLAYER_PLANNER_GET_WAYPOI NTS_REQ: {
way poi nt s _count = i s . r eadUns i gnedShor t ( ) ; / * number of waypoi nt s t o f ol l ow */
f or ( i nt i = 0; i < way poi nt s _count ; i ++) {
ppwt [ i ] . s et X ( i s . r eadI nt ( ) ) ; / * waypoi nt l ocat i on ( mm, mm, deg) */
ppwt [ i ] . s et Y ( i s . r eadI nt ( ) ) ;
ppwt [ i ] . s et A ( i s . r eadI nt ( ) ) ;
}

r eady PPWT = t r ue;
br eak;
}
cas e PLAYER_PLANNER_ENABLE_REQ: {
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ Pl anner ] : Unex pect ed r es pons e " + s ubt y pe +
" of s i z e = " + s i z e) ;
br eak;
}
}
Sy s t em. er r . pr i nt l n ( "[ Pl anner ] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Get t he number of waypoi nt s t o f ol l ow
* @r et ur n number of waypoi nt s t o f ol l ow as an i nt
*/
publ i c s y nchr oni z ed i nt get Way poi nt Count ( ) { r et ur n t hi s . way poi nt s _count ; }

/ **
* Get a speci f i ed waypoi nt f r oman ar r ay of waypoi nt s.
* @r et ur n an obj ect of t ype Pl ayer Pl anner Waypoi nt T cont ai ni ng t he r equest ed dat a
*/
publ i c s y nchr oni z ed Pl ay er Pl anner Way poi nt T get PPWT ( i nt i ) { r et ur n ppwt [ i ] ; }

/ **
* Get t he waypoi nt s.
* @r et ur n an ar r ay of obj ect s of t ype Pl ayer Pl anner Waypoi nt T cont ai ni ng t he r equest ed dat a
*/
publ i c s y nchr oni z ed Pl ay er Pl anner Way poi nt T[ ] get Al l PPWT ( ) { r et ur n ppwt ; }

/ **
* Check i f waypoi nt dat a i s avai l abl e.
*/
publ i c s y nchr oni z ed bool ean i s Ready PPWT ( ) {
i f ( r eady PPWT) {
r eady PPWT = f al s e;
r et ur n t r ue;
}
r et ur n f al s e;
}

}

j ja av va ac cl li ie en nt t/ /P Pl la ay ye er rC Cl li ie en nt t. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl ayer Cl i ent . j ava
*
*
*
*
Anexe
pagina LIX
*
* $I d: Pl ayer Cl i ent . j ava, v 1. 9 2005/ 06/ 01 19: 05: 48 veedee Exp $
*
*/

i mpor t j av a. net . Socket ;
i mpor t j av a. net . Socket Ex cept i on;
i mpor t j av a. i o. Dat aI nput St r eam;
i mpor t j av a. i o. Buf f er edOut put St r eam;
i mpor t j av a. i o. Dat aOut put St r eam;
i mpor t j av a. i o. EOFEx cept i on;
i mpor t j av acl i ent . s t r uct ur es . Pl ay er Dev i ceDr i v er I nf o;
i mpor t j av acl i ent . s t r uct ur es . Pl ay er Dev i ceDev l i s t T;
i mpor t j av acl i ent . s t r uct ur es . Pl ay er Dev i ceI dT;

/ **
* The Pl ayer Cl i ent i s t he mai n J avacl i ent cl ass. I t cont ai ns met hods f or i nt er act i ng wi t h t he
* pl ayer devi ce. The pl ayer devi ce r epr esent s t he ser ver i t sel f , and i s used i n conf i gur i ng
* t he behavi or of t he ser ver . Ther e i s onl y one such devi ce ( wi t h i ndex 0) and i t i s al ways
* open.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pl ay er Cl i ent ex t ends Thr ead {

pr ot ect ed s t at i c f i nal bool ean i s Debuggi ng =
( Sy s t em. get Pr oper t y ( "Pl ay er Cl i ent . debug") ! = nul l ) ? t r ue : f al s e;

pr i v at e s t at i c f i nal bool ean s t opOnEOFEx cept i on =
( Sy s t em. get Pr oper t y ( "Pl ay er Cl i ent . s t opOnEOFEx cept i on") ! = nul l ) ? f al s e : t r ue;

/ * t he message st ar t si gni f i er */
pr ot ect ed s t at i c f i nal s hor t PLAYER_STXX = 0x 5878;
/ * st r i ng t o mat ch t he cur r ent l y assi gned devi ces */
pr ot ect ed s t at i c f i nal s hor t PLAYER_MAX_DEVI CE_STRI NG_LEN = 64;

/ * t he r equest subt ypes ( see pl ayer . h) */
pr ot ect ed s t at i c f i nal s hor t PLAYER_PLAYER_DEVLI ST_REQ = 1;
pr ot ect ed s t at i c f i nal s hor t PLAYER_PLAYER_DRI VERI NFO_REQ = 2;
pr ot ect ed s t at i c f i nal s hor t PLAYER_PLAYER_DEV_REQ = 3;
pr ot ect ed s t at i c f i nal s hor t PLAYER_PLAYER_DATA_REQ = 4;
pr ot ect ed s t at i c f i nal s hor t PLAYER_PLAYER_DATAMODE_REQ = 5;
pr ot ect ed s t at i c f i nal s hor t PLAYER_PLAYER_DATAFREQ_REQ = 6;
pr ot ect ed s t at i c f i nal s hor t PLAYER_PLAYER_AUTH_REQ = 7;
pr ot ect ed s t at i c f i nal s hor t PLAYER_PLAYER_NAMESERVI CE_REQ = 8;

pr ot ect ed s t at i c f i nal s hor t PLAYER_MSGTYPE_DATA = 1;
pr ot ect ed s t at i c f i nal s hor t PLAYER_MSGTYPE_CMD = 2;
pr ot ect ed s t at i c f i nal s hor t PLAYER_MSGTYPE_REQ = 3;
pr ot ect ed s t at i c f i nal s hor t PLAYER_MSGTYPE_RESP_ACK = 4;
pr ot ect ed s t at i c f i nal s hor t PLAYER_MSGTYPE_SYNCH = 5;
pr ot ect ed s t at i c f i nal s hor t PLAYER_MSGTYPE_RESP_NACK = 6;
pr ot ect ed s t at i c f i nal s hor t PLAYER_MSGTYPE_RESP_ERR = 7;

/ * t he cur r ent assi gned i nt er f ace codes f or Pl ayer 1. 6. 2 */
pr ot ect ed s t at i c f i nal s hor t PLAYER_NULL_CODE = 256; // /dev /nul l anal ogue
pr ot ect ed s t at i c f i nal s hor t PLAYER_PLAYER_CODE = 1; // t he s er v er i t s el f
pr ot ect ed s t at i c f i nal s hor t PLAYER_POWER_CODE = 2; // power s ubs y s t em
pr ot ect ed s t at i c f i nal s hor t PLAYER_GRI PPER_CODE = 3; // gr i pper
pr ot ect ed s t at i c f i nal s hor t PLAYER_POSI TI ON_CODE = 4; // dev i ce t hat mov es
pr ot ect ed s t at i c f i nal s hor t PLAYER_SONAR_CODE = 5; // f i x ed r ange- f i nder
pr ot ect ed s t at i c f i nal s hor t PLAYER_LASER_CODE = 6; // s canni ng r ange- f i nder
pr ot ect ed s t at i c f i nal s hor t PLAYER_BLOBFI NDER_CODE = 7; // v i s ual bl obf i nder
pr ot ect ed s t at i c f i nal s hor t PLAYER_PTZ_CODE = 8; // pan- t i l t - z oom uni t
pr ot ect ed s t at i c f i nal s hor t PLAYER_AUDI O_CODE = 9; // audi o I /O
pr ot ect ed s t at i c f i nal s hor t PLAYER_FI DUCI AL_CODE = 10; // f i duci al det ect or
pr ot ect ed s t at i c f i nal s hor t PLAYER_SPEECH_CODE = 12; // s peech I /O
pr ot ect ed s t at i c f i nal s hor t PLAYER_GPS_CODE = 13; // GPS uni t
pr ot ect ed s t at i c f i nal s hor t PLAYER_BUMPER_CODE = 14; // bumper ar r ay
pr ot ect ed s t at i c f i nal s hor t PLAYER_TRUTH_CODE = 15; // gr ound- t r ut h ( St age)
pr ot ect ed s t at i c f i nal s hor t PLAYER_I DARTURRET_CODE = 16; // r angi ng + comms
pr ot ect ed s t at i c f i nal s hor t PLAYER_I DAR_CODE = 17; // r angi ng + comms
pr ot ect ed s t at i c f i nal s hor t PLAYER_DESCARTES_CODE = 18; // Des car t es pl at f or m
pr ot ect ed s t at i c f i nal s hor t PLAYER_DI O_CODE = 20; // di gi t al I /O
Anexe
pagina LX
pr ot ect ed s t at i c f i nal s hor t PLAYER_AI O_CODE = 21; // anal og I /O
pr ot ect ed s t at i c f i nal s hor t PLAYER_I R_CODE = 22; // I R ar r ay
pr ot ect ed s t at i c f i nal s hor t PLAYER_WI FI _CODE = 23; // wi f i car d s t at us
pr ot ect ed s t at i c f i nal s hor t PLAYER_WAVEFORM_CODE = 24; // f et ch r aw wav ef or ms
pr ot ect ed s t at i c f i nal s hor t PLAYER_LOCALI ZE_CODE = 25; // l ocal i z at i on
pr ot ect ed s t at i c f i nal s hor t PLAYER_MCOM_CODE = 26; // mul t i coms
pr ot ect ed s t at i c f i nal s hor t PLAYER_SOUND_CODE = 27; // s ound f i l e pl ay back
pr ot ect ed s t at i c f i nal s hor t PLAYER_AUDI ODSP_CODE = 28; // audi o ds p I /O
pr ot ect ed s t at i c f i nal s hor t PLAYER_AUDI OMI XER_CODE = 29; // audi o I /O
pr ot ect ed s t at i c f i nal s hor t PLAYER_POSI TI ON3D_CODE = 30; // 3- D pos i t i on
pr ot ect ed s t at i c f i nal s hor t PLAYER_SI MULATI ON_CODE = 31; // s i mul at or s
pr ot ect ed s t at i c f i nal s hor t PLAYER_SERVI CE_ADV_CODE = 32; // LAN adv er t i s ement
pr ot ect ed s t at i c f i nal s hor t PLAYER_BLI NKENLI GHT_CODE = 33; // bl i nki ng l i ght s
pr ot ect ed s t at i c f i nal s hor t PLAYER_NOMAD_CODE = 34; // Nomad r obot
pr ot ect ed s t at i c f i nal s hor t PLAYER_CAMERA_CODE = 40; // camer a dev i ce( gaz ebo)
pr ot ect ed s t at i c f i nal s hor t PLAYER_MAP_CODE = 42; // get a map
pr ot ect ed s t at i c f i nal s hor t PLAYER_PLANNER_CODE = 44; // 2D mot i on pl anner
pr ot ect ed s t at i c f i nal s hor t PLAYER_LOG_CODE = 45; // l og R/Wcont r ol
pr ot ect ed s t at i c f i nal s hor t PLAYER_ENERGY_CODE = 46; // ener gy char gi ng
pr ot ect ed s t at i c f i nal s hor t PLAYER_MOTOR_CODE = 47; // mot or i nt er f ace
pr ot ect ed s t at i c f i nal s hor t PLAYER_POSI TI ON2D_CODE = 48; // 2- D pos i t i on
pr ot ect ed s t at i c f i nal s hor t PLAYER_JOYSTI CK_CODE = 49; // Joy s t i ck
pr ot ect ed s t at i c f i nal s hor t PLAYER_SPEECH_RECOGNI TI ON_CODE = 50; // s peech r ecogni t i onI /O
pr ot ect ed s t at i c f i nal s hor t PLAYER_OPAQUE_CODE = 51; // pl ugi n i nt er f ace

/ * t he devi ce access modes */
publ i c s t at i c f i nal s hor t PLAYER_READ_MODE = 114; // ' r '
publ i c s t at i c f i nal s hor t PLAYER_WRI TE_MODE = 119; // ' w'
publ i c s t at i c f i nal s hor t PLAYER_ALL_MODE = 97; // ' a'
publ i c s t at i c f i nal s hor t PLAYER_CLOSE_MODE = 99; // ' c'
publ i c s t at i c f i nal s hor t PLAYER_ERROR_MODE = 101; // ' e'

/ * t ot al number of assi gned i nt er f ace codes ( see pl ayer . h) */
pr ot ect ed s t at i c i nt N_DEVI CES = 52;
/ * maxi mumdevi ces of t he same t ype */
pr ot ect ed s t at i c i nt MAX_DEVI CES = 5;

/ * maxi mumsi ze f or r equest / r epl y.
* t hi s i s a conveni ence so t hat t he Pl ayer Queue can used f i xed si ze el ement s.
*/
pr ot ect ed s t at i c f i nal s hor t PLAYER_MAX_REQREP_SI ZE = 4096; / * 4KB */

/ **
* Dat a del i ver y mode: Send dat a at a f i xed r at e ( def aul t 10Hz; see
* r equest Dat aDel i ver yFr equency( ) bel ow t o change t he r at e) f r omALL subscr i bed devi ces ,
* r egar dl ess of whet her t he dat a i s new or ol d. A PLAYER_MSGTYPE_SYNCH packet f ol l ows
* each set of dat a. Rar el y used.
* @see #r equest Dat aDel i ver yFr equency( shor t )
*/
publ i c s t at i c f i nal by t e PLAYER_DATAMODE_PUSH_ALL = 0;
/ **
* Dat a del i ver y mode: Onl y on r equest ( see r equest Dat a ( ) r equest bel ow) , send dat a f r om
*ALL subscr i bed devi ces, r egar dl ess of whet her t he dat a i s new or ol d. A PLAYER_MSGTYPE_SYNCH
* packet f ol l ows each set of dat a. Rar el y used.
* @see #r equest Dat a( )
*/
publ i c s t at i c f i nal by t e PLAYER_DATAMODE_PULL_ALL = 1;
/ **
* Dat a del i ver y mode: Send dat a at a f i xed r at e ( def aul t 10Hz; see
* r equest Dat aDel i ver yFr equency( ) bel ow t o change t he r at e) onl y f r omt hose subscr i bed devi ces
* t hat have pr oduced new dat a si nce t he l ast t i me dat a was pushed t o t hi s cl i ent . A
* PLAYER_MSGTYPE_SYNCH packet f ol l ows each set of dat a. Thi s i s t he def aul t mode.
*/
publ i c s t at i c f i nal by t e PLAYER_DATAMODE_PUSH_NEW = 2;
/ **
* Dat a del i ver y mode: Onl y on r equest ( see r equest Dat a ( ) r equest bel ow) , send dat a onl y
*f r omt hose subscr i bed devi ces t hat have pr oduced new dat a si nce l ast t i me dat a was pushed
* t o t hi s cl i ent . Use t hi s mode i f your cl i ent r uns sl owl y or at an upr edi ct abl e r at e ( e. g. , a
* GUI ) .
* A PLAYER_MSGTYPE_SYNCH packet f ol l ows each set of dat a.
* @see #r equest Dat a( )
*/
publ i c s t at i c f i nal by t e PLAYER_DATAMODE_PULL_NEW = 3;
/ **
* Dat a del i ver y mode: When a subscr i bed devi ce pr oduces new dat a, send i t . Thi s i s t he
* l owest - l at ency del i ver y mode; when a devi ce pr oduces dat a, t he ser ver ( al most ) i mmedi at el y
* sends i t on t he cl i ent . So t he cl i ent may r ecei ve dat a at an ar bi t r ar i l y hi gh r at e.
* PLAYER_MSGTYPE_SYNCH packet s ar e st i l l sent , but at a f i xed r at e ( see
* r equest Dat aDel i ver yFr equency ( ) t o change t hi s r at e) t hat i s unr el at ed t o r at e at whi ch
* dat a ar e del i ver ed f r omdevi ces.
*/
publ i c s t at i c f i nal by t e PLAYER_DATAMODE_PUSH_ASYNC = 4;

Anexe
pagina LXI
pr ot ect ed i nt dev i ceCnt [ ] = new i nt [ N_DEVI CES] ;
pr ot ect ed Pl ay er Dev i ce dev i ceLi s t [ ] [ ] = new Pl ay er Dev i ce[ N_DEVI CES] [ MAX_DEVI CES] ;

pr i v at e Pl ay er Dev i ceDr i v er I nf o pddi ;
pr i v at e Pl ay er Dev i ceDev l i s t T pddt ;
pr i v at e bool ean r eady PDDT = f al s e;
pr i v at e bool ean r eady PDDI = f al s e;
pr i v at e bool ean r ecei v edAut hent i cat i on = f al s e;
pr i v at e bool ean r eady Por t Number = f al s e;

pr ot ect ed Socket s ocket ;
pr ot ect ed Buf f er edOut put St r eam buf f er ;
/ **
* The i nput st r eamf or t he socket connect ed t o t he pl ayer ser ver .
*/
publ i c Dat aI nput St r eam i s ;
/ **
* The out put st r eamf or t he socket connect ed t o t he pl ayer ser ver . I t ' s buf f er ed, so r emember
* t o f l ush( ) !
*/
publ i c Dat aOut put St r eam os ;

pr i v at e i nt por t Number ;

pr i v at e l ong mi l l i s ;
pr i v at e i nt nanos ;
pr i v at e bool ean i s Thr eaded;

/ * used f or cr eat i ng Pl ayer Devi ce t ype obj ect s on r equest Devi ceAccess ( ) */
pr i v at e Pl ay er Dev i ce newpd;

pr i v at e f i nal i nt DI FFERENCE_SYNCH_FACTOR = 10;
pr i v at e i nt i ndex = 0;

/ **
* The Pl ayer Cl i ent const r uct or . Once cal l ed, i t wi l l cr eat e a socket wi t h t he Pl ayer ser ver
* r unni ng on host ser ver name on por t por t Number .
* @par amser ver Name ur l of t he host r unni ng Pl ayer
* @par ampor t Number t he por t number of t he Pl ayer ser ver
*/
publ i c Pl ay er Cl i ent ( St r i ng s er v er Name, i nt por t Number ) {
t r y {
/ * i ni t i al i ze net wor k connect i on */
s ocket = new Socket ( s er v er Name, por t Number ) ;
/ * open t he pr oper st r eams ( I / O) */
i s = new Dat aI nput St r eam ( s ocket . get I nput St r eam ( ) ) ;
buf f er = new Buf f er edOut put St r eam ( s ocket . get Out put St r eam ( ) , 128) ;
os = new Dat aOut put St r eam ( new Dat aOut put St r eam ( buf f er ) ) ;
/ * wr i t e t he pl ayer ver si on number ( manual says ver si on st r i ng i s 32 char s) */
f or ( i nt i = 0; i < 32; i ++) {
char c = ( char ) i s . r eadBy t e ( ) ;
Sy s t em. er r . pr i nt ( c) ;
}
Sy s t em. er r . pr i nt l n ( ) ;
/ * i ni t i al i ze t he devi ce l i st */
f or ( i nt i = 0; i < N_DEVI CES; i ++) {
dev i ceCnt [ i ] = 0;
f or ( i nt j = 0; j < MAX_DEVI CES; j ++)
dev i ceLi s t [ i ] [ j ] = nul l ;
}
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Er r or i n Pl ay er Cl i ent i ni t : " + e. t oSt r i ng ( ) ) ;
Sy s t em. ex i t ( 1) ;
}
}

/ **
* The Pl ayer Cl i ent " dest r uct or " . Once cal l ed, i t wi l l cl ose al l t he open st r eams/ socket s
* wi t h t he Pl ayer ser ver .
*/
publ i c v oi d cl os e ( ) {
t r y {
/ * cl ose al l socket s */
os . cl os e ( ) ;
buf f er . cl os e ( ) ;
i s . cl os e ( ) ;
s ocket . cl os e ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Er r or i n Pl ay er Cl i ent s t op: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Change t he mode J avacl i ent r uns t o non- t hr eaded.
Anexe
pagina LXII
*/
publ i c v oi d s et Not Thr eaded( ) {
i s Thr eaded = f al s e;
}

/ **
* St ar t a t hr eaded copy of J avacl i ent .
* @par ammi l l i s number of mi l i seconds t o sl eep bet ween cal l s
* @par amnanos number of nanoseconds t o sl eep bet ween cal l s
*/
publ i c v oi d r unThr eaded ( l ong mi l l i s , i nt nanos ) {
i f ( i s Thr eaded) {
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : A s econd cal l f or r unThr eaded, i gnor i ng! ") ;
r et ur n;
}
t hi s . mi l l i s = mi l l i s ;
t hi s . nanos = nanos ;
i s Thr eaded = t r ue;
t hi s . s t ar t ( ) ;
}

/ **
* St ar t t he J avacl i ent t hr ead. Ran aut omat i cal l y f r omr unThr eaded ( ) .
*/
publ i c v oi d r un ( ) {
t r y {
whi l e ( i s Thr eaded) {
whi l e ( r ead ( ) ! = PLAYER_MSGTYPE_SYNCH && i s Thr eaded) ;
i f ( mi l l i s < 0)
Thr ead. y i el d ( ) ;
el s e
i f ( nanos <= 0)
Thr ead. s l eep ( mi l l i s ) ;
el s e
Thr ead. s l eep ( mi l l i s , nanos ) ;
}
} cat ch ( Ex cept i on e) { e. pr i nt St ackTr ace ( ) ; }
}

/ **
* Request a Power devi ce.
* @par ami ndex t he devi ce i ndex
* @par amr access mode
* @r et ur n a Power devi ce i f successf ul , nul l ot her wi se
*/
publ i c Power I nt er f ace r eques t I nt er f acePower ( i nt i ndex , char r ) {
r et ur n ( Power I nt er f ace) r eques t Dev i ceAcces s ( PLAYER_POWER_CODE,
( s hor t ) dev i ceCnt [ 2] , r ) ;
}

/ **
* Request a Gr i pper devi ce.
* @r et ur n a Gr i pper devi ce i f successf ul , nul l ot her wi se
*/
publ i c Gr i pper I nt er f ace r eques t I nt er f aceGr i pper ( i nt i ndex , char r ) {
r et ur n ( Gr i pper I nt er f ace) r eques t Dev i ceAcces s ( PLAYER_GRI PPER_CODE,
}

/ **
* Request a Posi t i on devi ce.
* @r et ur n a Posi t i on devi ce i f successf ul , nul l ot her wi se
*/
publ i c Pos i t i onI nt er f ace r eques t I nt er f acePos i t i on ( i nt i ndex , char r ) {
r et ur n ( Pos i t i onI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_POSI TI ON_CODE,
}

/ **
* Request a Sonar devi ce.
* @r et ur n a Sonar devi ce i f successf ul , nul l ot her wi se
*/
publ i c Sonar I nt er f ace r eques t I nt er f aceSonar ( i nt i ndex , char r ) {
r et ur n ( Sonar I nt er f ace) r eques t Dev i ceAcces s ( PLAYER_SONAR_CODE,
}

/ **
* Request a Laser devi ce.
Anexe
pagina LXIII
* @r et ur n a Laser devi ce i f successf ul , nul l ot her wi se
*/
publ i c Las er I nt er f ace r eques t I nt er f aceLas er ( i nt i ndex , char r ) {
r et ur n ( Las er I nt er f ace) r eques t Dev i ceAcces s ( PLAYER_LASER_CODE,
}

/ **
* Request a Bl obf i nder devi ce.
* @r et ur n a Bl obf i nder devi ce i f successf ul , nul l ot her wi se
*/
publ i c Bl obf i nder I nt er f ace r eques t I nt er f aceBl obf i nder ( i nt i ndex , char r ) {
r et ur n ( Bl obf i nder I nt er f ace) r eques t Dev i ceAcces s ( PLAYER_BLOBFI NDER_CODE,
}

/ **
* Request a Pt z devi ce.
* @r et ur n a Pt z devi ce i f successf ul , nul l ot her wi se
*/
publ i c Pt z I nt er f ace r eques t I nt er f acePt z ( i nt i ndex , char r ) {
r et ur n ( Pt z I nt er f ace) r eques t Dev i ceAcces s ( PLAYER_PTZ_CODE,
}

/ **
* Request an Audi o devi ce.
* @r et ur n an Audi o devi ce i f successf ul , nul l ot her wi se
*/
publ i c Audi oI nt er f ace r eques t I nt er f aceAudi o ( i nt i ndex , char r ) {
r et ur n ( Audi oI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_AUDI O_CODE,
}

/ **
* Request a Fi duci al devi ce.
* @r et ur n a Fi duci al devi ce i f successf ul , nul l ot her wi se
*/
publ i c Fi duci al I nt er f ace r eques t I nt er f aceFi duci al ( i nt i ndex , char r ) {
r et ur n ( Fi duci al I nt er f ace) r eques t Dev i ceAcces s ( PLAYER_FI DUCI AL_CODE,
}

/ **
* Request a Speech devi ce.
* @r et ur n a Speech devi ce i f successf ul , nul l ot her wi se
*/
publ i c SpeechI nt er f ace r eques t I nt er f aceSpeech ( i nt i ndex , char r ) {
r et ur n ( SpeechI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_SPEECH_CODE,
}

/ **
* Request a GPS devi ce.
* @r et ur n a GPS devi ce i f successf ul , nul l ot her wi se
*/
publ i c GPSI nt er f ace r eques t I nt er f aceGPS ( i nt i ndex , char r ) {
r et ur n ( GPSI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_GPS_CODE,
}

/ **
* Request a Bumper devi ce.
* @r et ur n a Bumper devi ce i f successf ul , nul l ot her wi se
*/
publ i c Bumper I nt er f ace r eques t I nt er f aceBumper ( i nt i ndex , char r ) {
r et ur n ( Bumper I nt er f ace) r eques t Dev i ceAcces s ( PLAYER_BUMPER_CODE,
}
Anexe
pagina LXIV

/ **
* Request a Tr ut h devi ce.
* @r et ur n a Tr ut h devi ce i f successf ul , nul l ot her wi se
*/
publ i c Tr ut hI nt er f ace r eques t I nt er f aceTr ut h ( i nt i ndex , char r ) {
r et ur n ( Tr ut hI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_TRUTH_CODE,
}

/ **
* Request a DI O devi ce.
* @r et ur n a DI O devi ce i f successf ul , nul l ot her wi se
*/
publ i c DI OI nt er f ace r eques t I nt er f aceDI O ( i nt i ndex , char r ) {
r et ur n ( DI OI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_DI O_CODE,
}

/ **
* Request an AI O devi ce.
* @r et ur n an AI O devi ce i f successf ul , nul l ot her wi se
*/
publ i c AI OI nt er f ace r eques t I nt er f aceAI O ( i nt i ndex , char r ) {
r et ur n ( AI OI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_AI O_CODE,
}

/ **
* Request an I R devi ce.
* @r et ur n an I R devi ce i f successf ul , nul l ot her wi se
*/
publ i c I RI nt er f ace r eques t I nt er f aceI R ( i nt i ndex , char r ) {
r et ur n ( I RI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_I R_CODE,
}

/ **
* Request a Wi Fi devi ce.
* @r et ur n a Wi Fi devi ce i f successf ul , nul l ot her wi se
*/
publ i c Wi Fi I nt er f ace r eques t I nt er f aceWi Fi ( i nt i ndex , char r ) {
r et ur n ( Wi Fi I nt er f ace) r eques t Dev i ceAcces s ( PLAYER_WI FI _CODE,
}

/ **
* Request a Wavef or mdevi ce.
* @r et ur n a Wavef or mdevi ce i f successf ul , nul l ot her wi se
*/
publ i c Wav ef or mI nt er f ace r eques t I nt er f aceWav ef or m ( i nt i ndex , char r ) {
r et ur n ( Wav ef or mI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_WAVEFORM_CODE,
}

/ **
* Request a Local i ze devi ce.
* @r et ur n a Local i ze devi ce i f successf ul , nul l ot her wi se
*/
publ i c Local i z eI nt er f ace r eques t I nt er f aceLocal i z e ( i nt i ndex , char r ) {
r et ur n ( Local i z eI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_LOCALI ZE_CODE,
}

/ **
* Request a MCommdevi ce.
* @r et ur n a MCommdevi ce i f successf ul , nul l ot her wi se
*/

Anexe
pagina LXV
publ i c MComI nt er f ace r eques t I nt er f aceMCom ( i nt i ndex , char r ) {
r et ur n ( MComI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_MCOM_CODE,
}

/ **
* Request a Sound devi ce.
* @r et ur n a Sound devi ce i f successf ul , nul l ot her wi se
*/
publ i c SoundI nt er f ace r eques t I nt er f aceSound ( i nt i ndex , char r ) {
r et ur n ( SoundI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_SOUND_CODE,
}

/ **
* Request an Audi oDSP devi ce.
* @r et ur n an Audi oDSP devi ce i f successf ul , nul l ot her wi se
*/
publ i c Audi oDSPI nt er f ace r eques t I nt er f aceAudi oDSP ( i nt i ndex , char r ) {
r et ur n ( Audi oDSPI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_AUDI ODSP_CODE,
}

/ **
* Request an Audi oMi xer devi ce.
* @r et ur n an Audi oMi xer devi ce i f successf ul , nul l ot her wi se
*/
publ i c Audi oMi x er I nt er f ace r eques t I nt er f aceAudi oMi x er ( i nt i ndex , char r ) {
r et ur n ( Audi oMi x er I nt er f ace) r eques t Dev i ceAcces s ( PLAYER_AUDI OMI XER_CODE,
}

/ **
* Request a Posi t i on3D devi ce.
* @r et ur n a Posi t i on3D devi ce i f successf ul , nul l ot her wi se
*/
publ i c Pos i t i on3DI nt er f ace r eques t I nt er f acePos i t i on3D ( i nt i ndex , char r ) {
r et ur n ( Pos i t i on3DI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_POSI TI ON3D_CODE,
}

/ **
* Request a Si mul at i on devi ce.
* @r et ur n a Si mul at i on devi ce i f successf ul , nul l ot her wi se
*/
publ i c Si mul at i onI nt er f ace r eques t I nt er f aceSi mul at i on ( i nt i ndex , char r ) {
r et ur n ( Si mul at i onI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_SI MULATI ON_CODE,
}

/ **
* Request a Bl i nkenl i ght devi ce.
* @r et ur n a Bl i nkenl i ght devi ce i f successf ul , nul l ot her wi se
*/
publ i c Bl i nkenl i ght I nt er f ace r eques t I nt er f aceBl i nkenl i ght ( i nt i ndex , char r ) {
r et ur n ( Bl i nkenl i ght I nt er f ace) r eques t Dev i ceAcces s ( PLAYER_BLI NKENLI GHT_CODE,
}

/ **
* Request a Nomad devi ce.
* @r et ur n a Nomad devi ce i f successf ul , nul l ot her wi se
*/
publ i c NomadI nt er f ace r eques t I nt er f aceNomad ( i nt i ndex , char r ) {
r et ur n ( NomadI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_NOMAD_CODE,
}

/ **
* Request a Camer a devi ce.
Anexe
pagina LXVI
* @r et ur n a Camer a devi ce i f successf ul , nul l ot her wi se
*/
publ i c Camer aI nt er f ace r eques t I nt er f aceCamer a ( i nt i ndex , char r ) {
r et ur n ( Camer aI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_CAMERA_CODE,
}

/ **
* Request a Map devi ce.
* @r et ur n a Map devi ce i f successf ul , nul l ot her wi se
*/
publ i c MapI nt er f ace r eques t I nt er f aceMap ( i nt i ndex , char r ) {
r et ur n ( MapI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_MAP_CODE,
}

/ **
* Request a Pl anner devi ce.
* @r et ur n a Pl anner devi ce i f successf ul , nul l ot her wi se
*/
publ i c Pl anner I nt er f ace r eques t I nt er f acePl anner ( i nt i ndex , char r ) {
r et ur n ( Pl anner I nt er f ace) r eques t Dev i ceAcces s ( PLAYER_PLANNER_CODE,
}

/ **
* Request a Log devi ce.
* @r et ur n a Log devi ce i f successf ul , nul l ot her wi se
*/
publ i c LogI nt er f ace r eques t I nt er f aceLog ( i nt i ndex , char r ) {
r et ur n ( LogI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_LOG_CODE,
}

/ **
* Request a Ener gy devi ce.
* @r et ur n a Ener gy devi ce i f successf ul , nul l ot her wi se
*/
publ i c Ener gy I nt er f ace r eques t I nt er f aceEner gy ( i nt i ndex , char r ) {
r et ur n ( Ener gy I nt er f ace) r eques t Dev i ceAcces s ( PLAYER_ENERGY_CODE,
}

/ **
* Request a Mot or devi ce.
* @r et ur n a Mot or devi ce i f successf ul , nul l ot her wi se
*/
publ i c Mot or I nt er f ace r eques t I nt er f aceMot or ( i nt i ndex , char r ) {
r et ur n ( Mot or I nt er f ace) r eques t Dev i ceAcces s ( PLAYER_MOTOR_CODE,
}

/ **
* Request a Posi t i on2D devi ce.
* @r et ur n a Posi t i on2D devi ce i f successf ul , nul l ot her wi se
*/
publ i c Pos i t i on2DI nt er f ace r eques t I nt er f acePos i t i on2D ( i nt i ndex , char r ) {
r et ur n ( Pos i t i on2DI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_POSI TI ON2D_CODE,
}

/ **
* Request a J oyst i ck devi ce.
* @r et ur n a J oyst i ck devi ce i f successf ul , nul l ot her wi se
*/
publ i c Joy s t i ckI nt er f ace r eques t I nt er f aceJoy s t i ck ( i nt i ndex , char r ) {
r et ur n ( Joy s t i ckI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_JOYSTI CK_CODE,
}

Anexe
pagina LXVII
/ **
* Request a Speech Recogni t i on devi ce.
* @r et ur n a Speech Recogni t i on devi ce i f successf ul , nul l ot her wi se
*/
publ i c SpeechRecogni t i onI nt er f ace r eques t I nt er f aceSpeechRecogni t i on ( i nt i ndex , char r ) {
r et ur n ( SpeechRecogni t i onI nt er f ace) r eques t Dev i ceAcces s ( PLAYER_SPEECH_RECOGNI TI ON_CODE,
}

/ **
* Sends a Pl ayer message header f i l l ed wi t h t he gi ven val ues.
* @par amt ype t ype of message ( DATA, CMD, REQ, RESP_ACK, SYNCH, RESP_NACK, RESP_ERR)
* @par amdevi ce t ype of devi ce
* @par amsi ze si ze i n byt es of t he payl oad t o f ol l ow
*/
pr i v at e v oi d s endHeader ( s hor t t y pe, s hor t dev i ce, s hor t i ndex , i nt s i z e) {
t r y {
/ * see pl ayer . h / pl ayer _msghdr f or addi t i onal expl anat i ons */
os . wr i t eShor t ( PLAYER_STXX) ; / * 0x5878 - t he message st ar t si gni f i er */
os . wr i t eShor t ( t y pe) ; / * DATA, CMD, REQ, RESP_ACK, SYNCH, RESP_NACK, RESP_ERR */
os . wr i t eShor t ( dev i ce) ; / * what ki nd of devi ce */
os . wr i t eShor t ( i ndex ) ; / * whi ch devi ce of what ki nd */
os . wr i t eI nt ( 0) ; / * ser ver ' s cur r ent t i me ( seconds) */
os . wr i t eI nt ( 0) ; / * ser ver ' s cur r ent t i me ( mi cr oseconds) */
os . wr i t eI nt ( 0) ; / * t i me when t he cur r ent dat a was gener at ed ( seconds) */
os . wr i t eI nt ( 0) ; / * t i me when t he cur r ent dat a was gener at ed ( u seconds) */
os . wr i t eI nt ( 0) ; / * r eser ved */
os . wr i t eI nt ( s i z e) ; / * si ze i n byt es of t he payl oad t o f ol l ow */
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Er r or when r eadi ng header : " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Get t he l i st of avai l abl e devi ces.
* 
* I t ' s usef ul f or appl i cat i ons such as vi ewer pr ogr ams and t est sui t es t hat t ai l or behave
* di f f er ent l y dependi ng on whi ch devi ces ar e avai l abl e.
*/
publ i c v oi d r eques t Dev i ceLi s t ( ) {
t r y {
s endHeader ( PLAYER_MSGTYPE_REQ, PLAYER_PLAYER_CODE, ( s hor t ) 0, 2) ; / * 2 byt es payl oad */
os . wr i t eShor t ( PLAYER_PLAYER_DEVLI ST_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Coul dn' t r eques t dev i ce l i s t " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Get t he dr i ver name f or a par t i cul ar devi ce.
* @par amdevi ce t he devi ce i dent i f i er
*/
publ i c v oi d r eques t Dr i v er Name ( s hor t dev i ce) {
t r y {
os . wr i t eShor t ( PLAYER_PLAYER_DRI VERI NFO_REQ) ;
os . wr i t eShor t ( dev i ce) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Coul dn' t r eques t dr i v er name " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Get devi ce access.
* 
* Thi s i s t he most i mpor t ant r equest ! Bef or e i nt er act i ng wi t h a devi ce, t he cl i ent must
* r equest appr opr i at e access.
* 
* The access codes, whi ch ar e used i n bot h t he r equest and r esponse, ar e gi ven above. Read
* access means t hat t he ser ver wi l l st ar t sendi ng dat a f r omt he speci f i ed devi ce. For
* i nst ance, i f r ead access i s obt ai ned f or t he sonar devi ce Pl ayer wi l l st ar t sendi ng sonar
* dat a t o t he cl i ent . Wr i t e access means t hat t he cl i ent has per mi ssi on t o cont r ol t he
* act uat or s of t he devi ce. Ther e i s no l ocki ng mechani smso di f f er ent cl i ent s can have
* concur r ent wr i t e access t o t he same act uat or s. Al l access i s bot h of t he above and f i nal l y
* cl ose means t hat t her e i s no l onger any access t o t he devi ce. Devi ce r equest messages can
* be sent at any t i me, pr ovi di ng on t he f l y r econf i gur at i on f or cl i ent s t hat need di f f er ent
* devi ces dependi ng on t he t ask at hand.
* 
* Of cour se, not al l of t he access codes ar e appl i cabl e t o al l devi ces; f or i nst ance i t does
* not make sense t o wr i t e t o t he sonar s. However , a r equest f or such access wi l l not gener at e
Anexe
pagina LXVIII
* an er r or ; r at her , i t wi l l be gr ant ed, but any commands act ual l y sent t o t hat devi ce wi l l be
* i gnor ed. I n r esponse t o such a devi ce r equest , t he ser ver wi l l send a r epl y i ndi cat i ng t he
* act ual access t hat was gr ant ed f or t he devi ce. The gr ant ed access may be di f f er ent f r omt he
* r equest ed access; i n par t i cul ar , i f t her e was some er r or i n i ni t i al i zi ng t he devi ce t he
* gr ant ed access wi l l be er r or , and t he cl i ent shoul d not t r y t o r ead f r omor wr i t e t o t he
* devi ce.
* @par amr Devi ce t he i nt er f ace f or t he devi ce
* @par ami ndex t he i ndex f or t he devi ce
* @par amr t he r equest ed access
* @r et ur n an obj ect of Pl ayer Devi ce t ype
*/
pr i v at e Pl ay er Dev i ce r eques t Dev i ceAcces s ( s hor t r Dev i ce, s hor t i ndex , char r ) {
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] [ Debug] : Subs cr i bi ng t o " + r Dev i ce) ;
t r y {
/ * see pl ayer . h / pl ayer _devi ce_r eq f or addi t i onal expl anat i ons */
os . wr i t eShor t ( PLAYER_PLAYER_DEV_REQ) ; / * subt ype - must be PLAYER_PLAYER_DEV_REQ */
os . wr i t eShor t ( r Dev i ce) ; / * t he i nt er f ace f or t he devi ce */
os . wr i t eShor t ( i ndex ) ; / * t he i ndex f or t he devi ce */
os . wr i t eBy t e ( ( by t e) r ) ; / * r equest ed access r i ght s */
os . f l us h ( ) ;
s hor t r es ul t = 0;
whi l e ( ( r es ul t = r eques t Sat i s f y ( ) ) ! = PLAYER_MSGTYPE_RESP_ACK) {
i f ( r es ul t == - 2)
};
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Reques t dat a er r or : " + e. t oSt r i ng ( ) ) ;
}
r et ur n newpd;
}

/ **
* Handl e sever al Pl ayer r epl i es. I f PLAYER_MSGTYPE_RESP_ACK af t er a r equest Devi ceAccess ( ) ,
* cr eat es a newpd obj ect of a Pl ayer Devi ce t ype.
* @r et ur n t he message t ype t hat Pl ayer r epl i ed wi t h.
*/
pr i v at e s hor t r eques t Sat i s f y ( ) {
s hor t t y pe = - 1;
t r y {
whi l e ( i s . r eadShor t ( ) ! = PLAYER_STXX) / * wai t f or t he STX */
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] [ Debug] : Unr ecogni z ed header : ") ;

t y pe = i s . r eadShor t ( ) ; / * DATA, CMD, REQ, RESP_ACK, SYNCH, RESP_NACK, RESP_ERR */
s hor t dev i ce = i s . r eadShor t ( ) ; / * what ki nd of devi ce */
s hor t i ndex = i s . r eadShor t ( ) ; / * whi ch devi ce of what ki nd */
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] [ Debug] : Got a mes s age: t y pe = " + t y pe +
" dev i ce = " + dev i ce + " i ndex = " + i ndex ) ;

i f ( t y pe == PLAYER_MSGTYPE_DATA) {
t r y {
dev i ceLi s t [ ( i nt ) dev i ce] [ ( i nt ) i ndex ] . r eadDat a ( ) ;
} cat ch ( Nul l Poi nt er Ex cept i on npe) {
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Got a mes s age: t y pe = " + t y pe +
" dev i ce = " + dev i ce + " i ndex = " + i ndex ) ;
}
r et ur n - 1;
}

/ * i gnor e t he t i me_sec, t i me_usec, t i mest amp_sec, t i mest amp_usec and r eser ved f i el ds */
i s . r eadI nt ( ) ; i s . r eadI nt ( ) ; i s . r eadI nt ( ) ; i s . r eadI nt ( ) ; i s . r eadI nt ( ) ;

i f ( t y pe == PLAYER_MSGTYPE_SYNCH) {
f or ( i nt i = 0; i < s i z e; i ++, i s . r eadBy t e ( ) ) ;
r et ur n - 1;
} el s e i f ( t y pe == PLAYER_MSGTYPE_RESP_NACK)
dev i ceLi s t [ ( i nt ) dev i ce] [ ( i nt ) i ndex ] . handl eNARMes s age ( ) ;
el s e i f ( t y pe == PLAYER_MSGTYPE_RESP_ERR)
dev i ceLi s t [ ( i nt ) dev i ce] [ ( i nt ) i ndex ] . handl eEARMes s age ( ) ;

/ * PLAYER_MSGTYPE_RESP_ACK */
el s e {
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] [ Debug] : Got r es ponce: " + dev i ce +
" s i z e of pay l oad: " + s i z e) ;

/ * see pl ayer . h / pl ayer _devi ce_r esp f or addi t i onal expl anat i ons */
i s . r eadShor t ( ) ; / * subt ype - i gnor ed */
s hor t dev i ce2 = i s . r eadShor t ( ) ; / * t he i nt er f ace f or t he devi ce */
i s . r eadShor t ( ) ; / * t he i ndex f or t he devi ce - i gnor ed */
by t e r = i s . r eadBy t e ( ) ; / * r equest ed access r i ght s */
Anexe
pagina LXIX
Sy s t em. er r . pr i nt ( "[ Pl ay er Cl i ent ] : Got r es pons e f or dev i ce " + dev i ce2 + "( ") ;
f or ( i nt i = 0; i < PLAYER_MAX_DEVI CE_STRI NG_LEN; i ++) {
char c = ( char ) i s . r eadBy t e ( ) ;
i f ( c ! = 0)
Sy s t em. er r . pr i nt ( c) ;
}
Sy s t em. er r . pr i nt l n ( ") wi t h acces s : " + ( char ) r ) ;

i f ( r == PLAYER_ERROR_MODE) {
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Er r or was r epl i ed f r om t he s er v er ! ") ;
r et ur n - 2;
}
newpd = nul l ;

s wi t ch ( dev i ce2) {
cas e PLAYER_NULL_CODE: { / * / dev/ nul l anal ogue */
br eak;
}
cas e PLAYER_PLAYER_CODE: { / * t he ser ver i t sel f */
br eak;
}
cas e PLAYER_POWER_CODE: { / * power subsyst em*/
newpd = new Power I nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_GRI PPER_CODE: { / * gr i pper */
newpd = new Gr i pper I nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_POSI TI ON_CODE: { / * devi ce t hat moves */
newpd = new Pos i t i onI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_SONAR_CODE: { / * f i xed r ange- f i nder */
newpd = new Sonar I nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_LASER_CODE: { / * scanni ng r ange- f i nder */
newpd = new Las er I nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_BLOBFI NDER_CODE: { / * vi sual bl obf i nder */
newpd = new Bl obf i nder I nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_PTZ_CODE: { / * pan- t i l t - zoomuni t */
newpd = new Pt z I nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_AUDI O_CODE: { / * audi o I / O */
newpd = new Audi oI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_FI DUCI AL_CODE: { / * f i duci al det ect or */
newpd = new Fi duci al I nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_SPEECH_CODE: { / * speech I / O */
newpd = new SpeechI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_GPS_CODE: { / * GPS uni t */
newpd = new GPSI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_BUMPER_CODE: { / * bumper ar r ay */
newpd = new Bumper I nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_TRUTH_CODE: { / * gr ound- t r ut h ( St age) */
newpd = new Tr ut hI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_I DARTURRET_CODE: { / * r angi ng + comms */
/ * Pl ayer suppor t obsol et e? */
br eak;
}
cas e PLAYER_I DAR_CODE: { / * r angi ng + comms */
br eak;
}
cas e PLAYER_DESCARTES_CODE: { / * descar t es pl at f or m*/
br eak;
}
Anexe
pagina LXX
cas e PLAYER_DI O_CODE: { / * di gi t al I / O */
newpd = new DI OI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_AI O_CODE: { / * anal og I / O */
newpd = new AI OI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_I R_CODE: { / * I R ar r ay */
newpd = new I RI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_WI FI _CODE: { / * wi f i car d st at us */
newpd = new Wi Fi I nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_WAVEFORM_CODE: { / * f et ch r aw wavef or ms */
newpd = new Wav ef or mI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_LOCALI ZE_CODE: { / * l ocal i zat i on */
newpd = new Local i z eI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_MCOM_CODE: { / * mul t i coms */
newpd = new MComI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_SOUND_CODE: { / * sound f i l e pl ayback */
newpd = new SoundI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_AUDI ODSP_CODE: { / * audi o DSP I / O */
newpd = new Audi oDSPI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_AUDI OMI XER_CODE: { / * audi o I / O */
newpd = new Audi oMi x er I nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_POSI TI ON3D_CODE: { / * 3- D posi t i on */
newpd = new Pos i t i on3DI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_SI MULATI ON_CODE: { / * si mul at or s */
newpd = new Si mul at i onI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_SERVI CE_ADV_CODE: { / * LAN adver t i sement */
br eak;
}
cas e PLAYER_BLI NKENLI GHT_CODE: { / * bl i nki ng l i ght s */
newpd = new Bl i nkenl i ght I nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_NOMAD_CODE: { / * nomad r obot */
newpd = new NomadI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_CAMERA_CODE: { / * camer a devi ce ( gazebo) */
newpd = new Camer aI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_MAP_CODE: { / * get a map */
newpd = new MapI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_PLANNER_CODE: { / * 2D mot i on pl anner */
newpd = new Pl anner I nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_LOG_CODE: { / * l og R/ Wcont r ol */
newpd = new LogI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_ENERGY_CODE: { / * ener gy char gi ng */
newpd = new Ener gy I nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_MOTOR_CODE: { / * mot or i nt er f ace */
newpd = new Mot or I nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_POSI TI ON2D_CODE: { / * 2- D posi t i on */
Anexe
pagina LXXI
newpd = new Pos i t i on2DI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_JOYSTI CK_CODE: { / * j oyst i ck */
newpd = new Joy s t i ckI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_SPEECH_RECOGNI TI ON_CODE: { / * speech r ecogni t i on I / O */
newpd = new SpeechRecogni t i onI nt er f ace ( t hi s , ( s hor t ) dev i ceCnt [ dev i ce2] ) ;
br eak;
}
cas e PLAYER_OPAQUE_CODE: { / * pl ugi n i nt er f ace */
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Uns uppor t ed dev i ce er r or ! - " +
dev i ce2) ;
newpd = nul l ;
r = PLAYER_ERROR_MODE;
br eak;
}
}

i f ( newpd ! = nul l ) {
i f ( dev i ceCnt [ dev i ce2] >= MAX_DEVI CES) {
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Max number of dev i ces ex ceeded ! ") ;
r et ur n - 2;
}
el s e / * add t he devi ce t o t he l i st */
dev i ceLi s t [ dev i ce2] [ dev i ceCnt [ dev i ce2] ++] = newpd;
}
}
} cat ch ( EOFEx cept i on e) {
Sy s t em. er r . pr i nt l n( "[ Pl ay er Cl i ent ] : j av a. i o. EOFEx cept i on : I s t he Pl ay er s er v er s t i l l r unni ng?") ;
i f ( s t opOnEOFEx cept i on)
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Read er r or : " + e. t oSt r i ng ( ) ) ;
e. pr i nt St ackTr ace ( ) ;
}
r et ur n t y pe;
}

/ **
* Conf i gur at i on r equest : Get dat a.
* 
* When t he ser ver i s i n a PLAYER_DATAMODE_PULL_* dat a del i ver y mode, t he cl i ent can r equest a
* si ngl e r ound of dat a by sendi ng a zer o- ar gument r equest wi t h t ype code 0x0003. The r esponse
* wi l l be a zer o- l engt h acknowl edgement . The cl i ent onl y needs t o make t hi s r equest when a
* PLAYER_DATAMODE_PULL_* mode i s i n use.
*/
publ i c v oi d r eques t Dat a ( ) {
t r y {
os . wr i t eShor t ( PLAYER_PLAYER_DATA_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Coul dn' t r eques t one r ound dat a " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Change dat a del i ver y mode.
* 
*The Pl ayer ser ver suppor t s f our dat a modes, descr i bed above. By def aul t , t he ser ver oper at es
*i n PLAYER_DATAMODE_PUSH_NEWmode at a f r equency of 10Hz. To swi t ch t o a di f f er ent mode send a
* r equest wi t h t he f or mat gi ven bel ow. The ser ver ' s r epl y wi l l be a zer o- l engt h
* acknowl edgement .
* @par ammode t he r equest ed mode
*/
publ i c v oi d r eques t Dat aDel i v er y Mode ( by t e mode) {
t r y {
os . wr i t eShor t ( PLAYER_PLAYER_DATAMODE_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Coul dn' t r eques t change of dat a mode " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Change dat a del i ver y f r equency.
Anexe
pagina LXXII
* 
* By def aul t , t he f i xed f r equency f or t he PUSH dat a del i ver y modes i s 10Hz; t hus a cl i ent
* whi ch makes no conf i gur at i on changes wi l l r ecei ve sensor dat a appr oxi mat el y ever y 100ms.
* The ser ver can send dat a f ast er or sl ower ; t o change t he f r equency, send a r equest wi t h
* t hi s f or mat . The ser ver ' s r epl y wi l l be a zer o- l engt h acknowl edgement .
* @par amf r equency r equest ed f r equency i n Hz
*/
publ i c v oi d r eques t Dat aDel i v er y Fr equency ( s hor t f r equency ) {
t r y {
os . wr i t eShor t ( PLAYER_PLAYER_DATAFREQ_REQ) ;
os . wr i t eShor t ( f r equency ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Coul dn' t r eques t change of dat a f r equency " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Aut hent i cat i on.
* 
* I f ser ver aut hent i cat i on has been enabl ed ( by pr ovi di ng ' - key <key>' on t he command- l i ne;
* see Command l i ne opt i ons) ; t hen each cl i ent must aut hent i cat e i t sel f bef or e ot her wi se
* i nt er act i ng wi t h t he ser ver . To aut hent i cat e, send a r equest wi t h t hi s f or mat .
* 
* I f t he key mat ches t he ser ver ' s key t hen t he cl i ent i s aut hent i cat ed, t he ser ver wi l l r epl y
* wi t h a zer o- l engt h acknowl edgement , and t he cl i ent can cont i nue wi t h ot her oper at i ons. I f
* t he key does not mat ch, or i f t he cl i ent at t empt s any ot her ser ver i nt er act i ons bef or e
* aut hent i cat i ng, t hen t he connect i on wi l l be cl osed i mmedi at el y. I t i s onl y necessar y t o
* aut hent i cat e each cl i ent once.
* 
* Not e t hat t hi s suppor t f or aut hent i cat i on i s NOT a secur i t y mechani sm. The keys ar e al ways
* i n pl ai n t ext , bot h i n memor y and when t r ansmi t t ed over t he net wor k; f ur t her , si nce t he key
* i s gi ven on t he command- l i ne, t her e i s a ver y good chance t hat you can f i nd i t i n pl ai n t ext
* i n t he pr ocess t abl e ( i n Li nux t r y ' ps - ax | gr ep pl ayer ' ) . Thus you shoul d not use an
* i mpor t ant passwor d as your key, nor shoul d you r el y on Pl ayer aut hent i cat i on t o pr event bad
* guys f r omdr i vi ng your r obot s( use a f i r ewal l i nst ead) . Rat her , aut hent i cat i on was i nt r oduced
*i nt o Pl ayer t o pr event acci dent al l y connect i ng one' s cl i ent pr ogr amt o someone el se' s r obot .
*Thi s ki nd of acci dent occur s pr i mar i l y when St age i s r unni ng i n a mul t i - user envi r onment . I n
* t hi s case i t i s ver y l i kel y t hat t her e i s a Pl ayer ser ver l i st eni ng on por t 6665, & cl i ent s
* wi l l gener al l y connect t o t hat por t by def aul t , unl ess a speci f i c opt i on i s gi ven.
* 
* Thi s mechani smwas never r eal l y used, and may be r emoved.
* @par amkey t he aut hent i cat i on key
*/
publ i c v oi d r eques t Aut hent i cat i on ( by t e[ ] key ) {
t r y {
i f ( key . l engt h > 32)
t hr ow new Ex cept i on ( "[ Pl ay er Cl i ent ] : Suppl i ed aut hent i cat i on key i s " +
key . l engt h + " but s houl d be <= 32 by t es ") ;
os . wr i t eShor t ( PLAYER_PLAYER_AUTH_REQ) ;
f or ( i nt i = 0; i < key . l engt h; i ++)
os . wr i t eBy t e ( key [ i ] ) ;
f or ( i nt i = 0; i < 32 - key . l engt h; i ++)
os . wr i t eBy t e ( ( by t e) 0) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Coul dn' t r eques t aut hent i cat i on " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
* Use nameser vi ce t o get t he cor r espondi ng por t f or a r obot name ( onl y wi t h St age) .
* @par amname t he r obot name
*/
publ i c v oi d r eques t NameSer v i ce ( char [ ] name) {
t r y {
i nt t ot al Si z e = PLAYER_MAX_DEVI CE_STRI NG_LEN + 2;
/ * 4 byt es payl oad */
s endHeader ( PLAYER_MSGTYPE_REQ, PLAYER_PLAYER_CODE, ( s hor t ) 0, t ot al Si z e) ;
os . wr i t eShor t ( PLAYER_PLAYER_NAMESERVI CE_REQ) ;
f or ( i nt i = 0; i < name. l engt h; i ++) {
os . wr i t eBy t e ( ( by t e) name[ i ] ) ;
}
f or ( i nt i = 0; i < PLAYER_MAX_DEVI CE_STRI NG_LEN - name. l engt h; i ++) {
}
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Coul dn' t r eques t name s er v i ce " + e. t oSt r i ng ( ) ) ;
}
}
Anexe
pagina LXXIII

/ **
* Get t he devi ce l i st i ndex.
* @r et ur n devi ce l i st i ndex as an i nt eger
* @see #i ncI ( i nt )
*/
publ i c s y nchr oni z ed i nt get I ( ) { r et ur n i ndex ; }

/ **
* I ncr ement s t he devi ce l i st i ndex.
* @par ami ncr ement number of i ncr ement s
* @see #get I ( )
*/
publ i c s y nchr oni z ed v oi d i ncI ( i nt i ncr ement ) {
i f ( Mat h. abs ( i ndex - I nt eger . MAX_VALUE) <= DI FFERENCE_SYNCH_FACTOR)
i ndex = 0;
i ndex += i ncr ement ;
}

/ **
* Read t he Pl ayer ser ver r epl i es ( f or a t hr eaded J avacl i ent ) .
* 
* Message header f i el d and t ypes: 
* STX t ype devi ce i ndex t _sec t _usec t s_sec t s_usec r eser ved si ze 
* shor t shor t shor t shor t i nt i nt i nt i nt i nt i nt 
* @r et ur n t he message t ype code
*/
pr i v at e s hor t r ead ( ) {
s hor t t y pe = - 1;
t r y {
whi l e ( i s . r eadShor t ( ) ! = PLAYER_STXX) ; / * wai t f or t he STX */
t y pe = i s . r eadShor t ( ) ; / * r ead t he t ype of message */
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] [ Debug] : Ent er ed = " + t y pe) ;
/ * ver i f y t he message t ype code - see " Message For mat s" f r omt he Pl ayer manual */
s wi t ch ( t y pe) {
cas e PLAYER_MSGTYPE_DATA: { / * Dat a message */
s hor t dev i ce = i s . r eadShor t ( ) ;
s hor t i ndex = i s . r eadShor t ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] [ Debug] : Dat a f or " + dev i ce +
" " + i ndex ) ;
dev i ceLi s t [ ( i nt ) dev i ce] [ ( i nt ) i ndex ] . r eadDat a ( ) ;
br eak;
}
cas e PLAYER_MSGTYPE_CMD: { / * Command message */
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Cl i ent s houl dn' t r ecei v e a cmd mes s age! ") ;
br eak;
}
cas e PLAYER_MSGTYPE_REQ: { / * Request message */
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Cl i ent s houl dn' t r ecei v e a r eq mes s age! ") ;
br eak;
}
cas e PLAYER_MSGTYPE_RESP_ACK: { / * Acknowl edgement r esponse message */
/ * i gnor e t he t i me_sec, t i me_usec, t i mest amp_sec, t i mest amp_usec and r eser ved */
i f ( dev i ce == 1) { / * t he pl ayer ser ver devi ce i nt er f ace */
handl eRes pons e ( s i z e) ; / * handl e t he payl oad */
br eak;
}
dev i ceLi s t [ ( i nt ) dev i ce] [ ( i nt ) i ndex ] . handl eRes pons e ( s i z e) ;
br eak;
}
cas e PLAYER_MSGTYPE_SYNCH: { / * Synchr oni zat i on message */
/ * i gnor e t he devi ce and i ndex f i el ds */
i s . r eadShor t ( ) ; i s . r eadShor t ( ) ;
/ * i gnor e t he si ze f i el d */
i s . r eadI nt ( ) ;
br eak;
}
cas e PLAYER_MSGTYPE_RESP_NACK: { / * Negat i ve acknowl edgement r esponse message */
dev i ceLi s t [ ( i nt ) dev i ce] [ ( i nt ) i ndex ] . handl eNARMes s age ( ) ;
br eak;
}
cas e PLAYER_MSGTYPE_RESP_ERR: { / * Er r or r esponse message */
Anexe
pagina LXXIV
dev i ceLi s t [ ( i nt ) dev i ce] [ ( i nt ) i ndex ] . handl eEARMes s age ( ) ;
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Unknown mes s age t y pe r ecei v ed i n r ead( ) ") ;
br eak;
}
}
} cat ch ( EOFEx cept i on e) {
Sy s t em. er r . pr i nt l n( "[ Pl ay er Cl i ent ] : j av a. i o. EOFEx cept i on : I s t he Pl ay er s er v er s t i l l r unni ng?") ;
} cat ch ( Socket Ex cept i on e) {
Sy s t em. er r . pr i nt l n( "[ Pl ay er Cl i ent ] : j av a. i o. Socket Ex cept i on: I s t he Pl ay er s er v er s t i l l r unni ng?") ;
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Read er r or : " + e. t oSt r i ng ( ) ) ;
e. pr i nt St ackTr ace ( ) ;
}
r et ur n t y pe;
}

/ **
* Read t he Pl ayer ser ver r epl i es i n non- t hr eaded mode.
*/
publ i c v oi d r eadAl l ( ) {
i f ( i s Thr eaded) r et ur n;
whi l e ( r ead ( ) ! = PLAYER_MSGTYPE_SYNCH) ;
}

/ **
*/
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Cl i ent ] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
s hor t s ubt y pe = i s . r eadShor t ( ) ;
/ * Conf i gur at i on r equest : Get t he l i st of avai l abl e devi ces. */
cas e PLAYER_PLAYER_DEVLI ST_REQ: { / * get devi ce l i st */
pddt = new Pl ay er Dev i ceDev l i s t T ( ) ;

pddt . s et Dev i ceCount ( i s . r eadShor t ( ) ) ; / * t he number of devi ces */
Pl ay er Dev i ceI dT[ ] pl ay er Dev Li s t = new Pl ay er Dev i ceI dT[ pddt . get Dev i ceCount ( ) ] ;
f or ( i nt i = 0; i < pddt . get Dev i ceCount ( ) ; i ++) {
pl ay er Dev Li s t [ i ] = new Pl ay er Dev i ceI dT ( ) ;
pl ay er Dev Li s t [ i ] . s et Code ( i s . r eadShor t ( ) ) ;
pl ay er Dev Li s t [ i ] . s et I ndex ( i s . r eadShor t ( ) ) ;
pl ay er Dev Li s t [ i ] . s et Por t ( i s . r eadShor t ( ) ) ;
}
pddt . s et Dev Li s t ( pl ay er Dev Li s t ) ;
/ * i gnor e t he r est of t he payl oad - i f any */
f or ( i nt j = 0; j < s i z e - ( 4 + pddt . get Dev i ceCount ( ) * 6) ; j ++)
i s . r eadBy t e ( ) ;

r eady PDDT = t r ue;
br eak;
}

/ * Conf i gur at i on r equest : Get t he dr i ver name f or a par t i cul ar devi ce. */
cas e PLAYER_PLAYER_DRI VERI NFO_REQ: { / * get dr i ver name */
pddi = new Pl ay er Dev i ceDr i v er I nf o( ) ;
Pl ay er Dev i ceI dT pddi Dev I D = new Pl ay er Dev i ceI dT ( ) ;

pddi Dev I D. s et Code ( i s . r eadShor t ( ) ) ;
pddi Dev I D. s et I ndex ( i s . r eadShor t ( ) ) ;
pddi Dev I D. s et Por t ( i s . r eadShor t ( ) ) ;

pddi . s et Dev I D ( pddi Dev I D) ;

St r i ng pddi Dr i v er Name = new St r i ng ( ) ;

f or ( i nt j = 0; j < s i z e - 8; j ++)
pddi Dr i v er Name += i s . r eadChar ( ) ;

pddi . s et Dr i v er Name( pddi Dr i v er Name) ;
Anexe
pagina LXXV

r eady PDDI = t r ue;
br eak;
}
cas e PLAYER_PLAYER_DEV_REQ: { / * r equest devi ce access */
br eak;
}
cas e PLAYER_PLAYER_DATA_REQ: { / * r equest dat a */
br eak;
}
cas e PLAYER_PLAYER_DATAMODE_REQ: { / * change dat a del i ver y mode */
br eak;
}
cas e PLAYER_PLAYER_DATAFREQ_REQ: { / * change dat a del i ver y f r equency */
br eak;
}
cas e PLAYER_PLAYER_AUTH_REQ: { / * aut hent i cat i on */
r ecei v edAut hent i cat i on = t r ue;
br eak;
}
cas e PLAYER_PLAYER_NAMESERVI CE_REQ: {
por t Number = i s . r eadShor t ( ) ;
r eady Por t Number = t r ue;
br eak;
}
}
} cat ch ( Ex cept i on e) {}
}

/ **
* Get t he l i st of avai l abl e devi ces af t er a PLAYER_PLAYER_DEVLI ST_REQ r equest .
* @r et ur n an obj ect of Pl ayer Devi ceDevl i st T t ype
* @see #i sReadyPDDT( )
*/
publ i c Pl ay er Dev i ceDev l i s t T get PDDT ( ) { r et ur n pddt ; }
/ **
* Get t he dr i ver name f or a par t i cul ar devi ce af t er a PLAYER_PLAYER_DRI VERI NFO_REQ r equest .
* @r et ur n an obj ect of Pl ayer Devi ceDr i ver I nf o t ype
* @see #i sReadyPDDI ( )
*/
publ i c Pl ay er Dev i ceDr i v er I nf o get PDDI ( ) { r et ur n pddi ; }
/ **
* Get t he por t number f or t he speci f i ed r obot af t er a PLAYER_PLAYER_NAMESERVI CE_REQ r equest .
* @r et ur n t he por t number t he speci f i ed r obot r uns on
* @see #r equest NameSer vi ce( char [ ] )
* @see #i sReadyPor t Number ( )
*/
publ i c i nt get Por t Number ( ) { r et ur n por t Number ; }

/ **
* Check t o see i f t he Pl ayer ser ver r epl i ed wi t h a PLAYER_PLAYER_DEVLI ST_REQ successf ul l y.
* @r et ur n t r ue i f t he PLAYER_PLAYER_DEVLI ST_REQ occur ed, f al se ot her wi se
* @see #get PDDT( )
*/
publ i c bool ean i s Ready PDDT ( ) {
i f ( r eady PDDT) {
r eady PDDT = f al s e;
r et ur n t r ue;
}
r et ur n f al s e;
}
/ **
* Check t o see i f t he Pl ayer ser ver r epl i ed wi t h a PLAYER_PLAYER_DRI VERI NFO_REQ successf ul l y.
* @r et ur n t r ue i f t he PLAYER_PLAYER_DRI VERI NFO_REQ occur ed, f al se ot her wi se
* @see #get PDDI ( )
*/
publ i c bool ean i s Ready PDDI ( ) {
i f ( r eady PDDI ) {
r eady PDDI = f al s e;
r et ur n t r ue;
}
r et ur n f al s e;
}
/ **
* Check t o see i f t he cl i ent has aut hent i cat ed successf ul l y.
* @r et ur n t r ue i f cl i ent has aut hent i cat ed, f al se ot her wi se
*/
publ i c bool ean i s Aut hent i cat ed ( ) {
i f ( r ecei v edAut hent i cat i on) {
r ecei v edAut hent i cat i on = f al s e;
r et ur n t r ue;
}
r et ur n f al s e;
}
/ **
Anexe
pagina LXXVI
* Check t o see i f t he por t number has been i dent i f i ed.
* @r et ur n t r ue i f t he por t i s r eady t o be r ead, f al se ot her wi se
* @see #get Por t Number ( )
*/
publ i c bool ean i s Ready Por t Number ( ) {
i f ( r eady Por t Number ) {
r eady Por t Number = f al s e;
r et ur n t r ue;
}
r et ur n f al s e;
}
}

j ja av va ac cl li ie en nt t/ /P Pl la ay ye er rD De ev vi ic ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl ayer Devi ce. j ava
*
*
*
*
*
* $I d: Pl ayer Devi ce. j ava, v 1. 4 2005/ 05/ 25 21: 55: 48 veedee Exp $
*
*/

i mpor t j av a. i o. Dat aI nput St r eam;
i mpor t j av a. i o. Dat aOut put St r eam;

/ **
* Abst r act cl ass f or al l Pl ayer i nt er f aces.
* @ver si on
* <ul >
* </ ul >
*/
publ i c abs t r act cl as s Pl ay er Dev i ce {

pr ot ect ed f i nal s hor t PLAYER_STXX = Pl ay er Cl i ent . PLAYER_STXX;
pr ot ect ed f i nal i nt PLAYER_MAX_REQREP_SI ZE = 4096;
pr ot ect ed f i nal i nt DI FFERENCE_SYNCH_FACTOR = 10;

pr ot ect ed Pl ay er Cl i ent pc;
pr ot ect ed Dat aI nput St r eam i s ;
pr ot ect ed Dat aOut put St r eam os ;

pr ot ect ed i nt t _s ec = 0;
pr ot ect ed i nt t _us ec = 0;
pr ot ect ed i nt t s _s ec = 0;
pr ot ect ed i nt t s _us ec = 0;
pr ot ect ed i nt r es er v ed = 0;
pr ot ect ed i nt s i z e = 0;
pr ot ect ed s hor t dev i ce = 0x 0000;
pr ot ect ed s hor t i ndex = - 1;

/ / synchr oni zat i on st uf f . . .
pr i v at e i nt i ndSy nch = 0;
pr i v at e i nt t mpI = 0;

publ i c s y nchr oni z ed i nt get Ti meFor Dat aSampl ed_s ec ( ) { r et ur n t _s ec; };
publ i c s y nchr oni z ed i nt get Ti meFor Dat aSampl ed_us ec ( ) { r et ur n t _us ec; };
publ i c s y nchr oni z ed i nt get Ti meFor Dat aSent _s ec ( ) { r et ur n t s _s ec; };
publ i c s y nchr oni z ed i nt get Ti meFor Dat aSent _us ec ( ) { r et ur n t s _us ec; };

publ i c s y nchr oni z ed l ong get Ti meFor Dat aSampl ed ( ) {
r et ur n ( ( l ong) t _s ec) * 1000000 + t _us ec;
}
Anexe
pagina LXXVII

publ i c s y nchr oni z ed l ong get Ti meFor Dat aSent ( ) {
r et ur n ( ( l ong) t s _s ec) * 1000000 + t s _us ec;
}

/ **
* Abst r act const r uct or f or each Pl ayer Devi ce.
* @par ampl c a r ef er ence t o t he Pl ayer Cl i ent obj ect
*/
publ i c Pl ay er Dev i ce ( Pl ay er Cl i ent pl c) {
pc = pl c;
i s = pc. i s ;
os = pc. os ;
}

/ **
* Read t he t _sec, t _usec, t s_sec, t s_usec, r eser ved and si ze val ues f r omt he Pl ayer
* message header .
*/
pr ot ect ed s y nchr oni z ed v oi d r eadHeader ( ) {
t r y {
t _s ec = i s . r eadI nt ( ) ;
t _us ec = i s . r eadI nt ( ) ;
t s _s ec = i s . r eadI nt ( ) ;
t s _us ec = i s . r eadI nt ( ) ;
r es er v ed = i s . r eadI nt ( ) ;
s i z e = i s . r eadI nt ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Dev i ce] : Er r or when r eadi ng header : " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Sends a Pl ayer message header .
* @par amt ype t ype of message ( DATA, CMD, REQ, RESP_ACK, SYNCH, RESP_NACK, RESP_ERR)
* @par amsi ze si ze i n byt es of t he payl oad t o f ol l ow
*/
pr ot ect ed v oi d s endHeader ( s hor t t y pe, i nt s i z e) {
t r y {
/ * see pl ayer . h / pl ayer _msghdr f or addi t i onal expl anat i ons */
os . wr i t eShor t ( PLAYER_STXX) ; / * 0x5878 - t he message st ar t si gni f i er */
os . wr i t eShor t ( t y pe) ; / * DATA, CMD, REQ, RESP_ACK, SYNCH, RESP_NACK, RESP_ERR */
os . wr i t eShor t ( dev i ce) ; / * what ki nd of devi ce */
os . wr i t eShor t ( i ndex ) ; / * whi ch devi ce of what ki nd */
os . wr i t eI nt ( 0) ; / * ser ver ' s cur r ent t i me ( seconds) */
os . wr i t eI nt ( 0) ; / * ser ver ' s cur r ent t i me ( mi cr oseconds) */
os . wr i t eI nt ( 0) ; / * t i me when t he cur r ent dat a was gener at ed ( seconds) */
os . wr i t eI nt ( 0) ; / * t i me when t he cur r ent dat a was gener at ed ( mi cr oseconds) */
os . wr i t eI nt ( 0) ; / * r eser ved */
os . wr i t eI nt ( s i z e) ; / * si ze i n byt es of t he payl oad t o f ol l ow */
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Dev i ce] : Er r or when r eadi ng header : " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Read up t o si ze byt es of dat a.
*/
publ i c s y nchr oni z ed v oi d r eadDat a( ) {
r eadHeader ( ) ;
t r y {
f or ( i nt i = 0; i < s i z e; i ++)
i s . r eadBy t e ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Dev i ce] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Abst r act handl eNARMessage met hod.
*/
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Dev i ce] : Need t o handl e a NAR mes s age. ") ;
}

/ **
* Abst r act handl eEARMessage met hod.
*/
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Dev i ce] : Need t o handl e a EAR mes s age. ") ;
}

/ **
* Abst r act handl eResponse met hod ( t hr eaded mode) .
Anexe
pagina LXXVIII
*/
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Dev i ce] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
Sy s t em. er r . pr i nt l n ( "[ Pl ay er Dev i ce] : Gener al handl e r es ponce was t r i gger ed. ") ;
}
}

j ja av va ac cl li ie en nt t/ /P Po os si it ti io on n2 2D DI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Posi t i on2DI nt er f ace. j ava
*
*
*
*
*
* $I d: Posi t i on2DI nt er f ace. j ava, v 1. 7 2005/ 06/ 14 09: 41: 05 veedee Exp $
*
*/

i mpor t j av acl i ent . s t r uct ur es . Pl ay er Pos i t i on2DGeomT;

/ **
* The posi t i on2d i nt er f ace i s used t o cont r ol a pl anar mobi l e r obot base.
* Thi s i nt er f ace i s t he same as t he posi t i on i nt er f ace, but uses di f f er ent
* uni t s ( e. g. , mr ad i nst ead of deg) . The posi t i on2d i nt er f ace wi l l pr obabl y
* r epl ace t he posi t i on i nt er f ace ( event ual l y) .
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pos i t i on2DI nt er f ace ex t ends Abs t r act Pos i t i onDev i ce {


/ * 2- D posi t i on */
pr i v at e f i nal s hor t PLAYER_POSI TI ON2D_CODE = Pl ay er Cl i ent . PLAYER_POSI TI ON2D_CODE;


pr ot ect ed f i nal s hor t PLAYER_POSI TI ON2D_GET_GEOM_REQ = 1;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON2D_MOTOR_POWER_REQ = 2;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON2D_VELOCI TY_MODE_REQ = 3;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON2D_RESET_ODOM_REQ = 4;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON2D_POSI TI ON_MODE_REQ = 5;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON2D_SPEED_PI D_REQ = 6;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON2D_POSI TI ON_PI D_REQ = 7;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON2D_SPEED_PROF_REQ = 8;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON2D_SET_ODOM_REQ = 9;

/ * These ar e possi bl e Segway RMP conf i g commands; see t he st at us command i n
t he RMP manual */
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON2D_RMP_VELOCI TY_SCALE = 51;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON2D_RMP_ACCEL_SCALE = 52;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON2D_RMP_TURN_SCALE = 53;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON2D_RMP_GAI N_SCHEDULE = 54;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON2D_RMP_CURRENT_LI MI T = 55;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON2D_RMP_RST_I NTEGRATORS = 56;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON2D_RMP_SHUTDOWN = 57;

Anexe
pagina LXXIX
/ * These ar e used f or r eset t i ng t he Segway RMP' s i nt egr at or s. */
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_RMP_RST_I NT_RI GHT = 1;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_RMP_RST_I NT_LEFT = 2;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_RMP_RST_I NT_YAW = 3;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_RMP_RST_I NT_FOREAFT = 4;

/ * see pl ayer . h / pl ayer _posi t i on2d_dat a f or addi t i onal expl anat i ons */
pr i v at e i nt x Pos = 0; / * X posi t i on i n mm*/
pr i v at e i nt y Pos = 0; / * Y posi t i on i n mm*/
pr i v at e i nt y aw = 0; / * Yaw i n mr ad */
pr i v at e i nt x Speed = 0; / * X t r ansl at i onal vel oci t y i n mm/ sec */
pr i v at e i nt y Speed = 0; / * Y t r ansl at i onal vel oci t y i n mm/ sec */
pr i v at e i nt y awSpeed = 0; / * angul ar vel oci t y i n mr ad/ sec */
pr i v at e by t e s t al l s = 0; / * ar e t he mot or s st al l ed? */

/ * obj ect cont ai ni ng pl ayer _posi t i on2d_geomi n case of a t hr eaded cal l */
pr i v at e Pl ay er Pos i t i on2DGeomT ppgt ;
pr i v at e bool ean r eady PPGT = f al s e;

/ **
* Const r uct or f or Posi t i on2DI nt er f ace.
*/
publ i c Pos i t i on2DI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_POSI TI ON2D_CODE;
}

/ **
* Read t he posi t i on dat a val ues ( xPos, yPos, yaw, xSpeed, ySpeed, yawSpeed, st al l s) .
*/
t r y {
r eadHeader ( ) ;
x Pos = i s . r eadI nt ( ) ;
y Pos = i s . r eadI nt ( ) ;
y aw = i s . r eadI nt ( ) ;
x Speed = i s . r eadI nt ( ) ;
y Speed = i s . r eadI nt ( ) ;
y awSpeed = i s . r eadI nt ( ) ;
s t al l s = i s . r eadBy t e ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on2D] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng( ) ) ;
}
}

/ **
* Get X posi t i on i n mm.
* @r et ur n X posi t i on i n mm
*/
publ i c s y nchr oni z ed i nt get X ( ) { r et ur n x Pos ; }

/ **
* Get Y posi t i on i n mm.
* @r et ur n Y posi t i on i n mm
*/
publ i c s y nchr oni z ed i nt get Y ( ) { r et ur n y Pos ; }

/ **
* Get Headi ng i n mr ad.
* @r et ur n headi ng i n mr ad
*/
publ i c s y nchr oni z ed i nt get Yaw ( ) { r et ur n y aw; }

/ **
*/
publ i c s y nchr oni z ed by t e get St al l ( ) { r et ur n s t al l s ; }

/ **
* Get X t r ansl at i onal vel oci t y i n mm/ sec.
* @r et ur n X t r ansl at i onal vel oci t y i n mm/ sec
*/
publ i c s y nchr oni z ed i nt get XSpeed ( ) { r et ur n x Speed; }

/ **
* Get Y t r ansl at i onal vel oci t y i n mm/ sec.
* @r et ur n Y t r ansl at i onal vel oci t y i n mm/ sec
*/
publ i c s y nchr oni z ed i nt get YSpeed ( ) { r et ur n y Speed; }

/ **
Anexe
pagina LXXX
* Get angul ar vel oci t y i n mr ad/ sec.
* @r et ur n angul ar vel oci t y i n mr ad/ sec
*/
publ i c s y nchr oni z ed i nt get YawSpeed ( ) { r et ur n y awSpeed; }

/ **
* The posi t i on i nt er f ace accept s new posi t i ons and/ or vel oci t i es f or t he r obot ' s mot or s
* ( dr i ver s may suppor t posi t i on cont r ol , speed cont r ol or bot h) .
* 
* See t he pl ayer _posi t i on2d_cmd st r uct ur e f r ompl ayer . h
* @par amxP X posi t i on2d i n mm
* @par amyP Y posi t i on2d i n mm
* @par amyawT Yaw i n mr ad
* @par amxS X t r ansl at i onal vel oci t y i n mm/ sec
* @par amyS Y t r ansl at i onal vel oci t y i n mm/ sec
* @par amyawS angul ar vel oci t y i n mr ad/ sec
*/
publ i c v oi d s et Pos i t i on ( i nt x P, i nt y P, i nt y awT,
i nt x S, i nt y S, i nt y awS,
t r y {
os . wr i t eI nt ( x P) ;
os . wr i t eI nt ( y P) ;
os . wr i t eI nt ( y awT) ;
os . wr i t eI nt ( x S) ;
os . wr i t eI nt ( y S) ;
os . wr i t eI nt ( y awS) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on2D] : Coul dn' t s end pos i t i on commands : " + e. t oSt r i ng ( ) ) ;
}
}

/ ** Send posi t i on commands.
* @par amxP X posi t i on i n mm
* @par amyP Y posi t i on i n mm
* @par amyawT Yaw i n mr ad
*/
publ i c v oi d s et Pos i t i on ( i nt x P, i nt y P, i nt y awT) {
s et Pos i t i on ( x P, y P, y awT,
t hi s . x Speed, t hi s . y Speed, t hi s . y awSpeed, ( by t e) 1, ( by t e) 1) ;
}

/ **
* Set speed and t ur nr at e.
* @par amspeed X t r ansl at i onal vel oci t y i n mm/ sec
* @par amt ur nr at e angul ar vel oci t y i n mr ad/ sec
*/
publ i c v oi d s et Speed ( i nt s peed, i nt t ur nr at e) {
s et Pos i t i on ( t hi s . x Pos , t hi s . y Pos , t hi s . y aw,
s peed, t hi s . y Speed, t ur nr at e, ( by t e) 1, ( by t e) 0) ;
}

/ **
* Set speed, t ur nr at e and si deSpeed.
* @par amt ur nr at e angul ar vel oci t y i n mr ad/ sec
* @par amsi deSpeed Y t r ansl at i onal vel oci t y i n mm/ sec
*/
publ i c v oi d s et Speed ( i nt s peed, i nt t ur nr at e, i nt s i deSpeed) {
s et Pos i t i on( t hi s . x Pos , t hi s . y Pos , t hi s . y aw,
s peed, s i deSpeed, t ur nr at e, ( by t e) 1, ( by t e) 0) ;
}

/ **
*
*/
t r y {
os . wr i t e ( PLAYER_POSI TI ON2D_GET_GEOM_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on2D] : Coul dn' t s end " +
"PLAYER_POSI TI ON2D_GET_GEOM_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
Anexe
pagina LXXXI
*/
i f ( r eady PPGT) {
r eady PPGT = f al s e;
r et ur n t r ue;
}
r et ur n f al s e;
}

/ **
* @r et ur n an obj ect of t ype Pl ayer Posi t i on2DGeomT cont ai ni ng t he r equi r ed geomet r y dat a
*/
publ i c s y nchr oni z ed Pl ay er Pos i t i on2DGeomT get Geom ( ) { r et ur n ppgt ; }

/ **
*/
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on2D] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
cas e PLAYER_POSI TI ON2D_GET_GEOM_REQ: {
ppgt = new Pl ay er Pos i t i on2DGeomT ( ) ;

/ * pose of t he r obot base, i n t he r obot cs ( mm, mm, mr ad) */
s hor t [ ] ppos e = new s hor t [ 3] ;
/ * di mensi ons of t he base ( mm, mm) */
s hor t [ ] ps i z e = new s hor t [ 2] ;

ppos e[ 0] = i s . r eadShor t ( ) ; / * X pos i n mm*/
ppos e[ 1] = i s . r eadShor t ( ) ; / * Y pos i n mm*/
ppos e[ 2] = i s . r eadShor t ( ) ; / * Yaw i n mr ad */

//ps i z e[ 0] = ( s hor t ) i s . r eadUns i gnedShor t ( ) ; / * X base di mensi ons i n mm*/
ps i z e[ 0] = ( s hor t ) i s . r eadShor t ( ) ; / * X base di mensi ons i n mm*/
//ps i z e[ 1] = ( s hor t ) i s . r eadUns i gnedShor t ( ) ; / * Y base di mensi ons i n mm*/
ps i z e[ 1] = ( s hor t ) i s . r eadShor t ( ) ; / * Y base di mensi ons i n mm*/

ppgt . s et Pos e ( ppos e) ;
ppgt . s et Si z e ( ps i z e) ;
r eady PPGT = t r ue;
br eak;
}
cas e PLAYER_POSI TI ON2D_MOTOR_POWER_REQ: {
br eak;
}
cas e PLAYER_POSI TI ON2D_VELOCI TY_MODE_REQ: {
br eak;
}
cas e PLAYER_POSI TI ON2D_RESET_ODOM_REQ: {
br eak;
}
cas e PLAYER_POSI TI ON2D_POSI TI ON_MODE_REQ: {
br eak;
}
cas e PLAYER_POSI TI ON2D_SPEED_PI D_REQ: {
br eak;
}
cas e PLAYER_POSI TI ON2D_POSI TI ON_PI D_REQ: {
br eak;
}
cas e PLAYER_POSI TI ON2D_SPEED_PROF_REQ: {
br eak;
}
/ * Possi bl e Segway RMP conf i g commands. */
cas e PLAYER_POSI TI ON2D_RMP_VELOCI TY_SCALE: {
br eak;
}
cas e PLAYER_POSI TI ON2D_RMP_ACCEL_SCALE: {
br eak;
}
cas e PLAYER_POSI TI ON2D_RMP_TURN_SCALE: {
br eak;
}
cas e PLAYER_POSI TI ON2D_RMP_GAI N_SCHEDULE: {
Anexe
pagina LXXXII
br eak;
}
cas e PLAYER_POSI TI ON2D_RMP_CURRENT_LI MI T: {
br eak;
}
cas e PLAYER_POSI TI ON2D_RMP_RST_I NTEGRATORS: {
br eak;
}
cas e PLAYER_POSI TI ON2D_RMP_SHUTDOWN: {
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on2D] : Unex pect ed r es pons e " + s ubt y pe +
" of s i z e = " + s i z e) ;
br eak;
}
}
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on2D] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* 
* 
*/
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON2D_MOTOR_POWER_REQ) ;
os . wr i t eBy t e ( ( by t e) s t at e) ;
os . f l us h ( ) ;
"PLAYER_POSI TI ON2D_MOTOR_POWER_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Change vel oci t y cont r ol .
* 
* Some r obot s of f er di f f er ent vel oci t y cont r ol modes.
* 
* The p2os dr i ver of f er s t wo modes of vel oci t y cont r ol : separ at e t r ansl at i onal and r ot at i onal
* cont r ol and di r ect wheel cont r ol . When i n t he separ at e mode, t he r obot ' s mi cr ocont r ol l er
* i nt er nal l y comput es l ef t and r i ght wheel vel oci t i es based on t he cur r ent l y commanded
* t r ansl at i onal and r ot at i onal vel oci t i es and t hen at t enuat es t hese val ues t o mat ch a ni ce
* pr edef i ned accel er at i on pr of i l e. When i n t he di r ect mode, t he mi cr ocont r ol l er si mpl y passes
* on t he cur r ent l ef t and r i ght wheel vel oci t i es. Essent i al l y, t he separ at e mode of f er s
* smoot her but sl ower ( l ower accel er at i on) cont r ol , and t he di r ect mode of f er s f ast er but
* j er ki er ( hi gher accel er at i on) cont r ol . Pl ayer ' s def aul t i s t o use t he di r ect mode. Set mode
* t o zer o f or di r ect cont r ol and non- zer o f or separ at e cont r ol .
* 
* For t he r eb dr i ver , 0 i s di r ect vel oci t y cont r ol , 1 i s f or vel oci t y- based headi ng PD
* cont r ol l er .
* @par ammode dr i ver - speci f i c mode
*/
publ i c v oi d s et Vel oci t y Cont r ol ( by t e mode) {
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON2D_VELOCI TY_MODE_REQ) ;
os . f l us h ( ) ;
"PLAYER_POSI TI ON2D_VELOCI TY_MODE_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* 
* Reset s t he r obot ' s odomet r y t o ( x, y, t het a) = ( 0, 0, 0) .
*/
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON2D_RESET_ODOM_REQ) ;
os . f l us h ( ) ;
Anexe
pagina LXXXIII
"PLAYER_POSI TI ON2D_RESET_ODOM_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
*/
publ i c v oi d s et Cont r ol Mode ( i nt mode) {
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON2D_POSI TI ON_MODE_REQ) ;
os . wr i t eBy t e ( ( by t e) mode) ;
os . f l us h ( ) ;
"PLAYER_POSI TI ON2D_POSI TI ON_MODE_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* @par amxT X i n mm
* @par amyT Y i n mm
* @par amt het a Headi ng i n mr ad
*/
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON2D_SET_ODOM_REQ) ;
os . wr i t eI nt ( x T) ;
os . wr i t eI nt ( y T) ;
os . f l us h ( ) ;
"PLAYER_POSI TI ON2D_SET_ODOM_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
*/
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON2D_SPEED_PI D_REQ) ;
os . f l us h ( ) ;
"PLAYER_POSI TI ON2D_SPEED_PI D_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Set posi t i on PI D par amet er s.
*/
publ i c v oi d s et Pos i t i onPI DPar ams ( i nt kp, i nt ki , i nt kd) {
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON2D_POSI TI ON_PI D_REQ) ;
os . f l us h ( ) ;
"PLAYER_POSI TI ON2D_POSI TI ON_PI D_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Set speed pr of i l e par amet er s.
Anexe
pagina LXXXIV
*/
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON2D_SPEED_PROF_REQ) ;
os . f l us h ( ) ;
"PLAYER_POSI TI ON2D_SPEED_PROF_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
*/
t r y {
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on2D] : Handl i ng NAR of s i z e = " + s i z e) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on2D] : handl eRes pons ePos i t i on ERROR " + e. t oSt r i ng ( ) ) ;
}
}

/ **
*/
t r y {
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on2D] : Handl i ng EAR of s i z e = " + s i z e) ;
}
}
}

j ja av va ac cl li ie en nt t/ /P Po os si it ti io on n3 3D DI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Posi t i on3DI nt er f ace. j ava
*
*
*
*
*
* $I d: Posi t i on3DI nt er f ace. j ava, v 1. 6 2005/ 06/ 14 09: 41: 05 veedee Exp $
*
*/

i mpor t j av acl i ent . s t r uct ur es . Pl ay er Pos i t i on3DGeomT;

/ **
* The posi t i on3d i nt er f ace i s used t o cont r ol mobi l e r obot bases i n
* 3D ( i . e. , pi t ch and r ol l ar e i mpor t ant ) .
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pos i t i on3DI nt er f ace ex t ends Abs t r act Pos i t i onDev i ce {


/ * 3- D posi t i on */
pr i v at e f i nal s hor t PLAYER_POSI TI ON3D_CODE = Pl ay er Cl i ent . PLAYER_POSI TI ON3D_CODE;
Anexe
pagina LXXXV


pr ot ect ed f i nal s hor t PLAYER_POSI TI ON3D_GET_GEOM_REQ = 1;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON3D_MOTOR_POWER_REQ = 2;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON3D_VELOCI TY_MODE_REQ = 3;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON3D_RESET_ODOM_REQ = 4;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON3D_POSI TI ON_MODE_REQ = 5;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON3D_SPEED_PI D_REQ = 6;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON3D_POSI TI ON_PI D_REQ = 7;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON3D_SPEED_PROF_REQ = 8;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON3D_SET_ODOM_REQ = 9;

/ * see pl ayer . h / pl ayer _posi t i on3d_dat a f or addi t i onal expl anat i ons */
pr i v at e i nt x Pos = 0; / * X posi t i on i n mm*/
pr i v at e i nt y Pos = 0; / * Y posi t i on i n mm*/
pr i v at e i nt z Pos = 0; / * Y posi t i on i n mm*/
pr i v at e i nt r ol l = 0; / * r ol l i n mr ad */
pr i v at e i nt pi t ch = 0; / * pi t ch i n mr ad */
pr i v at e i nt y aw = 0; / * yaw i n mr ad */
pr i v at e i nt z Speed = 0; / * Z t r ansl at i onal vel oci t y i n mm/ sec */
pr i v at e i nt pi t chSpeed = 0; / * angul ar vel oci t y i n mr ad/ sec */
pr i v at e i nt r ol l Speed = 0; / * angul ar vel oci t y i n mr ad/ sec */
pr i v at e i nt y awSpeed = 0; / * angul ar vel oci t y i n mr ad/ sec */

/ * obj ect cont ai ni ng pl ayer _posi t i on3d_geomi n case of a t hr eaded cal l */
pr i v at e Pl ay er Pos i t i on3DGeomT ppgt ;

/ **
* Const r uct or f or Posi t i on3DI nt er f ace.
*/
publ i c Pos i t i on3DI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_POSI TI ON3D_CODE;
}

/ **
* Thi s i nt er f ace r et ur ns dat a r egar di ng t he odomet r i c pose and vel oci t y of
* t he r obot , as wel l as mot or st al l i nf or mat i on ( xPos, yPos, zPos, r ol l , pi t ch, yaw,
* xSpeed, ySpeed, zSpeed, r ol l Speed, pi t chSpeed, yawSpeed, st al l ) .
*/
t r y {
r eadHeader ( ) ;
x Pos = i s . r eadI nt ( ) ;
y Pos = i s . r eadI nt ( ) ;
z Pos = i s . r eadI nt ( ) ;
r ol l = i s . r eadI nt ( ) ;
pi t ch = i s . r eadI nt ( ) ;
z Speed = i s . r eadI nt ( ) ;
r ol l Speed = i s . r eadI nt ( ) ;
pi t chSpeed = i s . r eadI nt ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on3D] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng( ) ) ;
}
}

/ **
*/
publ i c s y nchr oni z ed i nt get X ( ) { r et ur n x Pos ; }

/ **
*/
publ i c s y nchr oni z ed i nt get Y ( ) { r et ur n y Pos ; }

/ **
Anexe
pagina LXXXVI
* Get Z posi t i on i n mm.
* @r et ur n Z posi t i on i n mm
*/
publ i c s y nchr oni z ed i nt get Z ( ) { r et ur n z Pos ; }

/ **
* Get angul ar vel oci t y ( r ol l ) i n mr ad.
* @r et ur n angul ar vel oci t y ( r ol l ) i n mr ad
*/
publ i c s y nchr oni z ed i nt get Rol l ( ) { r et ur n r ol l ; }

/ **
* Get angul ar vel oci t y ( pi t ch) i n mr ad.
* @r et ur n angul ar vel oci t y ( pi t ch) i n mr ad
*/
publ i c s y nchr oni z ed i nt get Pi t ch ( ) { r et ur n pi t ch; }

/ **
* Get angul ar vel oci t y ( yaw) i n mr ad.
* @r et ur n angul ar vel oci t y ( yaw) i n mr ad
*/

/ **
*/

/ **
*/

/ **
* Get Z t r ansl at i onal vel oci t y i n mm/ sec.
* @r et ur n Z t r ansl at i onal vel oci t y i n mm/ sec
*/
publ i c s y nchr oni z ed i nt get ZSpeed ( ) { r et ur n z Speed; }

/ **
* Get angul ar vel oci t y ( r ol l ) i n mr ad/ sec.
* @r et ur n angul ar vel oci t y ( r ol l ) i n mr ad/ sec
*/
publ i c s y nchr oni z ed i nt get Rol l Speed ( ) { r et ur n r ol l Speed; }

/ **
* Get angul ar vel oci t y ( pi t ch) i n mr ad/ sec.
* @r et ur n angul ar vel oci t y ( pi t ch) i n mr ad/ sec
*/
publ i c s y nchr oni z ed i nt get Pi t chSpeed ( ) { r et ur n pi t chSpeed; }

/ **
* Get angul ar vel oci t y ( yaw) i n mr ad/ sec.
* @r et ur n angul ar vel oci t y ( yaw) i n mr ad/ sec
*/

/ **
*/

/ **
* 
* See t he pl ayer _posi t i on3d_cmd st r uct ur e f r ompl ayer . h
* @par amzP Z posi t i on i n mm
* @par ampi t chP angul ar vel oci t y ( pi t ch) i n mr ad
* @par amr ol l P angul ar vel oci t y ( r ol l ) i n mr ad
* @par amyawP angul ar vel oci t y ( yaw) i n mr ad
* @par amzS Z t r ansl at i onal vel oci t y i n mm/ sec
* @par ampi t chS angul ar vel oci t y ( pi t ch) i n mr ad/ sec
* @par amr ol l S angul ar vel oci t y ( r ol l ) i n mr ad/ sec
* @par amyawS angul ar vel oci t y ( yaw) i n mr ad/ sec
*/
Anexe
pagina LXXXVII
publ i c v oi d s et Pos i t i on ( i nt x P, i nt y P, i nt z P,
i nt pi t chP, i nt r ol l P, i nt y awP,
i nt x S, i nt y S, i nt z S,
i nt pi t chS, i nt r ol l S, i nt y awS,
t r y {

/ * X posi t i on i n mm*/

/ * Y posi t i on i n mm*/

/ * Z posi t i on i n mm*/
os . wr i t eI nt ( z P) ;

/ * angul ar vel oci t y ( pi t ch) i n mr ad */
os . wr i t eI nt ( pi t chP) ;

/ * angul ar vel oci t y ( r ol l ) i n mr ad */
os . wr i t eI nt ( r ol l P) ;

/ * angul ar vel oci t y ( yaw) i n mr ad */
os . wr i t eI nt ( y awP) ;

/ * X t r ansl at i onal vel oci t y i n mm/ sec */

/ * Y t r ansl at i onal vel oci t y i n mm/ sec */

/ * Z t r ansl at i onal vel oci t y i n mm/ sec */
os . wr i t eI nt ( z S) ;

/ * angul ar vel oci t y ( pi t ch) i n mr ad/ sec */
os . wr i t eI nt ( pi t chS) ;

/ * angul ar vel oci t y ( r ol l ) i n mr ad/ sec */
os . wr i t eI nt ( r ol l S) ;

/ * angul ar vel oci t y ( yaw) i n mr ad/ sec */

os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on3D] : Coul dn' t s end pos i t i on commands : " + e. t oSt r i ng ( ) ) ;
}
}

*/
publ i c v oi d s et Pos i t i on ( i nt x P, i nt y P, i nt z P) {
s et Pos i t i on ( x P, y P, z P, t hi s . pi t ch, t hi s . r ol l , t hi s . y aw,
t hi s . x Speed, t hi s . y Speed, t hi s . z Speed,
t hi s . pi t chSpeed, t hi s . r ol l Speed, t hi s . y awSpeed,
( by t e) 1, ( by t e) 1) ;
}

* @par ampi t chP angul ar vel oci t y ( pi t ch) i n mr ad
* @par amr ol l P angul ar vel oci t y ( r ol l ) i n mr ad
* @par amyawP angul ar vel oci t y ( yaw) i n mr ad
*/
publ i c v oi d s et Pos i t i on ( i nt x P, i nt y P, i nt z P, i nt pi t chP, i nt r ol l P, i nt y awP) {
s et Pos i t i on ( x P, y P, z P, pi t chP, r ol l P, y awP,
t hi s . x Speed, t hi s . y Speed, t hi s . z Speed,
t hi s . pi t chSpeed, t hi s . r ol l Speed, t hi s . y awSpeed, ( by t e) 1, ( by t e) 1) ;
}

/ **
*/

Anexe
pagina LXXXVIII
publ i c v oi d s et Speed ( i nt x S, i nt y S) {
s et Pos i t i on ( t hi s . x Pos , t hi s . y Pos , t hi s . z Pos , t hi s . pi t ch, t hi s . r ol l , t hi s . y aw,
x S, y S, t hi s . z Speed, t hi s . pi t chSpeed, t hi s . r ol l Speed, t hi s . y awSpeed,
( by t e) 1, ( by t e) 0) ;
}

/ **
*/
publ i c v oi d s et Speed ( i nt x S, i nt y S, i nt z S) {
x S, y S, z S, t hi s . pi t chSpeed, t hi s . r ol l Speed, t hi s . y awSpeed,
( by t e) 1, ( by t e) 0) ;
}

/ **
* @par ampi t chS angul ar vel oci t y ( pi t ch) i n mr ad/ sec
* @par amr ol l S angul ar vel oci t y ( r ol l ) i n mr ad/ sec
* @par amyawS angul ar vel oci t y ( yaw) i n mr ad/ sec
*/
publ i c v oi d s et Speed ( i nt x S, i nt y S, i nt z S, i nt pi t chS, i nt r ol l S, i nt y awS) {
x S, y S, z S, pi t chS, r ol l S, y awS, ( by t e) 1, ( by t e) 0) ;
}

/ **
*
*/
t r y {
os . wr i t e ( PLAYER_POSI TI ON3D_GET_GEOM_REQ) ;
os . f l us h ( ) ;
"PLAYER_POSI TI ON3D_GET_GEOM_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
*/
r et ur n t r ue;
}
r et ur n f al s e;
}

/ **
* @r et ur n an obj ect of t ype Pl ayer Posi t i on3DGeomT cont ai ni ng t he r equi r ed geomet r y dat a
*/
publ i c s y nchr oni z ed Pl ay er Pos i t i on3DGeomT get Geom ( ) { r et ur n ppgt ; }

/ **
*/
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on2D] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
cas e PLAYER_POSI TI ON3D_GET_GEOM_REQ: {
ppgt = new Pl ay er Pos i t i on3DGeomT ( ) ;

/ * pose of t he r obot base, i n t he r obot cs ( mm, mm, mm, mr ad, mr ad, mr ad) */
/ * di mensi ons of t he base ( mm, mm, mm) */
Anexe
pagina LXXXIX

ppos e[ 2] = i s . r eadShor t ( ) ; / * Z pos i n mm*/
ppos e[ 3] = i s . r eadShor t ( ) ; / * Pi t ch i n mr ad */
ppos e[ 4] = i s . r eadShor t ( ) ; / * Rol l i n mr ad */
ppos e[ 5] = i s . r eadShor t ( ) ; / * Yaw i n mr ad */

ps i z e[ 0] = ( s hor t ) i s . r eadUns i gnedShor t ( ) ; / * X base di mensi ons i n mm*/
ps i z e[ 1] = ( s hor t ) i s . r eadUns i gnedShor t ( ) ; / * Y base di mensi ons i n mm*/
ps i z e[ 2] = ( s hor t ) i s . r eadUns i gnedShor t ( ) ; / * Z base di mensi ons i n mm*/

br eak;
}
cas e PLAYER_POSI TI ON3D_MOTOR_POWER_REQ: {
br eak;
}
cas e PLAYER_POSI TI ON3D_VELOCI TY_MODE_REQ: {
br eak;
}
cas e PLAYER_POSI TI ON3D_RESET_ODOM_REQ: {
br eak;
}
cas e PLAYER_POSI TI ON3D_POSI TI ON_MODE_REQ: {
br eak;
}
cas e PLAYER_POSI TI ON3D_SPEED_PI D_REQ: {
br eak;
}
cas e PLAYER_POSI TI ON3D_POSI TI ON_PI D_REQ: {
br eak;
}
cas e PLAYER_POSI TI ON3D_SPEED_PROF_REQ: {
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on3D] : Unex pect ed r es pons e " + s ubt y pe +
" of s i z e = " + s i z e) ;
br eak;
}
}
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on3D] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* 
* 
*/
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON3D_MOTOR_POWER_REQ) ;
os . f l us h ( ) ;
"PLAYER_POSI TI ON3D_MOTOR_POWER_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* 
* @par amst at e 0 f or vel oci t y mode, 1 f or posi t i on mode
*/
publ i c v oi d changePos i t i onCont r ol ( by t e s t at e) {
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON3D_POSI TI ON_MODE_REQ) ;
os . f l us h ( ) ;
"PLAYER_POSI TI ON3D_POSI TI ON_MODE_REQ command: " + e. t oSt r i ng ( ) ) ;
Anexe
pagina XC
}
}

/ **
* 
* Some r obot s of f er di f f er ent vel oci t y cont r ol modes. I t can be changed by sendi ng
* a r equest wi t h t he f or mat gi ven bel ow, i ncl udi ng t he appr opr i at e mode. No mat t er
* whi ch mode i s used, t he ext er nal cl i ent i nt er f ace t o t he posi t i on3d devi ce
* r emai ns t he same.
* 
* @par amval ue dr i ver - speci f i c
*/
publ i c v oi d s et Vel oci t y Cont r ol ( by t e v al ue) {
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON3D_VELOCI TY_MODE_REQ) ;
os . wr i t eBy t e ( v al ue) ;
os . f l us h ( ) ;
"PLAYER_POSI TI ON3D_VELOCI TY_MODE_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* @par amxP X i n mm
* @par amyP Y i n mm
* @par amzP Z i n mm
* @par amr ol l P Headi ng i n mr ad
* @par ampi t chP Headi ng i n mr ad
* @par amyawP Headi ng i n mr ad
*/
publ i c v oi d s et Odomet r y ( i nt x P, i nt y P, i nt z P,
i nt r ol l P, i nt pi t chP, i nt y awP) {
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON3D_SET_ODOM_REQ) ;
os . wr i t eI nt ( z P) ;
os . wr i t eI nt ( r ol l P) ;
os . wr i t eI nt ( pi t chP) ;
os . wr i t eI nt ( y awP) ;
os . f l us h ( ) ;
"PLAYER_POSI TI ON3D_SET_ODOM_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* 
*/
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON3D_RESET_ODOM_REQ) ;
os . f l us h ( ) ;
"PLAYER_POSI TI ON3D_RESET_ODOM_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
*/
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON3D_SPEED_PI D_REQ) ;
os . f l us h ( ) ;
"PLAYER_POSI TI ON3D_SPEED_PI D_REQ command: " + e. t oSt r i ng ( ) ) ;
Anexe
pagina XCI
}
}

/ **
*/
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON3D_POSI TI ON_PI D_REQ) ;
os . f l us h ( ) ;
"PLAYER_POSI TI ON3D_POSI TI ON_PI D_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* @par amsp max speed ( i n mr ad/ s)
* @par amacc max accel er at i on ( i n mr ad/ s/ s)
*/
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON3D_SPEED_PROF_REQ) ;
os . f l us h ( ) ;
"PLAYER_POSI TI ON3D_SPEED_PROF_REQ command: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
*/
t r y {
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on3D] : Handl i ng NAR of s i z e = " + s i z e) ;
}
}

/ **
*/
t r y {
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on3D] : Handl i ng EAR of s i z e = " + s i z e) ;
}
}
}

j ja av va ac cl li ie en nt t/ /P Po os si it ti io on nI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Posi t i onI nt er f ace. j ava
*
*
*
*
Anexe
pagina XCII
*
* $I d: Posi t i onI nt er f ace. j ava, v 1. 11 2005/ 06/ 14 09: 41: 05 veedee Exp $
*
*/

i mpor t j av acl i ent . s t r uct ur es . Pl ay er Pos i t i onGeomT;

/ **
* The posi t i on i nt er f ace i s used t o cont r ol mobi l e r obot bases i n 2D.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pos i t i onI nt er f ace ex t ends Abs t r act Pos i t i onDev i ce {


/ * devi ce t hat moves about */
pr i v at e s hor t PLAYER_POSI TI ON_CODE = Pl ay er Cl i ent . PLAYER_POSI TI ON_CODE;


pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_GET_GEOM_REQ = 1;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_MOTOR_POWER_REQ = 2;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_VELOCI TY_MODE_REQ = 3;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_RESET_ODOM_REQ = 4;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_POSI TI ON_MODE_REQ = 5;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_SPEED_PI D_REQ = 6;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_POSI TI ON_PI D_REQ = 7;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_SPEED_PROF_REQ = 8;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_SET_ODOM_REQ = 9;

/ * These ar e possi bl e Segway RMP conf i g commands; see t he st at us command i n
t he RMP manual */
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_RMP_VELOCI TY_SCALE = 51;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_RMP_ACCEL_SCALE = 52;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_RMP_TURN_SCALE = 53;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_RMP_GAI N_SCHEDULE = 54;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_RMP_CURRENT_LI MI T = 55;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_RMP_RST_I NTEGRATORS = 56;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_RMP_SHUTDOWN = 57;

/ * These ar e used f or r eset t i ng t he Segway RMP' s i nt egr at or s. */
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_RMP_RST_I NT_RI GHT = 1;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_RMP_RST_I NT_LEFT = 2;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_RMP_RST_I NT_YAW = 3;
pr ot ect ed f i nal s hor t PLAYER_POSI TI ON_RMP_RST_I NT_FOREAFT = 4;

/ * see pl ayer . h / pl ayer _posi t i on_dat a f or addi t i onal expl anat i ons */
pr i v at e i nt x = 0; / * X posi t i on i n mm*/
pr i v at e i nt y = 0; / * Y posi t i on i n mm*/
pr i v at e i nt y aw = 0; / * Yaw i n degr ees */
pr i v at e i nt y awSpeed = 0; / * angul ar vel oci t y i n degr ees/ sec */

/ * obj ect cont ai ni ng pl ayer _posi t i on_geomi n case of a t hr eaded cal l */
pr i v at e Pl ay er Pos i t i onGeomT ppgt ;

/ **
* Const r uct or f or Posi t i onI nt er f ace.
*/
publ i c Pos i t i onI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_POSI TI ON_CODE;
}

/ **
* Read t he posi t i on dat a val ues ( x, y, yaw, xSpeed, ySpeed, yawSpeed, st al l s) .
Anexe
pagina XCIII
*/
t r y {
r eadHeader ( ) ;
x = i s . r eadI nt ( ) ;
y = i s . r eadI nt ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng( ) ) ;
}
}

/ **
* 
* See t he pl ayer _posi t i on_cmd st r uct ur e f r ompl ayer . h
* @par amyawT Yaw i n degr ees
* @par amyawS angul ar vel oci t y i n degr ees/ sec
*/
publ i c v oi d s et Pos i t i on ( i nt x P, i nt y P, i nt y awT,
i nt x S, i nt y S, i nt y awS,
t r y {
os . wr i t eI nt ( y awT) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on] : Coul dn' t s end pos i t i on commands : " + e. t oSt r i ng ( ) ) ;
}
}

*/
publ i c v oi d s et Pos i t i on ( i nt x P, i nt y P, i nt y awT) {
s et Pos i t i on ( x P, y P, y awT,
}

/ ** Send t he headi ng of t he r obot .
*/
publ i c v oi d s et Headi ng ( i nt y awT) {
s et Pos i t i on ( t hi s . x , t hi s . y , y awT,
}

/ **
* @par amt ur nr at e angul ar vel oci t y i n degr ees/ sec
*/
publ i c v oi d s et Speed ( i nt s peed, i nt t ur nr at e) {
s et Pos i t i on ( t hi s . x , t hi s . y , t hi s . y aw,
s peed, t hi s . y Speed, t ur nr at e, ( by t e) 1, ( by t e) 0) ;
}

/ **
* @par amt ur nr at e angul ar vel oci t y i n degr ees/ sec
* @par amsi deSpeed Y t r ansl at i onal vel oci t y i n mm/ sec
*/

Anexe
pagina XCIV
publ i c v oi d s et Speed ( i nt s peed, i nt t ur nr at e, i nt s i deSpeed) {
s et Pos i t i on( t hi s . x , t hi s . y , t hi s . y aw, s peed, s i deSpeed, t ur nr at e, ( by t e) 1, ( by t e) 0) ;
}

/ **
*
*/
t r y {
os . wr i t e ( PLAYER_POSI TI ON_GET_GEOM_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on] : Coul dn' t s end PLAYER_POSI TI ON_GET_GEOM_REQ " +
}
}

/ **
*/
r et ur n t r ue;
}
r et ur n f al s e;
}

/ **
* @r et ur n an obj ect of t ype Pl ayer Posi t i onGeomT cont ai ni ng t he r equi r ed geomet r y dat a
*/
publ i c s y nchr oni z ed Pl ay er Pos i t i onGeomT get Geom ( ) { r et ur n ppgt ; }

/ **
*/
publ i c s y nchr oni z ed i nt get X ( ) { r et ur n x ; }

/ **
*/
publ i c s y nchr oni z ed i nt get Y ( ) { r et ur n y ; }

/ **
* Get Headi ng i n degr ees.
* @r et ur n headi ng i n degr ees
*/

/ **
*/

/ **
*/

/ **
* Get angul ar vel oci t y i n degr ees/ sec.
* @r et ur n angul ar vel oci t y i n degr ees/ sec
*/

/ **
*/

/ **
* 
* 
Anexe
pagina XCV
*/
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON_MOTOR_POWER_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on] : Coul dn' t s end PLAYER_POSI TI ON_MOTOR_POWER_REQ " +
}
}

/ **
* 
* Some r obot s of f er di f f er ent vel oci t y cont r ol modes.
* 
* The p2os dr i ver of f er s t wo modes of vel oci t y cont r ol : separ at e t r ansl at i onal and r ot at i onal
* cont r ol and di r ect wheel cont r ol . When i n t he separ at e mode, t he r obot ' s mi cr ocont r ol l er
* i nt er nal l y comput es l ef t and r i ght wheel vel oci t i es based on t he cur r ent l y commanded
* t r ansl at i onal and r ot at i onal vel oci t i es and t hen at t enuat es t hese val ues t o mat ch a ni ce
* pr edef i ned accel er at i on pr of i l e. When i n t he di r ect mode, t he mi cr ocont r ol l er si mpl y passes
* on t he cur r ent l ef t and r i ght wheel vel oci t i es. Essent i al l y, t he separ at e mode of f er s
* smoot her but sl ower ( l ower accel er at i on) cont r ol , and t he di r ect mode of f er s f ast er but
* j er ki er ( hi gher accel er at i on) cont r ol . Pl ayer ' s def aul t i s t o use t he di r ect mode. Set mode
* t o zer o f or di r ect cont r ol and non- zer o f or separ at e cont r ol .
* 
* For t he r eb dr i ver , 0 i s di r ect vel oci t y cont r ol , 1 i s f or vel oci t y- based headi ng PD
* cont r ol l er .
* @par ammode dr i ver - speci f i c mode
*/
publ i c v oi d s et Vel oci t y Cont r ol ( i nt mode) {
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON_VELOCI TY_MODE_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on] : Coul dn' t s end PLAYER_POSI TI ON_VELOCI TY_MODE_REQ " +
}
}

/ **
* 
*/
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON_RESET_ODOM_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on] : Coul dn' t s end PLAYER_POSI TI ON_RESET_ODOM_REQ " +
}
}

/ **
* Conf i gur at i on r equest : Change cont r ol mode.
*/
publ i c v oi d s et Cont r ol Mode ( i nt mode) {
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON_POSI TI ON_MODE_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on] : Coul dn' t s end PLAYER_POSI TI ON_POSI TI ON_MODE_REQ " +
}
}

/ **
* @par amxT X i n mm
* @par amyT Y i n m
* @par amt het a Headi ng i n degr ees
*/
Anexe
pagina XCVI
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON_SET_ODOM_REQ) ;
os . wr i t eI nt ( x T) ;
os . wr i t eI nt ( y T) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on] : Coul dn' t s end PLAYER_POSI TI ON_SET_ODOM_REQ " +
}
}

/ **
*/
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON_SPEED_PI D_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on] : Coul dn' t s end PLAYER_POSI TI ON_SPEED_PI D_REQ " +
}
}

/ **
*/
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON_POSI TI ON_PI D_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on] : Coul dn' t s end PLAYER_POSI TI ON_POSI TI ON_PI D_REQ " +
}
}

/ **
*/
publ i c v oi d s et SpeedPr of i l ePar ams ( s hor t s p, s hor t acc) {
t r y {
os . wr i t eBy t e ( PLAYER_POSI TI ON_SPEED_PROF_REQ) ;
os . wr i t eShor t ( s p) ;
os . wr i t eShor t ( acc) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on] : Coul dn' t s end PLAYER_POSI TI ON_SPEED_PROF_REQ " +
}
}

/ **
* Conf i gur at i on r equest : Segway RMP- speci f i c conf i gur at i on.
* 
* These ar e possi bl e Segway RMP conf i g commands; see t he st at us command i n t he RMP manual :
* <ul >
* <l i >PLAYER_POSI TI ON_RMP_VELOCI TY_SCALE = 51
* <l i >PLAYER_POSI TI ON_RMP_ACCEL_SCALE = 52
* <l i >PLAYER_POSI TI ON_RMP_TURN_SCALE = 53
* <l i >PLAYER_POSI TI ON_RMP_GAI N_SCHEDULE = 54
* <l i >PLAYER_POSI TI ON_RMP_CURRENT_LI MI T = 55
* <l i >PLAYER_POSI TI ON_RMP_RST_I NTEGRATORS = 56
* <l i >PLAYER_POSI TI ON_RMP_SHUTDOWN = 57
* <l i >PLAYER_POSI TI ON_RMP_RST_I NT_RI GHT = 1
* <l i >PLAYER_POSI TI ON_RMP_RST_I NT_LEFT = 2
Anexe
pagina XCVII
* <l i >PLAYER_POSI TI ON_RMP_RST_I NT_YAW = 3
* <l i >PLAYER_POSI TI ON_RMP_RST_I NT_FOREAFT = 4
* </ ul >
* @par amsubt ype must be of PLAYER_POSI TI ON_RMP_*
* @par amval ue hol ds var i ous val ues dependi ng on t he t ype of conf i g. See t he " St at us"
* command i n t he Segway manual .
*/
publ i c v oi d s et Segway RPMpar ams ( by t e s ubt y pe, s hor t v al ue) {
t r y {
os . wr i t eBy t e ( s ubt y pe) ;
os . wr i t eShor t ( v al ue) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on] : Coul dn' t s end Segway RPM conf i gur at i on r eques t : " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
*/
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
cas e PLAYER_POSI TI ON_GET_GEOM_REQ: {
ppgt = new Pl ay er Pos i t i onGeomT ( ) ;

/ * pose of t he r obot base, i n t he r obot cs ( mm, mm, degr ees) */

ppos e[ 2] = i s . r eadShor t ( ) ; / * Yaw i n degr ees */

ps i z e[ 0] = ( s hor t ) i s . r eadUns i gnedShor t ( ) ; / * X base di mensi ons i n mm*/
ps i z e[ 1] = ( s hor t ) i s . r eadUns i gnedShor t ( ) ; / * Y base di mensi ons i n mm*/

br eak;
}
cas e PLAYER_POSI TI ON_MOTOR_POWER_REQ: {
br eak;
}
cas e PLAYER_POSI TI ON_VELOCI TY_MODE_REQ: {
br eak;
}
cas e PLAYER_POSI TI ON_RESET_ODOM_REQ: {
br eak;
}
cas e PLAYER_POSI TI ON_POSI TI ON_MODE_REQ: {
br eak;
}
cas e PLAYER_POSI TI ON_SPEED_PI D_REQ: {
br eak;
}
cas e PLAYER_POSI TI ON_POSI TI ON_PI D_REQ: {
br eak;
}
cas e PLAYER_POSI TI ON_SPEED_PROF_REQ: {
br eak;
}
/ * Possi bl e Segway RMP conf i g commands. */
cas e PLAYER_POSI TI ON_RMP_VELOCI TY_SCALE: {
br eak;
}
cas e PLAYER_POSI TI ON_RMP_ACCEL_SCALE: {
br eak;
}
cas e PLAYER_POSI TI ON_RMP_TURN_SCALE: {
br eak;
}
cas e PLAYER_POSI TI ON_RMP_GAI N_SCHEDULE: {
Anexe
pagina XCVIII
br eak;
}
cas e PLAYER_POSI TI ON_RMP_CURRENT_LI MI T: {
br eak;
}
cas e PLAYER_POSI TI ON_RMP_RST_I NTEGRATORS: {
br eak;
}
cas e PLAYER_POSI TI ON_RMP_SHUTDOWN: {
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on] : Unex pect ed r es pons e " + s ubt y pe +
" of s i z e = " + s i z e) ;
br eak;
}
}
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
}
}

/ **
*/
t r y {
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on] : Handl i ng NAR of s i z e = " + s i z e) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on] : handl eRes pons ePos i t i on ERROR " + e. t oSt r i ng ( ) ) ;
}
}

/ **
*/
t r y {
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on] : Handl i ng EAR of s i z e = " + s i z e) ;
Sy s t em. er r . pr i nt l n ( "[ Pos i t i on] : handl eRes pons ePos i t i on ERROR " + e. t oSt r i ng ( ) ) ;
}
}

}

j ja av va ac cl li ie en nt t/ /P Po ow we er rI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Power I nt er f ace. j ava
*
*
*
*
*
* $I d: Power I nt er f ace. j ava, v 1. 2 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/

/ **
* The power i nt er f ace pr ovi des access t o a r obot ' s power subsyst em. Thi s i nt er f ace seems t o be
* depr ecat ed.
* @ver si on
* <ul >
Anexe
pagina XCIX
* </ ul >
*/
publ i c cl as s Power I nt er f ace ex t ends Pl ay er Dev i ce {

pr i v at e f i nal s hor t PLAYER_POWER_CODE = Pl ay er Cl i ent . PLAYER_POWER_CODE; / * power subsyst em*/


pr ot ect ed f i nal s hor t PLAYER_MAI N_POWER_REQ = 14;

/ * bat t er y vol t age, i n deci vol t s */
pr i v at e i nt char ge;

/ **
* Const r uct or f or Power I nt er f ace.
*/
publ i c Power I nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_POWER_CODE;
}

/ **
* Read t he bat t er y vol t age val ue.
*/
t r y {
r eadHeader ( ) ;
char ge = i s . r eadUns i gnedShor t ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Power ] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Get t he bat t er y char ge val ue.
* @r et ur n bat t er y vol t age as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Char ge ( ) { r et ur n char ge; }

/ **
* Request a bat t er y vol t age char ge val ue f r omt he Pl ayer ser ver .
*/
publ i c v oi d r eques t Char ge ( ) {
t r y {
os . wr i t eBy t e ( PLAYER_MAI N_POWER_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Power ] : Coul dn' t s end PLAYER_MAI N_POWER_REQ command: " +
e. t oSt r i ng ( ) ) ;
}
}
}

j ja av va ac cl li ie en nt t/ /P Pt tz zI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pt zI nt er f ace. j ava
*
*
*
*
*
* $I d: Pt zI nt er f ace. j ava, v 1. 4 2005/ 05/ 23 13: 13: 20 veedee Exp $
*
*/
Anexe
pagina C

/ **
* The pt z i nt er f ace i s used t o cont r ol a pan- t i l t - zoomuni t , such as a camer a.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pt z I nt er f ace ex t ends Pl ay er Dev i ce {


pr i v at e f i nal s hor t PLAYER_PTZ_CODE = Pl ay er Cl i ent . PLAYER_PTZ_CODE; / * pan- t i l t - zoomuni t */


/ ** maxi mumcommand l engt h f or use wi t h PLAYER_PTZ_GENERI C_CONFI G_REQ, based on t he
* Sony EVI D30 camer a r i ght now
*/
publ i c f i nal s hor t PLAYER_PTZ_MAX_CONFI G_LEN = 32;

/ * code f or gener i c conf i gur at i on r equest */
pr ot ect ed f i nal s hor t PLAYER_PTZ_GENERI C_CONFI G_REQ = 1;
/ * code f or cont r ol mode conf i gur at i on r equest */
pr ot ect ed f i nal s hor t PLAYER_PTZ_CONTROL_MODE_REQ = 2;
/ * code f or aut oser vo conf i gur at i on r equest */
pr ot ect ed f i nal s hor t PLAYER_PTZ_AUTOSERVO = 3;

pr i v at e s hor t pan; / * pan ( degr ees) */
pr i v at e s hor t t i l t ; / * t i l t ( degr ees) */
pr i v at e s hor t z oom; / * f i el d of vi ew ( degr ees) */
pr i v at e s hor t panSpeed; / * cur r ent pan vel oci t y ( deg/ sec) */
pr i v at e s hor t t i l t Speed; / * cur r ent t i l t vel oci t y ( deg/ sec) */

/ **
* Const r uct or f or Pt zI nt er f ace.
*/
publ i c Pt z I nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_PTZ_CODE;
}

/ **
* Read t he dat a r ef l ect i ng t he cur r ent st at e of t he Pan- Ti l t - Zoomuni t .
*/
r eadHeader ( ) ;
t r y {
pan = i s . r eadShor t ( ) ; / * pan ( degr ees) : - 100. . 100 */
t i l t = i s . r eadShor t ( ) ; / * t i l t ( degr ees) : - 25. . 25 */
z oom = i s . r eadShor t ( ) ; / * f i el d of vi ew ( degr ees) : 0. . 1023 */
panSpeed = i s . r eadShor t ( ) ; / * cur r ent pan vel oci t y ( deg/ sec) */
t i l t Speed = i s . r eadShor t ( ) ; / * cur r ent t i l t vel oci t y ( deg/ sec) */
Sy s t em. er r . pr i nt l n ( "[ Pt z ] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* The pt z i nt er f ace accept s commands t hat set change t he st at e of t he uni t . Not e t hat t he
* commands ar e absol ut e, not r el at i ve.
* @par ampan pan ( degr ees) : - 100. . 100
* @par amt i l t t i l t ( degr ees) : - 25. . 25
* @par amzoomf i el d of vi ew ( degr ees) : 0. . 1023
* @par ampanspeed cur r ent pan vel oci t y ( deg/ sec)
* @par amt i l t speed cur r ent t i l t vel oci t y ( deg/ sec)
*/
publ i c v oi d s et PTZ ( i nt pan, i nt t i l t , i nt z oom, i nt pans peed, i nt t i l t s peed) {
t r y {
os . wr i t eShor t ( ( s hor t ) pan) ; / * pan ( degr ees */
os . wr i t eShor t ( ( s hor t ) t i l t ) ; / * t i l t ( degr ees) */
os . wr i t eShor t ( ( s hor t ) z oom) ; / * f i el d of vi ew ( degr ees) */
os . wr i t eShor t ( ( s hor t ) pans peed) ; / * cur r ent pan vel oci t y ( deg/ sec) */
os . wr i t eShor t ( ( s hor t ) t i l t s peed) ; / * cur r ent t i l t vel oci t y ( deg/ sec) */
os . f l us h ( ) ;
Anexe
pagina CI
Sy s t em. er r . pr i nt l n ( "[ Pt z ] : Coul dn' t s end s et PTZ par amet er s command: " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Gener i c r equest .
* Thi s i oct l al l ows t he cl i ent t o send a uni t - speci f i c command t o t he uni t . Whet her dat a
* i s r et ur ned depends on t he command t hat was sent .
* @par aml engt h l engt h of dat a i n conf i g buf f er
* @par ambuf buf f er f or command/ r epl y
*/
publ i c v oi d gener i cReques t ( i nt l engt h, by t e[ ] buf ) {
i f ( l engt h > PLAYER_PTZ_MAX_CONFI G_LEN)
l engt h = PLAYER_PTZ_MAX_CONFI G_LEN;
t r y {
s endHeader ( PLAYER_MSGTYPE_REQ, l engt h + 3) ;
os . wr i t eBy t e ( PLAYER_PTZ_GENERI C_CONFI G_REQ) ;
os . wr i t eShor t ( ( s hor t ) l engt h) ; / * l engt h of dat a i n conf i g buf f er */

f or ( i nt i = 0; i < l engt h; i ++) {
os . wr i t eBy t e ( buf [ i ] ) ; / * buf f er f or command/ r epl y */
}
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pt z ] : Coul dn' t s end PLAYER_PTZ_GENERI C_CONFI G_REQ command: " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Cont r ol mode.
* Thi s i oct l al l ows t he cl i ent t o swi t ch bet ween posi t i on and vel oci t y cont r ol , f or t hose
* dr i ver s t hat suppor t i t . Not e t hat t hi s r equest changes how t he dr i ver i nt er pr et s
* f or t hcomi ng commands f r omal l cl i ent s.
* @par ammode mode t o use: must be ei t her PLAYER_PTZ_VELOCI TY_CONTROL ( 0) or
* PLAYER_PTZ_POSI TI ON_CONTROL ( 1)
*/
publ i c v oi d cont r ol Reques t ( i nt mode) {
t r y {
os . wr i t eBy t e ( PLAYER_PTZ_CONTROL_MODE_REQ) ;
os . wr i t eBy t e ( ( by t e) mode) ; / * 0 f or vel oci t y, 1 f or posi t i on */
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pt z ] : Coul dn' t s end PLAYER_PTZ_CONTROL_MODE_REQ command: " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Set Aut oSer vo mode.
* Enabl e/ Di sabl e Aut oSer vo mode on camer as such as t he CMUcam2.
* @par ammode 0=di sabl ed, 1=enabl ed
*/
publ i c v oi d s et Aut oSer v o ( i nt mode) {
t r y {
os . wr i t eBy t e ( PLAYER_PTZ_AUTOSERVO) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Pt z ] : Coul dn' t s end PLAYER_PTZ_AUTOSERVO command: " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
* Get pan i n degr ees.
* @r et ur n pan ( degr ees) as a shor t
*/
publ i c s y nchr oni z ed s hor t get Pan ( ) { r et ur n pan; }

/ **
* Get t i l t i n degr ees.
* @r et ur n t i l t ( degr ees) as a shor t
*/
publ i c s y nchr oni z ed s hor t get Ti l t ( ) { r et ur n t i l t ; }

/ **
* Get f i el d of vi ew i n degr ees.
* @r et ur n f i el d of vi ew ( degr ees) as a shor t
*/
publ i c s y nchr oni z ed s hor t get Zoom ( ) { r et ur n z oom; }
Anexe
pagina CII

/ **
* Get cur r ent pan vel oci t y i n deg/ sec.
* @r et ur n cur r ent pan vel oci t y ( deg/ sec) as a shor t
*/
publ i c s y nchr oni z ed s hor t get PanSpeed ( ) { r et ur n panSpeed; }

/ **
* Get cur r ent t i l t vel oci t y i n deg/ sec.
* @r et ur n cur r ent t i l t vel oci t y ( deg/ sec) as a shor t
*/
publ i c s y nchr oni z ed s hor t get Ti l t Speed ( ) { r et ur n t i l t Speed; }

/ **
*/
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ Pt z ] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
cas e PLAYER_PTZ_GENERI C_CONFI G_REQ: {
br eak;
}
cas e PLAYER_PTZ_CONTROL_MODE_REQ: {
br eak;
}
cas e PLAYER_PTZ_AUTOSERVO: {
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ Pt z ] : Unex pect ed r es pons e " + s ubt y pe +
" of s i z e = " + s i z e) ;
br eak;
}
}
Sy s t em. er r . pr i nt l n ( "[ Pt z ] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
}
}

}

j ja av va ac cl li ie en nt t/ /S Si im mu ul la at ti io on nI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Si mul at i onI nt er f ace. j ava
*
*
*
*
*
* $I d: Si mul at i onI nt er f ace. j ava, v 1. 4 2005/ 05/ 26 09: 14: 47 veedee Exp $
*
*/

i mpor t j av acl i ent . s t r uct ur es . Pl ay er Si mul at i onPos e2D;

/ **
* Pl ayer devi ces may ei t her be r eal har dwar e or vi r t ual devi ces gener at ed by a si mul at or such
* as St age or Gazebo. Thi s i nt er f ace pr ovi des di r ect access t o a si mul at or .
* @ver si on
* <ul >
Anexe
pagina CIII
* </ ul >
*/
publ i c cl as s Si mul at i onI nt er f ace ex t ends Pl ay er Dev i ce {


/ * si mul at or s */
pr i v at e f i nal s hor t PLAYER_SI MULATI ON_CODE = Pl ay er Cl i ent . PLAYER_SI MULATI ON_CODE;


pr ot ect ed f i nal s hor t PLAYER_SI MULATI ON_SET_POSE2D = 0;
pr ot ect ed f i nal s hor t PLAYER_SI MULATI ON_GET_POSE2D = 1;

/ ** t he maxi muml engt h of a st r i ng i ndent i f yi ng a si mul at i on obj ect */
publ i c s t at i c f i nal s hor t PLAYER_SI MULATI ON_I DENTI FI ER_MAXLEN = 64;

/ * obj ect cont ai ni ng pl ayer _si mul at i on_pose2d_r eq i n case of a t hr eaded cal l */
pr i v at e Pl ay er Si mul at i onPos e2D ps p2d;
pr i v at e bool ean r eady PSP2D = f al s e;

/ **
* Const r uct or f or Si mul at i onI nt er f ace.
*/
publ i c Si mul at i onI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_SI MULATI ON_CODE;
}

/ **
* Conf i gur at i on r equest : set 2D pose of a named si mul at i on obj ect .
* 
* To set or get t he pose of an obj ect i n a si mul at or , use t hi s message t ype. I f t he
* subt ype i s PLAYER_SI MULATI ON_SET_POSE2D, t he ser ver wi l l ask t he si mul at or t o move
* t he named obj ect t o t he l ocat i on speci f i ed by ( x, y, a) and r et ur n ACK. 
* See t he pl ayer _si mul at i on_pose2d_r eq st r uct ur e f r ompl ayer . h
*/
publ i c v oi d s et 2DPos e ( St r i ng i dent i f i er , i nt x , i nt y , i nt t het a) {
t r y {
i nt s i z e = 13 + PLAYER_SI MULATI ON_I DENTI FI ER_MAXLEN;
os . wr i t eBy t e ( PLAYER_SI MULATI ON_SET_POSE2D) ;
char [ ] i dent = i dent i f i er . t oChar Ar r ay ( ) ;
f or ( i nt i = 0; i < i dent i f i er . l engt h ( ) ; i ++)
os . wr i t eBy t e ( i dent [ i ] ) ;
f or ( i nt i = 0; i < PLAYER_SI MULATI ON_I DENTI FI ER_MAXLEN- i dent i f i er . l engt h ( ) ; i ++)
os . wr i t eI nt ( x ) ;
os . wr i t eI nt ( y ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Si mul at i on] : Coul dn' t s end PLAYER_SI MULATI ON_SET_POSE2D command: " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : get 2D pose of a named si mul at i on obj ect .
* 
* To set or get t he pose of an obj ect i n a si mul at or , use t hi s message t ype. I f t he
* subt ype i s PLAYER_SI MULATI ON_GET_POSE2D, t he ser ver wi l l at t empt t o l ocat e t he
* named obj ect and r epl y wi t h t he same packet wi t h ( x, y, a) f i l l ed i n. For al l message
* subt ypes, i f t he named obj ect does not exi st , or some ot her er r or occur s, t he r equest
* shoul d r epl y NACK. 
* See t he pl ayer _si mul at i on_pose2d_r eq st r uct ur e f r ompl ayer . h
*/
publ i c v oi d get 2DPos e ( St r i ng i dent i f i er ) {
t r y {
i nt s i z e = 1 + PLAYER_SI MULATI ON_I DENTI FI ER_MAXLEN;
os . wr i t eBy t e ( PLAYER_SI MULATI ON_GET_POSE2D) ;
char [ ] i dent = i dent i f i er . t oChar Ar r ay ( ) ;
f or ( i nt i = 0; i < i dent i f i er . l engt h ( ) ; i ++)
os . wr i t eBy t e ( i dent [ i ] ) ;
f or ( i nt i = 0; i < PLAYER_SI MULATI ON_I DENTI FI ER_MAXLEN- i dent i f i er . l engt h ( ) ; i ++)
os . f l us h ( ) ;
Anexe
pagina CIV
Sy s t em. er r . pr i nt l n ( "[ Si mul at i on] : Coul dn' t s end PLAYER_SI MULATI ON_GET_POSE2D command: " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
*/
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ Si mul at i on] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
cas e PLAYER_SI MULATI ON_GET_POSE2D: {
ps p2d = new Pl ay er Si mul at i onPos e2D ( ) ;
r eady PSP2D = t r ue;

/ * t he i dent i f i er of t he obj ect we want t o l ocat e */
St r i ng ps p2dName = new St r i ng ( ) ;
f or ( i nt j = 0; j < PLAYER_SI MULATI ON_I DENTI FI ER_MAXLEN ; j ++)
ps p2dName += ( char ) i s . r eadBy t e ( ) ;
ps p2d. s et Name ( ps p2dName) ;

/ * t he desi r ed pose or r et ur ned pose i n ( mm, mm, degr ees) */
ps p2d. s et X ( i s . r eadI nt ( ) ) ;
ps p2d. s et Y ( i s . r eadI nt ( ) ) ;
ps p2d. s et Thet a ( i s . r eadI nt ( ) ) ;
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ Si mul at i on] : Unex pect ed r es pons e " + s ubt y pe +
" of s i z e = " + s i z e) ;
br eak;
}
}
Sy s t em. er r . pr i nt l n ( "[ Si mul at i on] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Handl e negat i ve acknowl edgement r esponse messages.
*/
t r y {
Sy s t em. er r . pr i nt l n ( "[ Si mul at i on] : Handl i ng NAR of s i z e = " + s i z e) ;
Sy s t em. er r . pr i nt l n ( "[ Si mul at i on] : handl eRes pons ePos i t i on ERROR " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Get t he 2D pose of a named si mul at i on obj ect .
* @r et ur n an obj ect of t ype Pl ayer Si mul at i onPose2D cont ai ni ng t he r equi r ed pose dat a
*/
publ i c Pl ay er Si mul at i onPos e2D get Si mul at i onPos e2D ( ) { r et ur n ps p2d; }

/ **
* Check i f pose dat a i s avai l abl e.
*/
publ i c bool ean i s Pos e2DReady ( ) {
i f ( r eady PSP2D) {
r eady PSP2D = f al s e;
r et ur n t r ue;
}
r et ur n f al s e;
}

}

j ja av va ac cl li ie en nt t/ /S So on na ar rI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Sonar I nt er f ace. j ava
Anexe
pagina CV
*
*
*
*
*
* $I d: Sonar I nt er f ace. j ava, v 1. 5 2005/ 05/ 23 13: 13: 20 veedee Exp $
*
*/

i mpor t j av acl i ent . s t r uct ur es . Pl ay er Sonar GeomT;
/ **
* The sonar i nt er f ace pr ovi des access t o a col l ect i on of f i xed r ange sensor s, such as a sonar
* ar r ay. Thi s i nt er f ace accept s no commands.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Sonar I nt er f ace ex t ends Pl ay er Dev i ce {


/ * f i xed r ange- f i nder */
pr i v at e f i nal s hor t PLAYER_SONAR_CODE = Pl ay er Cl i ent . PLAYER_SONAR_CODE;

pr ot ect ed f i nal s hor t PLAYER_SONAR_GET_GEOM_REQ = 1;
pr ot ect ed f i nal s hor t PLAYER_SONAR_POWER_REQ = 2;


/ ** maxi mumnumber of sonar sampl es */
publ i c s t at i c f i nal s hor t PLAYER_SONAR_MAX_SAMPLES = 64;

pr i v at e i nt r ange[ ] = new i nt [ PLAYER_SONAR_MAX_SAMPLES] ;
pr i v at e i nt s ampl es Count = PLAYER_SONAR_MAX_SAMPLES;

/ * obj ect cont ai ni ng pl ayer _sonar _geomi n case of a t hr eaded cal l */
pr i v at e Pl ay er Sonar GeomT ps gt ;
pr i v at e bool ean r eady PSGT = f al s e;

/ **
* Const r uct or f or Sonar I nt er f ace.
*/
publ i c Sonar I nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_SONAR_CODE;
}

/ **
* Read t he sonar val ues.
*/
r eadHeader ( ) ;
t r y {
s ampl es Count = i s . r eadUns i gnedShor t ( ) ; / * t he number of val i d r ange r eadi ngs */
f or ( i nt i = 0; i < PLAYER_SONAR_MAX_SAMPLES; i ++)
r ange[ i ] = i s . r eadUns i gnedShor t ( ) ; / * t he r ange r eadi ngs */
Sy s t em. er r . pr i nt l n ( "[ Sonar ] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Ret ur ns t he sonar ar r ay dat a val ues up t o PLAYER_SONAR_MAX_SAMPLES.
Anexe
pagina CVI
* @r et ur n an ar r ay f i l l ed wi t h t he sonar val ues
*/
publ i c s y nchr oni z ed i nt [ ] get Ranges ( ) { r et ur n r ange; }

/ **
* Ret ur ns t he number of ul t r asoni c sensor s speci f i ed i n t he Pl ayer wor l d f i l e
* ( t he number of val i d r ange r eadi ngs) .
* @r et ur n t he number of ul t r asoni c sensor s speci f i ed i n t he Pl ayer wor l d f i l e as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Sampl es Count ( ) { r et ur n s ampl es Count ; }

/ **
* 
* See t he pl ayer _sonar _geomst r uct ur e f r ompl ayer . h
*/
t r y {
os . wr i t eBy t e ( PLAYER_SONAR_GET_GEOM_REQ) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Sonar ] : Coul dn' t s end PLAYER_SONAR_GET_GEOM_REQ command: " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
* Conf i gur at i on r equest : Sonar power . ( " avai l abl e onl y on r eal r obot s" ( TM) )
* 
* See t he pl ayer _sonar _power _conf i g st r uct ur e f r ompl ayer . h
* @par amval ue t ur n power of f ( 0) or on ( >0)
*/
publ i c v oi d s et Sonar Power ( i nt v al ue) {
t r y {
os . wr i t eBy t e ( PLAYER_SONAR_POWER_REQ) ;
os . wr i t eBy t e ( ( by t e) v al ue) ; / * t ur n power of f ( 0) or on ( >0) */
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Sonar ] : Coul dn' t s end PLAYER_SONAR_POWER_REQ command: " +
e. t oSt r i ng ( ) ) ;
}
}

/ **
*/
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ Sonar ] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {
cas e PLAYER_SONAR_GET_GEOM_REQ: {
ps gt = new Pl ay er Sonar GeomT ( ) ;

ps gt . s et Pos eCount ( i s . r eadShor t ( ) ) ;
s hor t s onar Pos es [ ] [ ] = new s hor t [ PLAYER_SONAR_MAX_SAMPLES] [ 3] ;
f or ( i nt i = 0; i < PLAYER_SONAR_MAX_SAMPLES; i ++) {
s onar Pos es [ i ] [ 0] = i s . r eadShor t ( ) ; / * X pos i n mm*/
s onar Pos es [ i ] [ 1] = i s . r eadShor t ( ) ; / * Y pos i n mm*/
s onar Pos es [ i ] [ 2] = i s . r eadShor t ( ) ; / * Yaw i n degr ees */
}
ps gt . s et Pos es ( s onar Pos es ) ;

r eady PSGT = t r ue;
br eak;
}
cas e PLAYER_SONAR_POWER_REQ: {
br eak;
}
def aul t : {
Sy s t em. er r . pr i nt l n ( "[ Sonar ] : Unex pect ed r es pons e " + s ubt y pe +
" of s i z e = " + s i z e) ;
br eak;
}
}
Sy s t em. er r . pr i nt l n ( "[ Sonar ] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
Anexe
pagina CVII
}
}

/ **
*/
publ i c bool ean i s PSGTReady ( ) {
i f ( r eady PSGT) {
r eady PSGT = f al s e;
r et ur n t r ue;
}
r et ur n f al s e;
}

/ **
* @r et ur n an obj ect of t ype Pl ayer Sonar GeomT cont ai ni ng t he r equi r ed geomet r y dat a
*/
publ i c Pl ay er Sonar GeomT get PSGT ( ) { r et ur n ps gt ; }
}

j ja av va ac cl li ie en nt t/ /S So ou un nd dI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - SoundI nt er f ace. j ava
* Copyr i ght ( C) 2003- 2005 J osh Ber s & Radu Bogdan Rusu
*
*
*
*
*
* $I d: SoundI nt er f ace. j ava, v 1. 3 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/

/ **
* The sound i nt er f ace al l ows pl ayback of a pr e- r ecor ded sound ( e. g. , on an Ami gobot ) .
* Thi s i nt er f ace pr ovi des no dat a.
* @aut hor J osh Ber s & Radu Bogdan Rusu
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s SoundI nt er f ace ex t ends Pl ay er Dev i ce {

/ * sound f i l e pl ayback */
pr i v at e f i nal s hor t PLAYER_SOUND_CODE = Pl ay er Cl i ent . PLAYER_SOUND_CODE;


/ **
* Const r uct or f or SoundI nt er f ace.
*/
publ i c SoundI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_SOUND_CODE;
}

/ **
* Send a pl ay command t o t he Sound devi ce.
* @par ami ndex i ndex of sound t o be pl ayed
*/
publ i c v oi d pl ay ( s hor t i ndex ) {
Anexe
pagina CVIII
t r y {
os . wr i t eShor t ( i ndex ) ; / * i ndex of sound t o be pl ayed */
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Sound] : Coul dn' t s end pl ay command r eques t : " +
e. t oSt r i ng ( ) ) ;
}
}
}

j ja av va ac cl li ie en nt t/ /S Sp pe ee ec ch hI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - SpeechI nt er f ace. j ava
*
*
*
*
*
* $I d: SpeechI nt er f ace. j ava, v 1. 2 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/

/ **
* The speech i nt er f ace pr ovi des access t o a speech synt hesi s syst em.
* The speech i nt er f ace r et ur ns no dat a.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s SpeechI nt er f ace ex t ends Pl ay er Dev i ce {

pr i v at e f i nal s hor t PLAYER_SPEECH_CODE = Pl ay er Cl i ent . PLAYER_SPEECH_CODE; / * speech I / O */

/ ** maxi mumst r i ng l engt h */
publ i c s t at i c f i nal s hor t PLAYER_SPEECH_MAX_STRI NG_LEN = 256;


/ **
* Const r uct or f or SpeechI nt er f ace.
*/
publ i c SpeechI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_SPEECH_CODE;
}

/ **
* The speech i nt er f ace accept s a command t hat i s a st r i ng t o be gi ven t o t he speech
* synt hesi zer . The f i r st PLAYER_SPEECH_MAX_STRI NG_LEN char act er s wi l l be sai d.
* @par amt ext t he st r i ng t o say
*/
publ i c v oi d s peech ( St r i ng t ex t ) {
St r i ng t emp = t ex t ;
i f ( t ex t . l engt h ( ) > PLAYER_SPEECH_MAX_STRI NG_LEN)
t emp = t ex t . s ubs t r i ng ( 0, PLAYER_SPEECH_MAX_STRI NG_LEN) ;
t r y {
s endHeader ( PLAYER_MSGTYPE_CMD, t emp. l engt h ( ) ) ;
os . wr i t eBy t es ( t emp) ; / * t he st r i ng t o say */
os . f l us h ( ) ;
Anexe
pagina CIX
Sy s t em. er r . pr i nt l n ( "[ Speech] : Coul dn' t s end s peech command r eques t : " +
e. t oSt r i ng ( ) ) ;
}
}
}

j ja av va ac cl li ie en nt t/ /S Sp pe ee ec ch hR Re ec co og gn ni it ti io on nI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - SpeechRecogni t i onI nt er f ace. j ava
*
*
*
*
*
* $I d: SpeechRecogni t i onI nt er f ace. j ava, v 1. 2 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/

/ **
* The speech r ecogni t i on i nt er f ace pr ovi des access t o a speech r ecogni t i on ser ver .
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s SpeechRecogni t i onI nt er f ace ex t ends Pl ay er Dev i ce {

/ * speech r ecogni t i on I / O */
pr i v at e f i nal s hor t PLAYER_SPEECH_RECOGNI TI ON_CODE =
Pl ay er Cl i ent . PLAYER_SPEECH_RECOGNI TI ON_CODE;

publ i c s t at i c f i nal s hor t SPEECH_RECOGNI TI ON_TEXT_LEN = 256;

/ * t he speech r ecogni t i on dat a packet */
pr i v at e char [ ] t ex t = new char [ SPEECH_RECOGNI TI ON_TEXT_LEN] ;

/ **
* Const r uct or f or SpeechRecogni t i onI nt er f ace.
*/
publ i c SpeechRecogni t i onI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_SPEECH_RECOGNI TI ON_CODE;
}

/ **
* Read t he speech r ecogni t i on dat a packet .
*/
r eadHeader ( ) ;
t r y {
f or ( i nt i = 0; i < SPEECH_RECOGNI TI ON_TEXT_LEN; i ++)
t ex t [ i ] = i s . r eadChar ( ) ; / * t he speech r ecogni t i on dat a packet */
Sy s t em. er r . pr i nt l n ( "[ SpeechRecogni t i on] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Ret ur ns t he speech r ecogni t i on dat a packet .
* @r et ur n an ar r ay of up t o SPEECH_RECOGNI TI ON_TEXT_LEN char s f i l l ed wi t h dat a
*/
publ i c s y nchr oni z ed char [ ] get Tex t ( ) { r et ur n t hi s . t ex t ; }
}
Anexe
pagina CX

j ja av va ac cl li ie en nt t/ /T Tr ru ut th hI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Tr ut hI nt er f ace. j ava
*
*
*
*
*
* $I d: Tr ut hI nt er f ace. j ava, v 1. 3 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/

/ **
* The t r ut h i nt er f ace pr ovi des access t o t he absol ut e st at e of ent i t i es. Not e t hat , unl ess your
* r obot has super power s, t r ut h devi ces ar e onl y avi l abl e i n si mul at i on.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Tr ut hI nt er f ace ex t ends Pl ay er Dev i ce {


/ * gr ound- t r ut h ( vi a St age) */
pr i v at e f i nal s hor t PLAYER_TRUTH_CODE = Pl ay er Cl i ent . PLAYER_TRUTH_CODE;


pr ot ect ed f i nal s hor t PLAYER_TRUTH_GET_POSE = 0;
pr ot ect ed f i nal s hor t PLAYER_TRUTH_SET_POSE = 1;
pr ot ect ed f i nal s hor t PLAYER_TRUTH_SET_POSE_ON_ROOT = 2;
pr ot ect ed f i nal s hor t PLAYER_TRUTH_GET_FI DUCI AL_I D = 3;
pr ot ect ed f i nal s hor t PLAYER_TRUTH_SET_FI DUCI AL_I D = 4;

/ * obj ect posi t i on i n t he wor l d ( x, y, z) i n mm*/
pr i v at e i nt x Pos = 0;
pr i v at e i nt y Pos = 0;
pr i v at e i nt z Pos = 0;
/ * obj ect or i ent at i on i n t he wor l d ( r , p, y) i n mi l l i r ad */
pr i v at e i nt r Head = 0;
pr i v at e i nt pHead = 0;
pr i v at e i nt y Head = 0;

pr i v at e bool ean i s Tel epor t ed = f al s e;

/ **
* Const r uct or f or Tr ut hI nt er f ace.
*/
publ i c Tr ut hI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_TRUTH_CODE;
}

/ **
* Read t he cur r ent st at e of t he ent i t y.
*/
r eadHeader ( ) ;
t r y {
Anexe
pagina CXI
x Pos = i s . r eadI nt ( ) ; / * X posi t i on i n t he wor l d i n mm*/
y Pos = i s . r eadI nt ( ) ; / * Y posi t i on i n t he wor l d i n mm*/
z Pos = i s . r eadI nt ( ) ; / * Z posi t i on i n t he wor l d i n mm*/
r Head = i s . r eadI nt ( ) ; / * R or i ent at i on i n t he wor l d i n mi l l i r ad */
pHead = i s . r eadI nt ( ) ; / * P or i ent at i on i n t he wor l d i n mi l l i r ad */
y Head = i s . r eadI nt ( ) ; / * Y or i ent at i on i n t he wor l d i n mi l l i r ad */
Sy s t em. er r . pr i nt l n ( "[ Tr ut h] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
*/
publ i c s y nchr oni z ed i nt get Xpos ( ) { r et ur n x Pos ; }

/ **
*/
publ i c s y nchr oni z ed i nt get Ypos ( ) { r et ur n y Pos ; }

/ **
* Get Z posi t i on i n mm.
* @r et ur n Z posi t i on i n mm
*/
publ i c s y nchr oni z ed i nt get Zpos ( ) { r et ur n z Pos ; }

/ **
* Get R or i ent at i on i n mi l l i r ad.
* @r et ur n R or i ent at i on i n mi l l i r ad
*/
publ i c s y nchr oni z ed i nt get Rhead ( ) { r et ur n r Head; }

/ **
* Get P or i ent at i on i n mi l l i r ad.
* @r et ur n P or i ent at i on i n mi l l i r ad
*/
publ i c s y nchr oni z ed i nt get Phead ( ) { r et ur n pHead; }

/ **
* Get Y or i ent at i on i n mi l l i r ad.
* @r et ur n Y or i ent at i on i n mi l l i r ad
*/
publ i c s y nchr oni z ed i nt get Yhead ( ) { r et ur n y Head; }

/ **
* Set t he pose of t he cur r ent ent i t y.
* @par amx X posi t i on i n mm
* @par amy Y posi t i on i n mm
*/
publ i c v oi d t el epor t ( i nt x , i nt y ) {
t r y {
os . wr i t eBy t e ( PLAYER_TRUTH_SET_POSE) ;
os . wr i t eI nt ( z Pos ) ;
os . wr i t eI nt ( r Head) ;
os . wr i t eI nt ( pHead) ;
os . wr i t eI nt ( y Head) ;
os . f l us h ( ) ;
Sy s t em. er r . pr i nt l n ( "[ Tr ut h] : Coul dn' t s end PLAYER_TRUTH_SET_POSE command: " +
e. t oSt r i ng ( ) ) ;
}
i s Tel epor t ed = ! i s Tel epor t ed;
}

/ **
* @par amx X posi t i on i n mm
* @par amy Y posi t i on i n mm
* @par amz Z posi t i on i n mm
*/
publ i c v oi d t el epor t ( i nt x , i nt y , i nt z ) {
t r y {
os . wr i t eI nt ( z ) ;
os . wr i t eI nt ( r Head) ;
os . wr i t eI nt ( pHead) ;
Anexe
pagina CXII
os . wr i t eI nt ( y Head) ;
os . f l us h ( ) ;
e. t oSt r i ng ( ) ) ;
}
}

/ **
* @par amxp X posi t i on i n mm
* @par amyp Y posi t i on i n mm
* @par amzp Z posi t i on i n mm
* @par amr o R or i ent at i on i n mi l l i r ad
* @par ampo P or i ent at i on i n mi l l i r ad
* @par amyo Y or i ent at i on i n mi l l i r ad
*/
publ i c v oi d t el epor t ( i nt x p, i nt y p, i nt z p, i nt r o, i nt po, i nt y o) {
t r y {
os . wr i t eI nt ( x p) ;
os . wr i t eI nt ( y p) ;
os . wr i t eI nt ( z p) ;
os . wr i t eI nt ( r o) ;
os . wr i t eI nt ( po) ;
os . wr i t eI nt ( y o) ;
os . f l us h ( ) ;
e. t oSt r i ng ( ) ) ;
}
}

/ **
* Check i f t he t el epor t at i on ( PLAYER_TRUTH_SET_POSE) was successf ul or not .
* @r et ur n t r ue i f successf ul , f al se i f not
*/
publ i c bool ean i s Tel epor t ed ( ) { r et ur n i s Tel epor t ed; }

/ **
*/
i f ( s i z e == 0) {
Sy s t em. er r . pr i nt l n ( "[ Tr ut h] [ Debug] : Unex pect ed r es pons e of s i z e 0! ") ;
r et ur n;
}
t r y {

Sy s t em. er r . pr i nt l n ( "[ Tr ut h] : Er r or when r eadi ng pay l oad " + e. t oSt r i ng ( ) ) ;
}
}
}

j ja av va ac cl li ie en nt t/ /W Wa av ve ef fo or rm mI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Wavef or mI nt er f ace. j ava
*
*
*
*
*
* $I d: Wavef or mI nt er f ace. j ava, v 1. 2 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
Anexe
pagina CXIII
*/

/ **
* The wavef or mi nt er f ace i s used t o r ecei ve ar bi t r ar y di gi t al sampl es, say f r om
* a di gi t al audi o devi ce.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Wav ef or mI nt er f ace ex t ends Pl ay er Dev i ce {

/ * f et ch r aw wavef or ms */
pr i v at e f i nal s hor t PLAYER_WAVEFORM_CODE = Pl ay er Cl i ent . PLAYER_WAVEFORM_CODE;

/ ** 4K - hal f t he packet max */
publ i c s t at i c f i nal s hor t PLAYER_WAVEFORM_DATA_MAX = 4096;

pr i v at e i nt r at e; / * bi t r at e - bi t s per second */
pr i v at e i nt dept h; / * dept h - bi t s per sampl e */
pr i v at e i nt s ampl es ; / * sampl es - t he number of byt es of r aw dat a */
/ * dat a - an ar r ay of r aw dat a */
pr i v at e by t e[ ] dat a = new by t e[ PLAYER_WAVEFORM_DATA_MAX] ;

/ **
* Const r uct or f or Wavef or mI nt er f ace.
*/
publ i c Wav ef or mI nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_WAVEFORM_CODE;
}

/ **
* The wavef or mi nt er f ace r eads a di gi t i zed wavef or mf r omt he t ar get devi ce.
*/
r eadHeader ( ) ;
t r y {
r at e = i s . r eadI nt ( ) ; / * bi t r at e - bi t s per second */
dept h = i s . r eadUns i gnedShor t ( ) ; / * dept h - bi t s per sampl e */
s ampl es = i s . r eadI nt ( ) ; / * sampl es - t he number of byt es of r aw dat a */
i f ( s ampl es > PLAYER_WAVEFORM_DATA_MAX)
s ampl es = PLAYER_WAVEFORM_DATA_MAX;
f or ( i nt i = 0; i < s ampl es ; i ++) {
dat a[ i ] = ( by t e) i s . r eadUns i gnedBy t e ( ) ; / * t he ar r ay of r aw dat a */
}
Sy s t em. er r . pr i nt l n ( "[ Wav ef or m] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}

/ **
* Ret ur ns t he cur r ent bi t r at e i n bi t s per second.
* @r et ur n t he cur r ent bi t r at e as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Rat e ( ) { r et ur n t hi s . r at e; }

/ **
* Ret ur ns t he cur r ent dept h i n bi t s per sampl e.
* @r et ur n t he cur r ent dept h as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Dept h ( ) { r et ur n t hi s . dept h; }

/ **
* Ret ur ns t he number of sampl es.
* @r et ur n t he number of byt es of r aw dat a t o f ol l ow as an i nt eger
*/
publ i c s y nchr oni z ed i nt get Sampl es ( ) { r et ur n t hi s . s ampl es ; }

/ **
* Ret ur ns t he dat a ar r ay
* @r et ur n t he ar r ay of r aw dat a as an ar r ay of byt es
*/
publ i c s y nchr oni z ed by t e[ ] get Dat a ( ) { r et ur n t hi s . dat a; }
}

Anexe
pagina CXIV

j ja av va ac cl li ie en nt t/ /W Wi iF Fi iI In nt te er rf fa ac ce e. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Wi Fi I nt er f ace. j ava
*
*
*
*
*
* $I d: Wi Fi I nt er f ace. j ava, v 1. 2 2005/ 05/ 10 12: 10: 23 veedee Exp $
*
*/

/ **
* The wi f i i nt er f ace pr ovi des access t o t he st at e of a wi r el ess net wor k i nt er f ace.
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Wi Fi I nt er f ace ex t ends Pl ay er Dev i ce {

pr i v at e f i nal s hor t PLAYER_WI FI _CODE = Pl ay er Cl i ent . PLAYER_WI FI _CODE; / * wi f i car d st at us */

pr i v at e s hor t l i nkCount = 0; / * l engt h of sai d l i st */
pr i v at e i nt t hr oughput ; / * myst er i ous t hr oughput cal cul at ed by dr i ver */
pr i v at e i nt bi t r at e; / * cur r ent bi t r at e of devi ce */
pr i v at e by t e mode; / * oper at i ng mode of devi ce */
pr i v at e by t e qual Ty pe; / * i ndi cat es t ype of l i nk qual i t y i nf o we have */
pr i v at e s hor t max Qual ; / * maxi mumval ues f or qual i t y, l evel and noi se */
pr i v at e s hor t max Lev el ;
pr i v at e s hor t max Noi s e;
pr i v at e char [ ] ap = new char [ 32] ; / * MAC addr ess of cur r ent access poi nt / cel l */
/ **
* Const r uct or f or Wi Fi I nt er f ace.
*/
publ i c Wi Fi I nt er f ace ( Pl ay er Cl i ent pc, s hor t i ndex Of Dev i ce) {
s uper ( pc) ;
dev i ce = PLAYER_WI FI _CODE;
}

/ **
* Read t he Wi Fi i nf or mat i on.
*/
r eadHeader ( ) ;
t r y {
l i nkCount = ( s hor t ) i s . r eadUns i gnedShor t ( ) ; / * l engt h of sai d l i st */
t hr oughput = i s . r eadI nt ( ) ; / * t hr oughput */
bi t r at e = i s . r eadI nt ( ) ; / * cur r ent bi t r at e of devi ce */
mode = ( by t e) i s . r eadUns i gnedBy t e ( ) ; / * oper at i ng mode of devi ce */
qual Ty pe = ( by t e) i s . r eadUns i gnedBy t e ( ) ; / * t ype of l i nk qual i t y i nf o */
max Qual = ( s hor t ) i s . r eadUns i gnedShor t ( ) ; / * maxi mumval ues f or qual i t y */
max Lev el = ( s hor t ) i s . r eadUns i gnedShor t ( ) ; / * maxi mumval ues f or l evel */
max Noi s e = ( s hor t ) i s . r eadUns i gnedShor t ( ) ; / * maxi mumval ues f or noi se */
f or ( i nt i = 0; i < 32; i ++)
ap[ i ] = i s . r eadChar ( ) ; / * MAC addr ess of cur r ent access poi nt / cel l */
Sy s t em. er r . pr i nt l n ( "[ Wi Fi ] : Er r or when r eadi ng pay l oad: " + e. t oSt r i ng ( ) ) ;
}
}
}
Anexe
pagina CXV

j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /B Bl lo ob b. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Bl ob. j ava
*
*
*
*
*
* $I d: Bl ob. j ava, v 1. 2 2005/ 05/ 10 12: 10: 24 veedee Exp $
*
*/

package j av acl i ent . s t r uct ur es ;

/ **
* St r uct ur e descr i bi ng a si ngl e bl ob. 
* ( see t he pl ayer _bl obf i nder _bl ob st r uct ur e f r ompl ayer . h)
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Bl ob {
pr i v at e s hor t i d; / * bl ob I D */

/ * a descr i pt i ve col or f or t he bl ob ( usef ul f or gui ' s) . The col or i s st or ed as packed 32- bi t
* RGB, i . e. , 0x00RRGGBB.
*/
pr i v at e i nt col or ;

pr i v at e i nt ar ea; / * t he bl ob ar ea ( pi xel s) */
pr i v at e s hor t x , y ; / * t he bl ob cent r oi d ( i mage coor ds) */
pr i v at e s hor t l ef t , r i ght , t op, bot t om; / * boundi ng box f or t he bl ob ( i mage coor ds) */
pr i v at e s hor t r ange; / * r ange ( mm) t o t he bl ob cent er */

/ **
*
* @r et ur n bl ob I D
*/
publ i c s y nchr oni z ed s hor t get I D ( ) {
r et ur n t hi s . i d;
}

/ **
*
* @par amnewi d bl ob I D
*/
publ i c s y nchr oni z ed v oi d s et I D ( s hor t newi d) {
t hi s . i d = newi d;
}

/ **
*
*@r et ur n a descr i pt i ve col or f or t he bl ob. The col or i s st or ed as packed 32- bi t
* RGB, i . e. , 0x00RRGGBB.
*/
publ i c s y nchr oni z ed i nt get Col or ( ) {
r et ur n t hi s . col or ;
}

/ **
*
* @par amnewcol or a descr i pt i ve col or f or t he bl ob ( usef ul f or gui ) . The col or must be packed
* as a 32- bi t RGB, i . e. , 0x00RRGGBB.
*/
Anexe
pagina CXVI
publ i c s y nchr oni z ed v oi d s et Col or ( i nt newcol or ) {
t hi s . col or = newcol or ;
}

/ **
*
* @r et ur n t he bl ob ar ea ( pi xel s)
*/
publ i c s y nchr oni z ed i nt get Ar ea( ) {
r et ur n t hi s . ar ea;
}

/ **
*
* @par amnewar ea t he bl ob ar ea ( pi xel s)
*/
publ i c s y nchr oni z ed v oi d s et Ar ea( i nt newar ea) {
t hi s . ar ea = newar ea;
}

/ **
*
* @r et ur n t he bl ob cent r oi d ( i mage coor ds, X)
*/
publ i c s y nchr oni z ed s hor t get X ( ) {
r et ur n t hi s . x ;
}

/ **
*
* @par amnewx t he bl ob cent r oi d ( i mage coor ds, X)
*/
publ i c s y nchr oni z ed v oi d s et X ( s hor t newx ) {
t hi s . x = newx ;
}

/ **
*
* @r et ur n t he bl ob cent r oi d ( i mage coor ds, Y)
*/
publ i c s y nchr oni z ed s hor t get Y ( ) {
r et ur n t hi s . y ;
}

/ **
*
* @par amnewy t he bl ob cent r oi d ( i mage coor ds, Y)
*/
publ i c s y nchr oni z ed v oi d s et Y ( s hor t newy ) {
t hi s . y = newy ;
}

/ **
*
* @r et ur n boundi ng box f or t he bl ob ( i mage coor ds, Lef t )
*/
publ i c s y nchr oni z ed s hor t get Lef t ( ) {
r et ur n t hi s . l ef t ;
}

/ **
*
* @par amnewl ef t boundi ng box f or t he bl ob ( i mage coor ds, Lef t )
*/
publ i c s y nchr oni z ed v oi d s et Lef t ( s hor t newl ef t ) {
t hi s . l ef t = newl ef t ;
}

/ **
*
* @r et ur n boundi ng box f or t he bl ob ( i mage coor ds, Ri ght )
*/
publ i c s y nchr oni z ed s hor t get Ri ght ( ) {
r et ur n t hi s . r i ght ;
}

/ **
*
* @par amnewr i ght boundi ng box f or t he bl ob ( i mage coor ds, Ri ght )
*/
publ i c s y nchr oni z ed v oi d s et Ri ght ( s hor t newr i ght ) {
t hi s . r i ght = newr i ght ;
}

/ **
Anexe
pagina CXVII
*
* @r et ur n boundi ng box f or t he bl ob ( i mage coor ds, Top)
*/
publ i c s y nchr oni z ed s hor t get Top ( ) {
r et ur n t hi s . t op;
}

/ **
*
* @par amnewt op boundi ng box f or t he bl ob ( i mage coor ds, Top)
*/
publ i c s y nchr oni z ed v oi d s et Top ( s hor t newt op) {
t hi s . t op = newt op;
}

/ **
*
* @r et ur n boundi ng box f or t he bl ob ( i mage coor ds, Bot t om)
*/
publ i c s y nchr oni z ed s hor t get Bot t om ( ) {
r et ur n t hi s . bot t om;
}

/ **
*
* @par amnewbot t omboundi ng box f or t he bl ob ( i mage coor ds, Bot t om)
*/
publ i c s y nchr oni z ed v oi d s et Bot t om ( s hor t newbot t om) {
t hi s . bot t om = newbot t om;
}

/ **
*
* @r et ur n r ange ( mm) t o t he bl ob cent er
*/
publ i c s y nchr oni z ed s hor t get Range ( ) {
r et ur n t hi s . r ange;
}

/ **
*
* @par amnewr ange r ange ( mm) t o t he bl ob cent er
*/
publ i c s y nchr oni z ed v oi d s et Range ( s hor t newr ange) {
t hi s . r ange = newr ange;
}
}

j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /H Hy yp po ot th he es si is s. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Hypot hesi s. j ava
*
*
*
*
*
* $I d: Hypot hesi s. j ava, v 1. 2 2005/ 05/ 10 12: 10: 24 veedee Exp $
*
*/

/ **
* Hypot hesi s f or mat . 
* Si nce t he r obot pose may be ambi guous ( i . e. , t he r obot may at any of a number of wi del y
* spaced l ocat i ons) , t he l ocal i ze i nt er f ace i s capabl e of r et ur ni ng mor e t hat one hypot hesi s. 
* ( see t he pl ayer _l ocal i ze_hypot h st r uct ur e f r ompl ayer . h)
* @ver si on
* <ul >
Anexe
pagina CXVIII
* </ ul >
*/
publ i c cl as s Hy pot hes i s {
pr i v at e i nt [ ] mean = new i nt [ 3] ; / *t he mean val ue of t he pose est i mat e ( mm, mm, ar c- seconds) */

/ * t he covar i ance mat r i x pose est i mat e ( mm$^2$, ar c- seconds$^2$) */
pr i v at e l ong[ ] [ ] cov = new l ong[ 3] [ 3] ;

pr i v at e i nt al pha; / * t he wei ght coef f i ci ent f or l i near combi nat i on ( al pha * 1e6) */

/ **
*
* @r et ur n t he mean val ue of t he pose est i mat e ( mm, mm, ar c- seconds)
*/
publ i c s y nchr oni z ed i nt [ ] get Mean ( ) {
r et ur n t hi s . mean;
}

/ **
*
* @par amnewmean t he mean val ue of t he pose est i mat e ( mm, mm, ar c- seconds)
*/
publ i c s y nchr oni z ed v oi d s et Mean ( i nt [ ] newmean) {
t hi s . mean = newmean;
}

/ **
*
* @r et ur n t he covar i ance mat r i x pose est i mat e ( mm$^2$, ar c- seconds$^2$)
*/
publ i c s y nchr oni z ed l ong[ ] [ ] get Cov ( ) {
r et ur n t hi s . cov ;
}

/ **
*
* @par amnewcov t he covar i ance mat r i x pose est i mat e ( mm$^2$, ar c- seconds$^2$)
*/
publ i c s y nchr oni z ed v oi d s et Cov ( l ong[ ] [ ] newcov ) {
t hi s . cov = newcov ;
}

/ **
*
* @r et ur n t he wei ght coef f i ci ent f or l i near combi nat i on ( al pha * 1e6)
*/
publ i c s y nchr oni z ed i nt get Al pha ( ) {
r et ur n t hi s . al pha;
}

/ **
*
* @par amnewal pha t he wei ght coef f i ci ent f or l i near combi nat i on ( al pha * 1e6)
*/
publ i c s y nchr oni z ed v oi d s et Al pha ( i nt newal pha) {
t hi s . al pha = newal pha;
}
}

j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rB Bu um mp pe er rD De ef fi in ne eT T. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl ayer Bumper Def i neT. j ava
*
*
*
*
*
Anexe
pagina CXIX
* $I d: Pl ayer Bumper Def i neT. j ava, v 1. 2 2005/ 05/ 10 12: 10: 24 veedee Exp $
*
*/

/ **
* The geomet r y of a si ngl e bumper . 
* ( see t he pl ayer _bumper _def i ne st r uct ur e f r ompl ayer . h)
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pl ay er Bumper Def i neT { / * t he geomet r y of a si ngl e buf f er */
pr i v at e s hor t x Of f s et , y Of f s et , t hOf f s et ; / * t he l ocal pose of a si ngl e bumper i n mm*/
pr i v at e s hor t l engt h; / * l engt h of t he sensor i n mm*/
pr i v at e s hor t r adi us ; / * r adi us of cur vat ur e i n mm- zer o f or st r ai ght l i nes */

/ **
*
* @r et ur n t he l ocal pose of a si ngl e bumper i n mm( X)
*/
publ i c s y nchr oni z ed s hor t get XOf f s et ( ) {
r et ur n t hi s . x Of f s et ;
}

/ **
*
* @par amnewXOf f set t he l ocal pose of a si ngl e bumper i n mm( X)
*/
publ i c s y nchr oni z ed v oi d s et XOf f s et ( s hor t newXOf f s et ) {
t hi s . x Of f s et = newXOf f s et ;
}

/ **
*
* @r et ur n t he l ocal pose of a si ngl e bumper i n mm( Y)
*/
publ i c s y nchr oni z ed s hor t get YOf f s et ( ) {
r et ur n t hi s . y Of f s et ;
}

/ **
*
* @par amnewYOf f set t he l ocal pose of a si ngl e bumper i n mm( Y)
*/
publ i c s y nchr oni z ed v oi d s et YOf f s et ( s hor t newYOf f s et ) {
t hi s . y Of f s et = newYOf f s et ;
}

/ **
*
* @r et ur n t he l ocal pose of a si ngl e bumper i n mm( Thet a)
*/
publ i c s y nchr oni z ed s hor t get ThOf f s et ( ) {
r et ur n t hi s . t hOf f s et ;
}

/ **
*
* @par amnewThOf f set t he l ocal pose of a si ngl e bumper i n mm( Thet a)
*/
publ i c s y nchr oni z ed v oi d s et ThOf f s et ( s hor t newThOf f s et ) {
t hi s . t hOf f s et = newThOf f s et ;
}

/ **
*
* @r et ur n l engt h of t he sensor i n mm
*/
publ i c s y nchr oni z ed s hor t get Lengt h ( ) {
r et ur n t hi s . l engt h;
}

/ **
*
* @par amnewLengt h l engt h of t he sensor i n mm
*/
publ i c s y nchr oni z ed v oi d s et Lengt h ( s hor t newLengt h) {
t hi s . l engt h = newLengt h;
}

Anexe
pagina CXX
/ **
*
* @r et ur n r adi us of cur vat ur e i n mm- zer o f or st r ai ght l i nes
*/
publ i c s y nchr oni z ed s hor t get Radi us ( ) {
r et ur n t hi s . r adi us ;
}

/ **
*
* @par amnewRadi us r adi us of cur vat ur e i n mm- zer o f or st r ai ght l i nes
*/
publ i c s y nchr oni z ed v oi d s et Radi us ( s hor t newRadi us ) {
t hi s . r adi us = newRadi us ;
}
}

j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rB Bu um mp pe er rG Ge eo om mT T. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl ayer Bumper GeomT. j ava
*
*
*
*
*
* $I d: Pl ayer Bumper GeomT. j ava, v 1. 2 2005/ 05/ 10 12: 10: 24 veedee Exp $
*
*/

/ **
* To quer y t he geomet r y of a bumper ar r ay, gi ve t he f ol l owi ng r equest , f i l l i ng i n onl y t he
* subt ype. The ser ver wi l l r epond wi t h t he ot her f i el ds f i l l ed i n. 
* ( see t he pl ayer _bumper _geomst r uct ur e f r ompl ayer . h)
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pl ay er Bumper GeomT {
pr i v at e s hor t bumper Count ; / * t he number of val i d bumper def i ni t i ons */
/ * geomet r y of each bumper */
pr i v at e Pl ay er Bumper Def i neT[ ] dat a =
new Pl ay er Bumper Def i neT[ j av acl i ent . Bumper I nt er f ace. PLAYER_BUMPER_MAX_SAMPLES] ;

/ **
*
* @r et ur n t he number of val i d bumper def i ni t i ons
*/
publ i c s y nchr oni z ed s hor t get Bumper Count ( ) {
r et ur n t hi s . bumper Count ;
}

/ **
*
* @par amnewbumper count t he number of val i d bumper def i ni t i ons
*/
publ i c s y nchr oni z ed v oi d s et Bumper Count ( s hor t newbumper count ) {
t hi s . bumper Count = newbumper count ;
}

/ **
*
* @r et ur n an ar r ay of bumper geomet r i es
*/
publ i c s y nchr oni z ed Pl ay er Bumper Def i neT[ ] get Dat a ( ) {
r et ur n t hi s . dat a;
Anexe
pagina CXXI
}

/ **
*
* @par amnewdat a ar r ay of bumper geomet r i es
*/
publ i c s y nchr oni z ed v oi d s et Dat a ( Pl ay er Bumper Def i neT[ ] newdat a) {
t hi s . dat a = newdat a;
}
}

j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rD De ev vi ic ce eD De ev vl li is st tT T. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl ayer Devi ceDevl i st T. j ava
*
*
*
*
*
* $I d: Pl ayer Devi ceDevl i st T. j ava, v 1. 2 2005/ 05/ 10 12: 10: 24 veedee Exp $
*
*/

/ **
* I t ' s usef ul f or appl i cat i ons such as vi ewer pr ogr ams and t est sui t es t hat t ai l or behave
* di f f er ent l y dependi ng on whi ch devi ces ar e avai l abl e. To r equest t he l i st , set t he subt ype t o
* PLAYER_PLAYER_DEVLI ST_REQ and l eave t he r est of t he f i el ds bl ank. Pl ayer wi l l r et ur n a packet
* wi t h subt ype PLAYER_PLAYER_DEVLI ST_REQ wi t h t he f i el ds f i l l ed i n. 
* ( see t he pl ayer _devi ce_devl i st st r uct ur e f r ompl ayer . h)
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pl ay er Dev i ceDev l i s t T {
pr i v at e s hor t dev i ceCount ; / * t he number of devi ces */
pr i v at e Pl ay er Dev i ceI dT[ ] dev Li s t ; / * t he l i st of avai l abl e devi ces */

/ **
*
* @r et ur n t he number of devi ces
*/
publ i c s y nchr oni z ed s hor t get Dev i ceCount ( ) {
r et ur n t hi s . dev i ceCount ;
}

/ **
*
* @par amnewdevi cecount number of devi ces
*/
publ i c s y nchr oni z ed v oi d s et Dev i ceCount ( s hor t newdev i cecount ) {
t hi s . dev i ceCount = newdev i cecount ;
}

/ **
*
* @r et ur n t he l i st of avai l abl e devi ces
*/
publ i c s y nchr oni z ed Pl ay er Dev i ceI dT[ ] get Dev Li s t ( ) {
r et ur n t hi s . dev Li s t ;
}

/ **
*
* @par amnewdevl i st l i st of avai l abl e devi ces
*/
Anexe
pagina CXXII
publ i c s y nchr oni z ed v oi d s et Dev Li s t ( Pl ay er Dev i ceI dT[ ] newdev l i s t ) {
t hi s . dev Li s t = newdev l i s t ;
}
}

j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rD De ev vi ic ce eD Dr ri iv ve er rI In nf fo o. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl ayer Devi ceDr i ver I nf o. j ava
*
*
*
*
*
* $I d: Pl ayer Devi ceDr i ver I nf o. j ava, v 1. 2 2005/ 05/ 10 12: 10: 24 veedee Exp $
*
*/

/ **
* To get a name, set t he subt ype t o PLAYER_PLAYER_DRI VERI NFO_REQ and set t he i d f i el d. Pl ayer
* wi l l r et ur n t he dr i ver i nf o. 
* ( see t he pl ayer _devi ce_dr i ver i nf o st r uct ur e f r ompl ayer . h)
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pl ay er Dev i ceDr i v er I nf o {
pr i v at e Pl ay er Dev i ceI dT dev I D; / * t he devi ce i dent i f i er */
pr i v at e St r i ng dr i v er Name; / * t he dr i ver name */

/ **
*
* @r et ur n t he devi ce i dent i f i er
*/
publ i c s y nchr oni z ed Pl ay er Dev i ceI dT get Dev I D ( ) {
r et ur n t hi s . dev I D;
}

/ **
*
* @par amnewdevi d devi ce i dent i f i er
*/
publ i c s y nchr oni z ed v oi d s et Dev I D ( Pl ay er Dev i ceI dT newdev i d) {
t hi s . dev I D = newdev i d;
}

/ **
*
* @r et ur n t he dr i ver name
*/
publ i c s y nchr oni z ed St r i ng get Dr i v er Name ( ) {
r et ur n t hi s . dr i v er Name;
}

/ **
*
* @par amnewdr i ver name dr i ver name
*/
publ i c s y nchr oni z ed v oi d s et Dr i v er Name ( St r i ng newdr i v er name) {
t hi s . dr i v er Name = newdr i v er name;
}
}

Anexe
pagina CXXIII

j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rD De ev vi ic ce eI Id dT T. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl ayer Devi ceI dT. j ava
*
*
*
*
*
* $I d: Pl ayer Devi ceI dT. j ava, v 1. 2 2005/ 05/ 10 12: 10: 24 veedee Exp $
*
*/

/ **
* Devi ces ar e di f f er ent i at ed i nt er nal l y i n Pl ayer by t hese i dent i f i er s, and some messages
* cont ai n t hem. 
* ( see t he pl ayer _devi ce_i d st r uct ur e f r ompl ayer . h)
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pl ay er Dev i ceI dT {
pr i v at e s hor t code; / * t he i nt er f ace pr ovi ded by t he devi ce */
pr i v at e s hor t i ndex ; / * t he i ndex of t he devi ce */
pr i v at e s hor t por t ; / * t he TCP por t of t he devi ce */

/ **
*
* @r et ur n t he i nt er f ace pr ovi ded by t he devi ce
*/
publ i c s y nchr oni z ed s hor t get Code ( ) {
r et ur n t hi s . code;
}

/ **
*
* @par amnewcode i nt er f ace pr ovi ded by t he devi ce
*/
publ i c s y nchr oni z ed v oi d s et Code ( s hor t newcode) {
t hi s . code = newcode;
}

/ **
*
* @r et ur n t he i ndex of t he devi ce
*/
publ i c s y nchr oni z ed s hor t get I ndex ( ) {
r et ur n t hi s . i ndex ;
}

/ **
*
* @par amnewi ndex i ndex of t he devi ce
*/
publ i c s y nchr oni z ed v oi d s et I ndex ( s hor t newi ndex ) {
t hi s . i ndex = newi ndex ;
}

/ **
*
* @r et ur n t he TCP por t of t he devi ce
*/
publ i c s y nchr oni z ed s hor t get Por t ( ) {
r et ur n t hi s . por t ;
}
Anexe
pagina CXXIV

/ **
*
* @par amnewpor t TCP por t of t he devi ce
*/
publ i c s y nchr oni z ed v oi d s et Por t ( s hor t newpor t ) {
t hi s . por t = newpor t ;
}

}

j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rF Fi id du uc ci ia al lI It te em m. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl ayer Fi duci al I t em. j ava
*
*
*
*
*
* $I d: Pl ayer Fi duci al I t em. j ava, v 1. 3 2005/ 05/ 10 12: 10: 24 veedee Exp $
*
*/

/ **
* I nf o on a si ngl e det ect ed f i duci al . 
* ( see t he pl ayer _f i duci al _i t emst r uct ur e f r ompl ayer . h)
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pl ay er Fi duci al I t em {

/ * The f i duci al i d. Fi duci al s t hat cannot be i dent i f i ed get i d - 1. */
pr i v at e s hor t i d;

/ * Fi duci al posi t i on r el at i ve t o t he det ect or ( x, y, z) i n mm. */
pr i v at e i nt [ ] pos = new i nt [ 3] ;

/ * Fi duci al or i ent at i on r el at i ve t o t he det ect or ( r , p, y) i n mi l l i r ad. */
pr i v at e i nt [ ] r ot = new i nt [ 3] ;

/ * Uncer t ai nt y i n t he measur ed pose ( x, y, z) i n mm. */
pr i v at e i nt [ ] upos = new i nt [ 3] ;

/ * Uncer t ai nt y i n f i duci al or i ent at i on r el at i ve t o t he det ect or ( r , p, y) i n mi l l i r ad. */
pr i v at e i nt [ ] ur ot = new i nt [ 3] ;

/ **
*
* @r et ur n The f i duci al i d. Fi duci al s t hat cannot be i dent i f i ed get i d - 1.
*/
publ i c s y nchr oni z ed i nt get I D ( ) {
r et ur n t hi s . i d;
}

/ **
*
* @par amnewI D t he f i duci al i d
*/
publ i c s y nchr oni z ed v oi d s et I D ( i nt newI D) {
t hi s . i d = ( s hor t ) newI D;
}

/ **
*
Anexe
pagina CXXV
* @r et ur n Fi duci al posi t i on r el at i ve t o t he det ect or ( x, y, z) i n mm.
*/
publ i c s y nchr oni z ed i nt [ ] get Pos ( ) {
r et ur n t hi s . pos ;
}

/ **
*
* @par amnewPos f i duci al posi t i on r el at i ve t o t he det ect or ( x, y, z) i n mm
*/
publ i c s y nchr oni z ed v oi d s et Pos ( i nt [ ] newPos ) {
t hi s . pos = newPos ;
}

/ **
*
* @r et ur n Fi duci al or i ent at i on r el at i ve t o t he det ect or ( r , p, y) i n mi l l i r ad.
*/
publ i c s y nchr oni z ed i nt [ ] get Rot ( ) {
r et ur n t hi s . r ot ;
}

/ **
*
* @par amnewRot f i duci al or i ent at i on r el at i ve t o t he det ect or ( r , p, y) i n mi l l i r ad
*/
publ i c s y nchr oni z ed v oi d s et Rot ( i nt [ ] newRot ) {
t hi s . r ot = newRot ;
}

/ **
*
* @r et ur n Uncer t ai nt y i n t he measur ed pose ( x, y, z) i n mm.
*/
publ i c s y nchr oni z ed i nt [ ] get UPos ( ) {
r et ur n t hi s . upos ;
}

/ **
*
* @par amnewUPos Uncer t ai nt y i n t he measur ed pose ( x, y, z) i n mm
*/
publ i c s y nchr oni z ed v oi d s et UPos ( i nt [ ] newUPos ) {
t hi s . upos = newUPos ;
}

/ **
*
* @r et ur n Uncer t ai nt y i n f i duci al or i ent at i on r el at i ve t o t he det ect or ( r , p, y)
* i n mi l l i r ad.
*/
publ i c s y nchr oni z ed i nt [ ] get URot ( ) {
r et ur n t hi s . ur ot ;
}

/ **
*
* @par amnewURot uncer t ai nt y i n f i duci al or i ent at i on r el at i ve t o t he det ect or
* ( r , p, y) i n mi l l i r ad
*/
publ i c s y nchr oni z ed v oi d s et URot ( i nt [ ] newURot ) {
t hi s . ur ot = newURot ;
}
}

j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rI IR RP Po os se eR Re eq qT T. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl ayer I RPoseReqT. j ava
*
*
*
*
Anexe
pagina CXXVI
*
* $I d: Pl ayer I RPoseReqT. j ava, v 1. 1 2005/ 06/ 01 19: 05: 51 veedee Exp $
*
*/

/ **
* To quer y t he pose of t he I Rs, use t hi s r equest , f i l l i ng i n onl y t he subt ype.
* The ser ver wi l l r espond wi t h t he ot her f i el ds f i l l ed i n. 
* ( see t he pl ayer _i r _pose_r eq st r uct ur e f r ompl ayer . h)
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pl ay er I RPos eReqT {
/ * t he number of i r sampl es r et ur ned by t hi s r obot */
pr i v at e s hor t pos eCount ;

/ * t he pose of each I R det ect or on t hi s r obot ( mm, mm, degr ees) */
pr i v at e s hor t pos es [ ] [ ] = new s hor t [ j av acl i ent . I RI nt er f ace. PLAYER_I R_MAX_SAMPLES] [ 3] ;

/ **
*
* @r et ur n t he number of val i d poses
*/
publ i c s y nchr oni z ed s hor t get Pos eCount ( ) {
r et ur n t hi s . pos eCount ;
}

/ **
*
* @par amnewposecount number of val i d poses
*/
publ i c s y nchr oni z ed v oi d s et Pos eCount ( s hor t newpos ecount ) {
t hi s . pos eCount = newpos ecount ;
}

/ **
*
* @r et ur n t he pose of each I R det ect or on t hi s r obot ( mm, mm, degr ees)
*/
publ i c s y nchr oni z ed s hor t [ ] [ ] get Pos es ( ) {
r et ur n t hi s . pos es ;
}

/ **
*
* @par amnewposes pose of each I R det ect or on t hi s r obot ( mm, mm, degr ees)
*/
publ i c s y nchr oni z ed v oi d s et Pos es ( s hor t [ ] [ ] newpos es ) {
t hi s . pos es = newpos es ;
}
}

j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rL La as se er rC Co on nf fi ig gT T. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl ayer Laser Conf i gT. j ava
*
*
*
*
*
* $I d: Pl ayer Laser Conf i gT. j ava, v 1. 2 2005/ 05/ 10 12: 10: 24 veedee Exp $
*
*/
Anexe
pagina CXXVII

/ **
* The scan conf i gur at i on ( r esol ut i on, aper t ur e, et c) can be quer i ed usi ng t he
* PLAYER_LASER_GET_CONFI G r equest and modi f i ed usi ng t he PLAYER_LASER_SET_CONFI G r equest .
* Read t he document at i on f or your dr i ver t o det er mi ne what conf i gur at i on val ues ar e per mi ssi bl e.
* ( see t he pl ayer _l aser _conf i g st r uct ur e f r ompl ayer . h)
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pl ay er Las er Conf i gT {
pr i v at e s hor t mi nAngl e; / * st ar t angl e ( 0. 01 degr ees) . Val i d r ange i s - 9000 t o +9000 */
pr i v at e s hor t max Angl e; / * end angl e ( 0. 01 degr ees) . Val i d r ange i s - 9000 t o +9000 */
pr i v at e i nt r es ol ut i on; / *scan r esol ut i on ( 0. 01 degr ees) . Val i d r esol ut i ons ar e 25, 50, 100 */
pr i v at e i nt r angeRes ; / * r ange r esol ut i on. Val i d: 1, 10, 100 ( For mm, cm, dm) */
pr i v at e by t e i nt ens i t y ; / * enabl e r ef l ect i on i nt ensi t y dat a */

/ **
*
* @r et ur n t he st ar t angl e
*/
publ i c s y nchr oni z ed s hor t get Mi nAngl e ( ) {
r et ur n t hi s . mi nAngl e;
}

/ **
*
* @par amnewmi nangl e st ar t angl e ( 0. 01 degr ees) . Val i d r ange i s - 9000 t o +9000
*/
publ i c s y nchr oni z ed v oi d s et Mi nAngl e ( s hor t newmi nangl e) {
t hi s . mi nAngl e = newmi nangl e;
}

/ **
*
* @r et ur n t he end angl e
*/
publ i c s y nchr oni z ed s hor t get Max Angl e ( ) {
r et ur n t hi s . max Angl e;
}

/ **
*
* @par amnewmaxangl e end angl e ( 0. 01 degr ees) . Val i d r ange i s - 9000 t o +9000
*/
publ i c s y nchr oni z ed v oi d s et Max Angl e ( s hor t newmax angl e) {
t hi s . max Angl e = newmax angl e;
}

/ **
*
* @r et ur n t he scan r esol ut i on
*/
publ i c s y nchr oni z ed i nt get Res ol ut i on ( ) {
r et ur n t hi s . r es ol ut i on;
}

/ **
*
* @par amnewr esol ut i on scan r esol ut i on ( 0. 01 degr ees) . Val i d r esol ut i ons ar e 25, 50, 100
*/
publ i c s y nchr oni z ed v oi d s et Res ol ut i on ( i nt newr es ol ut i on) {
t hi s . r es ol ut i on = newr es ol ut i on;
}

/ **
*
* @r et ur n t he r ange r esol ut i on
*/
publ i c s y nchr oni z ed i nt get RangeRes ( ) {
r et ur n t hi s . r angeRes ;
}

/ **
*
* @par amnewr anger es r ange r esol ut i on. Val i d: 1, 10, 100 ( For mm, cm, dm)
*/
publ i c s y nchr oni z ed v oi d s et RangeRes ( i nt newr anger es ) {
t hi s . r angeRes = newr anger es ;
}

Anexe
pagina CXXVIII
/ **
*
* @r et ur n whet her t he r ef l ect i on i nt ensi t y dat a i s di sabl ed or enabl ed ( 0/ 1)
*/
publ i c s y nchr oni z ed by t e get I nt ens i t y ( ) {
r et ur n t hi s . i nt ens i t y ;
}

/ **
*
* @par amnewi nt ensi t y enabl e r ef l ect i on i nt ensi t y dat a
*/
publ i c s y nchr oni z ed v oi d s et I nt ens i t y ( by t e newi nt ens i t y ) {
t hi s . i nt ens i t y = newi nt ens i t y ;
}

}

j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rL La as se er rG Ge eo om mT T. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl ayer Laser GeomT. j ava
*
*
*
*
*
* $I d: Pl ayer Laser GeomT. j ava, v 1. 2 2005/ 05/ 10 12: 10: 24 veedee Exp $
*
*/

/ **
* The l aser geomet r y ( posi t i on and si ze) can be quer i ed usi ng t he PLAYER_LASER_GET_GEOM
* r equest . The r equest and r epl y packet s have t he same f or mat . 
* ( see t he pl ayer _l aser _geomst r uct ur e f r ompl ayer . h)
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pl ay er Las er GeomT {
pr i v at e s hor t pos e[ ] = new s hor t [ 2] ; / * l aser pose, i n r obot cs ( mm, mm, degr ees) */
pr i v at e s hor t s i z e[ ] = new s hor t [ 3] ; / * l aser di mensi ons ( mm, mm) */

/ **
*
* @r et ur n t he l aser pose, i n r obot cs ( mm, mm, degr ees)
*/
publ i c s y nchr oni z ed s hor t [ ] get Pos e ( ) {
r et ur n t hi s . pos e;
}

/ **
*
* @par amnewpose l aser pose, i n r obot cs ( mm, mm, degr ees)
*/
publ i c s y nchr oni z ed v oi d s et Pos e ( s hor t [ ] newpos e) {
t hi s . pos e = newpos e;
}

/ **
*
* @r et ur n t he l aser di mensi ons ( mm, mm)
*/
publ i c s y nchr oni z ed s hor t [ ] get Si z e ( ) {
r et ur n t hi s . s i z e;
}
Anexe
pagina CXXIX

/ **
*
* @par amnewsi ze l aser di mensi ons ( mm, mm)
*/
publ i c s y nchr oni z ed v oi d s et Si z e ( s hor t [ ] news i z e) {
t hi s . pos e = news i z e;
}

}

j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rL Lo oc ca al li iz ze eC Co on nf fi ig gT T. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl ayer Local i zeConf i gT. j ava
*
*
*
*
*
* $I d: Pl ayer Local i zeConf i gT. j ava, v 1. 2 2005/ 05/ 10 12: 10: 24 veedee Exp $
*
*/

/ **
* To r et r i eve t he conf i gur at i on, set t he subt ype t o PLAYER_LOCALI ZE_GET_CONFI G_REQ and l eave
* t he ot her f i el ds empt y. The ser ver wi l l r epl y wi t h t he f ol l owi ng conf i gur at i on f i el ds f i l l ed
* i n. To change t he cur r ent conf i gur at i on, set t he subt ype t o PLAYER_LOCALI ZE_SET_CONFI G_REQ
* and f i l l t he conf i gur at i on f i el ds. 
* ( see t he pl ayer _l ocal i ze_conf i g st r uct ur e f r ompl ayer . h)
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pl ay er Local i z eConf i gT {
pr i v at e i nt numPar t i cl es ; / *maxi mumnumber of par t i cl es ( f or dr i ver s usi ng par t i cl e f i l t er s) */

/ **
*
* @r et ur n t he maxi mumnumber of par t i cl es ( f or dr i ver s usi ng par t i cl e f i l t er s)
*/
publ i c s y nchr oni z ed i nt get NumPar t i cl es ( ) {
r et ur n t hi s . numPar t i cl es ;
}

/ **
*
* @par amnewnumpar t i cl es maxi mumnumber of par t i cl es ( f or dr i ver s usi ng par t i cl e f i l t er s)
*/
publ i c s y nchr oni z ed v oi d s et NumPar t i cl es ( i nt newnumpar t i cl es ) {
t hi s . numPar t i cl es = newnumpar t i cl es ;
}
}

j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rP Pl la an nn ne er rW Wa ay yp po oi in nt tT T. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl ayer Pl anner Waypoi nt T. j ava
*
*
Anexe
pagina CXXX
*
*
*
* $I d: Pl ayer Pl anner Waypoi nt T. j ava, v 1. 2 2005/ 05/ 10 12: 10: 24 veedee Exp $
*
*/

/ **
* Waypoi nt st r uct ur e. 
* ( see t he pl ayer _pl anner _waypoi nt st r uct ur e f r ompl ayer . h)
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pl ay er Pl anner Way poi nt T {
pr i v at e i nt x , y , a;

publ i c s y nchr oni z ed i nt get X ( ) {
}

publ i c s y nchr oni z ed v oi d s et X ( i nt newx ) {
t hi s . x = newx ;
}

publ i c s y nchr oni z ed i nt get Y ( ) {
}

publ i c s y nchr oni z ed v oi d s et Y ( i nt newy ) {
t hi s . y = newy ;
}

publ i c s y nchr oni z ed i nt get A ( ) {
r et ur n t hi s . a;
}

publ i c s y nchr oni z ed v oi d s et A ( i nt newa) {
t hi s . a = newa;
}
}

j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rP Po os si it ti io on n2 2D DG Ge eo om mT T. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl ayer Posi t i on2DGeomT. j ava
*
*
*
*
*
* $I d: Pl ayer Posi t i on2DGeomT. j ava, v 1. 2 2005/ 05/ 10 12: 10: 24 veedee Exp $
*
*/

/ **
* To r equest r obot geomet r y, set t he subt ype t o PLAYER_POSI TI ON2D_GET_GEOM_REQ and l eave t he
* ot her f i el ds empt y. The ser ver wi l l r epl y wi t h t he pose and si ze f i el ds f i l l ed i n. 
Anexe
pagina CXXXI
* ( see t he pl ayer _posi t i on2d_geomst r uct ur e f r ompl ayer . h)
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pl ay er Pos i t i on2DGeomT {
/ * pose of t he r obot base, i n t he r obot cs ( mm, mm, mr ad) */
pr i v at e s hor t [ ] pos e = new s hor t [ 3] ;
pr i v at e s hor t [ ] s i z e = new s hor t [ 2] ;

/ **
*
* @r et ur n t he pose of t he r obot base, i n t he r obot cs ( mm, mm, mr ad)
*/
}

/ **
*
* @par amnewpose pose of t he r obot base, i n t he r obot cs ( mm, mm, mr ad)
*/
}

/ **
*
* @r et ur n t he di mensi ons of t he base ( mm, mm)
*/
}

/ **
*
* @par amnewsi ze di mensi ons of t he base ( mm, mm)
*/
t hi s . s i z e = news i z e;
}
}

j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rP Po os si it ti io on n3 3D DG Ge eo om mT T. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl ayer Posi t i on3DGeomT. j ava
*
*
*
*
*
* $I d: Pl ayer Posi t i on3DGeomT. j ava, v 1. 2 2005/ 05/ 10 12: 10: 24 veedee Exp $
*
*/

/ **
* To r equest r obot geomet r y, set t he subt ype t o PLAYER_POSI TI ON3D_GET_GEOM_REQ
* and l eave t he ot her f i el ds empt y. The ser ver wi l l r epl y wi t h t he pose and
* si ze f i el ds f i l l ed i n. 
* ( see t he pl ayer _posi t i on3d_geomst r uct ur e f r ompl ayer . h)
* @ver si on
* <ul >
Anexe
pagina CXXXII
* </ ul >
*/
publ i c cl as s Pl ay er Pos i t i on3DGeomT {
/ * pose of t he r obot base, i n t he r obot cs ( mm, mm, mm, mr ad, mr ad, mr ad) */
/ * di mensi ons of t he base ( mm, mm, mm) */

/ **
*
* @r et ur n t he pose of t he r obot base, i n t he r obot cs ( mm, mm, mr ad)
*/
}

/ **
*
* @par amnewpose pose of t he r obot base, i n t he r obot cs ( mm, mm, mr ad)
*/
}

/ **
*
*/
}

/ **
*
*/
}
}

j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rP Po os si it ti io on nG Ge eo om mT T. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl ayer Posi t i onGeomT. j ava
*
*
*
*
*
* $I d: Pl ayer Posi t i onGeomT. j ava, v 1. 3 2005/ 05/ 10 12: 10: 24 veedee Exp $
*
*/

/ **
* To r equest r obot geomet r y, set t he subt ype t o PLAYER_POSI TI ON_GET_GEOM_REQ and l eave t he
* ot her f i el ds empt y. The ser ver wi l l r epl y wi t h t he pose and si ze f i el ds f i l l ed i n. 
* ( see t he pl ayer _posi t i on_geomst r uct ur e f r ompl ayer . h)
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pl ay er Pos i t i onGeomT {
/ * pose of t he r obot base, i n t he r obot cs ( mm, mm, degr ees) */
Anexe
pagina CXXXIII

/ **
*
* @r et ur n t he pose of t he r obot base, i n t he r obot cs ( mm, mm, degr ees)
*/
}

/ **
*
* @par amnewpose pose of t he r obot base, i n t he r obot cs ( mm, mm, degr ees)
*/
}

/ **
*
*/
}

/ **
*
*/
}
}

j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rS Si im mu ul la at ti io on nP Po os se e2 2D D. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl ayer Si mul at i onPose2D. j ava
*
*
*
*
*
* $I d: Pl ayer Si mul at i onPose2D. j ava, v 1. 2 2005/ 05/ 10 12: 10: 24 veedee Exp $
*
*/

/ **
* To set or get t he pose of an obj ect i n a si mul at or , use t hi s message t ype. I f t he subt ype
* i s PLAYER_SI MULATI ON_SET_POSE2D, t he ser ver wi l l ask t he si mul at or t o move t he named
* obj ect t o t he l ocat i on speci f i ed by ( x, y, a) and r et ur n ACK. I f t he subt ype i s
* PLAYER_SI MULATI ON_GET_POSE2D, t he ser ver wi l l at t empt t o l ocat e t he named obj ect and r epl y
* wi t h t he same packet wi t h ( x, y, a) f i l l ed i n. For al l message subt ypes, i f t he named obj ect
* does not exi st , or some ot her er r or occur s, t he r equest shoul d r epl y NACK. 
* ( see t he pl ayer _si mul at i on_pose2d_r eq st r uct ur e f r ompl ayer . h)
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pl ay er Si mul at i onPos e2D {
pr i v at e St r i ng name; / * t he i dent i f i er of t he obj ect we want t o l ocat e */
pr i v at e i nt x , y , t het a; / * t he desi r ed pose or r et ur ned pose i n ( mm, mm, degr ees) */

/ **
Anexe
pagina CXXXIV
*
* @r et ur n t he i dent i f i er of t he obj ect we want t o l ocat e
*/
publ i c s y nchr oni z ed St r i ng get Name ( ) {
r et ur n t hi s . name;
}

/ **
*
* @par amnewname i dent i f i er of t he obj ect we want t o l ocat e
*/
publ i c s y nchr oni z ed v oi d s et Name ( St r i ng newname) {
t hi s . name = newname;
}

/ **
*
* @r et ur n t he desi r ed pose or r et ur ned pose ( X)
*/
publ i c s y nchr oni z ed i nt get X ( ) {
}

/ **
*
* @par amnewx desi r ed pose or r et ur ned pose ( X)
*/
publ i c s y nchr oni z ed v oi d s et X ( i nt newx ) {
t hi s . x = newx ;
}

/ **
*
* @r et ur n t he desi r ed pose or r et ur ned pose ( Y)
*/
publ i c s y nchr oni z ed i nt get Y ( ) {
}

/ **
*
* @par amnewy desi r ed pose or r et ur ned pose ( Y)
*/
publ i c s y nchr oni z ed v oi d s et Y ( i nt newy ) {
t hi s . y = newy ;
}

/ **
*
* @r et ur n t he desi r ed pose or r et ur ned pose ( Thet a)
*/
publ i c s y nchr oni z ed i nt get Thet a ( ) {
r et ur n t hi s . t het a;
}

/ **
*
* @par amnewt het a desi r ed pose or r et ur ned pose ( Thet a)
*/
publ i c s y nchr oni z ed v oi d s et Thet a ( i nt newt het a) {
t hi s . t het a = newt het a;
}
}

j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rS So on na ar rG Ge eo om mT T. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl ayer Sonar GeomT. j ava
*
*
*
*
Anexe
pagina CXXXV
*
* $I d: Pl ayer Sonar GeomT. j ava, v 1. 2 2005/ 05/ 10 12: 10: 24 veedee Exp $
*
*/

/ **
* To quer y t he geomet r y of t he sonar t r ansducer s, use t hi s r equest , but onl y f i l l i n t he subt ype.
* The ser ver wi l l r epl y wi t h t he ot her f i el ds f i l l ed i n. 
* ( see t he pl ayer _sonar _geomst r uct ur e f r ompl ayer . h)
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pl ay er Sonar GeomT {
pr i v at e s hor t pos eCount ; / * t he number of val i d poses */
/ * pose of each sonar , i n r obot cs ( mm, mm, degr ees) */
pr i v at e s hor t pos es [ ] [ ] = new s hor t [ j av acl i ent . Sonar I nt er f ace. PLAYER_SONAR_MAX_SAMPLES] [ 3] ;

/ **
*
* @r et ur n t he number of val i d poses
*/
publ i c s y nchr oni z ed s hor t get Pos eCount ( ) {
r et ur n t hi s . pos eCount ;
}

/ **
*
* @par amnewposecount number of val i d poses
*/
publ i c s y nchr oni z ed v oi d s et Pos eCount ( s hor t newpos ecount ) {
t hi s . pos eCount = newpos ecount ;
}

/ **
*
* @r et ur n t he pose of each sonar , i n r obot cs ( mm, mm, degr ees)
*/
publ i c s y nchr oni z ed s hor t [ ] [ ] get Pos es ( ) {
r et ur n t hi s . pos es ;
}

/ **
*
* @par amnewposes pose of each sonar , i n r obot cs ( mm, mm, degr ees)
*/
publ i c s y nchr oni z ed v oi d s et Pos es ( s hor t [ ] [ ] newpos es ) {
t hi s . pos es = newpos es ;
}
}

j ja av va ac cl li ie en nt t/ /s st tr ru uc ct tu ur re es s/ /P Pl la ay ye er rW Wi iF Fi iL Li in nk kT T. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Pl ayer Wi Fi Li nkT. j ava
*
*
*
*
*
* $I d: Pl ayer Wi Fi Li nkT. j ava, v 1. 2 2005/ 05/ 10 12: 10: 24 veedee Exp $
*
*/

Anexe
pagina CXXXVI
/ **
* The wi f i i nt er f ace r et ur ns dat a r egar di ng t he si gnal char act er i st i cs of r emot e host s as
* per cei ved t hr ough a wi r el ess net wor k i nt er f ace; t hi s i s t he f or mat of t he dat a f or each host .
* ( see t he pl ayer _wi f i _l i nk st r uct ur e f r ompl ayer . h)
* @ver si on
* <ul >
* </ ul >
*/
publ i c cl as s Pl ay er Wi Fi Li nkT {
pr i v at e char [ ] mac = new char [ 32] ; / * MAC addr ess */
pr i v at e char [ ] i p = new char [ 32] ; / * I P addr ess */
pr i v at e char [ ] es s i d = new char [ 32] ; / * ESSI D */
pr i v at e by t e mode; / * mode ( mast er , adhoc, et c) */
pr i v at e s hor t f r equency ; / * f r equency ( MHz) */
pr i v at e by t e encr y pt ; / * encr ypt ed */
pr i v at e s hor t qual ; / * l i nk qual i t y, l evel and noi se i nf or mat i on */
pr i v at e s hor t l ev el ;
pr i v at e s hor t noi s e;

/ **
*
* @r et ur n t he MAC addr ess
*/
publ i c s y nchr oni z ed char [ ] get MAC ( ) {
r et ur n t hi s . mac;
}

/ **
*
* @par amnewmac MAC addr ess
*/
publ i c s y nchr oni z ed v oi d s et MAC ( char [ ] newmac) {
t hi s . mac = newmac;
}

/ **
*
* @r et ur n t he I P addr ess
*/
publ i c s y nchr oni z ed char [ ] get I P ( ) {
r et ur n t hi s . i p;
}

/ **
*
* @par amnewi p I P addr ess
*/
publ i c s y nchr oni z ed v oi d s et I P ( char [ ] newi p) {
t hi s . i p = newi p;
}

/ **
*
* @r et ur n t he ESSI D
*/
publ i c s y nchr oni z ed char [ ] get ESSI D ( ) {
r et ur n t hi s . es s i d;
}

/ **
*
* @par amnewessi d ESSI D
*/
publ i c s y nchr oni z ed v oi d s et ESSI D ( char [ ] newes s i d) {
t hi s . es s i d = newes s i d;
}

/ **
*
* @r et ur n t he mode ( mast er , adhoc, et c)
*/
publ i c s y nchr oni z ed by t e get Mode ( ) {
r et ur n t hi s . mode;
}

/ **
*
* @par amnewmode mode ( mast er , adhoc, et c)
*/
publ i c s y nchr oni z ed v oi d s et Mode ( by t e newmode) {
t hi s . mode = newmode;
Anexe
pagina CXXXVII
}

/ **
*
* @r et ur n t he f r equency ( MHz)
*/
publ i c s y nchr oni z ed s hor t get Fr equency ( ) {
r et ur n t hi s . f r equency ;
}

/ **
*
* @par amnewf r equency f r equency ( MHz)
*/
publ i c s y nchr oni z ed v oi d s et Fr equency ( s hor t newf r equency ) {
t hi s . f r equency = newf r equency ;
}

/ **
*
* @r et ur n t he encr ypt i on st at us
*/
publ i c s y nchr oni z ed by t e get Encr y pt ( ) {
r et ur n t hi s . encr y pt ;
}

/ **
*
* @par amnewencr ypt encr i pt i on val ue
*/
publ i c s y nchr oni z ed v oi d s et Encr y pt ( by t e newencr y pt ) {
t hi s . encr y pt = newencr y pt ;
}

/ **
*
* @r et ur n t he l i nk qual i t y
*/
publ i c s y nchr oni z ed s hor t get Qual ( ) {
r et ur n t hi s . qual ;
}

/ **
*
* @par amnewqual l i nk qual i t y
*/
publ i c s y nchr oni z ed v oi d s et Qual ( s hor t newqual ) {
t hi s . qual = newqual ;
}

/ **
*
* @r et ur n t he l evel
*/
publ i c s y nchr oni z ed s hor t get Lev el ( ) {
r et ur n t hi s . l ev el ;
}

/ **
*
* @par amnewl evel l evel
*/
publ i c s y nchr oni z ed v oi d s et Lev el ( s hor t newl ev el ) {
t hi s . l ev el = newl ev el ;
}

/ **
*
* @r et ur n t he noi se i nf or mat i on
*/
publ i c s y nchr oni z ed s hor t get Noi s e ( ) {
r et ur n t hi s . noi s e;
}

/ **
*
* @par amnewnoi se noi se i nf or mat i on
*/
publ i c s y nchr oni z ed v oi d s et Noi s e ( s hor t newnoi s e) {
t hi s . noi s e = newnoi s e;
}
}

Anexe
pagina CXXXVIII

j ja av va ac cl li ie en nt t/ /e ex xt tr ra a/ /C Co on nt tr ro ol ll le er r. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Cont r ol l er . j ava
*
*
*
*
* $I d: Cont r ol l er . j ava, v 1. 2 2005/ 05/ 23 13: 13: 22 veedee Exp $
*
*/
package j av acl i ent . ex t r a;

/ **
* Abst r act cont r ol l er i mpl ement at i on. Used as a st ar t i ng poi nt f or P, PI , PD, PI D cont r ol l er s.
*/
publ i c abs t r act cl as s Cont r ol l er {

/ ** set t he cont r ol l er ' s goal */
pr ot ect ed doubl e goal ;

/ ** t he sumof al l er r or s so f ar */
pr ot ect ed doubl e eSum;

/ ** cur r ent er r or */
pr ot ect ed doubl e cur r E;
/ ** l ast er r or */
pr ot ect ed doubl e l as t E;

/ **
* Set a new goal f or t he cont r ol l er .
* @par amnewGoal t he new goal f or t he cont r ol l er
*/
publ i c v oi d s et Goal ( doubl e newGoal ) {
t hi s . goal = newGoal ;
}

/ **
* Get t he di f f er ence bet ween t he cur r ent er r or and t he l ast er r or .
* @r et ur n t he di f f er ence bet ween t he cur r ent er r or and t he l ast er r or
*/
pr ot ect ed doubl e del t aE ( ) {
r et ur n ( cur r E - l as t E) ;
}
}

j ja av va ac cl li ie en nt t/ /e ex xt tr ra a/ /H He ea ad di in ng gC Co on nt tr ro ol l. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Headi ngCont r ol . j ava
* Copyr i ght ( C) 2005 Radu Bogdan Rusu & Mar i us Bor odi
*
*
*
*
Anexe
pagina CXXXIX
*
* $I d: Headi ngCont r ol . j ava, v 1. 1 2005/ 06/ 14 09: 44: 32 veedee Exp $
*
*/

i mpor t j av acl i ent . Abs t r act Pos i t i onDev i ce;

/ **
* Headi ng cont r ol i nt er f ace f or Posi t i on, Posi t i on2D and Posi t i on3D Pl ayer
* i nt er f aces. Uses met hods f r ombot h pl ayer i nt er f aces and PI DCont r ol l er .
* @aut hor Radu Bogdan Rusu & Mar i us Bor odi
*/
publ i c cl as s Headi ngCont r ol ex t ends PI DCont r ol l er {

pr i v at e Abs t r act Pos i t i onDev i ce dev i ce = nul l ;

/ * PI D coef f i ci ent s */
pr i v at e i nt Kp = 1;
pr i v at e i nt Ki = 0;
pr i v at e i nt Kd = 0;

pr i v at e bool ean s t op = f al s e;

/ * mi ni mumand maxi mumadmi ssi bl e commands */
pr i v at e i nt mi nCommand = 1;
pr i v at e i nt max Command = 180;

/ **
* Const r uct or f or Headi ngCont r ol .
* @par ampd a r ef er ence t o a Pl ayer Devi ce i nt er f ace ( Posi t i on, Posi t i on2D
* or Posi t i on3D) .
*/
publ i c Headi ngCont r ol ( Abs t r act Pos i t i onDev i ce pd) {
s uper ( 1, 0, 0) ;
t hi s . dev i ce = pd;
}

/ **
* @par amkp t he pr opor t i onal const ant
* @par amki t he i nt egr al const ant
* @par amkd t he der i vat i ve const ant
*/
publ i c Headi ngCont r ol ( Abs t r act Pos i t i onDev i ce pd,
i nt kp, i nt ki , i nt kd) {
s uper ( kp, ki , kd) ;
t hi s . Kp = kp;
t hi s . Ki = ki ;
t hi s . Kd = kd;
}

/ **
* @par ammi nC mi ni mumadmi ssi bl e command f or t he r obot ' s mot or s
* @par ammaxC maxi mumadmi ssi bl e command f or t he r obot ' s mot or s
*/
publ i c Headi ngCont r ol ( Abs t r act Pos i t i onDev i ce pd, i nt mi nC, i nt max C) {
s uper ( 1, 0, 0) ;
t hi s . mi nCommand = mi nC;
t hi s . max Command = max C;
}

/ **
*/
publ i c Headi ngCont r ol ( Abs t r act Pos i t i onDev i ce pd, i nt mi nC, i nt max C,
t hi s . Kp = kp;
Anexe
pagina CXL
t hi s . Ki = ki ;
t hi s . Kd = kd;
}

/ **
* Set t he mi ni mumadmi ssi bl e command f or t he r obot ' s mot or s.
* @par ammi nC mi ni mumadmi ssi bl e command as an i nt eger
*/
publ i c v oi d s et Mi ni mumCommand ( i nt mi nC) {
}

/ **
* Set t he maxi mumadmi ssi bl e command f or t he r obot ' s mot or s.
* @par ammaxC maxi mumadmi ssi bl e command as an i nt eger
*/
publ i c v oi d s et Max i mumCommand ( i nt max C) {
}

/ **
* St op t he r obot f r ommovi ng.
*/
publ i c v oi d s t opRobot ( ) {
t hi s . s t op = t r ue;
}

/ **
* Cal cul at e and r et ur n t he cont r ol l er ' s command f or t he cont r ol l ed syst em.
* @par amcur r ent Out put t he cur r ent out put of t he syst em
* @r et ur n t he new cal cul at ed command f or t he syst em
*/
publ i c doubl e get Command ( doubl e cur r ent Out put ) {
t hi s . cur r E = t hi s . goal - cur r ent Out put ;

/ * Angl e adj ust ment s */
i f ( cur r E <= - 180 )
cur r E = 360 + cur r E;
el s e
i f ( cur r E >= 180 && cur r E <= 360)
cur r E = cur r E - 360;
el s e
i f ( cur r E > 360)
cur r E = cur r E - 360;

eSum += cur r E;

l as t E = cur r E;

doubl e Pgai n = t hi s . Kp * cur r E;
doubl e I gai n = t hi s . Ki * eSum;
doubl e Dgai n = t hi s . Kd * del t aE ( ) ;

r et ur n Pgai n + I gai n + Dgai n;
}

/ **
* Bound t he out put command t o t he mi ni mumand maxi mumadmi ssi bl e commands.
* @par amcommand command t o bound
* @r et ur n new bounded command
*/
pr i v at e i nt boundCommand ( i nt command) {
i f ( command == 0)
r et ur n 0;
i f ( command < 0) {
i f ( command > - mi nCommand)
command = - mi nCommand;
i f ( command < - max Command)
command = - max Command;
}
el s e {
i f ( command < mi nCommand)
command = mi nCommand;
i f ( command > max Command)
command = max Command;
}
r et ur n command;
}

/ **
* Angl e t r ansf or mat i ons, used i nt er nal l y.
* @par amangl e angl e t o t r ansf or m
* @r et ur n new t r ansf or med angl e
*/
Anexe
pagina CXLI
pr i v at e i nt t r ans f or mAngl e ( i nt angl e) {
angl e = angl e % 360;
i f ( angl e < 0)
angl e = 360 + angl e;
r et ur n angl e;
}

/ **
* Rot at e t he r obot on spot ( di f f er ent i al headi ng) wi t h a desi r ed headi ng.
* @par amangl e angl e f or r ot at i on
* @r et ur n f al se i n case t he r ot at i on was i nt er r upt ed, t r ue ot her wi se
*/
publ i c bool ean s et Di f f Headi ng ( i nt angl e) {
i f ( angl e == 0)
r et ur n t r ue;

s t op = f al s e;
bool ean r et = t r ue;
/ * get t he cur r ent headi ng */
doubl e cur r ent Head = t r ans f or mAngl e ( dev i ce. get Yaw ( ) ) ;
/ * cal cul at e t he goal headi ng */
doubl e newGoal = t r ans f or mAngl e ( ( i nt ) ( cur r ent Head + angl e) ) ;

s et Goal ( newGoal ) ;

doubl e now = t r ans f or mAngl e ( dev i ce. get Yaw ( ) ) ;

/ * keep r ot at i ng whi l e t he goal was not r eached */
whi l e ( now ! = newGoal ) {
i f ( s t op == t r ue) {
r et = f al s e;
br eak;
}

i f ( Mat h. abs ( now - newGoal ) <= 1 && newGoal == 180)
br eak; / * exi t i f we r eached our dest i nat i on */

now = t r ans f or mAngl e ( dev i ce. get Yaw ( ) ) ;

/ * get t he mot or command and check i f wi t hi n t he desi r ed l i mi t s */
i nt command = ( i nt ) get Command ( now) ;
command = boundCommand ( command) ;
dev i ce. s et Speed ( 0, command) ;

t r y { Thr ead. s l eep ( 100) ; } cat ch ( Ex cept i on e) {}
}
dev i ce. s et Speed ( 0, 0) ; / * st op t he r obot f r omr ot at i ng */

r et ur n r et ;
}

/ **
* Rot at e t he r obot on spot ( absol ut e headi ng) t o t he desi r ed headi ng.
* @par amangl e goal angl e
* @r et ur n f al se i n case t he r ot at i on was i nt er r upt ed, t r ue ot her wi se
*/
publ i c bool ean s et Headi ng ( i nt angl e) {
i nt cur r ent Angl e = t r ans f or mAngl e ( dev i ce. get Yaw ( ) ) ;

/ * di f f er ence bet ween t he cur r ent headi ng and t he goal headi ng */
i nt del t aAngl e = ( angl e - cur r ent Angl e) ;

i f ( del t aAngl e ! = 0) {
i f ( del t aAngl e <= 180 && del t aAngl e > 0)
r et ur n s et Di f f Headi ng ( del t aAngl e) ;
el s e
i f ( del t aAngl e > - 180)
r et ur n s et Di f f Headi ng ( - 360 + del t aAngl e) ;
el s e
r et ur n s et Di f f Headi ng ( 360 + del t aAngl e) ;
}
r et ur n t r ue;
}
}

j ja av va ac cl li ie en nt t/ /e ex xt tr ra a/ /P PC Co on nt tr ro ol ll le er r. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - PCont r ol l er . j ava
*
Anexe
pagina CXLII
*
*
*
* $I d: PCont r ol l er . j ava, v 1. 3 2005/ 06/ 14 09: 41: 07 veedee Exp $
*
*/

/ **
* Pr opor t i onal cont r ol l er i mpl ement at i on.
*/
publ i c cl as s PCont r ol l er ex t ends Cont r ol l er {

/ ** Pr opor t i onal const ant */
pr ot ect ed doubl e kp;

/ **
* Const r uct or f or PCont r ol l er .
* @par amKp t he pr opor t i onal const ant
*/
publ i c PCont r ol l er ( doubl e Kp) {
t hi s . kp = Kp;
}

/ **
*/
eSum += cur r E;

l as t E = cur r E;
doubl e Pgai n = t hi s . kp * cur r E;

r et ur n Pgai n;
}

/ **
* Get t he cur r ent val ue of t he pr opor t i onal const ant .
* @r et ur n Kp as a doubl e
*/
publ i c doubl e get Kp ( ) {
r et ur n t hi s . kp;
}

/ **
* Set a new val ue f or t he pr opor t i onal const ant .
* @par amnewKp t he new val ue f or Kp
*/
publ i c v oi d s et Kp ( doubl e newKp) {
t hi s . kp = newKp;
}
}

j ja av va ac cl li ie en nt t/ /e ex xt tr ra a/ /P PD DC Co on nt tr ro ol ll le er r. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - PDCont r ol l er . j ava
*
*
Anexe
pagina CXLIII
*
*
* $I d: PDCont r ol l er . j ava, v 1. 4 2005/ 06/ 14 09: 41: 16 veedee Exp $
*
*/

/ **
* Pr opor t i onal - Der i vat i ve cont r ol l er i mpl ement at i on.
*/
publ i c cl as s PDCont r ol l er ex t ends Cont r ol l er {

/ ** Der i vat i ve const ant */
pr ot ect ed doubl e kd;

/ **
* Const r uct or f or PDCont r ol l er .
* @par amKd t he der i vat i ve const ant
*/
publ i c PDCont r ol l er ( doubl e Kp, doubl e Kd) {
t hi s . kp = Kp;
t hi s . kd = Kd;
}

/ **
*/
eSum += cur r E;

l as t E = cur r E;
doubl e Dgai n = t hi s . kd * del t aE ( ) ;

r et ur n Pgai n + Dgai n;
}

/ **
*/
}

/ **
*/
}

/ **
* Get t he cur r ent val ue of t he der i vat i ve const ant .
* @r et ur n Kd as a doubl e
*/
publ i c doubl e get Kd ( ) {
r et ur n t hi s . kd;
}

/ **
* Set a new val ue f or t he der i vat i ve const ant .
* @par amnewKd t he new val ue f or Kd
*/
publ i c v oi d s et Kd ( doubl e newKd) {
t hi s . kd = newKd;
}
}

j ja av va ac cl li ie en nt t/ /e ex xt tr ra a/ /P PI IC Co on nt tr ro ol ll le er r. .j ja av va a
/ *
Anexe
pagina CXLIV
* Pl ayer J ava Cl i ent - PI Cont r ol l er . j ava
*
*
*
*
* $I d: PI Cont r ol l er . j ava, v 1. 4 2005/ 06/ 14 09: 41: 16 veedee Exp $
*
*/

/ **
* Pr opor t i onal - I nt egr al cont r ol l er i mpl ement at i on.
*/
publ i c cl as s PI Cont r ol l er ex t ends Cont r ol l er {

/ ** I nt egr al const ant */
pr ot ect ed doubl e ki ;

/ **
* Const r uct or f or PI Cont r ol l er .
* @par amKi t he i nt egr al const ant
*/
publ i c PI Cont r ol l er ( doubl e Kp, doubl e Ki ) {
t hi s . kp = Kp;
t hi s . ki = Ki ;
}

/ **
*/
eSum += cur r E;

l as t E = cur r E;
doubl e I gai n = t hi s . ki * eSum;

r et ur n Pgai n + I gai n;
}

/ **
*/
}

/ **
*/
}

/ **
* Get t he cur r ent val ue of t he i nt egr al const ant .
* @r et ur n Ki as a doubl e
*/
publ i c doubl e get Ki ( ) {
r et ur n t hi s . ki ;
}

/ **
* Set a new val ue f or t he i nt egr al const ant .
Anexe
pagina CXLV
* @par amnewKi t he new val ue f or Ki
*/
publ i c v oi d s et Ki ( doubl e newKi ) {
t hi s . ki = newKi ;
}
}

j ja av va ac cl li ie en nt t/ /e ex xt tr ra a/ /P PI ID DC Co on nt tr ro ol ll le er r. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - PI DCont r ol l er . j ava
*
*
*
*
* $I d: PI DCont r ol l er . j ava, v 1. 4 2005/ 06/ 14 09: 41: 16 veedee Exp $
*
*/

/ **
* Pr opor t i onal - I nt egr al - Der i vat i ve cont r ol l er i mpl ement at i on.
*/
publ i c cl as s PI DCont r ol l er ex t ends Cont r ol l er {

/ ** I nt egr al const ant */
pr ot ect ed doubl e ki ;
/ ** Der i vat i ve const ant */
pr ot ect ed doubl e kd;

/ **
* Const r uct or f or PI DCont r ol l er .
* @par amKi t he i nt egr al const ant
* @par amKd t he der i vat i ve const ant
*/
publ i c PI DCont r ol l er ( doubl e Kp, doubl e Ki , doubl e Kd) {
t hi s . kp = Kp;
t hi s . ki = Ki ;
t hi s . kd = Kd;
}

/ **
*/
eSum += cur r E;

l as t E = cur r E;
doubl e I gai n = t hi s . ki * eSum;
doubl e Dgai n = t hi s . kd * del t aE ( ) ;

r et ur n Pgai n + I gai n + Dgai n;
}

/ **
*/
}

/ **
Anexe
pagina CXLVI
*/
}

/ **
* Get t he cur r ent val ue of t he i nt egr al const ant .
* @r et ur n Ki as a doubl e
*/
publ i c doubl e get Ki ( ) {
r et ur n t hi s . ki ;
}

/ **
* Set a new val ue f or t he i nt egr al const ant .
* @par amnewKi t he new val ue f or Ki
*/
publ i c v oi d s et Ki ( doubl e newKi ) {
t hi s . ki = newKi ;
}

/ **
* Get t he cur r ent val ue of t he der i vat i ve const ant .
* @r et ur n Kd as a doubl e
*/
publ i c doubl e get Kd ( ) {
r et ur n t hi s . kd;
}

/ **
* Set a new val ue f or t he der i vat i ve const ant .
* @par amnewKd t he new val ue f or Kd
*/
publ i c v oi d s et Kd ( doubl e newKd) {
t hi s . kd = newKd;
}
}

j ja av va ac cl li ie en nt t/ /e ex xt tr ra a/ /P Po os si it ti io on nC Co on nt tr ro ol l. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Posi t i onCont r ol . j ava
* Copyr i ght ( C) 2005 Mar i us Bor odi & Radu Bogdan Rusu
*
*
*
*
* $I d: Posi t i onCont r ol . j ava, v 1. 1 2005/ 06/ 14 09: 44: 32 veedee Exp $
*
*/

i mpor t j av acl i ent . Abs t r act Pos i t i onDev i ce;
i mpor t j av a. awt . Poi nt ;

/ **
* Posi t i on cont r ol i nt er f ace f or Posi t i on, Posi t i on2D and Posi t i on3D Pl ayer
* i nt er f aces. Uses met hods f r ombot h pl ayer i nt er f aces and PI DCont r ol l er .
* @aut hor Mar i us Bor odi & Radu Bogdan Rusu
*/
publ i c cl as s Pos i t i onCont r ol ex t ends PI DCont r ol l er {

pr i v at e Abs t r act Pos i t i onDev i ce dev i ce = nul l ;

/ * PI D coef f i ci ent s */
pr i v at e i nt Kp = 1;
pr i v at e i nt Ki = 0;
pr i v at e i nt Kd = 0;

pr i v at e bool ean s t op = f al s e;
Anexe
pagina CXLVII

/ * mi ni mumand maxi mumadmi ssi bl e commands */
pr i v at e i nt mi nCommand = 1;
pr i v at e i nt max Command = 100;

/ **
* Const r uct or f or Posi t i onCont r ol .
*/
publ i c Pos i t i onCont r ol ( Abs t r act Pos i t i onDev i ce pd) {
s uper ( 1, 0, 0) ;
}

/ **
*/
publ i c Pos i t i onCont r ol ( Abs t r act Pos i t i onDev i ce pd,
t hi s . Kp = kp;
t hi s . Ki = ki ;
t hi s . Kd = kd;
}

/ **
*/
publ i c Pos i t i onCont r ol ( Abs t r act Pos i t i onDev i ce pd, i nt mi nC, i nt max C) {
s uper ( 1, 0, 0) ;
}

/ **
*/
publ i c Pos i t i onCont r ol ( Abs t r act Pos i t i onDev i ce pd, i nt mi nC, i nt max C,
t hi s . Kp = kp;
t hi s . Ki = ki ;
t hi s . Kd = kd;
}

/ **
* Set t he mi ni mumadmi ssi bl e command f or t he r obot ' s mot or s.
* @par ammi nC mi ni mumadmi ssi bl e command as an i nt eger
*/
publ i c v oi d s et Mi ni mumCommand ( i nt mi nC) {
}

/ **
* Set t he maxi mumadmi ssi bl e command f or t he r obot ' s mot or s.
* @par ammaxC maxi mumadmi ssi bl e command as an i nt eger
*/
publ i c v oi d s et Max i mumCommand ( i nt max C) {
}

/ **
* St op t he r obot f r ommovi ng.
Anexe
pagina CXLVIII
*/
publ i c v oi d s t opRobot ( ) {
t hi s . s t op = t r ue;
}

/ **
* Get t he cur r ent r obot posi t i on as a Poi nt ( AWT) .
* @r et ur n t he cur r ent r obot posi t i on
*/
publ i c Poi nt get Robot Pos i t i on ( ) {
r et ur n new Poi nt ( dev i ce. get X ( ) , dev i ce. get Y ( ) ) ;
}

/ **
* Bound t he out put command t o t he mi ni mumand maxi mumadmi ssi bl e commands.
* @par amcommand command t o bound
* @r et ur n new bounded command
*/
pr i v at e i nt boundCommand ( i nt command) {
i f ( command == 0)
r et ur n 0;
i f ( command < 0) {
i f ( command > - mi nCommand)
command = - mi nCommand;
i f ( command < - max Command)
command = - max Command;
}
el s e {
i f ( command < mi nCommand)
command = mi nCommand;
i f ( command > max Command)
command = max Command;
}
r et ur n command;
}

/ **
* Move t he r obot f or a gi ven di st ance t o a new dest i nat i on.
* @par amdi st ance t he desi r ed di st ance
* @r et ur n f al se i n case t he movement was i nt er r upt ed, t r ue ot her wi se
*/
publ i c bool ean mov eRobot ( i nt di s t ance) {
s t op = f al s e;
bool ean r et = t r ue;
i nt di s t = di s t ance;
i nt angl e = dev i ce. get Yaw ( ) ; / * get t he cur r ent headi ng */
Poi nt r oboPos = get Robot Pos i t i on ( ) ; / * get t he cur r ent posi t i on */

/ * f i nd out t he dest i nat i on poi nt usi ng t he di st ance and t he cur r ent angl e */
Poi nt des t = Pos i t i onGeomet r y Tool s . cal cDi s t Poi nt ( r oboPos , di s t ance, angl e) ;
/ * get t he di st ance f r omDest t o r oboPos */
di s t = Pos i t i onGeomet r y Tool s . cal cDi s t ( des t , r oboPos ) ;

s et Goal ( 0) ;
i nt s gn = - 1;
i f ( di s t ance < 0 )
s gn = 1;

/ * move t o t he goal , mi ni mi ze di st ance */
whi l e ( di s t > 0) {
i f ( s t op == t r ue) {
r et = f al s e;
br eak;
}
r oboPos = get Robot Pos i t i on ( ) ; / * get cur r ent posi t i on */
/ * get t he di st ance f r omDest t o r oboPos */
i nt er X = r oboPos . x - des t . x ;
i nt er Y = r oboPos . y - des t . y ;
i f ( ( er X == 0 && Mat h. abs ( er Y) <= 10) | | ( er Y == 0 && Mat h. abs ( er X) <= 10) )
br eak; / * exi t i f we r eached our dest i nat i on */

/ * get t he mot or command and check i f wi t hi n t he desi r ed l i mi t s */
i nt command = ( i nt ) get Command ( di s t ) ;
command = boundCommand ( command) ;
dev i ce. s et Speed ( ( s gn) * command, 0) ;

t r y { Thr ead. s l eep( 100) ; } cat ch ( Ex cept i on e) { }
}
dev i ce. s et Speed ( 0, 0) ; / * st op t he r obot f r ommovi ng */
r oboPos = get Robot Pos i t i on ( ) ; / * get cur r ent r obot posi t i on */
i f ( di s t ! = 0) / * send a war ni ng i n case of er r or s */
Sy s t em. er r . pr i nt l n ( "[ Pos i t i onCont r ol ] Di s t ance er r or : " + di s t ) ;

Anexe
pagina CXLIX
r et ur n r et ;
}
}

j ja av va ac cl li ie en nt t/ /e ex xt tr ra a/ /P Po os si it ti io on nG Ge eo om me et tr ry yT To oo ol ls s. .j ja av va a
/ *
* Pl ayer J ava Cl i ent - Posi t i onGeomet r yTool s. j ava
* Copyr i ght ( C) 2005 Mar i us Bor odi & Radu Bogdan Rusu
*
*
*
*
*
* $I d: Posi t i onGeomet r yTool s. j ava, v 1. 1 2005/ 06/ 14 09: 44: 32 veedee Exp $
*
*/


i mpor t j av a. awt . *;

/ **
* Sever al met hods f or posi t i on geomet r i c cal cul us.
* @aut hor Mar i us Bor odi & Radu Bogdan Rusu
*/

publ i c cl as s Pos i t i onGeomet r y Tool s {

/ **
* Cal cul at e t he di st ance bet ween t wo X and Y poi nt s assumi ng
* t hat t hei r count er par t s ar e 0 usi ng Pi t agor a' s t heor em.
* @par amx X' s coor donat e of t he f i r st poi nt ( Y=0)
* @par amy Y' s coor donat e of t he second poi nt ( X=0)
* @r et ur n di st ance bet ween [ X, 0] and [ 0, Y] as an i nt eger
*/
publ i c s t at i c i nt cal cDi s t ( i nt x , i nt y ) {
doubl e d = ( Mat h. s qr t ( Mat h. pow( x , 2) + Mat h. pow ( y , 2) ) ) ;
r et ur n ( i nt ) Mat h. r ound ( d) ;
}

/ **
* Cal cul at e t he di st ance bet ween t wo poi nt s ( p1 and p2) .
* @par amp1 f i r st poi nt
* @par amp2 second poi nt
* @r et ur n t he di st ance bet ween p1 and p2
*/
publ i c s t at i c i nt cal cDi s t ( Poi nt p1, Poi nt p2) {
i nt x = p2. x - p1. x ;
i nt y = p2. y - p1. y ;
r et ur n cal cDi s t ( x , y ) ;
}

/ **
* Cal cul at e t he X coor di nat e of a poi nt si t uat ed at di st ance
* di st , angl e angl e f r oma gi ven poi nt i ni t P.
* @par ami ni t P r ef er ence poi nt
* @par amdi st di st ance f r omt he r ef er ence poi nt t o t he desi r ed poi nt
* @par amangl e angl e f r omt he r ef er ence poi nt t o t he desi r ed poi nt
* @r et ur n t he X coor di nat e of t he poi nt
*/
publ i c s t at i c i nt cal cX ( Poi nt i ni t P, i nt di s t , i nt angl e) {
doubl e t mp = ( di s t * Mat h. cos ( Mat h. t oRadi ans ( angl e) ) ) ;
r et ur n ( i ni t P. x + ( i nt ) Mat h. r ound ( t mp) ) ;
}

/ **
* Cal cul at e t he Y coor di nat e of a poi nt si t uat ed at di st ance
Anexe
pagina CL
* @r et ur n t he Y coor di nat e of t he poi nt
*/
publ i c s t at i c i nt cal cY ( Poi nt i ni t P, i nt di s t , i nt angl e) {
doubl e t mp = ( di s t * Mat h. s i n ( Mat h. t oRadi ans ( angl e) ) ) ;
r et ur n ( i ni t P. y + ( i nt ) Mat h. r ound ( t mp) ) ;
}

/ **
* Cal cul at e t he coor di nat es of a poi nt si t uat ed at di st ance
* @r et ur n t he coor di nat es of t he new poi nt as a Poi nt ( AWT)
*/
publ i c s t at i c Poi nt cal cDi s t Poi nt ( Poi nt i ni t P, i nt di s t , i nt angl e) {
doubl e t mp = ( di s t * Mat h. cos ( Mat h. t oRadi ans ( angl e) ) ) ;
i nt x = i ni t P. x + ( i nt ) Mat h. r ound ( t mp) ;
t mp = ( di s t * Mat h. s i n ( Mat h. t oRadi ans ( angl e) ) ) ;
i nt y = i ni t P. y + ( i nt ) Mat h. r ound ( t mp) ;
r et ur n new Poi nt ( x , y ) ;
}
}
Anexe
pagina I

C C. . C Co od du ul l s su ur rs s a al l d dr ri iv ve er ru ul lu ui i C CM MU Uc ca am m2 2
c ca am me er ra a. .h h
#i f ndef CAMERA_H_
#def i ne CAMERA_H_

#i f def __cpl us pl us
ex t er n "C"
{
#endi f

#i ncl ude <f cnt l . h>
#i ncl ude <t er mi os . h>
#i ncl ude <s t di o. h>
#i ncl ude <t i me. h>
#i ncl ude <s t r i ng. h>
#i ncl ude <s t dl i b. h>
#i ncl ude <uni s t d. h>

/ * These shoul d NOT be r edef i ned. */
/ / t ypedef unsi gned i nt ui nt 32_t ;
/ / t ypedef unsi gned shor t ui nt 16_t ;
/ / t ypedef unsi gned char ui nt 8_t ;

/ * I nst ead #i ncl ude pl ayer conf i g. h, whi ch get s t hemi n a por t abl e way */
#i ncl ude "pl ay er conf i g. h"

/ **************************************************************************
*** CONSTANST ***
**************************************************************************/
#def i ne I MAGE_WI DTH 174 / / t he wi dt h of t he f r ame camer a sends
#def i ne I MAGE_HEI GHT 143 / / t he hei ght of t he f r ame camer a sends
#def i ne CONTRAST 5 / / camer a' s cont r ast r egi st er #
#def i ne BRI GHTNESS 6 / / camer a' s br i ght ness r egi st er #
#def i ne COLORMODE 18 / / camer a' s col or mode r egi st er #
#def i ne RGB_AWT_ON 44 / / camer a' s RGB aut o whi t e bal ance on
#def i ne RGB_AWT_OFF 40 / / camer a' sRGB aut o whi t e bal ance of f
#def i ne YCRCB_AWT_ON 36 / / camer a' sYCr Cb aut o whi t e bal ance on
#def i ne YCRCB_AWT_OFF 32 / / camer a' sYCr Cb aut o whi t e bal ance of f
#def i ne AUTOGAI N 19 / / camer a' s aut ogai n r egi st er #
#def i ne AUTOGAI N_ON 33 / / camer a' s aut ogai n on
#def i ne AUTOGAI N_OFF 32 / / camer a' s aut ogai n of f
#def i ne ZERO_POSI TI ON 128 / / ser vos' mi ddl e posi t i on as def i end by camer a
#def i ne MI N_RGB 16 / / camer a' s mi n r gb val ue
#def i ne MAX_RGB 240 / / camer a' s max r gb val ue
#def i ne T_PACKET_LENGTH 50 / / max l engt h of T packet t hat camer a r et ur ns
#def i ne F_PACKET_LENGTH 37474 / / max l engt h of T packet t hat camer a r et ur ns

/ **************************************************************************
*** T PACKET ***
**************************************************************************/
t y pedef s t r uct / / camer a' s out put packet f or t r acki ng bl obs
{
i nt mi ddl e_x , mi ddl e_y ; / / t he bl ob ent r oi d ( i mage coor ds)
i nt l ef t _x ; / / t he l ef t most cor ner ' s x val ue
i nt l ef t _y ; / / t he l ef t most cor ner ' s y val ue
i nt r i ght _x ; / / t he r i ght most cor ner ' s x vl aue
i nt r i ght _y ; / / t he r i ght most cor ner ' s y val ue
i nt bl ob_ar ea; / / number of pi xel s i n t he t r acked r egi on,
/ / scal ed and capped at 255: ( pi xel s+4) / 8
i nt conf i dence; / / t he ( # of pi xel s/ ar ea) *256 of t he bounded
/ / r ect angl e and capped at 255
} packet _t ;

/ **************************************************************************
*** F PACKET ***
**************************************************************************/
t y pedef s t r uct
{
i nt r , g, b;
} r gb_t y pe;

t y pedef s t r uct
{
Anexe
pagina II
i nt r owby t e;
r gb_t y pe r gb[ I MAGE_WI DTH] ;
} r ow_t y pe;

t y pedef s t r uct
{
i nt f i r s t ;
i nt x s i z e, y s i z e;
r ow_t y pe r ows [ I MAGE_HEI GHT] ;
i nt l as t ;
} packet _f ;

/ **************************************************************************
*** I MAGER CONFI G ***
**************************************************************************/
t y pedef s t r uct / / camer a' s i nt er nal r egi st er cont r ol l i ng i mage qual i t y
{
ui nt 8_t s ubt y pe; / / must be PLAYER_BLOBFI NDER_SET_I MAGER_PARAMS_REQ.
i nt 16_t br i ght nes s ; / / cont r ast : - 1 = no change. ( 0- 255)
i nt 16_t cont r as t ; / / br i ght ness: - 1 = no change. ( 0- 255)
i nt 8_t col or mode; / / col or mode: - 1 = no change.
/ / 0 = RGB/ aut o whi t e bal ance Of f ,
/ / 1 = RGB/ Aut oWhi t eBal ance On,
/ / 2 = YCr CB/ Aut oWhi t eBal ance Of f ,
/ / 3 = YCr Cb/ AWB On)
i nt 8_t aut ogai n; / / aut o gai n: - 1 = no change.
/ / 0 = of f , o
/ / 1 = on.
} i mager _conf i g;

/ **************************************************************************
*** CONFI G CONFI G ***
**************************************************************************/
t y pedef s t r uct
{
ui nt 8_t s ubt y pe; / / must be PLAYER_BLOBFI NDER_SET_COLOR_REQ.
i nt 16_t r mi n, r max ; / / RGB mi ni mumand max val ues ( 0- 255)
i nt 16_t gmi n, gmax ;
i nt 16_t bmi n, bmax ;
} col or _conf i g;

/ **************************************************************************
*** RGB ***
**************************************************************************/
t y pedef s t r uct / / RGB val ues
{
i nt r ed;
i nt gr een;
i nt bl ue;
} r gb;

/ **************************************************************************
*** CONFI G CONFI G ***
**************************************************************************/
t y pedef s t r uct / / camer a' s i mage
{
i nt wi dt h;
i nt hei ght ;
r gb **pi x el ;
} i mage;

col or _conf i g r ange;

/ **************************************************************************
*** FUNCTI ON PROTOTYPES ***
**************************************************************************/
i nt get _t _packet ( i nt f d, packet _t *t packet ) ;
i nt s et _i mager _conf i g( i nt f d, i mager _conf i g i c) ;
i nt get _by t es ( i nt f d, char *buf , s i z e_t l en) ;
i nt open_por t ( char *dev i cepat h) ;
v oi d cl os e_por t ( i nt f d) ;
v oi d r ead_t _packet ( i nt f d, char *t packChar s ) ;
i nt r ead_f _packet ( i nt f d, char *f packChar s ) ;
i nt s et _t _packet ( packet _t *t packet , char t pack_char s [ ] ) ;
i nt s et _f _packet ( packet _f *f packet , char f pack_char s [ ] , i nt chan_num) ;
i nt s et _s er v o_pos i t i on( i nt f d, i nt s er v o_num, i nt angl e) ;
i nt get _s er v o_pos i t i on( i nt f d, i nt s er v o_num) ;
v oi d s t op_t r acki ng( i nt f d) ;
i nt wr i t e_check( i nt f d, char *ms g, i nt r es pond_s i z e) ;
i nt pol l _mode( i nt f d, i nt on) ;
v oi d make_command( char *cmd, i nt *n, s i z e_t s i z e, char *f ul l Command) ;
i nt aut o_s er v oi ng( i nt f d, i nt on) ;
v oi d t r ack_bl ob( i nt f d, col or _conf i g cc) ;
i nt r ead_i mage ( i nt f d, i nt chan_num, packet _f *f packet ) ;

Anexe
pagina III
}
#endi f

#endi f

c ca am me er ra a. .c c
#i ncl ude "camer a. h"
#i ncl ude "r epl ace. h"

/ ************************************************************************
*** WRI TE CHECK ***
**************************************************************************/
/ * Descr i pt i on: Thi s f unct i on wr i t es a command t o t he camer a and checks i f t he
wr i t e was done successf ul l y by checki ng camer a' s r esponse.
Par amet er s: f d: ser i al por t f i l e descr i pt or
msg: t he command t o be send t o t he camer a
Ret ur ns: 1: i f t he wr i t e was successf ul
0: ot her wi se
*/
i nt wr i t e_check( i nt f d, char *ms g, i nt r es pond_s i z e)
{
char r es pond[ 5] ;

i f ( wr i t e( f d, ms g, s t r l en( ms g) ) ! = ( i nt ) s t r l en( ms g) ) / / wr i t e t he command t o t he camer a
{
pr i nt f ( "Cmucam2: wr i t i ng t o s er i al dev i ce f ai l ed. \n" ) ;
r et ur n 0;
}
i f ( ( get _by t es ( f d, r es pond, r es pond_s i z e) ) < 1 )
{
pr i nt f ( "Cmucam2: get by t es f ai l ed\n" ) ;
r et ur n 0;
}

i f ( r es pond[ 0] == ' N' ) / / I f NCK i s r et ur ned, t her e was an er r or i n wr i t e
{
pr i nt f ( "Cmucam2: r ecei v ed NCK! \n") ;
r et ur n 0;
}
r et ur n 1;
}

i nt power ( i nt f d, i nt on)
{
i f ( on)
r et ur n wr i t e_check( f d, "CP 1\r ", 5) ;
r et ur n wr i t e_check( f d, "CP 0\r ", 5) ;
}

/ **************************************************************************
*** SET I MAGER CONFI G ***
**************************************************************************/
/ * Descr i pt i on: Thi s f unct i on set s t he camer a' s i nt er nal r esi ger val ues
f or cont r ol l i ng i mage qual i t i es.
pl ayer _bl obf i nder _i mager _conf i g: a Pl ayer packet cont ai ni ng
i nf or mat i on f or camer a' s i nt er nal r egi st er :
cont r ast , br i ght ness, col or mode, Exposur e
Ret ur ns: 1: I f t he command was successf ul l y sent t o t he camer a
0: Ot her wi se
*/

i nt s et _i mager _conf i g( i nt f d, i mager _conf i g i c)
{
i nt v al ue[ 8] , s i z e = 0; / / The number s used i n t he command:
char command[ 26] ; / / ex. CR 5 255 19 33
i f ( i c. cont r as t ! = - 1) / / I f t her i s a change set t he val ues
{
v al ue[ s i z e++] = CONTRAST;
v al ue[ s i z e++] = i c. cont r as t ;
}
i f ( i c. br i ght nes s ! = - 1)
{
v al ue[ s i z e++] = BRI GHTNESS;
v al ue[ s i z e++] = i c. br i ght nes s ;
}
i f ( i c. col or mode ! = - 1)
{
v al ue[ s i z e++] = COLORMODE;
i f ( i c. col or mode == 0)
v al ue[ s i z e++] = RGB_AWT_OFF;
Anexe
pagina IV
v al ue[ s i z e++] = RGB_AWT_ON;
v al ue[ s i z e++] = YCRCB_AWT_OFF;
v al ue[ s i z e++] = YCRCB_AWT_ON;
}
i f ( i c. aut ogai n ! = - 1)
{
v al ue[ s i z e++] = AUTOGAI N;
i f ( i c. aut ogai n == 0)
v al ue[ s i z e++] = AUTOGAI N_OFF;
i f ( i c. aut ogai n == 1)
v al ue[ s i z e++] = AUTOGAI N_ON;
}
make_command( "CR ", v al ue, s i z e, command) ; / / Put t he val ues i nt o camer a' s command f or mat :
/ / ex. CR 6 105 18 44
r et ur n wr i t e_check( f d, command, 5) ; / / send t he command t o t he camer a
}

/ **************************************************************************
number [ i ] = c; *** GET T PACKET ***
**************************************************************************/
/ * Descr i pt i on: Thi s f unct i on put s t he camer a' s out put dur i ng t r acki ng i nt o
a T packet , whi ch cont r ai ns i nf or mat i on about t he bl ob.
t packet : t he packet t hat wi l l cont ai n t he bl ob i nf o
Ret ur ns: voi d
*/
i nt get _t _packet ( i nt f d, packet _t *t packet )
{
char t pack_char s [ T_PACKET_LENGTH] ;
r ead_t _packet ( f d, t pack_char s ) ; / / r ead t he out put of t he camer a
r et ur n s et _t _packet ( t packet , t pack_char s ) ; / / conver t i t i nt o T packet
}

/ **************************************************************************
*** POLL MODE ***
**************************************************************************/
/ * Descr i pt i on: Thi s f unct i ons det er mi nes whet her t he camer a shoul d send a
cont i nuous st r eamof packet s or j ust one packet .
on: i f on == 1, onl y one packet i s send
i f on == 0, a cont i nuous st r eamof packet s i s send
0: Ot her wi se
*/
i nt pol l _mode( i nt f d, i nt on)
{
i f ( on)
r et ur n wr i t e_check( f d, "PM 1\r ", 5) ;
el s e
r et ur n wr i t e_check( f d, "PM 0\r ", 5) ;
}

/ **************************************************************************
*** SET SERVO POSI TI ON ***
**************************************************************************/
/ * Descr i pt i on: Thi s f unct i ons set s t he ser vo posi t i on gi ven t he ser vo number
and t he angl e ( not e: angl e = 0 denot es ser vo posi t i on = 128
i n t er ms of camer a' s val ues)
Par amet er s: number [ i ] = c; f d: ser i al por t f i l e descr i pt or
ser vo_num: t he ser vo whi ch we ar e set t i ng t he posi t i on
I amusi ng 0: pan 1: t i l t
0: Ot her wi se
*/
i nt s et _s er v o_pos i t i on( i nt f d, i nt s er v o_num, i nt angl e)
{
i nt pos i t i on = ZERO_POSI TI ON + angl e; / / change t he angl e i nt o camer a' s
f or mat
char comm[ 10] ; / / f or ser vo posi t i on. I amusi ng
angl e 0
i nt v al ue[ ] = {s er v o_num, pos i t i on}; / / cor r espondi ng t o t he def aul t ser vo pos.
128
make_command( "SV ", v al ue, s i z eof ( v al ue) /s i z eof ( i nt ) , comm) ; / / gener at e t he command usi ng t he
val ues
pr i nt f ( "s er v o %d new pos i t i on: %d\n", s er v o_num, angl e) ;
r et ur n wr i t e_check( f d, comm, 5) ; / / wr i t e t he command t o t he
camer a
}

/ **************************************************************************
*** MAKE COMMAND ***
**************************************************************************/
Anexe
pagina V
/ * Descr i pt i on: Thi s f unct i on get s a set s of val ues and a camer a command header
number [ i ] = c; t o gener at e t he command f or t he camer a.
Par amet er s: cmd: t he command header , f or exampl e SF or CR ( see CMUcam2 user gui de)
n: t he set of val ues t o be used i n t he command
si ze: t he number of val ues used
f ul l _command: t he f i nal command i n char act er s t o be send t o t he camer a
Ret ur ns: voi d
*/
v oi d make_command( char *cmd, i nt *n, s i z e_t s i z e, char *f ul l _command)
{
char v al ue[ 3] ; / / t he val ues ar e al l wi t hi ng 3 di gi t s
i nt l engt h, i ;
f or ( i = 0; i < 10; i ++) / / set al l t o nul l so t hat i f t her e ar e
f ul l _command[ i ] = ' \0' ; / / unsed char act er s at t he end, t he camer a
/ / does not compl ai n about t he command.
/ / t her e i s pr obabl y a bet t er way t o do t hi s!
s t r cat ( f ul l _command, cmd) ; / / at t ach t he command header , ex. SF
f or ( i = 0; i < ( i nt ) s i z e; i ++) / / f or al l t he val ues, conver t t hemi nt o char
{ / / and at t ach t hemt o t he end of t he command
l engt h = s pr i nt f ( v al ue, "%d", n[ i ] ) ; / / pl us a space
s t r cat ( f ul l _command, v al ue) ;
s t r cat ( f ul l _command, " ") ;
}
s t r cat ( f ul l _command, "\r ") ; / / at t ach t he r et ur n char act er t o t he end
}

/ **************************************************************************
*** OPEN PORT ***
**************************************************************************/
/ * Descr i pt i on: Thi s f unct i on opens t he ser i al por t f or communi cat i on wi t h t he camer a.
Par amet er s: NONE
Ret ur ns: t he f i l e descr i pt or
*/
i nt open_por t ( char *dev i cepat h)
{
i nt f d = open( dev i cepat h, O_RDWR ) ; // open t he s er i al por t
s t r uct t er mi os t er m;
s t r uct pol l f d f ds [ 1] ;
char cam_r es pons e[ 5] ;

i f ( t cget at t r ( f d, &t er m ) < 0 ) / / get devi ce at t r i but es
{
put s ( "Cmucam2: unabl e t o get dev i ce at t r i but es . ") ;
r et ur n - 1;
}

cf maker aw( &t er m ) ;
cf s et i s peed( &t er m, B115200 ) ; / / set baudr at e t o 115200
cf s et os peed( &t er m, B115200 ) ;

i f ( t cs et at t r ( f d, TCSAFLUSH, &t er m ) < 0 )
{
put s ( "Cmucam2: unabl e t o s et dev i ce at t r i but es ") ;
r et ur n - 1;
}

/ / Make sur e queue i s empt y
t cf l us h( f d, TCI OFLUSH) ;

wr i t e( f d, "\r ", 1) ;
f ds [ 0] . f d = f d;
f ds [ 0] . ev ent s = 1;
pol l ( f ds , 1, 100) ;
i f ( ! f ds [ 0] . r ev ent s )
{
pr i nt f ( "ERROR: CMUCAM2 I S OFF! \n") ;
r et ur n - 1;
}
get _by t es ( f d, cam_r es pons e, 5) ;

r et ur n f d;
}

/ **************************************************************************
*** CLOSE PORT ***
**************************************************************************/
/ * Descr i pt i on: Thi s f unct i on cl oses t he ser i al por t .
Par amet er s: f d: ser i al por t handl er
Ret ur ns: voi d
*/
v oi d cl os e_por t ( i nt f d)
{
cl os e( f d) ;
}

Anexe
pagina VI
/ **************************************************************************
*** GET BYTES ***
**************************************************************************/
/ * Descr i pt i on: Thi s f unct i on r eads a speci f i ed number of byt es f r omt he ser i al por t
Par amet er s: f d: ser i al por t handl er , buf : byt es r ead, l en: byt es t o r ead
Ret ur ns: 0: i f coul d not r ead t he number of byt es speci f i ed 1: ot her wi se
*/
i nt get _by t es ( i nt f d, char *buf , s i z e_t l en)
{
i nt by t es _r ead = 0, r et _v al ;
whi l e( by t es _r ead < ( i nt ) l en)
{
r et _v al = r ead( f d, buf +by t es _r ead, l en- by t es _r ead) ;
i f ( r et _v al < 0)
{
per r or ( "Cmucam2: get t i ng by t es f ai l ed. \n") ;
r et ur n 0;
}
el s e i f ( r et _v al > 0)
by t es _r ead += r et _v al ;
}
r et ur n by t es _r ead;
}

/ **************************************************************************
*** GET SERVO POSI TI ON ***
**************************************************************************/
/ * Descr i pt i on: Thi s f unct i on get s t he posi t i on of t he speci f i ed ser vo.
Par amet er s: f d: ser i al por t handl er , ser vo_num: t he ser vo number whose si ze i s r equi r ed
Ret ur ns: t he posi t i on of t he ser vo
*/
i nt get _s er v o_pos i t i on( i nt f d, i nt s er v o_num)
{
i nt s er v o_pos i t i on;
i nt i ;
char number [ 3] ;
char c = 0;

i f ( s er v o_num) / / set posi t i on of ser vo 1
wr i t e_check( f d, "GS 1\r ", 4) ;
el s e / / set posi t i on of ser vo 0
wr i t e_check( f d, "GS 0\r ", 4) ;

f or ( i = 0; 1; i ++)
{
r ead( f d, &c, 1) ;
i f ( c == ' \r ' )
br eak;
number [ i ] = c;
}
r ead( f d, &c, 1) ; / / r ead t he : at t he end
s er v o_pos i t i on = at oi ( number ) ;
r et ur n s er v o_pos i t i on - ZERO_POSI TI ON;
}

/ **************************************************************************
*** TRACK BLOB ***
**************************************************************************/
/ * Descr i pt i on: Thi s f unct i ons st ar t s t o Tr ack a Col or . I t t akes i n t he mi ni mumand
maxi mumRGB val ues and out put s a t ype T packet . Thi s packet by def ual t
r et ur ns t he mi ddl e mass x and y coor di nat es, t he boundi ng box, t he
number of pi xl es t r acked, and a conf i dence val ues.
cc: t he mi n & max RGB val ues of t he bl ob t o be t r acked.
*/
v oi d t r ack_bl ob( i nt f d, col or _conf i g cc )
{
char cmd[ 28] ;
i nt v al ue[ ] = {cc. r mi n, cc. r max , cc. gmi n, cc. gmax , cc. bmi n, cc. bmax };
r ange = cc;

make_command( "TC ", v al ue, s i z eof ( v al ue) /s i z eof ( i nt ) , cmd) ;
i f ( ! wr i t e_check( f d, cmd, 4) )
{
pr i nt f ( "ERROR; t r ack col or f ai l ed. \n") ;
r et ur n;
}
}

/ **************************************************************************
*** GET SERVO POSI TI ON ***
**************************************************************************/
/ * Descr i pt i on: The f unct i on enabl es/ di sabl es aut omat i c ser voi ng
Par amet er s: f d: ser i al por t handl er , on: det er mi nes whet her e t o enabl e( on=1) or di sabl e( on=0)
Ret ur ns: 0: i f t he camer a f ai l s t o wr i t e command 1: ot her wi se
*/
Anexe
pagina VII
i nt aut o_s er v oi ng( i nt f d, i nt on)
{
/ / Enabl i ng Aut o Ser voi ng Mode f or bot h ser vos
i f ( on)
{
i f ( ! wr i t e_check( f d, "SM 15\r ", 5) )
{
pr i nt f ( "CMUCAM I I ERROR: Enabl i ng aut o- s er v o f ai l ed. \n") ;
r et ur n 0;
}
r et ur n 1;
}
el s e
{
i f ( ! wr i t e_check( f d, "SM 0\r ", 5) )
{
pr i nt f ( "CMUCAM I I ERROR: Di s abl i ng aut o- s er v o f ai l d. \n") ;
r et ur n 0;
}
r et ur n 1;
}
}

/ **************************************************************************
*** STOP TRACKI NG ***
**************************************************************************/
/ * Descr i pt i on: The f unct i on st ops t he camer a f r omt r acki ng bl obs and sendi ng dat a
Par amet er s: f d: ser i al por t handl er
Ret ur ns: none
*/
v oi d s t op_t r acki ng( i nt f d)
{
char c = 0;
wr i t e( f d, "\r ", 1) ;
whi l e( c ! = ' : ' )
r ead( f d, &c, 1) ;
}

/ **************************************************************************
*** READ T PACKET ***
**************************************************************************/
/ * Descr i pt i on: The f unct i on r eads a t - packet f r omcamer a, ex. when camer a i s t r acki ng
Par amet er s: f d: ser i al por t handl er , t pack_char s: t he char act er s r ead
Ret ur ns: none
*/
v oi d r ead_t _packet ( i nt f d, char *t pack_char s )
{
char c = 0;
i nt k = 0;
whi l e( 1)
{
r ead( f d, &c, 1) ;
t pack_char s [ k++] = c;
i f ( c == ' \r ' )
br eak;
}
i f ( t pack_char s [ k- 1] ! = ' \r ' )
pr i nt f ( "ERROR: r eadi ng T packet f ai l ed. \n") ;
t pack_char s [ k] = ' \0' ;
}

/ **************************************************************************
*** READ T PACKET ***
**************************************************************************/
/ * Descr i pt i on: The f unct i on r eads a t - packet f r omcamer a, ex. when camer a i s t r acki ng
Par amet er s: f d: ser i al por t handl er , out put :
Ret ur ns: none
*/
i nt s et _t _packet ( packet _t *t packet , char t pack_char s [ ] )
{
char packet _t y pe;
s s canf ( t pack_char s , "%c %d %d %d %d %d %d %d %d", &packet _t y pe, &t packet - >mi ddl e_x ,
&t packet - >mi ddl e_y ,
&t packet - >l ef t _x , &t packet - >l ef t _y , &t packet - >r i ght _x , &t packet - >r i ght _y ,
&t packet - >bl ob_ar ea, &t packet - >conf i dence) ;
i f ( packet _t y pe ! = ' T' )
{
pr i nt f ( "ERROR: cmucam2 f ai l ed t o t r ans mi t t packet . \n") ;
r et ur n 0;
}
r et ur n 1;
}

/ **************************************************************************
*** READ F PACKET ***
Anexe
pagina VIII
**************************************************************************/
/ * Descr i pt i on: The f unct i on r eads a f - packet f r omcamer a
Par amet er s: f d: ser i al por t handl er , f pack_char s: t he char act er s r ead
Ret ur ns: none
*/
i nt r ead_f _packet ( i nt f d, char *f pack_char s )
{
char c = 0;
i nt k = 0;

whi l e ( ( c ! = 0) | | ( c ! = 1) )
{
r ead( f d, &c, 1) ;

i f ( c == 0) {
pr i nt f ( "Cmucam2: f r ame gr ab f ai l ed. \n") ;
r et ur n - 1;
}

i f ( c == 1) {
f pack_char s [ k++] = c;
r ead( f d, &c, 1) ;
/ / char xsi ze = c;
r ead( f d, &c, 1) ;
/ / char ysi ze = c;
/ / pr i nt f ( " Cmucam2: get t i ng a f r ame of X=%d and Y=%d pi xel s. \ n" ,
/ / ( ui nt 8_t ) xsi ze*2,
/ / ( ui nt 8_t ) ysi ze) ;
br eak;
}
}

whi l e( 1)
{
r ead ( f d, &c, 1) ;
i f ( c == 3) {
/ / pr i nt f ( " Cmucam2: got a f r ame of %d byt es. \ n" , k) ;
br eak;
}
}

i f ( f pack_char s [ k- 1] ! = 3) {
pr i nt f ( "ERROR: r eadi ng F packet f ai l ed. \n") ;
r et ur n - 1;
}
f pack_char s [ k] = ' \0' ;
r et ur n 0;
}

/ **************************************************************************
*** READ I MAGE ***
**************************************************************************/
/ * Descr i pt i on: Thi s f unct i on get s an i mage f r omt he camer a usi ng t he
speci f i ed channel as a f i l t er .
Par amet er s: f d: ser i al por t handl er , chan_num: t he channel number
Ret ur ns: t he i mage as an F packet
*/
i nt r ead_i mage ( i nt f d, i nt chan_num, packet _f *f packet )
{
s wi t ch ( chan_num)
{
cas e 0:
{
wr i t e_check ( f d, "SF 0\r ", 4) ;
br eak;
}
cas e 1:
{
br eak;
}
cas e 2:
{
br eak;
}
cas e - 1:
{
wr i t e_check ( f d, "SF \r ", 4) ;
br eak;
}
def aul t :
Anexe
pagina IX
{
pr i nt f ( "Cmucam2: i nv al i d channel number ! \n") ;
br eak;
}
}

char f pack_char s [ F_PACKET_LENGTH] ;

i f ( r ead_f _packet ( f d, f pack_char s ) ! = 0)
r et ur n - 1;

r et ur n s et _f _packet ( f packet , f pack_char s , chan_num) ;
}

/ **************************************************************************
*** SET F PACKET ***
**************************************************************************/
i nt s et _f _packet ( packet _f *f packet , char f pack_char s [ ] , i nt chan_num)
{
f packet - >f i r s t = ( ui nt 8_t ) f pack_char s [ 0] ;
f packet - >x s i z e = ( ui nt 8_t ) f pack_char s [ 1] ;
f packet - >y s i z e = ( ui nt 8_t ) f pack_char s [ 2] ;

s wi t ch ( chan_num)
{
cas e - 1:
{
i nt i = 0;
i nt j = 0;

f or ( i = 0; i r ows [ i ] . r owby t e = ( ui nt 8_t ) f pack_char s [ 3 + ( i *I MAGE_WI DTH/2) ] ;
f or ( j = 0; j r ows [ i ] . r gb[ j ] . r = ( ui nt 8_t ) f pack_char s [ 4 + i *j ] ;
f packet - >r ows [ i ] . r gb[ j ] . g = ( ui nt 8_t ) f pack_char s [ 5 + i *j ] ;
f packet - >r ows [ i ] . r gb[ j ] . b = ( ui nt 8_t ) f pack_char s [ 6 + i *j ] ;
}
}

f packet - >l as t = ( ui nt 8_t ) f pack_char s [ 3 + I MAGE_HEI GHT*( I MAGE_WI DTH/2*3 + 1) ] ;

br eak;
}
def aul t :
{
i nt i = 0;
i nt j = 0;

f or ( i = 0; i r ows [ i ] . r owby t e = ( ui nt 8_t ) f pack_char s [ 3 + ( i *I MAGE_WI DTH/2) ] ;
f or ( j = 0; j r ows [ i ] . r gb[ j ] . r = ( ui nt 8_t ) f pack_char s [ 4 + i *j ] ;
f packet - >r ows [ i ] . r gb[ j ] . g = ( ui nt 8_t ) f pack_char s [ 4 + i *j ] ;
f packet - >r ows [ i ] . r gb[ j ] . b = ( ui nt 8_t ) f pack_char s [ 4 + i *j ] ;
}
}

f packet - >l as t = ( ui nt 8_t ) f pack_char s [ 3 + I MAGE_HEI GHT*( I MAGE_WI DTH/2 + 1) ] ;
br eak;
}
}
r et ur n 0;
}

c cm mu uc ca am m2 2. .c c
/ *
* Pl ayer - One Hel l of a Robot Ser ver
* Copyr i ght ( C) 2000
* Br i an Ger key, Kasper St oy, Ri char d Vaughan, & Andr ew Howar d
* Radu Bogdan Rusu
*
*
*
Anexe
pagina X
*
*
*/

/ *
* $I d: cmucam2. cc, v 1. 8 2005/ 04/ 24 19: 07: 14 ger key Exp $
*/

/ ** @addt ogr oup dr i ver s Dr i ver s */
/ ** @{ */
/ ** @def gr oup pl ayer _dr i ver _cmucam2 cmucam2

The cmucam2 dr i ver connect s over a ser i al por t t o a CMUCam2. Pr esent s a
@r ef pl ayer _i nt er f ace_bl obf i nder i nt er f ace and a @r ef pl ayer _i nt er f ace_pt z
i nt er f ace and can t r ack mul t i pl e col or bl obs. Col or t r acki ng par amet er s ar e
def i ned i n Pl ayer ' s conf i g f i l e ( see bel ow f or an exampl e) .

@par Compi l e- t i me dependenci es

- none

@par Pr ovi des

- @r ef pl ayer _i nt er f ace_bl obf i nder : t he bl obs det ect ed by t he CMUCam2
- @r ef pl ayer _i nt er f ace_pt z : cont r ol of t he ser vos t hat pan and t i l t
t he CMUCam2
- @r ef pl ayer _i nt er f ace_camer a : snapshot i mages t aken by t he CMUCam2

@par Requi r es

- none

@par Suppor t ed conf i gur at i on r equest s

- The @r ef pl ayer _i nt er f ace_pt z i nt er f ace suppor t s:
- PLAYER_PTZ_AUTOSERVO

- The @r ef pl ayer _i nt er f ace_bl obf i nder i nt er f ace suppor t s:
- PLAYER_BLOBFI NDER_SET_COLOR_REQ
- PLAYER_BLOBFI NDER_SET_I MAGER_PARAMS_REQ

@par Conf i gur at i on f i l e opt i ons

- devi cepat h ( st r i ng)
- Def aul t : NULL
- Ser i al por t wher e t he CMUCam2 i s connect ed
- num_bl obs ( i nt eger )
- Def aul t : 1
- Number of col or s t o t r ack; you must al so i ncl ude t hi s many col or %d opt i ons
- col or %d ( f l oat t upl e)
- Def aul t : none
- Each col or %d i s a t upl e [ r mi n r max gmi n gmax bmi n bmax] of mi n/ max
val ues f or r ed, gr een, and bl ue, whi ch def i nes a r egi on i n RGB space
t hat t he CMUCam2 wi l l t r ack.
- bl obor camer a ( i nt eger )
- Def aul t : 1
- Set bl obor camer a t o 1 i f you want t he bl obf i nder / pt z act i ve, or set i t
t o 2 i f you want camer a/ pt z act i ve. ( t hi s wi l l be changed i n t he f ut ur e)

@par Exampl e

@ver bat i m
dr i ver
(
name " cmucam2"
pr ovi des [ " bl obf i nder : 0" " pt z: 0" " camer a: 0" ]
devi cepat h " / dev/ t t yS1"
bl obor camer a 1
num_bl obs 2
# val ues must be bet ween 40 and 240 ( ! )
col or 0 [ r ed_mi n r ed_max bl ue_mi n bl ue_max gr een_mi n gr een_max] )
# val ues must be bet ween 40 and 240 ( ! )
col or 1 [ r ed_mi n r ed_max bl ue_mi n bl ue_max gr een_mi n gr een_max] )
)
@endver bat i m

@par Aut hor s

Anexe
pagina XI
Pouya Bast ani , Ri char d Vaughan, Radu Bogdan Rusu
*/
/ ** @} */

#i ncl ude <as s er t . h>
#i ncl ude <s t di o. h>
#i ncl ude <uni s t d. h> / * cl ose( 2) , f cnt l ( 2) , get pi d( 2) , usl eep( 3) , execvp( 3) , f or k( 2) */
#i ncl ude <net db. h> / * f or get host byname( 3) */
#i ncl ude <net i net /i n. h> / * f or st r uct sockaddr _i n, ht ons( 3) */
#i ncl ude <s y s /t y pes . h> / * f or socket ( 2) */
#i ncl ude <s y s /s ocket . h> / * f or socket ( 2) */
#i ncl ude <s i gnal . h> / * f or ki l l ( 2) */
#i ncl ude <f cnt l . h> / * f or f cnt l ( 2) */
#i ncl ude <s t r i ng. h> / * f or st r ncpy( 3) , memcpy( 3) */
#i ncl ude <s t dl i b. h> / * f or at exi t ( 3) , at oi ( 3) */
#i ncl ude <pt hr ead. h> / * f or pt hr ead st uf f */
#i ncl ude <s ocket _ut i l . h>

#i ncl ude <er r or . h>
#i ncl ude <dr i v er . h>
#i ncl ude <dr i v er t abl e. h>
#i ncl ude <pl ay er . h>
#i ncl ude <mat h. h>

#i ncl ude "camer a. h"

#def i ne MAX_CHANNELS 32

cl as s Cmucam2: publ i c Dr i v er
{
pr i v at e:

/ / Descr i pt i ve col or s f or each channel .
ui nt 32_t col or s [ MAX_CHANNELS] ;
v oi d get _bl ob ( packet _t cam_packet ,
pl ay er _bl obf i nder _bl ob_t *bl ob,
col or _conf i g r ange) ;
v oi d get _i mage ( packet _f cam_packet ,
pl ay er _camer a_dat a_t *i mage) ;
i nt f d;
i nt num_of _bl obs ;

cons t char * dev i cepat h;
col or _conf i g col or [ PLAYER_BLOBFI NDER_MAX_BLOBS] ;
/ / Camer a cam;

/ / Bl obf i nder i nt er f ace ( pr ovi des)
pl ay er _dev i ce_i d_t bl obf i nder _i d;
pl ay er _bl obf i nder _dat a_t bl obf i nder _dat a;

/ / PTZ i nt er f ace ( pr ovi des)
pl ay er _dev i ce_i d_t pt z _i d;
pl ay er _pt z _dat a_t pt z _dat a;

/ / Camer a i nt er f ace ( pr ovi des)
pl ay er _dev i ce_i d_t cam_i d;
pl ay er _camer a_dat a_t cam_dat a;

v oi d CheckConf i gPt z ( ) ;
v oi d CheckConf i gBl obf i nder ( ) ;

v oi d Ref r es hDat aBl obf i nder ( ) ;
v oi d Ref r es hDat aPt z ( ) ;
v oi d Ref r es hDat aCamer a ( ) ;

i nt Bl obORCamer a;

publ i c:

/ / const r uct or
/ /
Cmucam2 ( Conf i gFi l e* cf , i nt s ect i on) ;

v i r t ual v oi d Mai n ( ) ;

i nt Set up ( ) ;
i nt Shut down ( ) ;
};

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / a f act or y cr eat i on f unct i on
Dr i v er * Cmucam2_I ni t ( Conf i gFi l e* cf , i nt s ect i on)
{
r et ur n ( ( Dr i v er *) ( new Cmucam2 ( cf , s ect i on) ) ) ;
Anexe
pagina XII
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / a dr i ver r egi st r at i on f unct i on
v oi d Cmucam2_Regi s t er ( Dr i v er Tabl e* t abl e)
{
t abl e- >AddDr i v er ( "cmucam2", Cmucam2_I ni t ) ;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / Const r uct or
Cmucam2: : Cmucam2 ( Conf i gFi l e* cf , i nt s ect i on)
: Dr i v er ( cf , s ect i on)
{

Bl obORCamer a = cf - >ReadI nt ( s ect i on, "bl obor camer a", 1) ;
i f ( ( Bl obORCamer a ! = 1) | | ( Bl obORCamer a ! = 2) )
Bl obORCamer a = 1;

/ / zer o I Ds
mems et ( &t hi s - >bl obf i nder _i d, 0, s i z eof ( t hi s - >bl obf i nder _i d) ) ;
mems et ( &t hi s - >pt z _i d, 0, s i z eof ( t hi s - >pt z _i d ) ) ;
mems et ( &t hi s - >cam_i d, 0, s i z eof ( t hi s - >cam_i d ) ) ;

/ / Out goi ng pt z i nt er f ace
i f ( cf - >ReadDev i ceI d ( &( t hi s - >pt z _i d) , s ect i on, "pr ov i des ",
PLAYER_PTZ_CODE, - 1, NULL) == 0)
{
i f ( t hi s - >AddI nt er f ace ( t hi s - >pt z _i d, PLAYER_ALL_MODE,
s i z eof ( pl ay er _pt z _dat a_t ) ,
s i z eof ( pl ay er _pt z _cmd_t ) , 5, 5) ! = 0)
{
t hi s - >Set Er r or ( - 1) ;
r et ur n;
}
}

s wi t ch ( Bl obORCamer a)
{
cas e 1:
{
/ / Out goi ng bl obf i nder i nt er f ace
i f ( cf - >ReadDev i ceI d ( &( t hi s - >bl obf i nder _i d) , s ect i on, "pr ov i des ",
PLAYER_BLOBFI NDER_CODE, - 1, NULL) == 0)
{
i f ( t hi s - >AddI nt er f ace ( t hi s - >bl obf i nder _i d, PLAYER_ALL_MODE,
s i z eof ( pl ay er _bl obf i nder _dat a_t ) , 0, 5, 5) ! = 0)
{
r et ur n;
}
}

num_of _bl obs = cf - >ReadI nt ( s ect i on, "num_bl obs ", 1) ;
char v ar i abl e[ 20] ;

f or ( i nt i = 0; i < num_of _bl obs ; i ++)
{
s pr i nt f ( v ar i abl e, "col or %d", i ) ;
col or [ i ] . r mi n = ( i nt ) cf - >ReadTupl eFl oat ( s ect i on, v ar i abl e, 0, 16) ;
col or [ i ] . r max = ( i nt ) cf - >ReadTupl eFl oat ( s ect i on, v ar i abl e, 1, 16) ;
col or [ i ] . gmi n = ( i nt ) cf - >ReadTupl eFl oat ( s ect i on, v ar i abl e, 2, 16) ;
col or [ i ] . gmax = ( i nt ) cf - >ReadTupl eFl oat ( s ect i on, v ar i abl e, 3, 16) ;
col or [ i ] . bmi n = ( i nt ) cf - >ReadTupl eFl oat ( s ect i on, v ar i abl e, 4, 16) ;
col or [ i ] . bmax = ( i nt ) cf - >ReadTupl eFl oat ( s ect i on, v ar i abl e, 5, 16) ;
}

br eak;
}

cas e 2:
{
/ / Out goi ng camer a i nt er f ace
i f ( cf - >ReadDev i ceI d ( &( t hi s - >cam_i d) , s ect i on, "pr ov i des ",
PLAYER_CAMERA_CODE, - 1, NULL) == 0)
{
i f ( t hi s - >AddI nt er f ace ( t hi s - >cam_i d, PLAYER_ALL_MODE,
s i z eof ( pl ay er _camer a_dat a_t ) , 0, 5, 5) ! = 0)
{
r et ur n;
}
}
br eak;
}
Anexe
pagina XIII
def aul t :
{
PLAYER_WARN ( "Cmucam2 i nv al i d bl obor camer a mode. ") ;
br eak;
}
}

i f ( ! ( t hi s - >dev i cepat h =
( char *) cf - >ReadSt r i ng ( s ect i on, "dev i cepat h", NULL) ) )
{
PLAYER_ERROR ( "mus t s peci f y dev i cepat h") ;
r et ur n;
}
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
i nt Cmucam2: : Set up ( )
{
f f l us h ( s t dout ) ;

f d = open_por t ( ( char *) dev i cepat h) ; / / openi ng t he ser i al por t
i f ( f d < 0) / / i f not successf ul , st op
{
PLAYER_ERROR ( "Camer a connect i on f ai l ed! ") ;
r et ur n - 1;
}
aut o_s er v oi ng ( f d, 0) ;

/ * now spawn r eadi ng t hr ead */
St ar t Thr ead ( ) ;

r et ur n ( 0) ;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
i nt Cmucam2: : Shut down ( )
{
St opThr ead ( ) ;
s t op_t r acki ng ( f d) ;
cl os e_por t ( f d) ; / / cl ose t he ser i al por t

r et ur n ( 0) ;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
v oi d Cmucam2: : Mai n ( )
{
mems et ( &bl obf i nder _dat a, 0, s i z eof ( bl obf i nder _dat a) ) ;
mems et ( &pt z _dat a, 0, s i z eof ( pt z _dat a ) ) ;
mems et ( &cam_dat a, 0, s i z eof ( cam_dat a ) ) ;

pl ay er _pt z _cmd_t command;

i nt pan_pos i t i on = 0;
i nt t i l t _pos i t i on = 0;

f or ( ; ; )
{

/ / handl e commands - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
pt hr ead_t es t cancel ( ) ;
Get Command ( ( uns i gned char *) &command, s i z eof ( pl ay er _pt z _cmd_t ) , NULL) ;

i f ( pan_pos i t i on ! = ( s hor t ) nt ohs ( ( uns i gned s hor t ) ( command. pan) ) )
{
pan_pos i t i on = ( s hor t ) nt ohs ( ( uns i gned s hor t ) ( command. pan) ) ;
i f ( abs ( pan_pos i t i on) <= 90 )
/ / Pan val ue must be negat ed.
s et _s er v o_pos i t i on ( f d, 0, - 1 * pan_pos i t i on) ;
}

i f ( t i l t _pos i t i on ! = ( s hor t ) nt ohs ( ( uns i gned s hor t ) ( command. t i l t ) ) )
{
t i l t _pos i t i on = ( s hor t ) nt ohs ( ( uns i gned s hor t ) ( command. t i l t ) ) ;
i f ( abs ( t i l t _pos i t i on) <= 90 )
/ / Ti l t val ue must be negat ed.
s et _s er v o_pos i t i on ( f d, 1, - 1 * t i l t _pos i t i on) ;
}

/ / handl e conf i gs - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
t hi s - >CheckConf i gPt z ( ) ;
Anexe
pagina XIV
t hi s - >CheckConf i gBl obf i nder ( ) ;

/ / get dat a - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

t hi s - >Ref r es hDat aPt z ( ) ;

s wi t ch ( Bl obORCamer a)
{
cas e 1: {
t hi s - >Ref r es hDat aBl obf i nder ( ) ;
br eak;
}
cas e 2: {
t hi s - >Ref r es hDat aCamer a ( ) ;
br eak;
}
}
}

pt hr ead_ex i t ( NULL) ;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
v oi d Cmucam2: : Ref r es hDat aBl obf i nder ( )
{
packet _t bl ob_i nf o;
pl ay er _bl obf i nder _bl ob_t bl ob;
i nt bl obs _obs er v ed;

mems et ( &t hi s - >bl obf i nder _dat a, 0, s i z eof ( t hi s - >bl obf i nder _dat a) ) ;

bl obf i nder _dat a. wi dt h = ht ons ( ( ui nt 16_t ) I MAGE_WI DTH ) ;
bl obf i nder _dat a. hei ght = ht ons ( ( ui nt 16_t ) I MAGE_HEI GHT) ;
bl obf i nder _dat a. bl ob_count = ht ons ( ( ui nt 16_t ) num_of _bl obs ) ;

bl obs _obs er v ed = 0;

f or ( i nt i = 0; i < num_of _bl obs ; i ++)
{
t r ack_bl ob ( f d, col or [ 0] ) ;
i f ( ! get _t _packet ( f d, &bl ob_i nf o) )


get _bl ob ( bl ob_i nf o, &bl ob, col or [ i ] ) ;
i f ( bl ob. ar ea > 0)
bl obs _obs er v ed++;

bl ob. i d = ht ons ( 0 ) ;
bl ob. col or = ht onl ( bl ob. col or ) ;
bl ob. ar ea = ht onl ( bl ob. ar ea ) ;
bl ob. x = ht ons ( bl ob. x ) ;
bl ob. y = ht ons ( bl ob. y ) ;
bl ob. l ef t = ht ons ( bl ob. l ef t ) ;
bl ob. r i ght = ht ons ( bl ob. r i ght ) ;
bl ob. t op = ht ons ( bl ob. t op ) ;
bl ob. bot t om = ht ons ( bl ob. bot t om) ;
bl ob. r ange = ht ons ( bl ob. r ange ) ;

memcpy ( &bl obf i nder _dat a. bl obs [ i ] , &bl ob, s i z eof ( bl ob) ) ;
}

bl obf i nder _dat a. bl ob_count = ht ons ( bl obs _obs er v ed) ;

Put Dat a ( bl obf i nder _i d, &bl obf i nder _dat a,
s i z eof ( bl obf i nder _dat a) - s i z eof ( bl obf i nder _dat a. bl obs ) +
nt ohs ( bl obf i nder _dat a. bl ob_count ) *
s i z eof ( bl obf i nder _dat a. bl obs [ 0] ) , NULL) ;
r et ur n;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
v oi d Cmucam2: : Ref r es hDat aPt z ( )
{
mems et ( &t hi s - >pt z _dat a, 0, s i z eof ( t hi s - >pt z _dat a) ) ;

pt z _dat a. z oom = 45; // cmucam does not hav e t hes e
pt z _dat a. pans peed = 0;
pt z _dat a. t i l t s peed = 0;
pt z _dat a. z oom = ht ons ( pt z _dat a. z oom ) ;
pt z _dat a. pans peed = ht ons ( pt z _dat a. pans peed ) ;
pt z _dat a. t i l t s peed = ht ons ( pt z _dat a. t i l t s peed) ;

pt z _dat a. pan = - 1*get _s er v o_pos i t i on ( f d, 0) ;
Anexe
pagina XV
pt z _dat a. t i l t = - 1*get _s er v o_pos i t i on ( f d, 1) ;
pt z _dat a. pan = ht ons ( pt z _dat a. pan ) ;
pt z _dat a. t i l t = ht ons ( pt z _dat a. t i l t ) ;

Put Dat a ( pt z _i d, &pt z _dat a, s i z eof ( pt z _dat a) , NULL) ;
r et ur n;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
v oi d Cmucam2: : Ref r es hDat aCamer a ( )
{
packet _f camer a_packet ;

mems et ( &t hi s - >cam_dat a, 0, s i z eof ( t hi s - >cam_dat a) ) ;
mems et ( &camer a_packet , 0, s i z eof ( camer a_packet ) ) ;

i f ( r ead_i mage ( f d, - 1, &camer a_packet ) ! = 0)

get _i mage ( camer a_packet , &cam_dat a) ;

Put Dat a ( cam_i d, &cam_dat a, s i z eof ( cam_dat a) , NULL) ;
r et ur n;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
v oi d Cmucam2: : CheckConf i gPt z ( )
{
v oi d* cl i ent ;
s i z e_t conf i g_s i z e = 0;
uns i gned char conf i g[ PLAYER_MAX_REQREP_SI ZE] ;

i f ( ( conf i g_s i z e = Get Conf i g ( pt z _i d, &cl i ent , ( v oi d*) conf i g,
s i z eof ( conf i g) , NULL) ) > 0)
{
s wi t ch ( conf i g[ 0] )
{
cas e PLAYER_PTZ_AUTOSERVO:
{
/ / check t hat t he conf i g i s t he r i ght l engt h
i f ( conf i g_s i z e ! = s i z eof ( pl ay er _pt z _cont r ol mode_conf i g) )
{
/ / compl ai n!
PLAYER_WARN ( "Cmucam2_pt z aut os er v o r eques t i s wr ong s i z e! ") ;
i f ( Put Repl y ( cl i ent , PLAYER_MSGTYPE_RESP_NACK, NULL) )
PLAYER_ERROR ( "f ai l ed t o Put Repl y ") ;
br eak;
}

pl ay er _pt z _cont r ol mode_conf i g *s er v o =
( pl ay er _pt z _cont r ol mode_conf i g*) conf i g;

aut o_s er v oi ng ( f d, s er v o- >mode) ;
i f ( s er v o- >mode)
PLAYER_MSG0 ( 1, "Aut o s er v oi ng i s enabl ed. ") ;
el s e
PLAYER_MSG0 ( 1, "Aut o s er v oi ng i s di s abl ed. ") ;

/ / r epl y OK
i f ( Put Repl y ( cl i ent , PLAYER_MSGTYPE_RESP_ACK, NULL) )

br eak;
}

def aul t :
{
PLAYER_WARN ( "Cmucam2_pt z got unknown conf i g r eques t ") ;
br eak;
}
}
}
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
v oi d Cmucam2: : CheckConf i gBl obf i nder ( )
{
v oi d* cl i ent ;
s i z e_t conf i g_s i z e = 0;

i f ( ( conf i g_s i z e = Get Conf i g ( bl obf i nder _i d, &cl i ent , ( v oi d*) conf i g,
s i z eof ( conf i g) , NULL) ) > 0)
Anexe
pagina XVI
{
s wi t ch( conf i g[ 0] )
{
cas e PLAYER_BLOBFI NDER_SET_COLOR_REQ:
{
i f ( conf i g_s i z e ! = s i z eof ( pl ay er _bl obf i nder _col or _conf i g_t ) )
{
/ / compl ai n!
PLAYER_WARN ( "Cmucam2_bl obf i nder s et _col or r eques t i s wr ong s i z e! ") ;
br eak;
}

pl ay er _bl obf i nder _col or _conf i g_t col or _conf i g;
col or _conf i g = *( ( pl ay er _bl obf i nder _col or _conf i g_t *) conf i g) ;

col or [ 0] . r mi n = nt ohs ( col or _conf i g. r mi n) ;
col or [ 0] . r max = nt ohs ( col or _conf i g. r max ) ;
col or [ 0] . gmi n = nt ohs ( col or _conf i g. gmi n) ;
col or [ 0] . gmax = nt ohs ( col or _conf i g. gmax ) ;
col or [ 0] . bmi n = nt ohs ( col or _conf i g. bmi n) ;
col or [ 0] . bmax = nt ohs ( col or _conf i g. bmax ) ;

/ / r epl y OK

PLAYER_MSG6 ( 1,
"Cmucam2_bl obf i nder r ecei v ed new t r acki ng col or : [ %d, %d, %d, %d, %d, %d] ",
col or [ 0] . r mi n, col or [ 0] . r max ,
col or [ 0] . gmi n, col or [ 0] . gmax ,
col or [ 0] . bmi n, col or [ 0] . bmax ) ;

br eak;
}

cas e PLAYER_BLOBFI NDER_SET_I MAGER_PARAMS_REQ:
{
i f ( conf i g_s i z e ! = s i z eof ( pl ay er _bl obf i nder _i mager _conf i g_t ) )
{
/ / compl ai n!
PLAYER_WARN
( "Cmucam2_bl obf i nder s et _i mager _par ams r eques t i s wr ong s i z e! ") ;
br eak;
}

pl ay er _bl obf i nder _i mager _conf i g_t pl ay er _i c;
pl ay er _i c = *( ( pl ay er _bl obf i nder _i mager _conf i g_t *) conf i g) ;


i mager _conf i g i c;

i c. br i ght nes s = nt ohs ( ( i nt 16_t ) pl ay er _i c. br i ght nes s ) ;
i c. cont r as t = nt ohs ( ( i nt 16_t ) pl ay er _i c. cont r as t ) ;
i c. aut ogai n = ( i nt 8_t ) pl ay er _i c. aut ogai n;
i c. col or mode = ( i nt 8_t ) pl ay er _i c. col or mode;

i f ( s et _i mager _conf i g ( f d, i c) ! = 1) {
PLAYER_WARN
( "Cmucam2_bl obf i nder s et _i mager _par ams f ai l ed! ") ;
br eak;
}

/ / t r ack_bl ob ( f d, col or [ 0] ) ;

/ / r epl y OK

PLAYER_MSG4 ( 1,
"Cmucam2_bl obf i nder r ecei v ed new i mager par ams : [ %d, %d, %d, %d] ",
i c. br i ght nes s , i c. cont r as t , i c. aut ogai n, i c. col or mode) ;

br eak;
}

def aul t :
Anexe
pagina XVII
{
PLAYER_WARN ( "Cmucam2_bl obf i nder got unknown conf i g r eques t ") ;
br eak;
}
}
}
}

/ **************************************************************************
*** GET BLOB ***
**************************************************************************/
/ * Descr i pt i on: Thi s f unct i on uses CMUcam' s T packet f or t r acki ng t o get
t he bl ob i nf or mat i on as descr i bed by Pl ayer
Par amet er s: cam_packet : camer a' s T packet gener at ed dur i ng t r acki ng
col or : t he col or r ange used i n t r acki ng
Ret ur ns: The Pl ayer f or mat f or bl ob i nf or mat i on
*/

v oi d Cmucam2: : get _bl ob ( packet _t cam_packet ,
pl ay er _bl obf i nder _bl ob_t *bl ob,
col or _conf i g r ange)
{
/ / a descr i pt i ve col or f or t he bl ob
uns i gned char r ed = ( r ange. r mi n + r ange. r max ) /2;
uns i gned char gr een = ( r ange. gmi n + r ange. gmax ) /2;
uns i gned char bl ue = ( r ange. bmi n + r ange. bmax ) /2;

( *bl ob) . col or = r ed << 16 + gr een << 8 + bl ue;
/ / t he number of pi xel s i n t he bl ob
( *bl ob) . ar ea = cam_packet . bl ob_ar ea;
/ / set t i ng t he boundi ng box f or t he bl ob
( *bl ob) . x = 2*cam_packet . mi ddl e_x ;
( *bl ob) . y = cam_packet . mi ddl e_y ;
( *bl ob) . l ef t = 2*cam_packet . l ef t _x ;
/ / hi ghest and l owest y- val ue f or t op and bot t om
( *bl ob) . r i ght = 2*cam_packet . r i ght _x ;
( *bl ob) . t op = ( cam_packet . l ef t _y > cam_packet . r i ght _y ) ?
cam_packet . l ef t _y : cam_packet . r i ght _y ;
( *bl ob) . bot t om = ( cam_packet . l ef t _y <= cam_packet . r i ght _y ) ?
cam_packet . l ef t _y : cam_packet . r i ght _y ;
}

/ **************************************************************************
*** GET I MAGE ***
**************************************************************************/
/ * Descr i pt i on: Thi s f unct i on uses CMUcam' s F packet t o get an i mage
Par amet er s: packet : camer a' s F packet i n r aw f or mat
Ret ur ns: The Pl ayer f or mat f or i mage dat a
*/

v oi d Cmucam2: : get _i mage ( packet _f cam_packet ,
pl ay er _camer a_dat a_t *cam_dat a)
{
i nt x = 0;
i nt y = 0;

( *cam_dat a) . wi dt h = ht ons ( ( ui nt 16_t ) cam_packet . x s i z e) ;
( *cam_dat a) . hei ght = ht ons ( ( ui nt 16_t ) cam_packet . y s i z e) ;
( *cam_dat a) . bpp = ( ui nt 8_t ) 24;
( *cam_dat a) . f or mat = ( ui nt 8_t ) 5; / / PLAYER_CAMERA_FORMAT_RGB888
( *cam_dat a) . f di v = ht ons ( ( ui nt 16_t ) 1) ;
( *cam_dat a) . compr es s i on = ( ui nt 8_t ) 0; // PLAYER_CAMERA_COMPRESS_RAW
( *cam_dat a) . i mage_s i z e = ht onl ( ( ui nt 32_t )
( ( *cam_dat a) . wi dt h * 2 * ( *cam_dat a) . hei ght ) ) ;

f or ( y = 0; y < ( *cam_dat a) . hei ght ; y ++)
{
f or ( x = 0; x < ( *cam_dat a) . wi dt h; x ++)
{
i nt r ed = ( ui nt 8_t ) cam_packet . r ows [ y ] . r gb[ x ] . r << 16;
i nt gr een = ( ui nt 8_t ) cam_packet . r ows [ y ] . r gb[ x ] . g << 8;
i nt bl ue = ( ui nt 8_t ) cam_packet . r ows [ y ] . r gb[ x ] . b;
( *cam_dat a) . i mage[ y * ( *cam_dat a) . wi dt h * 2 + x * 2]
= ( ui nt 8_t ) ( r ed + gr een + bl ue) ;
( *cam_dat a) . i mage[ y * ( *cam_dat a) . wi dt h * 2 + x * 2 + 1]
= ( ui nt 8_t ) ( r ed + gr een + bl ue) ;
}
}
r et ur n;
}
Anexe
pagina I
D D. . C Co od du ul l s su ur rs s a al l d dr ri iv ve er ru ul lu ui i Z Ze ee eR RO O
b br ra ai in ns st te em m. .h h
/ *
* ZeeRO Pl ayer Dr i ver - br ai nst em. h
*
*
*
*
* $I d: br ai nst em. h, v 1. 0 2005/ 05/ 22 14: 56: 25 veedee Exp $
*
*/

#i ncl ude <acr oname/aCommon/aUt i l . h>
#i ncl ude <acr oname/aCommon/aI O. h>
#i ncl ude <acr oname/aCommon/aSt em. h>

ex t er n "C"
{
#endi f

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ * - - - [ Const ant def i ni t i ons ] - - - */
#def i ne DEFAULTPORTNAME "t t y USB0"
#def i ne DEFAULTBAUDRATE 38400
#def i ne DEFAULTSLOT 0
#def i ne MAXSETTI NG 32

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ * - - - [ Robot const ant s ] - - - */
/ ** Basi c names f or t he ser vomot or s */
#def i ne aSERVO1 0
#def i ne aSERVO2 1

/ ** Base di amet er i n mm */
#def i ne BASEDI AMETER 220

/ ** Wheel di amet er i n mm */
#def i ne WHEELDI AMETER 72

/ ** Di st ance bet ween t he t wo wheel s */
#def i ne WHEELDI STANCE 162

/ ** Number of degr ees/ second f or t he l ef t wheel */
#def i ne DEGSECLEFT 290

/ ** Number of degr ees/ second f or t he r i ght wheel */
#def i ne DEGSECRI GHT 365

/ ** Number of avai l abl e SONAR ( ul t r asoni c) sensor s */
#def i ne SONAR_SAMPLES 4

/ ** Number of avai l abl e I R ( i nf r ar ed) sensor s */
#def i ne I R_SAMPLES 2

/ ** Number of avai l abl e Pyr oel ect r i c sensor s */
#def i ne PYRO_SAMPLES 1

/ ** I 2C addr ess f or t he Acr oname Br ai nSt emGP 1. 0 */
#def i ne GPMODULE 2

/ ** I 2C addr ess f or t he Acr oname Br ai nSt emMot o 1. 0 */
#def i ne MOTOMODULE 4

Anexe
pagina II
/ ** Sl ot number wher e t he TEA pr ogr amf or t he f i r st I R sensor r esi des */
#def i ne I RSLOT1 1

/ ** Sl ot number wher e t he TEA pr ogr amf or t he second I R sensor r esi des */
#def i ne I RSLOT2 3

/ ** Sl ot number wher e t he TEA pr ogr amf or t he SONAR sensor s r esi des */
#def i ne SONARSLOT 8

/ ** I ndex of t he anal og i nput wher e t he pyr oel ect r i c sensor i s i nst al l ed */
#def i ne PYROPORT 4

/ ** Const ant f or t he " I I C Baud Rat e" syst empar amet er ( f r omaModul e. t ea) */
#def i ne aMODULE_VAL_I I CBAUD 3

/ ** Const ant f or t he " RS232 Baud Rat e" syst empar amet er ( f r omaModul e. t ea) */
#def i ne aMODULE_VAL_SERBAUD 4

/ ** Const ant f or t he " Hear t beat Mode" syst empar amet er ( f r omaModul e. t ea) */
#def i ne aMODULE_VAL_HBFLAG 5

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ * - - - [ St r uct ur e t ypes ] - - - */
t y pedef s t r uct z eer o_s t r uct
{
aI OLi b i oRef ;
aSt emLi b s t emRef ;
aSet t i ngFi l eRef s et t i ngFi l e;
uns i gned char modul e;
uns i gned char s l ot ;
i nt baudRat e;
char por t Name[ MAXSETTI NG] ;
char hbModeSav e;
aSt r eamRef l i nkSt r eam;
aSt r eamRef dat aSt r eam;
aSt r eamRef out put St r eam;
} z eer o_s t r uct ;

t y pedef s t r uct s onar _pos e
{
doubl e x ;
doubl e y ;
doubl e t h;
} s onar _pos e;

t y pedef s t r uct ZeeRO_s onar _pos e {
s onar _pos e s onar s [ SONAR_SAMPLES] ;
} ZeeRO_s onar _pos e;

t y pedef s t r uct i r _pos e
{
doubl e x ;
doubl e y ;
doubl e t h;
} i r _pos e;

t y pedef s t r uct ZeeRO_i r _pos e {
i r _pos e i r s [ I R_SAMPLES] ;
} ZeeRO_i r _pos e;

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ * - - - [ Funct i on pr ot ot ypes ] - - - */
aEr r i ni t Br ai ns t em ( z eer o_s t r uct * z ) ;
aEr r connect Br ai ns t em ( z eer o_s t r uct * z , cons t char * por t Name, i nt por t Speed) ;
aEr r cl os eBr ai ns t em ( z eer o_s t r uct * z ) ;

aEr r i ni t Ser v os ( z eer o_s t r uct z ) ;

aEr r changeI 2CBaudRat e ( z eer o_s t r uct z , char newI 2CBaudRat e) ;
aEr r changeRS232BaudRat e ( z eer o_s t r uct z , char newRS232BaudRat e) ;
aEr r changeBaudRat e ( z eer o_s t r uct z ) ;

aEr r get I RVal ues ( z eer o_s t r uct z , doubl e I RVol t age[ ] , doubl e I RRange[ ] ) ;

aEr r s et Sonar Power ( z eer o_s t r uct z ) ;
aEr r get Sonar Val ues ( z eer o_s t r uct z , s hor t Sonar Range[ ] ) ;

aEr r get Py r oVal ue ( z eer o_s t r uct z , i nt Py r oVal ue[ ] ) ;

aEr r s t opSer v os ( z eer o_s t r uct z ) ;
aEr r s et Lef t Wheel Speed ( z eer o_s t r uct z , i nt s peed) ;
aEr r s et Ri ght Wheel Speed ( z eer o_s t r uct z , i nt s peed) ;
aEr r s et XSpeed ( z eer o_s t r uct z , i nt s peed) ;
aEr r s et YawSpeed ( z eer o_s t r uct z , i nt y aws peed) ;
Anexe
pagina III
doubl e s et Yaw ( z eer o_s t r uct z , i nt s peed, char di r ect i on) ;

}
#endi f

b br ra ai in ns st te em m. .c c
/ *
* ZeeRO Pl ayer Dr i ver - br ai nst em. c
*
*
*
*
* $I d: br ai nst em. c, v 1. 0 2005/ 05/ 21 23: 36: 25 veedee Exp $
*
*/

#i ncl ude "br ai ns t em. h"
#i ncl ude <acr oname/aCommon/aUt i l . h>
#i ncl ude <acr oname/aCommon/aSt em. h>
#i ncl ude <acr oname/aCommon/aSer v o. h>
#i ncl ude <acr oname/aCommon/aModul eVal . h>
#i ncl ude <acr oname/aCommon/aModul eVM. h>
#i ncl ude <acr oname/aCommon/aSRF08. h>
#i ncl ude <acr oname/aCommon/aAnal og. h>
#i ncl ude <acr oname/aCommon/aGP2D02. h>

aSt emLi b s t emLi b;
aI OLi b i oLi b;

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ **
* i ni t Br ai nst em- get t he appr opr i at e Acr oname l i br ar y r ef er ences
*/
aEr r i ni t Br ai ns t em ( z eer o_s t r uct * z )
{
aEr r er r = aEr r None;

aI O_Get Li bRef ( &z - >i oRef , &er r ) ;
i f ( er r ! = aEr r None)
r et ur n er r ;

aSt em_Get Li bRef ( &z - >s t emRef , &er r ) ;
r et ur n er r ;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ **
* changeI 2CBaudRat e - change t he I 2C baud r at e of t he modul e ( def aul t = 1Mbps)
* 0=100Kbps, 1=400Kbps, 2=1Mbps
*/
aEr r changeI 2CBaudRat e ( z eer o_s t r uct z , char newI 2CBaudRat e)
{
char ol dI 2CBaudRat e;

er r = aModul eVal _Get ( z . s t emRef , z . modul e,
aMODULE_VAL_I I CBAUD, &ol dI 2CBaudRat e) ;
r et ur n er r ;

/ / check i f t he new I 2C r at e equal s t he ol d one
i f ( newI 2CBaudRat e ! = ol dI 2CBaudRat e)
er r = aModul eVal _Set ( z . s t emRef , z . modul e,
aMODULE_VAL_I I CBAUD, ( char ) newI 2CBaudRat e) ;

r et ur n er r ;
}

/ **
Anexe
pagina IV
* changeRS232BaudRat e - change t he RS232 baud r at e of t he ser i al
* modul e i nt er f ace ( def aul t = 9600bps)
* 0=2400, 1=4800, *2=9600, 3=19200, 4=38400, 5=57600, 6=115200
*/
aEr r changeRS232BaudRat e ( z eer o_s t r uct z , char newRS232BaudRat e)
{
char ol dRS232BaudRat e;

er r = aModul eVal _Get ( z . s t emRef , z . modul e,
aMODULE_VAL_SERBAUD, &ol dRS232BaudRat e) ;
r et ur n er r ;

/ / check i f t he new RS232 r at e equal s t he ol d one
i f ( newRS232BaudRat e ! = ol dRS232BaudRat e)
er r = aModul eVal _Set ( z . s t emRef , z . modul e,
aMODULE_VAL_SERBAUD, ( char ) newRS232BaudRat e) ;

r et ur n er r ;
}

/ **
* changeBaudRat e - change t he RS232/ I 2C baud r at e t o 115200bps/ 400Kbps
*/
aEr r changeBaudRat e ( z eer o_s t r uct z )
{

/ / change t he I 2C baud r at e t o 400Kbps
er r = changeI 2CBaudRat e ( z , ( char ) 1) ;
r et ur n er r ;

/ / change t he RS232 baud r at e t o 115200bps
/ / er r = changeRS232BaudRat e ( z, ( char ) 6) ;

r et ur n er r ;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ **
* set Sonar Power - set t he maxi mumr ange and anal og gai n
*/
aEr r s et Sonar Power ( z eer o_s t r uct z )
{
i nt pRet ur n;

/ / set t he maxi mumr ange and anal og gai n of t he sonar s ( maxi mumdi st ance = 155cm)
er r = aModul eVM_Ex ecut e ( z . s t emRef , z . modul e, SONARSLOT,
0, NULL, &pRet ur n) ;
r et ur n er r ;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ **
* i ni t Ser vos - i ni t i al i ze cer t ai n ser vomot or par amet er s
*/
aEr r i ni t Ser v os ( z eer o_s t r uct z )
{
char cBy t es [ 4] ;

/ / enabl e ser vo1 and set i t as i nver t ed ( 128 + 64)
er r = aSer v o_Set Conf i g ( z . s t emRef , z . modul e, aSERVO1, ( uns i gned char ) 192) ;
r et ur n er r ;

/ / enabl e ser vo2
er r = aSer v o_Set Conf i g ( z . s t emRef , z . modul e, aSERVO2, ( uns i gned char ) 128) ;
r et ur n er r ;

cBy t es [ 0] = 20; // of f s et
cBy t es [ 1] = 30; // r ange

/ / set t he of f set and r ange f or t he f i r st ser vomot or
er r = aSer v o_Set Li mi t s ( z . s t emRef , z . modul e, aSERVO1, ( uns i gned char *) cBy t es ) ;
r et ur n er r ;

/ / set t he of f set and r ange f or t he second ser vomot or
er r = aSer v o_Set Li mi t s ( z . s t emRef , z . modul e, aSERVO2, ( uns i gned char *) cBy t es ) ;

Anexe
pagina V
r et ur n er r ;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ **
* connect Br ai nst em- set up a ser i al l i nk t o t he Acr oname Br ai nst emboar d
*/
aEr r connect Br ai ns t em ( z eer o_s t r uct * z , cons t char * por t Name, i nt por t Speed)
{

z - >modul e = ( uns i gned char ) GPMODULE;
z - >s l ot = ( uns i gned char ) DEFAULTSLOT;
z - >baudRat e = ( i nt ) por t Speed;
s t r cpy ( z - >por t Name, por t Name) ;

/ / cr eat e t he st r eamt o t he ser i al por t
aSt r eam_Cr eat eSer i al ( z - >i oRef , z - >por t Name, ( uns i gned i nt ) z - >baudRat e,
&z - >l i nkSt r eam, &er r ) ;
i f ( er r ! = aEr r None) {
r et ur n er r ;
}

/ / set t hi s as t he l i nk por t f or t he aSt eml i br ar y
aSt em_Set St r eam ( z - >s t emRef , z - >l i nkSt r eam, kSt emModul eSt r eam, &er r ) ;

/ / make sur e t he modul e i s i n pl ace
i f ( ( er r == aEr r None) && ! aModul eUt i l _Ens ur eModul e ( z - >s t emRef , z - >modul e) )
r et ur n er r ;

/ / st or e t he ol d hear t beat mode and set aut o hear t beat
i f ( er r == aEr r None)
{
er r = aModul eVal _Get ( z - >s t emRef , z - >modul e, aMODULE_VAL_HBFLAG, &z - >hbModeSav e) ;
i f ( ( er r == aEr r None) && ( z - >hbModeSav e ! = 1) )
{
er r = aModul eVal _Set ( z - >s t emRef , z - >modul e, aMODULE_VAL_HBFLAG, 1) ;
}
}

r et ur n er r ;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ **
* cl oseBr ai nst em- cl ose down and r el ease al l Acr oname l i br ar y r esour ces
*/
aEr r cl os eBr ai ns t em ( z eer o_s t r uct * z )
{

/ / r est or e t he pr evi ous hear t beat mode
i f ( z - >l i nkSt r eam)
aModul eVal _Set ( z - >s t emRef , z - >modul e, aMODULE_VAL_HBFLAG, z - >hbModeSav e) ;

i f ( z - >dat aSt r eam)
aSt r eam_Des t r oy ( z - >i oRef , z - >dat aSt r eam, &er r ) ;
r et ur n er r ;

i f ( z - >s t emRef )
aSt em_Rel eas eLi bRef ( z - >s t emRef , &er r ) ;
r et ur n er r ;

i f ( z - >i oRef )
aI O_Rel eas eLi bRef ( z - >i oRef , &er r ) ;

r et ur n er r ;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ **
* get I RVal ues - r ead and r et ur n t he val ues ( vol t ages & mm) f or t he t wo I R sensor s
*/
aEr r get I RVal ues ( z eer o_s t r uct z , doubl e I RVol t age[ ] , doubl e I RRange[ ] )
{
i nt pRet ur n;

/ / Launch t he I R TEA pr ocess f or t he f i r st I R sensor ( modul e 2, sl ot 1)
er r = aModul eVM_Ex ecut e ( z . s t emRef , z . modul e, I RSLOT1,
r et ur n er r ;
Anexe
pagina VI

I RVol t age[ 0] = pRet ur n;
I RRange [ 0] = 2. 54 * aGP2D02_RawToI nches ( ( char ) pRet ur n) ;

/ / Launch t he I R TEA pr ocess f or t he second I R sensor ( modul e 2, sl ot 3)
er r = aModul eVM_Ex ecut e ( z . s t emRef , z . modul e, I RSLOT2,
r et ur n er r ;

I RVol t age[ 1] = pRet ur n;
I RRange [ 1] = 2. 54 * aGP2D02_RawToI nches ( ( char ) pRet ur n) ;

r et ur n 0;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ **
* get Sonar Val ues - r ead and r et ur n t he val ues i n cmf or t he f our ul t r asoni c sensor s
*/
aEr r get Sonar Val ues ( z eer o_s t r uct z , s hor t Sonar Range[ ] )
{
s hor t pVal s ;

/ / r ead t he f i r st ul t r asoni c sensor ( E4h)
er r = aSRF08_Get Range ( z . s t emRef , z . modul e, ( char ) 0x E4, ( char ) aSRF08_CM, &pVal s , 1) ;
r et ur n er r ;

Sonar Range[ 0] = pVal s ;

/ / r ead t he second ul t r asoni c sensor ( E6h)
r et ur n er r ;


/ / r ead t he t hi r d ul t r asoni c sensor ( E0h)
r et ur n er r ;


/ / r ead t he f our t h ul t r asoni c sensor ( E2h)
r et ur n er r ;


r et ur n er r ;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ **
* get Pyr oVal ue - r ead and r et ur n t he anal ogue val ue of t he pyr oel ect r i c sensor
*/
aEr r get Py r oVal ue ( z eer o_s t r uct z , i nt Py r oVal ue[ ] )
{
i nt pAnal ogVal ue;

er r = aAnal og_ReadI nt ( z . s t emRef , ( char ) MOTOMODULE, ( char ) PYROPORT, &pAnal ogVal ue) ;

Py r oVal ue[ 0] = pAnal ogVal ue;

r et ur n er r ;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ **
* st opSer vos - st op t he ser vomot or s f r ommovi ng
*/
aEr r s t opSer v os ( z eer o_s t r uct z )
{

er r = aSer v o_Set Pos i t i onAbs ( z . s t emRef , z . modul e, aSERVO1, 128) ;
r et ur n er r ;

Anexe
pagina VII
r et ur n er r ;

er r = aSer v o_Set Conf i g ( z . s t emRef , z . modul e, aSERVO1, 0) ;
r et ur n er r ;

er r = aSer v o_Set Conf i g ( z . s t emRef , z . modul e, aSERVO2, 0) ;

r et ur n er r ;
}

/ **
* set Ri ght Wheel Speed - moves t he r i ght wheel wi t h a gi ven vel oci t y
*/
aEr r s et Ri ght Wheel Speed ( z eer o_s t r uct z , i nt s peed)
{

i f ( s peed > 0)
{
i nt com = 100 - ( s peed - 5) * 10;

/ / set t he speed f or t he f i r st ser vo mot or
er r = aSer v o_Set Pos i t i onAbs ( z . s t emRef , z . modul e, aSERVO1,
( uns i gned char ) 255- com) ;
}
el s e
{
s peed = - s peed;

i nt com = 100 - ( s peed - 4) * 10;

/ / set t he speed f or t he f i r st ser vo mot or
( uns i gned char ) com) ;
}

r et ur n er r ;
}

/ **
* set Lef t Wheel Speed - moves t he l ef t wheel wi t h a gi ven vel oci t y
*/
aEr r s et Lef t Wheel Speed ( z eer o_s t r uct z , i nt s peed)
{

i f ( s peed > 0)
{
i nt com = 100 - ( s peed - 5) * 10;
i nt of f s et = - 2 + ( s peed - 5) * 2. 1;

/ / set t he speed f or t he second ser vo mot or
( uns i gned char ) 255- of f s et - com) ;
}
el s e
{
s peed = - s peed;

i nt com = 100 - ( s peed - 4) * 10;
i nt of f s et = - 5 + ( s peed - 4) * 1. 7;

/ / set t he speed f or t he second ser vo mot or
( uns i gned char ) com- of f s et ) ;
}

r et ur n er r ;
}

/ **
* set XSpeed - moves t he r obot f or war d or backwar d wi t h a gi ven vel oci t y
* ( bounded)
* mi ni mumf or war d speed = 5cm/ s, maxi mumf or war d speed = 15cm/ s
* mi ni mumbackwar d speed = 4cm/ s, maxi mumbackwar d speed = 10cm/ s
*/
aEr r s et XSpeed ( z eer o_s t r uct z , i nt s peed)
{

i ni t Ser v os ( z ) ;

i f ( s peed == 0)
Anexe
pagina VIII
{
er r = s t opSer v os ( z ) ;
r et ur n er r ;
}

i f ( s peed > 0)
{
/ / bound t he vel oci t i es t o mat ch t he mi n/ max accept abl e speed of t he r obot
i f ( s peed < 5)
s peed = 5;
i f ( s peed > 15)
s peed = 15;

er r = s et Ri ght Wheel Speed ( z , s peed) ;
er r = s et Lef t Wheel Speed ( z , s peed) ;
}
el s e
{
/ / bound t he vel oci t i es t o mat ch t he mi n/ max accept abl e speed of t he r obot
i f ( s peed < - 10)
s peed = - 10;
i f ( s peed > - 4)
s peed = - 4;

er r = s et Ri ght Wheel Speed ( z , s peed) ;
er r = s et Lef t Wheel Speed ( z , s peed) ;
}

r et ur n er r ;
}

/ **
* set YawSpeed - moves t he r obot wi t h a gi ven angul ar vel oci t y
*/
aEr r s et YawSpeed ( z eer o_s t r uct z , i nt y aws peed)
{


i f ( y aws peed == 0)
{
er r = s t opSer v os ( z ) ;
r et ur n er r ;
}

er r = s et Ri ght Wheel Speed ( z , y aws peed) ;
er r = s et Lef t Wheel Speed ( z , - y aws peed) ;

r et ur n er r ;
}

/ **
* set YawHead - t ur ns t he r obot on t he spot wi t h t he gi ven angl e
*/
doubl e s et Yaw ( z eer o_s t r uct z , i nt angl e, char di r ect i on)
{


doubl e s l Sec = 0;
i nt wheel di s = WHEELDI STANCE;
i nt degl ef t = DEGSECLEFT;
i nt degr i ght = DEGSECRI GHT;
i nt wheel di a = WHEELDI AMETER;

/ / Lef t ?
i f ( di r ect i on == 0)
{
s l Sec = ( 2 * wheel di s * angl e * 1000) / ( degl ef t * wheel di a) ;

/ / set t he t ur ni ng speed f or t he f i r st ser vo mot or
er r = aSer v o_Set Pos i t i onAbs ( z . s t emRef , z . modul e, aSERVO1, 255- 60) ;

/ / set t he t ur ni ng speed f or t he second ser vo mot or
}

/ / Ri ght ?
i f ( di r ect i on == 1)
{
s l Sec = ( 2 * wheel di s * angl e * 1000) / ( degr i ght * wheel di a) ;

/ / set t he t ur ni ng speed f or t he f i r st ser vo mot or
Anexe
pagina IX

/ / set t he t ur ni ng speed f or t he second ser vo mot or
er r = aSer v o_Set Pos i t i onAbs ( z . s t emRef , z . modul e, aSERVO2, 255- 60) ;
}

r et ur n s l Sec;
}

z ze ee er ro o. .h h
/ *
* ZeeRO Pl ayer Dr i ver - zeer o. h
*
*
*
*
* $I d: zeer o. h, v 1. 0 2005/ 05/ 21 20: 30: 25 veedee Exp $
*
*/

#i ncl ude "br ai ns t em. h"

#i ncl ude <pl ay er /dr i v er t abl e. h>
#i ncl ude <pl ay er /dr i v er . h>
#i ncl ude <pl ay er /er r or . h>
#i ncl ude <pl ay er /pl ay er t i me. h>
#i ncl ude <pl ay er . h>

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / The cl ass f or t he dr i ver
cl as s ZeeRODr i v er : publ i c Dr i v er
{
publ i c:

/ / Const r uct or ; need t hat
ZeeRODr i v er ( Conf i gFi l e* cf , i nt s ect i on) ;

/ / Must i mpl ement t he f ol l owi ng met hods.
i nt Set up ( ) ;
i nt Shut down ( ) ;

pr i v at e:

/ / ZeeRO dat a st r uct ur e ( acr oname)
z eer o_s t r uct ZeeRO;
cons t char * por t Name;
i nt por t Speed;

/ / Posi t i on i nt er f ace
pl ay er _dev i ce_i d_t pos i t i on_i d;
pl ay er _pos i t i on2d_dat a_t pos i t i on_dat a;
pl ay er _pos i t i on2d_cmd_t pos i t i on_cmd;
pl ay er _pos i t i on2d_geom_t pos i t i on_geom;
pl ay er _pos i t i on2d_power _conf i g pos i t i on_power ;

/ / Sonar i nt er f ace
pl ay er _dev i ce_i d_t s onar _i d;
pl ay er _s onar _dat a_t s onar _dat a;
pl ay er _s onar _geom_t s onar _geom;
pl ay er _s onar _power _conf i g_t s onar _power ;

/ / I R i nt er f ace
pl ay er _dev i ce_i d_t i r _i d;
pl ay er _i r _dat a_t i r _dat a;
pl ay er _i r _pos e_r eq_t i r _geom;
pl ay er _i r _power _r eq_t i r _power ;

/ / AI O ( Pyr o) i nt er f ace
pl ay er _dev i ce_i d_t ai o_i d;
Anexe
pagina X
pl ay er _ai o_dat a_t ai o_dat a;

bool s onar Power ;
bool i r Power ;

/ / Mai n f unct i on f or devi ce t hr ead.
v i r t ual v oi d Mai n ( ) ;
i nt Acr onameI ni t ( ) ;
v oi d CheckConf i g ( ) ;
v oi d CheckConf i gPos i t i on ( ) ;
v oi d CheckConf i gSonar ( ) ;
v oi d CheckConf i gI R ( ) ;
v oi d CheckCommands ( ) ;
v oi d Ref r es hDat a ( ) ;
};

z ze ee er ro o. .c cp pp p
/ *
* ZeeRO Pl ayer Dr i ver - zeer o. cpp
*
*
*
*
* $I d: zeer o. cpp, v 1. 0 2005/ 05/ 21 20: 23: 25 veedee Exp $
*
*/

/ ** @addt ogr oup dr i ver s Dr i ver s */
/ ** @{ */
/ ** @def gr oup pl ayer _dr i ver _zeer o zeer o

Pl ayer dr i ver f or t he ZeeRO r obot . Check ht t p: / / www. r obot ux. i nf o/ zeer o f or mor e
i nf or mat i on about t he r obot .

@par Compi l e- t i me dependenci es

- none

@par Pr ovi des

The zeer o dr i ver pr ovi des t he f ol l owi ng devi ce i nt er f aces, some of
t hemnamed:

- @r ef pl ayer _i nt er f ace_posi t i on2d
- Thi s i nt er f ace accept s vel oci t y commands.

- @r ef pl ayer _i nt er f ace_sonar
- Ret ur ns dat a f r omsonar ar r ays.

- @r ef pl ayer _i nt er f ace_i r
- Ret ur ns dat a f r omi r ar r ays.

- @r ef pl ayer _i nt er f ace_ai o
- Ret ur ns dat a f r omt he pyr oel ect r i c sensor .

@par Suppor t ed conf i gur at i on r equest s

- @r ef pl ayer _i nt er f ace_posi t i on2d:
- PLAYER_POSI TI ON2D_GET_GEOM_REQ
- PLAYER_POSI TI ON2D_MOTOR_POWER_REQ
- @r ef pl ayer _i nt er f ace_sonar :
- PLAYER_SONAR_GET_GEOM_REQ
- PLAYER_SONAR_POWER_REQ
- @r ef pl ayer _i nt er f ace_i r
- PLAYER_I R_POSE_REQ
- PLAYER_I R_POWER_REQ

@par Conf i gur at i on f i l e opt i ons

- por t ( st r i ng)
Anexe
pagina XI
- Def aul t : " t t yUSB0"
- Ser i al por t used t o communi cat e wi t h t he r obot .
- speed ( i nt )
- Def aul t : 38400
- The speed of t he ser i al por t . Must be conf i gur ed on bot h t he Acr oname
and t he Gumst i x.

@par Exampl e

@ver bat i m
dr i ver
(
name " zeer o"
por t " t t yUSB0"
speed 38400
pl ugi n " l i bzeer odr i ver . so"
pr ovi des [ " posi t i on2d: 0" " sonar : 0" " i r : 0" " ai o: 0" ]
)
@endver bat i m

@par Aut hor s

Radu Bogdan Rusu
*/
/ ** @} */

#i ncl ude "z eer o. h"

#i ncl ude <s t r i ng. h>
#i ncl ude <net i net /i n. h>
#i ncl ude <t i me. h>

ZeeRO_s onar _pos e ZeeRO_s onar s =
{
{
{ 0. 065, 0. 065, - 22 },
{ 0. 065, - 0. 065, 21 },
{ - 0. 05 , - 0. 05 , 168 },
{ - 0. 05 , 0. 05 , - 160 }
}
};

ZeeRO_i r _pos e ZeeRO_i r s =
{
{
{ 0. 035, 0. 035, 0 },
{ 0. 035, - 0. 035, 0 }
}
};

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / A f act or y cr eat i on f unct i on, decl ar ed out si de of t he cl ass so t hat i t
/ / can be i nvoked wi t hout any obj ect cont ext ( al t er nat i vel y, you can
/ / decl ar e i t st at i c i n t he cl ass) . I n t hi s f unct i on, we cr eat e and r et ur n
/ / ( as a gener i c Dr i ver *) a poi nt er t o a new i nst ance of t hi s dr i ver .
Dr i v er *
ZeeRODr i v er _I ni t ( Conf i gFi l e* cf , i nt s ect i on)
{
/ / Cr eat e and r et ur n a new i nst ance of t hi s dr i ver
r et ur n ( ( Dr i v er *) ( new ZeeRODr i v er ( cf , s ect i on) ) ) ;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / A dr i ver r egi st r at i on f unct i on, agai n decl ar ed out si de of t he cl ass so
/ / t hat i t can be i nvoked wi t hout obj ect cont ext . I n t hi s f unct i on, we add
/ / t he dr i ver i nt o t he gi ven dr i ver t abl e, i ndi cat i ng whi ch i nt er f ace t he
/ / dr i ver can suppor t and how t o cr eat e a dr i ver i nst ance.
v oi d ZeeRODr i v er _Regi s t er ( Dr i v er Tabl e* t abl e)
{
t abl e- >AddDr i v er ( "z eer o", ZeeRODr i v er _I ni t ) ;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / Const r uct or . Ret r i eve opt i ons f r omt he conf i gur at i on f i l e and do any
/ / pr e- Set up( ) set up.
ZeeRODr i v er : : ZeeRODr i v er ( Conf i gFi l e* cf , i nt s ect i on)
: Dr i v er ( cf , s ect i on)
{
/ / Zer o I Ds
mems et ( &t hi s - >pos i t i on_i d, 0, s i z eof ( pos i t i on_i d) ) ;
mems et ( &t hi s - >s onar _i d, 0, s i z eof ( s onar _i d ) ) ;
mems et ( &t hi s - >i r _i d, 0, s i z eof ( i r _i d ) ) ;
Anexe
pagina XII

/ / Cr eat e a Posi t i on2D i nt er f ace
i f ( cf - >ReadDev i ceI d ( &( t hi s - >pos i t i on_i d) , s ect i on, "pr ov i des ",
PLAYER_POSI TI ON2D_CODE, 0, NULL) == 0)
{
i f ( t hi s - >AddI nt er f ace ( t hi s - >pos i t i on_i d, PLAYER_ALL_MODE,
s i z eof ( pl ay er _pos i t i on2d_dat a_t ) ,
s i z eof ( pl ay er _pos i t i on2d_cmd_t ) ,
1, 1) ! = 0)
{
r et ur n;
}
}

/ / Cr eat e a Sonar i nt er f ace
i f ( cf - >ReadDev i ceI d ( &( t hi s - >s onar _i d) , s ect i on, "pr ov i des ",
PLAYER_SONAR_CODE, 0, NULL) == 0)
{
i f ( t hi s - >AddI nt er f ace ( t hi s - >s onar _i d, PLAYER_READ_MODE,
s i z eof ( pl ay er _s onar _dat a_t ) ,
0, 1, 1) ! = 0)
{
r et ur n;
}
}

/ / Cr eat e an I R ( I nf r ar ed) i nt er f ace
i f ( cf - >ReadDev i ceI d ( &( t hi s - >i r _i d) , s ect i on, "pr ov i des ",
PLAYER_I R_CODE, 0, NULL) == 0)
{
i f ( t hi s - >AddI nt er f ace ( t hi s - >i r _i d, PLAYER_READ_MODE,
s i z eof ( pl ay er _i r _dat a_t ) ,
0, 1, 1) ! = 0)
{
r et ur n;
}
}

/ / Cr eat e an AI O ( f or t he Pyr oel ect r i c sensor ) i nt er f ace
i f ( cf - >ReadDev i ceI d ( &( t hi s - >ai o_i d) , s ect i on, "pr ov i des ",
PLAYER_AI O_CODE, 0, NULL) == 0)
{
i f ( t hi s - >AddI nt er f ace ( t hi s - >ai o_i d, PLAYER_READ_MODE,
s i z eof ( pl ay er _ai o_dat a_t ) ,
0, 1, 1) ! = 0)
{
r et ur n;
}
}
/ / Read opt i ons f r omt he conf i gur at i on f i l e
t hi s - >por t Name = cf - >ReadSt r i ng ( s ect i on, "por t ", DEFAULTPORTNAME) ;
t hi s - >por t Speed = cf - >ReadI nt ( s ect i on, "s peed", DEFAULTBAUDRATE) ;

r et ur n;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / Acr oname i ni t i al i zat i on f unct i on, r et ur n - 1 i f any er r or occur s
i nt ZeeRODr i v er : : Acr onameI ni t ( )
{
i nt br ai ns t emRes ul t = 0;

br ai ns t emRes ul t = i ni t Br ai ns t em ( &ZeeRO) ;

i f ( br ai ns t emRes ul t ! = 0)
{
PLAYER_ERROR ( "> ZeeRO dr i v er s et up. . . [ f ai l ed! ] ") ;
r et ur n ( - 1) ;
}
PLAYER_MSG0 ( 1, "> ZeeRO dr i v er s et up. . . [ done] ") ;

br ai ns t emRes ul t = connect Br ai ns t em ( &ZeeRO, t hi s - >por t Name, t hi s - >por t Speed) ;

{
PLAYER_ERROR ( "> Connect i ng t o Acr oname Br ai ns t em. . . [ f ai l ed! ] ") ;

br ai ns t emRes ul t = cl os eBr ai ns t em ( &ZeeRO) ;

{
Anexe
pagina XIII
PLAYER_ERROR ( "> ZeeRO dr i v er s hut t i ng down. . . [ f ai l ed! ] ") ;
r et ur n ( - 1) ;
}
PLAYER_MSG0 ( 1, "> ZeeRO dr i v er s hut t i ng down. . . [ done] ") ;
r et ur n ( - 1) ;
}
PLAYER_MSG0 ( 1, "> Connect i ng t o Acr oname Br ai ns t em. . . [ done] ") ;

br ai ns t emRes ul t = changeBaudRat e ( ZeeRO) ;

{
PLAYER_ERROR ( "> Set t i ng I 2C s peed t o 400Kbps . . . [ f ai l ed! ] ") ;
r et ur n ( - 1) ;
}
PLAYER_MSG0 ( 1, "> Set t i ng I 2C s peed t o 400Kbps . . . [ done] ") ;

br ai ns t emRes ul t = i ni t Ser v os ( ZeeRO) ;

{
PLAYER_ERROR ( "> I ni t i al i z i ng s er v omot or s . . . [ f ai l ed! ] ") ;
r et ur n ( - 1) ;
}
PLAYER_MSG0 ( 1, "> I ni t i al i z i ng s er v omot or s . . . [ done] ") ;

r et ur n ( 0) ;
}

/ / Set up t he devi ce. Ret ur n 0 i f t hi ngs go wel l , and - 1 ot her wi se.
i nt ZeeRODr i v er : : Set up ( )
{

br ai ns t emRes ul t = Acr onameI ni t ( ) ;

{
PLAYER_ERROR ( "> Tr y connect i ng wi t h y our cl i ent agai n! ") ;
r et ur n ( - 1) ;
}

s t opSer v os ( ZeeRO) ;

br ai ns t emRes ul t = s et Sonar Power ( ZeeRO) ;

{
PLAYER_ERROR ( "> Set t i ng max i mum s onar r ange. . . [ f ai l ed! ] ") ;
r et ur n ( - 1) ;
}
PLAYER_MSG0 ( 1, "> Set t i ng max i mum s onar r ange. . . [ done] ") ;

/ / St ar t t he devi ce t hr ead; spawns a new t hr ead and execut es
/ / ZeeRODr i ver : : Mai n( ) , whi ch cont ai ns t he mai n l oop f or t he dr i ver .
t hi s - >St ar t Thr ead ( ) ;

r et ur n ( 0) ;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / Shut down t he devi ce
i nt ZeeRODr i v er : : Shut down ( )
{

/ / St op and j oi n t he dr i ver t hr ead
t hi s - >St opThr ead ( ) ;

/ / Her e you woul d shut t he devi ce down by, f or exampl e, cl osi ng a
/ / ser i al por t .

br ai ns t emRes ul t = cl os eBr ai ns t em ( &ZeeRO) ;

{
PLAYER_ERROR ( "> ZeeRO dr i v er s hut t i ng down. . . [ f ai l ed! ] ") ;
pr i nt f ( "[ f ai l ed! ] \n") ;
r et ur n ( - 1) ;
}
PLAYER_MSG0 ( 1, "> ZeeRO dr i v er s hut t i ng down. . . [ done] ") ;

r et ur n ( 0) ;
}
Anexe
pagina XIV

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / Mai n f unct i on f or devi ce t hr ead
v oi d ZeeRODr i v er : : Mai n ( )
{
/ / Zer o dat a
mems et ( &t hi s - >pos i t i on_dat a, 0, s i z eof ( t hi s - >pos i t i on_dat a) ) ;
mems et ( &t hi s - >s onar _dat a, 0, s i z eof ( t hi s - >s onar _dat a ) ) ;
mems et ( &t hi s - >i r _dat a, 0, s i z eof ( t hi s - >i r _dat a ) ) ;
mems et ( &t hi s - >ai o_dat a, 0, s i z eof ( t hi s - >ai o_dat a ) ) ;

t hi s - >s onar Power = t r ue;
t hi s - >i r Power = t r ue;

t i mes pec s l eepTi me = {0, 0};

/ / The mai n l oop; i nt er act wi t h t he devi ce her e
f or ( ; ; )
{
/ / t est i f we ar e supposed t o cancel

/ / Check f or and handl e conf i gur at i on r equest s, usi ng Dr i ver : : Get Conf i g ( )
t hi s - >CheckConf i g ( ) ;

/ / Check f or and execut e commands, usi ng Dr i ver : : Get Command( )
t hi s - >CheckCommands ( ) ;

/ / I nt er act wi t h t he devi ce, and push out t he r esul t i ng dat a, usi ng
/ / Dr i ver : : Put Dat a( )
t hi s - >Ref r es hDat a ( ) ;

nanos l eep ( &s l eepTi me, NULL) ;
}
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / CheckConf i g f unct i on
v oi d ZeeRODr i v er : : CheckConf i g ( )
{

t hi s - >CheckConf i gPos i t i on ( ) ;
t hi s - >CheckConf i gSonar ( ) ;
t hi s - >CheckConf i gI R ( ) ;

r et ur n;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
v oi d ZeeRODr i v er : : CheckConf i gPos i t i on ( )
{
v oi d* cl i ent ;
i nt conf i g_s i z e = 0;

whi l e ( ( conf i g_s i z e = t hi s - >Get Conf i g ( t hi s - >pos i t i on_i d, &cl i ent ,
&conf i g, s i z eof ( conf i g) , NULL) > 0) )
{
{
cas e PLAYER_POSI TI ON2D_GET_GEOM_REQ:
{
/ * Ret ur n t he r obot geomet r y. */

i f ( conf i g_s i z e ! = 1)
{
PLAYER_WARN ( "Ar g get r obot geom i s wr ong s i z e; i gnor i ng") ;
t hi s - >Put Repl y ( t hi s - >pos i t i on_i d, cl i ent ,
PLAYER_MSGTYPE_RESP_NACK, NULL, 0, NULL) ;
br eak;
}

pos i t i on_geom. s ubt y pe = PLAYER_POSI TI ON2D_GET_GEOM_REQ;
/ / get r obot pose
pos i t i on_geom. pos e[ 0] = nt ohs ( ( s hor t ) 0) ;
/ / get r obot si ze
pos i t i on_geom. s i z e[ 0] = nt ohs ( ( s hor t ) BASEDI AMETER) ;
pos i t i on_geom. s i z e[ 1] = nt ohs ( ( s hor t ) BASEDI AMETER) ;

PLAYER_MSGTYPE_RESP_ACK, &t hi s - >pos i t i on_geom,
s i z eof ( t hi s - >pos i t i on_geom) , NULL) ;
Anexe
pagina XV
br eak;
}

cas e PLAYER_POSI TI ON2D_MOTOR_POWER_REQ:
{
/ * mot or st at e change r equest
* 1 = enabl e mot or s
* 0 = di sabl e mot or s ( def aul t )
*/

{
PLAYER_WARN
( "Ar g t o mot or s t at e change r eques t wr ong s i z e; i gnor i ng") ;
br eak;
}

pos i t i on_power = *( ( pl ay er _pos i t i on2d_power _conf i g_t *) conf i g) ;
i f ( pos i t i on_power . v al ue == 0)
{
PLAYER_MSG0 ( 1, "> ZeeRO mot or s s t at e. . . [ di s abl ed] ") ;
}
el s e
{
PLAYER_MSG0 ( 1, "> ZeeRO mot or s s t at e. . . [ enabl ed] ") ;
}

br eak;
}

cas e PLAYER_POSI TI ON2D_VELOCI TY_MODE_REQ:
{
br eak;
}

cas e PLAYER_POSI TI ON2D_RESET_ODOM_REQ:
{
br eak;
}

cas e PLAYER_POSI TI ON2D_POSI TI ON_MODE_REQ:
{
br eak;
}

cas e PLAYER_POSI TI ON2D_SPEED_PI D_REQ:
{
br eak;
}

cas e PLAYER_POSI TI ON2D_POSI TI ON_PI D_REQ:
{
br eak;
}

cas e PLAYER_POSI TI ON2D_SPEED_PROF_REQ:
{
br eak;
}

cas e PLAYER_POSI TI ON2D_SET_ODOM_REQ:
{
br eak;
}

def aul t :
{
PLAYER_WARN ( "Pos i t i on got unknown conf i g r eques t ") ;
t hi s - >Put Repl y ( cl i ent , PLAYER_MSGTYPE_RESP_NACK, NULL, 0, NULL) ;
br eak;
}
}
br eak;
}
r et ur n;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
v oi d ZeeRODr i v er : : CheckConf i gSonar ( )
{
v oi d* cl i ent ;
Anexe
pagina XVI

whi l e ( ( conf i g_s i z e = t hi s - >Get Conf i g ( t hi s - >s onar _i d, &cl i ent ,
{
{
cas e PLAYER_SONAR_GET_GEOM_REQ:
{
{
PLAYER_WARN ( "Ar g get s onar geom i s wr ong s i z e; i gnor i ng") ;
t hi s - >Put Repl y ( t hi s - >s onar _i d, cl i ent ,
br eak;
}

s onar _geom. s ubt y pe = PLAYER_SONAR_GET_GEOM_REQ;
s onar _geom. pos e_count = ht ons ( SONAR_SAMPLES) ;
f or ( i nt i = 0; i < SONAR_SAMPLES; i ++)
{
s onar _pos e pos e = ZeeRO_s onar s . s onar s [ i ] ;
s onar _geom. pos es [ i ] [ 0] = ht ons ( ( s hor t ) ( pos e. x * 1000) ) ;
s onar _geom. pos es [ i ] [ 1] = ht ons ( ( s hor t ) ( pos e. y * 1000) ) ;
s onar _geom. pos es [ i ] [ 2] = ht ons ( ( s hor t ) ( pos e. t h * 1000) ) ;
}

PLAYER_MSGTYPE_RESP_ACK, &t hi s - >s onar _geom,
s i z eof ( t hi s - >s onar _geom) , NULL) ;
br eak;
}

cas e PLAYER_SONAR_POWER_REQ:
{
{
PLAYER_WARN
( "Ar g t o s onar s t at e change r eques t wr ong s i z e; i gnor i ng") ;
br eak;
}

s onar _power = *( ( pl ay er _s onar _power _conf i g_t *) conf i g) ;

i f ( s onar _power . v al ue == 0)
t hi s - >s onar Power = f al s e;
el s e
t hi s - >s onar Power = t r ue;

PLAYER_MSGTYPE_RESP_ACK, NULL) ;
br eak;
}

def aul t :
{
PLAYER_WARN ( "Sonar got unknown conf i g r eques t ") ;
br eak;
}
}
}
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
v oi d ZeeRODr i v er : : CheckConf i gI R ( )
{
v oi d* cl i ent ;

whi l e ( ( conf i g_s i z e = t hi s - >Get Conf i g ( t hi s - >i r _i d, &cl i ent ,
{
{
cas e PLAYER_I R_POSE_REQ:
{
{
PLAYER_WARN ( "Ar g get I R geom i s wr ong s i z e; i gnor i ng") ;
t hi s - >Put Repl y ( t hi s - >i r _i d, cl i ent ,
Anexe
pagina XVII
br eak;
}

i r _geom. s ubt y pe = PLAYER_I R_POSE_REQ;
i r _geom. pos es . pos e_count = ht ons ( I R_SAMPLES) ;
f or ( i nt i = 0; i < I R_SAMPLES; i ++)
{
i r _pos e pos e = ZeeRO_i r s . i r s [ i ] ;
i r _geom. pos es . pos es [ i ] [ 0] = ht ons ( ( s hor t ) ( pos e. x * 1000) ) ;
i r _geom. pos es . pos es [ i ] [ 1] = ht ons ( ( s hor t ) ( pos e. y * 1000) ) ;
i r _geom. pos es . pos es [ i ] [ 2] = ht ons ( ( s hor t ) ( pos e. t h * 1000) ) ;
}

PLAYER_MSGTYPE_RESP_ACK, &t hi s - >i r _geom,
s i z eof ( t hi s - >i r _geom) , NULL) ;
br eak;
}

cas e PLAYER_I R_POWER_REQ:
{
{
PLAYER_WARN
( "Ar g t o i r s t at e change r eques t wr ong s i z e; i gnor i ng") ;
br eak;
}

i r _power = *( ( pl ay er _i r _power _r eq_t *) conf i g) ;

i f ( i r _power . s t at e == 0)
t hi s - >i r Power = f al s e;
el s e
t hi s - >i r Power = t r ue;

PLAYER_MSGTYPE_RESP_ACK, NULL) ;
br eak;
}

def aul t :
{
PLAYER_WARN ( "I R got unknown conf i g r eques t ") ;
br eak;
}
}
}
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / CheckCommands f unct i on
v oi d ZeeRODr i v er : : CheckCommands ( )
{
i f ( t hi s - >Get Command ( t hi s - >pos i t i on_i d, &t hi s - >pos i t i on_cmd,
s i z eof ( t hi s - >pos i t i on_cmd) , NULL) > 0)
{
i nt x s peed = nt ohl ( t hi s - >pos i t i on_cmd. x s peed) ;
i nt y aws peed = nt ohl ( t hi s - >pos i t i on_cmd. y aws peed) ;
i nt y aw = nt ohl ( t hi s - >pos i t i on_cmd. y aw) ;

s wi t ch ( pos i t i on_cmd. t y pe)
{
cas e 0:
{
/ * Vel oci t y mode */
i f ( y aws peed ! = 0)
s et YawSpeed ( ZeeRO, y aws peed) ;
el s e
s et XSpeed ( ZeeRO, x s peed) ;

br eak;
}

cas e 1:
{
/ * Posi t i on mode */

i f ( y aw == 0) {
br eak;
}

Anexe
pagina XVIII
doubl e s ec;

i f ( y aw < 0)
s ec = - s et Yaw ( ZeeRO, y aw, 0) ;
el s e
s ec = s et Yaw ( ZeeRO, y aw, 1) ;

l ong s l p = ( l ong) s ec*10000;
us l eep ( s l p) ;


br eak;
}

def aul t :
{
PLAYER_WARN ( "Pos i t i on got unknown command t y pe") ;
br eak;
}
}
}

r et ur n;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / Ref r eshDat a f unct i on
v oi d ZeeRODr i v er : : Ref r es hDat a ( )
{
i nt i ;

mems et ( &t hi s - >pos i t i on_dat a, 0, s i z eof ( t hi s - >pos i t i on_dat a) ) ;
mems et ( &t hi s - >s onar _dat a, 0, s i z eof ( t hi s - >s onar _dat a ) ) ;
mems et ( &t hi s - >i r _dat a, 0, s i z eof ( t hi s - >i r _dat a ) ) ;
mems et ( &t hi s - >ai o_dat a, 0, s i z eof ( t hi s - >ai o_dat a ) ) ;

/ / - - - [ Posi on2D I nt er f ace ] - - -
// To do: add odomet r y
pos i t i on_dat a. x pos = 0;
pos i t i on_dat a. y pos = 0;
pos i t i on_dat a. y aw = 0;
pos i t i on_dat a. x s peed = 0;
pos i t i on_dat a. y s peed = 0;
pos i t i on_dat a. y aws peed = 0;
pos i t i on_dat a. s t al l = 0;
/ / Wr i t e Posi t i on dat a
t hi s - >Put Dat a ( t hi s - >pos i t i on_i d, &t hi s - >pos i t i on_dat a, s i z eof ( t hi s - >pos i t i on_dat a) , NULL) ;

/ / - - - [ Sonar I nt er f ace ] - - -
s hor t Sonar Ranges [ PLAYER_SONAR_MAX_SAMPLES] ;
i nt Sonar Res ul t = 0;

i f ( t hi s - >s onar Power == t r ue)
{
Sonar Res ul t = get Sonar Val ues ( ZeeRO, Sonar Ranges ) ;

i f ( Sonar Res ul t ! = 0)
pr i nt f ( "Er r or %d whi l e r eadi ng Sonar s ens or s ! \n", Sonar Res ul t ) ;

s onar _dat a. r ange_count = ht ons ( SONAR_SAMPLES) ;

f or ( i = 0; i < SONAR_SAMPLES; i ++)
{
s onar _dat a. r anges [ i ] = ht ons ( ( uns i gned s hor t ) Sonar Ranges [ i ] ) ;
}
}
/ / Wr i t e Sonar dat a
t hi s - >Put Dat a ( t hi s - >s onar _i d, &t hi s - >s onar _dat a, s i z eof ( t hi s - >s onar _dat a) , NULL) ;

/ / - - - [ I R I nt er f ace ] - - -
doubl e I RVol t ages [ PLAYER_I R_MAX_SAMPLES] ;
doubl e I RRanges [ PLAYER_I R_MAX_SAMPLES] ;
i nt I RRes ul t = 0;

i f ( t hi s - >i r Power == t r ue)
{
I RRes ul t = get I RVal ues ( ZeeRO, I RVol t ages , I RRanges ) ;

i f ( I RRes ul t ! = 0)
pr i nt f ( "Er r or %d whi l e r eadi ng I R s ens or s ! \n", I RRes ul t ) ;

i r _dat a. r ange_count = ht ons ( I R_SAMPLES) ;

Anexe
pagina XIX
f or ( i = 0; i < I R_SAMPLES; i ++)
{
i r _dat a. v ol t ages [ i ] = ht ons ( ( uns i gned s hor t ) I RVol t ages [ i ] ) ;
i r _dat a. r anges [ i ] = ht ons ( ( uns i gned s hor t ) I RRanges [ i ] ) ;
}
}
/ / Wr i t e I R dat a
t hi s - >Put Dat a ( t hi s - >i r _i d, &t hi s - >i r _dat a, s i z eof ( t hi s - >i r _dat a) , NULL) ;

/ / - - - [ AI O ( Pyr o) I nt er f ace ] - - -
i nt Py r oVal ue[ PLAYER_AI O_MAX_SAMPLES] ;
i nt Py r oRes ul t = 0;

Py r oRes ul t = get Py r oVal ue ( ZeeRO, Py r oVal ue) ;

i f ( Py r oRes ul t ! = 0)
pr i nt f ( "Er r or %d whi l e r eadi ng Py r oel ect r i c s ens or ! \n", Py r oRes ul t ) ;

ai o_dat a. count = ( uns i gned char ) PYRO_SAMPLES;

f or ( i = 0; i < PYRO_SAMPLES; i ++)
{
ai o_dat a. ani n[ i ] = ht onl ( Py r oVal ue[ i ] ) ;
}

/ / Wr i t e AI O ( Pyr o) dat a
t hi s - >Put Dat a ( t hi s - >ai o_i d, &t hi s - >ai o_dat a, s i z eof ( t hi s - >ai o_dat a) , NULL) ;

r et ur n;
}

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / Ext r a st uf f f or bui l di ng a shar ed obj ect .

/ * need t he ext er n t o avoi d C++ name- mangl i ng */
ex t er n "C" {
i nt pl ay er _dr i v er _i ni t ( Dr i v er Tabl e* t abl e)
{
ZeeRODr i v er _Regi s t er ( t abl e) ;
PLAYER_MSG0 ( 1, "\n> ZeeRO dr i v er r egi s t er . . . [ done] ") ;
r et ur n ( 0) ;
}
}

Rusu Dissertation Thesis

Încărcat de

Informații document

Drepturi de autor

Formate disponibile

Partajați acest document

Partajați sau inserați document

Opțiuni de partajare

Vi se pare util acest document?

Este necorespunzător acest conținut?

Drepturi de autor:

Formate disponibile

Rusu Dissertation Thesis

Încărcat de

Drepturi de autor:

Formate disponibile

UNIVERSITATEA TEHNIC DIN CLUJ-NAPOCA

FACULTATEA DE AUTOMATIC I CALCULATOARE

S-ar putea să vă placă și