AUTONOMOUS OBJECT RETRIEVAL WITH HUMANOID ROBOT

NICK RHINEHARTMATT ZUCKER

DEPARTMENT OF ENGINEERINGSWARTHMORE COLLEGE

ABSTRACT. Object retrieval is a useful task for machines to perform, enabling robots to find a desired objectand bring it back to a human user or place it in a specified location. However, it requires sophisticated coordi-nation on multiple levels of control. In order to determine the direction in which to move to retrieve an object,localization is used to establish an estimate of the robot’s location in the environment. An Extended KalmanFilter is implemented on a humanoid robot to enable the robot to localize with sensory input from both coloredcylinders, and more recently, data matrices [1] of arbitrary size and pose that map to known locations. Therobot can autonomously navigate with both forms of input, and can also perform simple grasping under manualcontrol by the user.

CONTENTS

1. Introduction 22. Theory 33. Approach 63.1. Sensory Input 63.2. Maintaining Near-Real Time Operation 63.3. Flow Diagram of Program 73.4. Autonomous Controller 74. Results 85. Future Work 86. Appendices 96.1. main.cpp 96.2. Initialization.cpp 236.3. GlobalVariables.h 316.4. TagPositionWriter.java 33References 35

1

1. INTRODUCTION

The humanoid robot is a DARwIn-OP (Dynamic Anthropomorphic Robot with Intelligence - Open Plat-form). Sensing is performed with the single camera, located between the robot’s eye structures. The robothas many interfaces and features, some of which are labled in Figure 1b. DARwIn comes with a low-levelAPI and Framework in C++, including a Walking module whose member variables may be modified tocontrol the robot’s forward, sideways, and turning velocities, as well as a ColorFinder module and othermodules to track the blob with the head and by walking. The Walking, ColorFinder, and Head mod-ules were used, with all other functionality implemented as part of this work. Because the Framework is instandard C++, integration of both C and C++ libraries such as the socket libraries and Boost is possible.

(A) Darwin (B) Diagram of features

FIGURE 1. DARwIn-OP

Object retrieval is a general behavior that underlies more complex robotic behaviors, however, it is quitecomplex in and of itself. The task requires robustly navigating to locations in the environment, identifyingand grasping an object firmly, and then navigating to a different location to deposit the retrieved object. Thetwo main complex parts behind autonomous retrieval are localization and grasping. With a good estimateof the robot’s state (location and orientation) in the environment, the robot can navigate itself to a differentlocation, given that it can continue to maintain a good state estimate.

One method of developing this estimate is to use a motion model that incorporates a pre-calibrated func-tional mapping between input controls and changes in state. Using this mapping for estimation is known asodometry. However, the state estimate provided by the motion model grows worse over time, as small errorsaccumulate and state estimate diverges from reality, therefore becoming unusable.

To solve the problem of the odometry alone not being accurate enough to estimate the robot’s state,information from the camera is used to influence the estimate of the robot’s state. In the case of a knownenviroment (having the environment “mapped”), identifying parts of the environment that are known canyield another estimate of the robot’s state. Known parts of the environment are, in our case, called beacons,because much like a lighthouse beacon, they provide the recipient with knowledge about their location in the

2

environment. Integration of both the motion model and a sensing paradigm is performed with an ExtendedKalman Filter [2].

2. THEORY

An Extended Kalman filter is a recursive state-estimation algorithm used to produce a statistically optimalstate estimation. In our case, the state of the robot at time t is expressed as a 3× 1 vector of the x location,y location, and orientation:

xt =

xyθ

The motion model to update the robot’s state at the next time step t is:

xmt =

xtytθt

=xt−1 + ρ · cos(θt−1)yt−1 + ρ · sin(θt−1)

θt−1 + φ

where ρ is the desired forward translation, and φ is the desired rotation.An example of the motion model at work is shown below:

2D Cartesian Space

(xt−1, yt−1)

(xt, yt)

θt

θt−1

Given an initial position and a control of

xt−1 =

00−45◦

, µ = [ρφ

]=

[310◦

]The next state estimate then becomes:

xmt =

xtytθt

=xt−1 + ρ cos(θt−1)yt−1 + ρ sin(θt−1)

θt−1 + φ

=0 + 3 cos(−45◦)0 + 3 sin(−45◦)−45◦ + 10◦

≈ 2.12−2.12−35◦

3

The state update from the motion model, xm, is then used in the update of the state estimate, in combi-nation with any sensory update:

xp = xm +Kδ

where K is the Kalman Gain Matrix, and δ is the innovation vector:

δ = y − z

which is the difference between the state estimate based on the estimation of state from the currentobservation y), and what the state should be given the current observation (z). Both y and z are vectorsof distances to the object (ranges) and horizontal offests of the object from the center of the robot’s imageplane (bearings). The Kalman Gain matrix K is a function of the current covariances P in position given themotion model, the Jacobian of the motion model’s state F, the Jacobian H of the observed state, z, and theunderlying covariances in beacon observation, R. The full EKF algorithm for our situation is as follows:

4

Algorithm 1 EKF for Localization by Beacon Extraction from ImagesInitialize Pp, xp, Q, cov map, b locst← 0while true dot← t+ 1if a control u from user or autonomous controller does not exist, u← [0, 0]xmt ← motion model(xpt−1 ,u)F← Jacobian of xmtPmt ← FPpt−1FT +QImage← new image()Observed Beacons← extract ranges and bearings(Image)if Observed Beacons.length() is 0 thenxpt ← xmtPpt ← Pmtcontinue

end ifR← eye(Observed Beacons.length())i← 0for n = 0 to Observed Beacons.length() doBeacon← Observed Beacons[n]id← Beacon.id()Ri,i ← cov map[id][0]

y[[i,i+1],0] =

[Beacon.range()Beacon.bearing()

]Ri+1,i+1 ← cov map[id][1]i← i+ 1

end for[x, y, θ]T ← xmtfor n = 0 to Observed Beacons.length() do

[a, b]T ← b locs[Observed Beacons[n]]z[2·n,0] ← sqrt((a− x)2 + (b− y)2)z[2·n+1,0] ← atan2(b− y, a− x)− θ

end forH← Jacobian of zS← HPmtHT +RK← PmtHTS−1δ ← y − zxpt ← xmt +KδPpt ← (I−KH)Pmt

end while

By knowing the real-world locations of beacons (which is the data encapsulated by the map b locs), andobserving at least one beacon in an image, we can update the state estimate of the robot. The state estimategets better over time, as more beacon observations gives the algorithm more information about the robot’sstate. If no control is given, then the motion model takes an input of [0, 0]. If no beacon is observed inan image, the motion model’s state estimate is carried through to be the final state estimate at the end of aspecific loop iteration.

5

3. APPROACH

3.1. Sensory Input. The Extended Kalman Filter algorithm is useful, but needs robust sensory data toperform well. Initially, colored beacons were used as sensory input: measuring the real-world location ofknown height beacons enabled a range and bearing estimate to be extracted from beacons indentified withinimages. However, simple color blob detection is limiting in the sense that in order to be light-tolerant,there are a limited amount of colored beacons that can be used as input. Usage of more robust beacons isdesireable. The April Tag [1] Framework has been selected as useful for environment-sensing. By creatinga mapping between the tag numbers and their real-world locations, hundreds of tags can be placed anddetected in the environment.

(A) Color Beacon Range and Bearing Extraction (B) Robust Multiple Tag Detection [1]

FIGURE 2. April Tag Framework Demonstration

3.2. Maintaining Near-Real Time Operation. The main source for controlling the robot is written inC++, as it uses a C++ Framework developed by ROBOTIS R© for the low-level camera integration and ac-tuator control. The April Tag Framework, written in Java, was adapted and tweaked to interact with themain control program by opening several sockets: a socket to send images from the C++ program to theJava source that extracts the homography and tag information, and another socket to communicate the ho-mography and tag information back to the C++ program. Writing and reading the images with the standardC++ file writing and Java file reading functionality proved to be much too slow to facilitate near-real timeoperation.

6

3.3. Flow Diagram of Program.

sudo ./start control

Read inprogramoptions

with Boost

Use AprilTags?

Forknew JavaProcess

InitializeColor

Finders

Java TagDetectionProcess

EstablishImage

Sockets

Initialize Kalman Algorithm Data,Robot Pose, Global Variables, etc

Take an image

Sent entireimage?Send image

Autonomouscontrol?

Developcontrolbased

on state

Developcontrol

from userinput

Executecontrol

motionModel()

senseUpdate()

no

yes

no

yes

yes

no

3.4. Autonomous Controller. A simple proportional controller is used to navigate the robot to the goal.The controller selects a control by examining the estimated distance from the goal and the offset orientationto the goal. If the distance is less than a certian threshold distance, we say that the robot has arrived at thegoal. In practice, a threshold distance of .2 meters was selected. Several of the other cases are as follows:

7

Algorithm 2 Control selection for autonomous navigationCalculate δ, θ, the distance and angular offset from goal, respectivelymin thresh← .2close thresh← .3Set maximum turning angle, φSet maximum translation, ρif δ < min thresh thenµ =

[00

]else {(δ < close thresh and θ > 22.5◦) or θ > 90◦}

µ =

[0

φ · sgn(θ)

]else {θ > 40◦}

µ =

[0

φ · sgn(θ) ·√|θ|π

]else

µ =

[ρ · π−|θ|π

φ2 · sgn(θ) ·

√|θ|π

]end if

This algorithm is geared toward turning the robot to nearly be facing the goal before it translates at all.The only case in which the robot moves forwards is if the offset angle, θ, is less than 40◦. The sgn() operatorreturns the sign of the angle.

4. RESULTS

The robot is able to navigate autonomously using both colored beacons and April Tags as sensory in-put. After navigating to a goal, the robot may possibly continue onto further goals obtained from a queueof locations. As the performance of autonomous localization and navigation is quite dynamic, it does noteasily lend itself to proof by static images. At the time of writing, a video of the robot autonomously nav-igating can be found at http://www.youtube.com/watch?feature=player_embedded&v=Pk5xeWYpgQg. The reader may take note that many environmental complexities exist that were not ex-plicitly handled by this work - the transition from carpet to smooth floor, lack of stability on the smoothfloor, and obstacles within the environment. The robot currently does not avoid or even “see” obstacles, inthe sense that the controller assumes a completely flat and empty environment.

5. FUTURE WORK

To fully complete autonomous object retrieval, robust grasping must be developed. Simple grasping wasperformed by manually selecting several sets of joint angles were determined by trial-and-error and weretriggered manually by using a joystick.

8

http://www.youtube.com/watch?feature=player_embedded&v=Pk5xeWYpgQghttp://www.youtube.com/watch?feature=player_embedded&v=Pk5xeWYpgQg

6. APPENDICES

6.1. main.cpp.1 /∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ C o n t r o l DARWIN w i t h j o y s t i c k , or autonomous beacon∗ gu id ed n a v i g a t i o n . Many c o n f i g u r a t i o n s e t t i n g s∗ s p e c i f i e d i n ” c o n f i g m a s t e r . c f g ”∗∗ Author : Nick R h i n e h a r t ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ /

# i n c l u d e ” Tinyman . h ”us ing namespace Robot ;us ing namespace s t d ;

11 us ing namespace cv ;namespace po = b o o s t : : p r o g r a m o p t i o n s ;

/∗ # i n c l u d e ’ i n g a cpp ?!? t h i s t r i c k i s c a l l e d ” u n i t y ” b u i l d i n g .I t i s ” n e c e s s a r y ” because o f t h e usage o f g l o b a l v a r i a b l e s a c r o s s s e v e r a l cpp f i l e s .( t h i s one and ” I n i t i a l i z a t i o n . cpp ”) . Also , has t h e n i c e s i d e e f f e c to f s p e e d i n g up c o m p i l e t i m e a l o t ! Be c a r e f u l t o ”make c l e a n ” i f you changeI n i t i l i z a t i o n . h or I n i t i a l i z a t i o n . cpp ∗ /

# i n c l u d e ” I n i t i a l i z a t i o n . cpp ”

21 /∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ //∗ A c t i o n methods . ∗ //∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ /void s e t u p h a n d l e r ( ) ;void s t o p w a l k i n g ( ) ;bool r e a d f r o m j o y s t i c k ( J o y s t i c k ∗ joy , f l o a t ∗x amp , f l o a t ∗a amp ) ;void p a i n t f o u n d r e g i o n ( C o l o r F i n d e r ∗ b a l l f i n d e r , Mat image , f l o a t rgb [ 3 ] , bool f u l l m a s k = f a l s e ) ;void t e x t t o s p e e c h ( c o n s t char∗ s t r i n g , bool s p a n i s h ) ;void s p e e c h a c t i o n w r a p p e r ( i n t a c t i o n i d , c o n s t char∗ l i n e , bool s p a n i s h ) ;void g e t u p i f f a l l e n ( ) ;

31 i n t m a k e c o l o r ( f l o a t components [ 3 ] ) ;i n t s e t l e d c o l o r ( i n t cho i ce , f l o a t components [ 3 ] ) ;void s e t w a l k i n g s e t t i n g s ( f l o a t x amp , f l o a t a amp ) ;void s e r v e r s y n c ( ) ;void e x i t w i t h e r r o r ( s t r i n g e r r o r ) ;void o u t p u t s t a t e ( Mat xp ) ;void o u t p u t l o o p t i m e ( ) ;void o u t p u t t i m e d i f f e r e n c e ( c l o c k t s t a r t ) ;f l o a t c a l c u l a t e t i m e d i f f e r e n c e ( c l o c k t s t a r t ) ;void c o l o r b e a c o n s e n s e ( Mat o r i g i m a g e , v e c t o r

a l l c o l o r f i n d e r s , map<

s t r i n g , v e c t o r > s p o t t e d b e a c o n s ) ;41 s t r i n g r e a d f r o m a p r i l t a g ( Mat th , i n t ∗ t ag number , bool ∗ r e a d t a g h o m o g r a p h y , bool ∗ r e a d y t o s e n d i m a g e ) ;

void s e n d i m a g e t o a p r i l t a g ( Mat o r i g i m a g e ) ;void u n r e c o g n i z e d l o c a t i o n h a l t ( s t r i n g t a g n u m b e r s t r i n g ) ;void u n r e c o g n i z e d c o v a r i a n c e h a l t ( s t r i n g t a g n u m b e r s t r i n g ) ;void p r o m p t f o r n e w g o a l ( f l o a t g o a l [ 2 ] ) ;void s a y n e x t d e s t i n a t i o n ( f l o a t g o a l [ 2 ] ) ;f l o a t q u i c k s g n ( f l o a t a n g l e ) ;i n t s e t s e r v o a n g l e ( i n t ID , f l o a t a n g l e ) ;void f i x l e f t s e r v o a n g l e ( i n t ID , f l o a t ∗ a n g l e ) ;void c h e c k a n g l e l i m i t ( i n t id , f l o a t ∗v a l u e ) ;

51/∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ //∗ I m p o r t a n t v i s i o n + Kalman methods ∗ //∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ /void a u t o n o m o u s c o n t r o l l e r ( Mat xp o ld , f l o a t g o a l [ 2 ] , Mat u , f l o a t ∗x amp , f l o a t ∗a amp , bool c o l l i s i o n w a r n ) ;void angle mod ( f l o a t ∗ b e a r i n g ) ;void mot ion mode l ( Mat xp o ld , Mat u , Mat xm , Mat F , f l o a t a v g l o o p t i m e ) ;void s e n s e u p d a t e ( Mat xm , Mat o b s e r v e d b e a c o n s , Mat z , Mat H,

bool u p d a t e h = f a l s e ) ;void s e n s e w i t h b e a c o n s ( map s p o t t e d b e a c o n s ,

61 v e c t o r beacon LUT ,Mat xp , Mat xp o ld ,Mat xm , Mat xm old ,Mat Pm, Mat Pp , Mat Pp old ,Mat Q) ;

v e c t o r c a l c c o l o r e d b e a c o n d i s t a n c e ( Mat image , f l o a t c o l o r [ 3 ] , v e c t o r s t a c k e d b e a c o n s ) ;v e c t o r c a l c b e a c o n d i s t a n c e ( Mat frame2 , f l o a t o b j e c t h e i g h t ) ;v e c t o r e x t r a c t s t a t e f r o m h o m o g r a p h y ( Mat t h ) ;void c o n t r o l l e r s e l e c t i o n a n d e x e c u t i o n ( Mat xp o ld , f l o a t ∗x amp , f l o a t ∗a amp , Mat u , bool m o v e w i t h j o y s t i c k ,

i n t count , bool c o l l i s i o n w a r n ) ;Mat a c q u i r e i m a g e ( I p l I m a g e ∗cv image ) ;

71/∗ Here we go ∗ /i n t main ( i n t argc , char ∗∗ a rgv ) {

9

p r i n t f ( ”\n===== Tinyman 0 . 5 =====\n ” ) ;

/∗ T h i s b l o c k o f f u n c t i o n c a l l s has been c r e a t e d t o re du c e b u l k i n t h e∗ ”main” o f t h e program . The names o f f u n c t i o n s are i n t e n d e d t o be∗ d e s c r i p t i v e enough t o m a i n t a i n a high−l e v e l v iew o f t h e program ,∗ and many are e x p l a i n e d i n more d e t a i l i n t h e i r d e f i n i t i o n s . The f u n c t i o n s∗ are a l l i n i t i a l i z a t i o n−r e l a t e d , and occur b e f o r e t h e pr imary o p e r a t i o n a l loop ∗ /

81 v e c t o r

a l l c o l o r f i n d e r s ;t r y { p r o c e s s i n p u t ( a rgc , argv , &vm) ;}catch ( b o o s t : : e x c e p t i o n c o n s t &e ) { c e r r

i f ( ( r e t = a s p r i n t f (& s e n t e n c e , ” Beg inn ing n a v i g a t i o n . N a v i g a t i n g t o %.1 f %.1 f ” , g o a l [ 0 ] , g o a l [ 1 ] ) ) == −1)c e r r

i f ( n u m s p o t t e d > 0) s e n s e w i t h b e a c o n s ( s p o t t e d b e a c o n s , beacon LUT , xp , xp o ld , xm , xm old , Pm, Pp , Pp old ,Q) ;

231 e l s e {xm . copyTo ( xp ) ; xm . copyTo ( x p o l d ) ;} / / i f no beacons s p o t t e d , c a r r y mot ion model t h r o u g h/∗∗∗∗∗∗∗∗∗∗∗∗∗∗END OF KALMAN UPDATE∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ /

/ / i f ( n u m s p o t t e d > 0 && t a g d e t e c t i o n l e d p e r m a n e n c e < 0) {/ / t a g d e t e c t i o n l e d p e r m a n e n c e = 10;/ / s e t l e d c o l o r ( 1 , green ) ;/ / }/ / e l s e i f (!(−− t a g d e t e c t i o n l e d p e r m a n e n c e ) ) s e t l e d c o l o r ( 1 , b l u e ) ;

241i f ( o u t p u t t e r m i n a l s t a t e ) o u t p u t s t a t e ( xp ) ;i f ( p y t h o n p l o t t i n g ) p l o t w r i t i n g s t r e a m

K Gain = Pm∗H. t ( ) ∗S . i n v ( ) ;

d e l t a = y − z ;311

f o r ( i n t i =0 ; i

/∗ P o i n t t h e camera ’ s b u f f e r back t o t h e new image da ta so we can s t r eami t t o a p o r t w i t h an m j p e g s t r e a m e r ∗ /

/ / LinuxCamera : : G e t I n s t a n c e ( )−>f b u f f e r−>m RGBFrame−>m ImageData = frame2 . da ta ;re turn s t a t e ;

391 }

/∗ c a l c u l a t e t h e d i s t a n c e t o an o b j e c t w i t h an image t h a to n l y has t h e d e s i r e d c o l o r p a i n t e d ∗ /

v e c t o r c a l c b e a c o n d i s t a n c e ( Mat frame2 , f l o a t o b j e c t h e i g h t ) {v e c t o r c o n t o u r s ;v e c t o r s t a t e ;v e c t o r n o s t a t e ;n o s t a t e . p u s h b a c k (−1) ;n o s t a t e . p u s h b a c k (−1) ;

401 f l o a t b o u n d a r y t h r e s h o l d = 3 ;

/∗ f i n d C o n t o u r s needs a BINARY image . ∗ /Mat g ray ( f rame2 . rows , f rame2 . c o l s , CV 8UC1 ) ;c v t C o l o r ( frame2 , gray , CV RGB2GRAY) ;

/∗ f i n d t h e c o n n e c t e d components i n t h e image ∗ /f i n d C o n t o u r s ( gray , c o n t o u r s , CV RETR CCOMP , CV CHAIN APPROX SIMPLE ) ;f l o a t max area = 0 ;i n t max idx = −1;

411/∗ p o i n t l o c a t i o n s o f c o n n e c t e d component ’ s t o p and bo t tom ∗ /Mat t o p = Mat ( 3 , 1 , CV 32F ) ;Mat bot tom = Mat ( 3 , 1 , CV 32F ) ;

c o n s t f l o a t m i n h e i g h t = 2 0 ;/∗ use t h e l a r g e s t c o n n e c t e d component ∗ /f o r ( s i z e t i =0 ; i < c o n t o u r s . s i z e ( ) ; i ++) {

Rect r = bound ingRec t ( c o n t o u r s [ i ] ) ;

421 /∗ i f i t s t o o s m a l l or on t h e b o u n d a r i e s o f t h e image , s k i p ∗ /i f ( ( r . h e i g h t < m i n h e i g h t ) ) c o n t i nu e ;e l s e i f ( ( ( r . x+ r . wid th ) >= ( f rame2 . c o l s − b o u n d a r y t h r e s h o l d ) ) | |

( ( r . y+ r . h e i g h t ) >= ( f rame2 . rows − b o u n d a r y t h r e s h o l d ) ) | |( r . x = 0) {

Rect m a x r e c t = bound ingRec t ( c o n t o u r s [ max idx ] ) ;f l o a t t o p x = m a x r e c t . x + m a x r e c t . w id th / 2 ;f l o a t t o p y = m a x r e c t . y ;f l o a t bo t tom x = m a x r e c t . x + m a x r e c t . w id th / 2 ;f l o a t bo t tom y = m a x r e c t . y + m a x r e c t . h e i g h t ;

t o p . a t(0 ,0) = t o p x ;t o p . a t(0 ,1) = t o p y ;

451 t o p . a t(0 ,2) = 1 ;

bot tom . a t(0 ,0) = bo t tom x ;bot tom . a t(0 ,1) = bo t tom y ;bot tom . a t(0 ,2) = 1 ;

/∗ p u t t h e bounding box on t h e image i n red . ∗ /r e c t a n g l e ( frame2 , max rec t , S c a l a r ( 2 5 5 , 0 , 0 ) ) ;

/∗ Draw a l i n e from t h e p o i n t s t h a t w i l l be t r a n s f o r m e d ∗ /461 l i n e ( frame2 , P o i n t ( t op x , t o p y ) , P o i n t ( bo t tom x , bo t tom y ) , S c a l a r ( 2 5 5 , 2 5 5 , 0 ) ) ;

}e l s e {

re turn n o s t a t e ;}

14

/∗ b u c k e t s f o r t h e p o i n t s i n r e a l i t y ∗ /Mat t o p r e a l i t y = Mat ( 3 , 1 , CV 32F ) ;Mat b o t t o m r e a l i t y = Mat ( 3 , 1 , CV 32F ) ;

471 /∗ use t h e camera c a l i b r a t i o n m a t r i x t o d e t e r m i n e l o c a t i o n ∗ /t o p r e a l i t y = K inv∗ t o p ;b o t t o m r e a l i t y = K inv∗bot tom ;

/∗ e x t r a c t t h e h e i g h t s f o r d i s t a n c e c a l c u l a t i o n ∗ /f l o a t y0 = b o t t o m r e a l i t y . a t(0 ,1) ;f l o a t y1 = t o p r e a l i t y . a t(0 ,1) ;f l o a t X = t o p r e a l i t y . a t(0 ,0) ;

f l o a t Zc = o b j e c t h e i g h t / ( y0−y1 ) ;481 f l o a t Xc = X∗Zc ;

f l o a t t h e t a = a t a n (X / 1 . 0 ) ;

/∗ d i s t a n c e t o o b j i s t h e h y p o t e n u s e ∗ /f l o a t d i s t a n c e = s q r t ( pow ( Zc , 2 ) +pow ( Xc , 2 ) ) ;s t a t e . p u s h b a c k ( d i s t a n c e ) ;s t a t e . p u s h b a c k(− t h e t a ) ;re turn s t a t e ;

}

491void a u t o n o m o u s c o n t r o l l e r ( Mat xp o ld , f l o a t g o a l [ 2 ] , Mat u , f l o a t ∗x amp , f l o a t ∗a amp , bool c o l l i s i o n w a r n ) {

/∗ Keep s t a t i c da ta abou t how long t o a v o i d a c o l l i s i o n ∗ /s t a t i c i n t c o l l i s i o n w a r n i n g p e r m a n e n c e = 0 ;c o n s t i n t i t e r a t i o n s t o h e e d w a r n i n g = 4 0 ;bool c o l l i s i o n w a r n i n g a c t i v e = f a l s e ;c o n s t f l o a t a r r i v a l t h r e s h o l d = . 2 5 ; / / w i t h i n t h i s d i s t , we ’ re t h e r e

f l o a t g o a l d i s t = s q r t ( pow ( g o a l [ 0 ] − x p o l d . a t(0 ,0) , 2 ) + pow ( g o a l [ 1 ] − x p o l d . a t(1 ,0) , 2 ) ) ;f l o a t g o a l o r i e n t a t i o n = a t a n 2 ( g o a l [1]− x p o l d . a t(1 ,0) , g o a l [0]− x p o l d . a t(0 ,0) ) ;

501 f l o a t d e s i r e d a n g l e = g o a l o r i e n t a t i o n−x p o l d . a t(2 ,0) ;

/∗ d e s i r e d a n g l e i s now i n range −p i t o p i ∗ /angle mod (& d e s i r e d a n g l e ) ;f l o a t t u rn mag = abs ( d e s i r e d a n g l e ) ;f l o a t o f f s e t s c a l i n g =( pi−t u rn mag ) / p i ;

511 i f ( c o l l i s i o n w a r n ) {c o l l i s i o n w a r n i n g p e r m a n e n c e = i t e r a t i o n s t o h e e d w a r n i n g ;c o l l i s i o n w a r n i n g a c t i v e = t rue ;

}e l s e i f ( c o l l i s i o n w a r n i n g p e r m a n e n c e > 0) {

c o l l i s i o n w a r n i n g p e r m a n e n c e−−;c o l l i s i o n w a r n i n g a c t i v e = t rue ;

}

i f ( d e b u g c o n t r o l l e r ) {521 i f ( c o l l i s i o n w a r n i n g a c t i v e ) c o u t

u . a t(0 ,0) = −m a x f o r w a r d v e l∗ o f f s e t s c a l i n g ; / / c o l l i s i o n warning a c t i v a t e d by beacon . . . backup frombeacon

u . a t(1 ,0) = 0 ;}e l s e i f ( ( g o a l d i s t > 1 && turn mag > p i / 2 ) | | ( g o a l d i s t < . 5 && turn mag > p i / 4 ) | | ( g o a l d i s t < . 3 &&

turn mag > p i / 8 ) ) {u . a t(0 ,0) = 0 ;u . a t(1 ,0) = max ang ve l∗ q u i c k s g n ( d e s i r e d a n g l e ) ;

551 }e l s e i f ( g o a l d i s t > a r r i v a l t h r e s h o l d ) {

i f ( t u rn mag > (70∗ p i / 1 8 0 ) ) {u . a t(0 ,0) = 0 ;u . a t(1 ,0) = max ang ve l∗ q u i c k s g n ( d e s i r e d a n g l e )∗ s q r t ( t u rn mag / p i ) ;

}e l s e i f ( t u rn mag > (40∗ p i / 1 8 0 ) ) {

u . a t(0 ,0) = 0 ;u . a t(1 ,0) = max ang ve l∗ q u i c k s g n ( d e s i r e d a n g l e )∗ s q r t ( t u rn mag / p i ) ;

}561 e l s e {

u . a t(0 ,0) = m a x f o r w a r d v e l∗ o f f s e t s c a l i n g ;u . a t(1 ,0) = max ang ve l /2∗ q u i c k s g n ( d e s i r e d a n g l e )∗ s q r t ( t u rn mag / p i ) ;

}}/∗ we ’ ve a r r i v e d ∗ /e l s e i f ( g o a l d i s t 0){

v e c t o r new goa l = g o a l q u e u e . f r o n t ( ) ;571 g o a l q u e u e . pop ( ) ;

g o a l [ 0 ] = new goa l [ 0 ] ;g o a l [ 1 ] = new goa l [ 1 ] ;p r i n t f ( ” n e x t g o a l : %.1 f , %.1 f ” , g o a l [ 0 ] , g o a l [ 1 ] ) ;s a y n e x t d e s t i n a t i o n ( g o a l ) ;

}e l s e p r o m p t f o r n e w g o a l ( g o a l ) ;

}e l s e p r o m p t f o r n e w g o a l ( g o a l ) ;

}581

e l s e {s t o p w a l k i n g ( ) ;c o u t

621 C o l o r F i n d e r ∗ s i n g l e f i n d e r = a l l c o l o r f i n d e r s [ i ] . f i r s t [ j ] ;Poin t2D pos = s i n g l e f i n d e r−>G e t P o s i t i o n ( LinuxCamera : : G e t I n s t a n c e ( )−>f b u f f e r−>m HSVFrame ) ;

}

s t r i n g c o l o r = a l l c o l o r f i n d e r s [ i ] . s econd ;Mat p a i n t i m a g e = Mat : : z e r o s ( o r i g i m a g e . rows , o r i g i m a g e . c o l s , CV 8UC3 ) ;v e c t o r s t a t e = c a l c c o l o r e d b e a c o n d i s t a n c e ( p a i n t i m a g e , whi te , a l l c o l o r f i n d e r s [ i ] . f i r s t ) ;i f ( s t a t e [ 0 ] > . 1 ) {

i f ( o u t p u t b e a c o n d a t a ) {p r i n t f ( ”%s beacon\n−−−−−−−−−−−−\n d i s t :% f\n a n g l e :% f\n ” ,

631 c o l o r . c s t r ( ) ,s t a t e [ 0 ] ,s t a t e [ 1 ]∗1 8 0 / p i ) ;

}s p o t t e d b e a c o n s [ c o l o r ] = s t a t e ;

}o r i g i m a g e += p a i n t i m a g e ;

}}

641 /∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗Reading and C o n t r o l F u n c t i o n s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ /bool r e a d f r o m j o y s t i c k ( J o y s t i c k ∗ joy , f l o a t ∗x amp , f l o a t ∗a amp ) {

/∗ up da t e t h e da ta from t h e j o y s t i c k ∗ /joy−>u p d a t e ( ) ;i f ( d e b u g j o y s t i c k ) {

p r i n t f ( ” J o y s t i c k b u t t o n v a l u e s\n−−−−−−−−−−−−−−−\n ” ) ;f o r ( i n t i =0 ; inumButtons ( ) ; i ++) {

p r i n t f ( ” Bu t t on %d : %d\n ” , i , joy−>g e t B u t t o n ( i ) ) ;}

651 f o r ( i n t i =0 ; inumAxes ( ) ; i ++) {p r i n t f ( ” Axis %d : %d\n ” , i , joy−>g e t A x i s ( i ) ) ;

}}

f l o a t r i g h t a x i s u p d o w n v a l = joy−>g e t A x i s ( RIGHT AXIS UP DOWN ) ;f l o a t r i g h t a x i s l e f t r i g h t v a l = joy−>g e t A x i s ( RIGHT AXIS LEFT RIGHT ) ;f l o a t l e f t a x i s u p d o w n v a l = joy−>g e t A x i s ( LEFT AXIS UP DOWN ) ;f l o a t l e f t a x i s l e f t r i g h t v a l = joy−>g e t A x i s ( LEFT AXIS LEFT RIGHT ) ;f l o a t pan =0;

661 f l o a t t i l t =0 ;f l o a t t r a n s l a t e =0;f l o a t r o t a t e =0 ;bool move head= f a l s e , walk = f a l s e ;

/∗ read v a l u e from t h e h a t c o n t r o l l e r ∗ // / i n t h a t v a l u e =0;/ / h a t v a l u e = joy−>ge tH a t ( 0 ) ;

/∗∗∗∗∗∗∗∗∗∗∗∗∗∗BUTTON PRESS CODE∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ /671 /∗ i f ”A” b u t t o n h i t , c l o s e t h e program n i c e l y ∗ /

i f ( joy−>g e t B u t t o n (A BUTTON) == 1) {Body : : G e t I n s t a n c e ( )−>m J o i n t . S e t E n a b l e ( J o i n t D a t a : : ID R SHOULDER ROLL , t rue ) ;Body : : G e t I n s t a n c e ( )−>m J o i n t . S e t E n a b l e ( J o i n t D a t a : : ID L SHOULDER ROLL , t rue ) ;Body : : G e t I n s t a n c e ( )−>m J o i n t . Se tAng le ( J o i n t D a t a : : ID R SHOULDER ROLL,−60) ;Body : : G e t I n s t a n c e ( )−>m J o i n t . Se tAng le ( J o i n t D a t a : : ID L SHOULDER ROLL , 6 0 ) ;

}/∗ i f S t a r t b u t t o n h i t , c l o s e t h e program and don ’ t move ∗ /e l s e i f ( joy−>g e t B u t t o n (START BUTTON) ==1) {

p r i n t f ( ” E x i t i n g program from J o y s t i c k \n ” ) ;681 c l e a n s h u t d o w n ( ) ;

}e l s e i f ( joy−>g e t B u t t o n (X BUTTON) == 1 && p l a y i n g a u d i o == f a l s e ) {

i f ( Body : : G e t I n s t a n c e ( )−>m J o i n t . Ge tEnab le ( J o i n t D a t a : : ID R SHOULDER PITCH ) == f a l s e ) {Body : : G e t I n s t a n c e ( )−>m J o i n t . S e t E n a b l e ( J o i n t D a t a : : ID R SHOULDER PITCH , t rue ) ;Body : : G e t I n s t a n c e ( )−>m J o i n t . S e t E n a b l e ( J o i n t D a t a : : ID L SHOULDER PITCH , t rue ) ;

}Body : : G e t I n s t a n c e ( )−>m J o i n t . S e t E n a b l e ( J o i n t D a t a : : ID L SHOULDER PITCH , t rue ) ;Body : : G e t I n s t a n c e ( )−>m J o i n t . Se tAng le ( J o i n t D a t a : : ID R SHOULDER PITCH , 9 0 ) ;Body : : G e t I n s t a n c e ( )−>m J o i n t . Se tAng le ( J o i n t D a t a : : ID L SHOULDER PITCH,−90) ;

691}e l s e i f ( joy−>g e t B u t t o n (Y BUTTON) == 1 && p l a y i n g a u d i o == f a l s e ) {

Body : : G e t I n s t a n c e ( )−>m J o i n t . S e t E n a b l e ( J o i n t D a t a : : ID R ELBOW , t rue ) ;Body : : G e t I n s t a n c e ( )−>m J o i n t . S e t E n a b l e ( J o i n t D a t a : : ID L ELBOW , t rue ) ;Body : : G e t I n s t a n c e ( )−>m J o i n t . Se tAng le ( J o i n t D a t a : : ID R ELBOW,−90) ;Body : : G e t I n s t a n c e ( )−>m J o i n t . Se tAng le ( J o i n t D a t a : : ID L ELBOW , 9 0 ) ;

}e l s e i f ( joy−>g e t B u t t o n (B BUTTON) == 1 && p l a y i n g a u d i o == f a l s e ) {

17

}701 e l s e i f ( joy−>g e t B u t t o n (RB BUTTON) == 1 && p l a y i n g a u d i o == f a l s e ) {

Body : : G e t I n s t a n c e ( )−>m J o i n t . Se tAng le ( J o i n t D a t a : : ID R SHOULDER PITCH , 1 5 0 ) ;Body : : G e t I n s t a n c e ( )−>m J o i n t . Se tAng le ( J o i n t D a t a : : ID L SHOULDER PITCH,−150) ;

}

e l s e i f ( joy−>g e t B u t t o n (LB BUTTON) == 1 && l e f t b a c k b u t t o n d o w n == f a l s e ) {Body : : G e t I n s t a n c e ( )−>m J o i n t . S e t E n a b l e ( J o i n t D a t a : : ID R HIP PITCH , t rue ) ;

Body : : G e t I n s t a n c e ( )−>m J o i n t . S e t E n a b l e ( J o i n t D a t a : : ID L HIP PITCH , t rue ) ;Body : : G e t I n s t a n c e ( )−>m J o i n t . Se tAng le ( J o i n t D a t a : : ID R HIP PITCH ,−25) ;Body : : G e t I n s t a n c e ( )−>m J o i n t . Se tAng le ( J o i n t D a t a : : ID L HIP PITCH , 2 5 ) ;

711}/∗ randomly change t h e r o b o t s e y e s t o d i f f e r e n t c o l o r s w h i l e t h e

l e f t t r i g g e r i s h e l d down ∗ /e l s e i f ( joy−>g e t B u t t o n (LB BUTTON) == 1) {

}e l s e i f ( joy−>g e t B u t t o n (LB BUTTON) == 0) {

l e f t b a c k b u t t o n d o w n = f a l s e ;}

721/∗∗∗∗∗∗∗∗∗∗∗∗JOYSTICK AXIS MOVEMENT CODE∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ /i f ( abs ( r i g h t a x i s u p d o w n v a l ) > 30 && l e f t b a c k b u t t o n d o w n == f a l s e ) {

t i l t = ( r i g h t a x i s u p d o w n v a l / 32768 .0 ∗ 180 / HEAD MOVEMENT SCALING) ;move head= t rue ;

}i f ( abs ( r i g h t a x i s l e f t r i g h t v a l ) > 30 && l e f t b a c k b u t t o n d o w n == f a l s e ) {

pan = −( r i g h t a x i s l e f t r i g h t v a l / 32768 .0 ∗ 180 / HEAD MOVEMENT SCALING) ;move head= t rue ;

}731 i f ( move head ) {

i f ( d e b u g j o y s t i c k ) p r i n t f ( ” Pan : %f , T i l t : %f\n ” , pan , t i l t ) ;Head : : G e t I n s t a n c e ( )−>MoveByAngleOffset ( pan , t i l t ) ;

}i f ( abs ( l e f t a x i s u p d o w n v a l ) > 1000) {

t r a n s l a t e = (− l e f t a x i s u p d o w n v a l / 32768 .0 ∗ t r a n s l a t i o n s c a l i n g ) ;walk= t rue ;

}i f ( abs ( l e f t a x i s l e f t r i g h t v a l ) > 1000) {

r o t a t e = (− l e f t a x i s l e f t r i g h t v a l / 32768 .0 ∗ r o t a t i o n s c a l i n g ) ;741 walk= t rue ;

}

/∗ Only s e t t h e v a l u e s i f we ’ re supposed t o w a l k i n g w i t hnonzero t r a n s l a t i o n or r o t a t i o n ∗ /

i f ( walk && ( t r a n s l a t e | | r o t a t e ) && ! a u t o n o m o u s n a v i g a t i o n ) {i f ( Walking : : G e t I n s t a n c e ( )−>I sRunn ing ( ) == f a l s e ) {

Walking : : G e t I n s t a n c e ( )−>S t a r t ( ) ;}∗x amp = t r a n s l a t e ;

751 ∗a amp = r o t a t e ;/ / c o u t

p r i n t f ( ” Ac t io n f a i l e d \n ” ) ;

781 /∗ Talk , perhaps i n s p a n i s h . ∗ /t e x t t o s p e e c h ( l i n e , s p a n i s h ) ;whi le ( Ac t i on : : G e t I n s t a n c e ( )−>I sRunn ing ( ) ) u s l e e p (8∗1000) ;

/∗ Give c o n t r o l back t o t h e j o y s t i c k . ∗ /m a n u a l c o n t r o l ( ) ;

}

/∗ t a k e s a c−s t y l e s t r i n g i n and p l a y s i t w i t h t h e e s pe ak s y n t h e s i sprogram . f o r k e n a b l e s r e s t o f r o b o t t o s t i l l r e c e i v e commands ∗ /

791 void t e x t t o s p e e c h ( c o n s t char ∗ l i n e , bool s p a n i s h ) {i f ( p l a y i n g a u d i o ) re turn ;char∗ command ;

char∗ e n g l i s h l a n g =( char ∗) ” ” ;char∗ s p a n i s h l a n g =( char ∗) ” −ves ” ;/ / char∗ f e m a l e = ( char ∗)”−ven+f ”;char∗ l a n g u a g e = e n g l i s h l a n g ;i f ( s p a n i s h ) l a n g u a g e = s p a n i s h l a n g ;

i n t p i d = f o r k ( ) ;801 i f ( p i d ==0) {

i n t r e t = a s p r i n t f (&command , ”%s %s%s%s%s ” , ” e s p ea k −s 130 −g 1 ” ,l anguage , ”\” ” , l i n e , ”\” ” ) ;

i f ( r e t == −1) {p r i n t f ( ” a s p r i n t f e r r o r \n ” ) ;e x i t ( 0 ) ;

}p r i n t f ( ” Running : %s\n ” , command ) ;i n t r e t 2 = sys tem ( command ) ;i f ( r e t 2 == −1) p r i n t f ( ” E r r o r r u n n i n g t e x t−to−s pe ec h command\n ” ) ;

811 f r e e ( command ) ;e x i t ( 0 ) ;

}e l s e { p l a y i n g a u d i o = t rue ;}

}

void d i s a b l e a l l c o n t r o l ( ) {821 i n t t i m e r = 1 0 ;

i f ( u n d e r m a n u a l c o n t r o l ) {Walking : : G e t I n s t a n c e ( )−>Stop ( ) ;whi le ( ( Walking : : G e t I n s t a n c e ( )−>I sRunn ing ( ) ) | | ( t i m e r−−)) u s l e e p ( 1 0 0 ) ;u n d e r m a n u a l c o n t r o l = f a l s e ;

}e l s e i f ( u n d e r a c t i o n c o n t r o l ) {

whi le ( Ac t i on : : G e t I n s t a n c e ( )−>I sRunn ing ( ) | | ( t i m e r−−)) u s l e e p ( 1 0 0 ) ;u n d e r a c t i o n c o n t r o l = f a l s e ;

831 }MotionManager : : G e t I n s t a n c e ( )−>S e t E n a b l e ( f a l s e ) ;LinuxMot ionTimer : : S top ( ) ;

}

void g e t u p i f f a l l e n ( ) {i f ( M o t i o n S t a t u s : : FALLEN != STANDUP) {

/∗ change t h i s i f you want ∗ /t e x t t o s p e e c h ( ”Ouch . ” , f a l s e ) ;

841Walking : : G e t I n s t a n c e ( )−>Stop ( ) ;whi le ( Walking : : G e t I n s t a n c e ( )−>I sRunn ing ( ) == 1) u s l e e p ( 8 0 0 0 ) ;Ac t i on : : G e t I n s t a n c e ( )−>m J o i n t . Se tEnableBody ( true , t rue ) ;

i f ( M o t i o n S t a t u s : : FALLEN == FORWARD)Ac t i on : : G e t I n s t a n c e ( )−>S t a r t ( 1 0 ) ; / / FORWARD GETUP

e l s e i f ( M o t i o n S t a t u s : : FALLEN == BACKWARD)Ac t i on : : G e t I n s t a n c e ( )−>S t a r t ( 1 1 ) ; / / BACKWARD GETUP

whi le ( Ac t i on : : G e t I n s t a n c e ( )−>I sRunn ing ( ) == 1) u s l e e p ( 8 0 0 0 ) ;851 Head : : G e t I n s t a n c e ( )−>m J o i n t . Se tEnableHeadOnly ( true , t rue ) ;

Walking : : G e t I n s t a n c e ( )−>m J o i n t . Se tEnableBodyWithoutHead ( true , t rue ) ;Walking : : G e t I n s t a n c e ( )−>L o a d I N I S e t t i n g s ( d e f a u l t i n i ) ;

}}

/∗ c o n v e r t rgb t o f o r m a t t o send t o an LED on r o b o t ∗ /

19

i n t m a k e c o l o r ( f l o a t components [ 3 ] ) {i n t rgb [ 3 ] ;i n t i ;

861 f o r ( i =0 ; i 1) components [ i ] = 1 ;components [ i ] ∗= 3 1 ;rgb [ i ] = ( unsigned char ) components [ i ] ;

}

re turn ( ( rgb [ 2 ] 0 && w a l k i n g f o r w a r d s == f a l s e ) {

p r i n t f ( ” Loading f o r w a r d s s e t t i n g s \n ” ) ;Walking : : G e t I n s t a n c e ( )−>L o a d I N I S e t t i n g s ( f o r w a r d i n i ) ;

901 Walking : : G e t I n s t a n c e ( )−>Stop ( ) ;whi le ( Walking : : G e t I n s t a n c e ( )−>I sRunn ing ( ) == 1) u s l e e p ( 8 0 0 0 ) ;w a l k i n g f o r w a r d s = t rue ;

}e l s e i f ( x amp < 0 && w a l k i n g f o r w a r d s == t rue ) {

p r i n t f ( ” Loading backwards s e t t i n g s \n ” ) ;Walking : : G e t I n s t a n c e ( )−>L o a d I N I S e t t i n g s ( b a c k w a r d i n i ) ;Walking : : G e t I n s t a n c e ( )−>Stop ( ) ;whi le ( Walking : : G e t I n s t a n c e ( )−>I sRunn ing ( ) == 1) u s l e e p ( 8 0 0 0 ) ;w a l k i n g f o r w a r d s = f a l s e ;

911 }

/∗ Move forward X , t u r n A . ∗ /i f ( a amp > 25) a amp = 2 5 ;e l s e i f ( abs ( a amp ) > 25) a amp = −25;

/ / c o u t

}}buf [ numbytes ] = ’\0 ’ ;p r i n t f ( ” c l i e n t : r e c e i v e d ’%s ’\n ” , buf ) ;

941 u s l e e p ( 1 ) ;

}

/∗ a c h i l d i s f o r k e d i n t e x t−to−s pe ec h . once done r e a d i n g t h e t e x t ,s e t g l o b a l v a r i a b l e t o i n d i c a t e we ’ re done p l a y i n g aud io . ∗ /

void h a n d l e r ( i n t signum ) {i f ( signum == SIGCHLD) {

p l a y i n g a u d i o = f a l s e ;}

951 e l s e i f ( signum == SIGINT ) {c l e a n s h u t d o w n ( ) ;

}}

/∗ s e t up a s i g n a l h a n d l e r ∗ /void s e t u p h a n d l e r ( ) {

s i g n a l (SIGCHLD , &h a n d l e r ) ;s i g n a l ( SIGINT , &h a n d l e r ) ;

}961

void angle mod ( f l o a t ∗ b e a r i n g ) {i f (∗ b e a r i n g > p i ) {∗ b e a r i n g −= 2∗ p i ;

}e l s e i f (∗ b e a r i n g < −p i ) {∗ b e a r i n g += 2∗ p i ;

}}

971 void o u t p u t s t a t e ( Mat xp ) {p r i n t f ( ”\n\ t STATE\n\ t−−−−−−−−−\n\ t %.3 f\n\ t %.3 f\n\ t %.3 f\n ” ,

xp . a t(0 ,0) ,xp . a t(0 ,1) ,xp . a t(0 ,2) ∗180/ p i ) ;

}

void o u t p u t t i m e d i f f e r e n c e ( c l o c k t s t a r t ) {c l o c k t d i f f 2 ;d i f f 2 = c l o c k ( ) − s t a r t ;

981 i n t msec = d i f f 2 ∗ 1000 / CLOCKS PER SEC ;p r i n t f ( ” Time d i f f e r e n c e : %d s e c o n d s %d m i l l i s e c o n d s\n ” ,

msec / 1 0 0 0 , msec %1000) ;}

f l o a t c a l c u l a t e t i m e d i f f e r e n c e ( c l o c k t s t a r t ) {c l o c k t d i f f 2 ;d i f f 2 = c l o c k ( ) − s t a r t ;f l o a t s e c = d i f f 2 ∗ 1 . 0 / CLOCKS PER SEC ;re turn s e c ;

991 }

s t r i n g r e a d f r o m a p r i l t a g ( Mat th , i n t ∗ t ag number , bool ∗ r e a d t a g h o m o g r a p h y , bool ∗ r e a d y t o s e n d i m a g e ) {/∗we haven ’ t read t h e homography y e t ∗ /∗ r e a d t a g h o m o g r a p h y = f a l s e ;s t r i n g t a g n u m b e r s t r i n g ;char t a g b u f [ 5 1 2 ] ;char r e s p o n s e b u f [ 1 2 8 ] ;i n t num read = 0 ;i n t num read 2 = 0 ;

1001/∗ s t u f f s o c k e t da ta i n t o b u f f e r ∗ /i f ( ( num read = r e c v ( t a g s o c k e t f d , t a g b u f , 512 , 0 ) = 0 && i d s t a r t i d x < 20) {

s t r i n g s t r e a m s s ;i d s t a r t i d x += t a g s e a r c h . l e n g t h ( ) ;i d s t r i n g = t a g s t r i n g . s u b s t r ( i d s t a r t i d x , i d e n d i d x−i d s t a r t i d x ) ;

21

t a g n u m b e r s t r i n g = ” t a g ”+ i d s t r i n g ;∗ t ag number = a t o i ( i d s t r i n g . c s t r ( ) ) ;i f ( d e b u g t a g ) p r i n t f ( ”We saw t a g %d\n ” ,∗ t ag number ) ;s s > t h . a t(0 ,0) >> t h . a t(0 ,1) >> t h . a t(0 ,2) >> t h . a t(0 ,3) ;

1021 s s >> t h . a t(1 ,0) >> t h . a t(1 ,1) >> t h . a t(1 ,2) >> t h . a t(1 ,3) ;s s >> t h . a t(2 ,0) >> t h . a t(2 ,1) >> t h . a t(2 ,2) >> t h . a t(2 ,3) ;s s >> t h . a t(3 ,0) >> t h . a t(3 ,1) >> t h . a t(3 ,2) >> t h . a t(3 ,3) ;∗ r e a d t a g h o m o g r a p h y = t rue ;

}}/∗ t h e j a v a s p i t s messages back when i t s ready f o r a new image ∗ /i f ( ( num read 2 = r e c v ( r e s p o n s e s o c k e t f d , r e s p o n s e b u f , 128 , 0 ) ) 0) {

1031 ∗ r e a d y t o s e n d i m a g e = t rue ;}re turn t a g n u m b e r s t r i n g ;

}

void s e n d i m a g e t o a p r i l t a g ( Mat o r i g i m a g e ) {Mat s o c k e t c l o n e d i m a g e = o r i g i m a g e . c l o n e ( ) ;

S i z e dims = s o c k e t c l o n e d i m a g e . s i z e ( ) ;i n t e l e m e n t s i z e = s o c k e t c l o n e d i m a g e . e l e m S i z e ( ) ;

1041 s i z e t b y t e s t o w r i t e = ( dims . wid th∗dims . h e i g h t )∗ e l e m e n t s i z e ∗3;

/∗ w r i t e t h e image on one s o c k e t ( which t a k e s s e v e r a l ” read ” c a l l s t o compose t h e∗ image da ta s t r u c t u r e i n j a v a ) , and communicate t h e number o f b y t e s t h e a p r i l t a g∗ s h o u l d e x p e c t i n a n o t h e r ∗ /

i n t b y t e s w r i t t e n = w r i t e ( ( t a g s o c k e t f d ) , s o c k e t c l o n e d i m a g e . da t a , b y t e s t o w r i t e ) ;i n t s o c k e t i z e d = h t o n l ( b y t e s w r i t t e n ) ;i n t r e t = w r i t e ( r e s p o n s e s o c k e t f d ,& s o c k e t i z e d , s i z e o f ( s o c k e t i z e d ) ) ;i f ( r e t < 0) c e r r I sRunn ing ( ) == t rue ) {

1071 /∗ odometry ∗ /u . a t(0 ,0) = (∗ x amp )∗m a x t r a n s l a t i o n i n c h e s i t e r a t i o n / t r a n s l a t i o n s c a l i n g ;u . a t(1 ,0) = (∗ a amp )∗m a x r o t a t i o n r a d s i t e r a t i o n / r o t a t i o n s c a l i n g ;

}/∗ Otherwise , no movement ∗ /e l s e i f ( ! a u t o n o m o u s n a v i g a t i o n ) {

u . a t(0 ,0) = 0 ;u . a t(1 ,0) = 0 ;

}/∗ N a v i g a t e t o t h e goa l l o c a t i o n i f s p e c i f i e d ∗ /

1081 e l s e i f ( c o u n t > 50 && a u t o n o m o u s n a v i g a t i o n ) {a u t o n o m o u s c o n t r o l l e r ( xp o ld , goa l , u , x amp , a amp , c o l l i s i o n w a r n ) ;

}}

Mat a c q u i r e i m a g e ( I p l I m a g e ∗cv image ) {LinuxCamera : : G e t I n s t a n c e ( )−>CaptureFrame ( ) ;cv image−>imageData = ( char ∗) ( LinuxCamera : : G e t I n s t a n c e ( )−>f b u f f e r−>m RGBFrame−>m ImageData ) ;Mat o r i g i m a g e ( cv image ) ;re turn o r i g i m a g e ;

1091 }

f l o a t q u i c k s g n ( f l o a t a n g l e ) {

22

re turn ( a n g l e < 0) ? −1 : ( a n g l e > 0) ;}

void u n r e c o g n i z e d l o c a t i o n h a l t ( s t r i n g t a g n u m b e r s t r i n g ) {c e r r

/ / u s i n g namespace s t d ;/ / u s i n g namespace cv ;/ / namespace po = b o o s t : : p r o g r a m o p t i o n s ;

6 /∗ S o c k e t da ta ∗ /s t r u c t a d d r i n f o h i n t s , ∗ s e r v i n f o , ∗p ;i n t rv ;char s [ INET6 ADDRSTRLEN ] ;

void c h a n g e c u r r e n t d i r ( ) {char e x e p a t h [ 1 0 2 4 ] = {0} ;i f ( r e a d l i n k ( ” / p roc / s e l f / exe ” , exepa th , s i z e o f ( e x e p a t h ) ) != −1) {

i f ( c h d i r ( d i rname ( e x e p a t h ) ) != 0 ) p r i n t f ( ” E r r o r c h a n g i n g d i r e c t o r y t o : %s ” ,e x e p a t h ) ;

16 e l s e p r i n t f ( ” Changed i n t o %s\n ” , e x e p a t h ) ;}

}

/∗∗∗∗∗∗∗∗∗∗∗∗∗ I n i t i a l i z a t i o n F u n c t i o n s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ /void p r o c e s s i n p u t ( i n t argc , char ∗∗argv , po : : v a r i a b l e s m a p ∗vm) {

s t r i n g o p t i o n s f i l e = ” c o n f i g m a s t e r . c f g ” ;po : : o p t i o n s d e s c r i p t i o n desc ( ” Allowed o p t i o n s ” ) ;i f s t r e a m c o n f i g i n p u t s t r e a m ;

26 c o n f i g i n p u t s t r e a m . open ( o p t i o n s f i l e . c s t r ( ) , i o s : : i n ) ;

/∗ I f you ’ re add ing g l o b a l v a r i a b l e s , make s u r e t h e y are p r o c e s s e d somewhere∗ e l s e ! T h i s j u s t l o a d s t h e c o n f i g o p t i o n s i n t o t h e v a r i a b l e map ∗ /

t r y {desc . a d d o p t i o n s ( )

( ” u s e g o a l q u e u e ” , po : : va lue() , ” a r e we u s i n g a queue o f g o a l s ” )( ” g o a l q u e u e u n p a r s e d ” , po : : va lue() , ” queue of g o a l l o c a t i o n s ” )( ” p l o t f i l e n a m e ” , po : : va lue() , ” w r i t e d a t a h e r e ” )

36 ( ” o u t p u t b e a c o n d a t a ” , po : : va lue()−>d e f a u l t v a l u e ( f a l s e ) , ” d i s p l a y beacon d i s t / o r i e n t a t i o n ” )( ” a u t o n o m o u s n a v i g a t i o n ” , po : : va lue()−>d e f a u l t v a l u e ( f a l s e ) , ” s h o u l d n a v i g a t e au tonomous ly ” )( ” u s e c o l o r b e a c o n s ” , po : : va lue()−>d e f a u l t v a l u e ( f a l s e ) , ” debug ? ” )( ” d e b u g j o y s t i c k ” , po : : va lue()−>d e f a u l t v a l u e ( f a l s e ) , ” debug ? ” )( ” d e b u g t a g ” , po : : va lue()−>d e f a u l t v a l u e ( f a l s e ) , ” debug ? ” )( ” d e b u g c o n t r o l l e r ” , po : : va lue()−>d e f a u l t v a l u e ( f a l s e ) , ” debug ? ” )( ” t a g s o c k e t o n ” , po : : va lue()−>d e f a u l t v a l u e ( t rue ) , ” t a g s o c k e t ” )( ” r e m o t e p r o c e s s i n g ” , po : : va lue()−>d e f a u l t v a l u e ( f a l s e ) , ” debug ? ” )( ” w e b f i n d e r ” , po : : va lue()−>d e f a u l t v a l u e ( ” g r e e n ” ) , ”web f i n d e r ” )( ” o r a n g e d a y ” , po : : va lue() , ” o r a ng e f i n d e r ” )

46 ( ” g r e e n d a y ” , po : : va lue() , ” o r a ng e f i n d e r ” )( ” y e l l o w d a y ” , po : : va lue() , ” o r a ng e f i n d e r ” )( ” o r a n g e n i g h t ” , po : : va lue() , ” o r a ng e f i n d e r ” )( ” g r e e n n i g h t ” , po : : va lue() , ” o r a ng e f i n d e r ” )( ” y e l l o w n i g h t ” , po : : va lue() , ” o r a ng e f i n d e r ” )( ” o u t p u t t e r m i n a l s t a t e ” , po : : va lue()−>d e f a u l t v a l u e ( t rue ) , ” o u t p u t ” )( ” p y t h o n p l o t t i n g ” , po : : va lue()−>d e f a u l t v a l u e ( t rue ) , ” 2d p l o t t i n g ” )( ” p y t h o n p l o t t i n g e x e c u t a b l e ” , po : : va lue()−>d e f a u l t v a l u e ( ” p l o t t e r . py ” ) , ”name of . py exe ” )( ” o r a n g e c o v ” , po : : va lue() , ” b e a c o n c o v a r i a n c e s ” )( ” g r e e n c o v ” , po : : va lue() , ” b e a c o n c o v a r i a n c e s ” )

56 ( ” y e l l o w c o v ” , po : : va lue() , ” b e a c o n c o v a r i a n c e s ” )( ” o r a n g e l o c ” , po : : va lue() , ” b e a c o n l o c a t i o n ” )( ” g r e e n l o c ” , po : : va lue() , ” b e a c o n l o c a t i o n ” )( ” y e l l o w l o c ” , po : : va lue() , ” b e a c o n l o c a t i o n ” )( ” i n i t i a l s t a t e ” , po : : va lue() , ” i n i t i a l s t a t e ( xp ) ” )( ” c o v a r i a n c e m a t r i x ” , po : : va lue() , ” c o v a r i a n c e o f x , y , t h ” )( ” i n i t i a l g o a l ” , po : : va lue() , ” i n i t i a l l o c a t i o n o f g o a l ” )( ” t a g 0 l o c ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )( ” t a g 1 l o c ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )( ” t a g 2 l o c ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )

66 ( ” t a g 3 l o c ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )( ” t a g 4 l o c ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )( ” t a g 5 l o c ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )( ” t a g 6 l o c ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )( ” t a g 7 l o c ” , po : : va lue() , ” t a g 7 l o c a t i o n ” )( ” t a g 8 l o c ” , po : : va lue() , ” t a g 7 l o c a t i o n ” )( ” t a g 9 l o c ” , po : : va lue() , ” t a g 7 l o c a t i o n ” )( ” t a g 1 0 l o c ” , po : : va lue() , ” t a g 7 l o c a t i o n ” )( ” t a g 1 0 0 l o c ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )( ” t a g 0 c o v ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )

76 ( ” t a g 1 c o v ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )( ” t a g 2 c o v ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )( ” t a g 3 c o v ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )( ” t a g 4 c o v ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )( ” t a g 5 c o v ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )

24

( ” t a g 6 c o v ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )( ” t a g 7 c o v ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )( ” t a g 8 c o v ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )( ” t a g 9 c o v ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )( ” t a g 1 0 c o v ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )

86 ( ” t a g 1 0 0 c o v ” , po : : va lue() , ” t a g 0 l o c a t i o n ” )( ” b e a c o n c o l l i s i o n a d v i s e d i s t ” , po : : va lue() , ” watch o u t ! ” )( ” v e l o c i t y t o c o n t r o l v e l o c i t y ” , po : : va lue() , ” mapping ” )( ” t u r n t o c o n t r o l t u r n ” , po : : va lue() , ” mapping ” );

i f ( ! c o n f i g i n p u t s t r e a m ) {f p r i n t f ( s t d e r r , ” Couldn ’ t open c o n f i g f i l e %s ” , o p t i o n s f i l e . c s t r ( ) ) ;

}e l s e {

96 s t o r e ( p a r s e c o n f i g f i l e ( c o n f i g i n p u t s t r e a m , desc , t rue ) ,∗vm) ;n o t i f y (∗vm) ;

}}catch ( i n t e r r o r ) { p e r r o r ( ”EXCEPTION : ” ) ;} ;

}

void s e t u p p l o t t e r ( s t r i n g f i l e n a m e , f s t r e a m ∗ p l o t w r i t i n g s t r e a m ) {i f s t r e a m i f i l e ( f i l e n a m e . c s t r ( ) ) ;i f ( i f i l e ) remove ( f i l e n a m e . c s t r ( ) ) ;

106 p l o t w r i t i n g s t r e a m−>open ( f i l e n a m e . c s t r ( ) , f s t r e a m : : o u t ) ;}

/ / g e t sockaddr , IPv4 or IPv6 :void ∗ g e t i n a d d r ( s t r u c t s o c k a d d r ∗ sa ){

i f ( sa−>s a f a m i l y == AF INET ) {re turn &(( ( s t r u c t s o c k a d d r i n ∗) s a )−>s i n a d d r ) ;

}

116 re turn &(( ( s t r u c t s o c k a d d r i n 6 ∗) s a )−>s i n 6 a d d r ) ;}

void a d d m o t i o n m o d u l e s ( ) {MotionManager : : G e t I n s t a n c e ( )−>AddModule ( ( MotionModule ∗) A c t i o n : : G e t I n s t a n c e ( ) ) ;MotionManager : : G e t I n s t a n c e ( )−>AddModule ( ( MotionModule ∗) Head : : G e t I n s t a n c e ( ) ) ;MotionManager : : G e t I n s t a n c e ( )−>

AddModule ( ( MotionModule ∗) Walking : : G e t I n s t a n c e ( ) ) ;/ / hmm . .MotionManager : : G e t I n s t a n c e ( )−>AddModule ( ( MotionModule ∗) Body : : G e t I n s t a n c e ( ) ) ;

126 }

void a c t i o n i n i t ( ) {

/∗ c u r r e n t v e r s i o n from p r e v i o u s d e t e c t i o n ∗ /i n t f i r m v e r = 2 9 ;

/∗ mot ion f i l e i s a b i n a r y o f p r e l o a d e d a c t i o n s . ∗ /136 i f ( ( Ac t i on : : G e t I n s t a n c e ( )−>L o a d F i l e ( ( char ∗)MOTION FILE PATH ) ) == f a l s e )

p r i n t f ( ” E r r o r l o a d i n g mot ion f i l e .\ n ” ) ;

e l s e p r i n t f ( ” Loaded Motion f i l e : %s , f i r m w a r e v e r s i o n : %d\n ” ,MOTION FILE PATH , f i r m v e r ) ;

}

void c r e a t e j o i n t i n i t i a l i z a t i o n d a t a ( i n t param [ ] ) {i n t n = 0 ;

146 i n t wGoa lPos i t i on , w S t a r t P o s i t i o n , wDis tance ;

/ / I n i t i a l i z e s a l l o f t h e j o i n t si n t i d = J o i n t D a t a : : ID R SHOULDER PITCH ;f o r ( ; idm J o i n t . GetValue ( i d ) ;i f ( w S t a r t P o s i t i o n > w G o a l P o s i t i o n )

wDis tance = w S t a r t P o s i t i o n − w G o a l P o s i t i o n ;156 e l s e

wDis tance = w G o a l P o s i t i o n − w S t a r t P o s i t i o n ;

wDis tance >>= 2 ;

25

i f ( wDis tance < 8 )wDis tance = 8 ;

param [ n ++] = i d ;param [ n ++] = CM730 : : GetLowByte ( w G o a l P o s i t i o n ) ;param [ n ++] = CM730 : : GetHighByte ( w G o a l P o s i t i o n ) ;

166 param [ n ++] = CM730 : : GetLowByte ( wDis tance ) ;param [ n ++] = CM730 : : GetHighByte ( wDis tance ) ;

}}

void j o i n t s i n i t ( i n t param [ ] , CM730 ∗cm730 ) {cm730−>SyncWri te (MX28 : : P GOAL POSITION L ,

5 ,J o i n t D a t a : : NUMBER OF JOINTS − 1 ,param ) ;

176 p r i n t f ( ” I n i t i a l i z e d a l l j o i n t s .\ n ” ) ;}

void s d l i n t e r f a c e s i n i t ( ) {SDL Ini tSubSys tem ( SDL INIT JOYSTICK ) ;S D L J o y s t i c k E v e n t S t a t e (SDL ENABLE) ;

}

void c a m e r a i n i t ( m i n I n i ∗ i n i ) {LinuxCamera : : G e t I n s t a n c e ( )−> I n i t i a l i z e ( 0 ) ;

186 LinuxCamera : : G e t I n s t a n c e ( )−>L o a d I N I S e t t i n g s ( i n i ) ;}

void m o t i o n m a n a g e r i n i t ( m i n I n i ∗ i n i , CM730 ∗cm730 ) {i f ( MotionManager : : G e t I n s t a n c e ( )−> I n i t i a l i z e ( cm730 ) == f a l s e ) {

p r i n t f ( ” F a i l t o i n i t i a l i z e Motion Manager !\n ” ) ;e x i t ( 1 ) ;

}LinuxMot ionTimer : : I n i t i a l i z e ( MotionManager : : G e t I n s t a n c e ( ) ) ;MotionManager : : G e t I n s t a n c e ( )−>L o a d I N I S e t t i n g s ( i n i ) ;

196 }

/∗ S e t up web s t r e a m e r t o c o n f i g u r e t h e c o l o r f i n d e r ∗ /void c o l o r f i n d e r i n i t ( m i n I n i ∗ i n i , C o l o r F i n d e r ∗ b a l l f i n d e r ){

h t t p d : : b a l l f i n d e r = b a l l f i n d e r ;}

/∗v o i d s o c k e t s e t u p ( char ∗hos t , i n t ∗ s o c k f d , char ∗ p o r t ) {

206 i n t n , p o r t n o ;s t r u c t s o c k a d d r i n s e r v a d d r ;s t r u c t h o s t e n t ∗ s e r v e r ;p o r t n o = a t o i ( p o r t ) ;∗ s o c k f d = s o c k e t ( AF INET , SOCK STREAM , 0) ;i f (∗ s o c k f d < 0) p e r r o r (” Error open ing s o c k e t ”) ;s e r v e r = g e t h o s t b y n a m e ( h o s t ) ;i f ( s e r v e r == NULL) { p e r r o r (” Error , no such h o s t\n ”) ;}b z e r o ( ( char ∗) &s e r v a d d r , s i z e o f ( s e r v a d d r ) ) ;s e r v a d d r . s i n f a m i l y = AF INET ;

216 bcopy ( ( char ∗) s e r v e r−>h addr ,( char ∗)&s e r v a d d r . s i n a d d r . s addr ,s e r v e r−>h l e n g t h ) ;

s e r v a d d r . s i n p o r t = h t o n s ( p o r t n o ) ;i f ( c o n n e c t (∗ s o c k f d , ( s t r u c t s o c k a d d r ∗) &s e r v a d d r , s i z e o f ( s e r v a d d r ) ) < 0)

p e r r o r (” Error c o n n e c t i n g ”) ;}∗ /

void s o c k e t s e t u p ( char ∗arg , i n t ∗ sockfd , char ∗ p o r t ) {226 memset(& h i n t s , 0 , s i z e o f h i n t s ) ;

h i n t s . a i f a m i l y = PF UNSPEC ;h i n t s . a i s o c k t y p e = SOCK STREAM;

i f ( ( rv = g e t a d d r i n f o ( arg , p o r t , &h i n t s , &s e r v i n f o ) ) != 0 ) {

f p r i n t f ( s t d e r r , ” g e t a d d r i n f o : %s\n ” , g a i s t r e r r o r ( rv ) ) ;e x i t ( 1 ) ;

}

236 f o r ( p = s e r v i n f o ; p != NULL; p = p−>a i n e x t ) {i f ( (∗ s o c k f d = s o c k e t ( p−>a i f a m i l y , p−>a i s o c k t y p e ,

p−>a i p r o t o c o l ) ) == −1) {

26

p e r r o r ( ” C l i e n t : s o c k e t ” ) ;c o n t i nu e ;

}i f ( c o n n e c t (∗ sockfd , p−>a i a d d r , p−>a i a d d r l e n ) == −1) {

/ / c l o s e (∗ s o c k f d ) ;p e r r o r ( ” C l i e n t : c o n n e c t ” ) ;/ / c o u t a i a d d r ) ,

s ,s i z e o f s ) ;

s t r u c t t i m e v a l t v ;t v . t v s e c = 1 ;s e t s o c k o p t (∗ sockfd , SOL SOCKET , SO RCVTIMEO , ( s t r u c t t i m e v a l ∗)&tv , s i z e o f ( s t r u c t t i m e v a l ) ) ;p r i n t f ( ” C l i e n t : c o n n e c t e d t o %s\n ” , s ) ;f r e e a d d r i n f o ( s e r v i n f o ) ;

}

266

void c l e a n s h u t d o w n ( ) {s t o p w a l k i n g ( ) ;

MotionManager : : G e t I n s t a n c e ( )−>RemoveModule ( ( MotionModule ∗) Head : : G e t I n s t a n c e ( ) ) ;MotionManager : : G e t I n s t a n c e ( )−>RemoveModule ( ( MotionModule ∗) Walking : : G e t I n s t a n c e ( ) ) ;

SDL Quit ( ) ;k i l l ( 0 ,SIGTERM) ;

276 e x i t ( 0 ) ;}

void s e t u p a n d s t a r t p l o t t e r ( ) {s e t u p p l o t t e r ( p l o t f i l e n a m e ,& p l o t w r i t i n g s t r e a m ) ;s t a r t p l o t t e r ( p y t h o n p l o t t i n g e x e c u t a b l e , p l o t f i l e n a m e ) ;

}

void s t a r t p l o t t e r ( s t r i n g p y t h o n f i l e n a m e , s t r i n g d a t a f i l e n a m e ) {i n t p i d = f o r k ( ) ;

286 i f ( p i d ==0) {char ∗command ;a s p r i n t f (&command , ” py thon %s %s ” , p y t h o n f i l e n a m e . c s t r ( ) , d a t a f i l e n a m e . c s t r ( ) ) ;i n t r e t 2 = sys tem ( command ) ;i f ( r e t 2 != 0) c e r r

y e l l o w p a r a m s = vm[ ” y e l l o w n i g h t ” ] . as() ;o r a n g e f i n d e r = new C o l o r F i n d e r ( expand vec6 ( o r a n g e p a r a m s ) ) ;g r e e n f i n d e r = new C o l o r F i n d e r ( expand vec6 ( g r e e n p a r a m s ) ) ;y e l l o w f i n d e r = new C o l o r F i n d e r ( expand vec6 ( y e l l o w p a r a m s ) ) ;

}e l s e {

v e c t o r orange pa rams , g reen pa rams , y e l l o w p a r a m s ;o r a n g e p a r a m s = vm[ ” o r a n g e d a y ” ] . as() ;

326 g r e e n p a r a m s = vm[ ” g r e e n d a y ” ] . as() ;y e l l o w p a r a m s = vm[ ” y e l l o w d a y ” ] . as() ;o r a n g e f i n d e r = new C o l o r F i n d e r ( expand vec6 ( o r a n g e p a r a m s ) ) ;g r e e n f i n d e r = new C o l o r F i n d e r ( expand vec6 ( g r e e n p a r a m s ) ) ;y e l l o w f i n d e r = new C o l o r F i n d e r ( expand vec6 ( y e l l o w p a r a m s ) ) ;

}v e c t o r s o l i d y e l l o w , s o l i d g r e e n , s o l i d o r a n g e ;s o l i d y e l l o w . p u s h b a c k ( y e l l o w f i n d e r ) ;s o l i d y e l l o w . p u s h b a c k ( y e l l o w f i n d e r ) ;s o l i d g r e e n . p u s h b a c k ( g r e e n f i n d e r ) ;

336 s o l i d g r e e n . p u s h b a c k ( g r e e n f i n d e r ) ;s o l i d o r a n g e . p u s h b a c k ( o r a n g e f i n d e r ) ;s o l i d o r a n g e . p u s h b a c k ( o r a n g e f i n d e r ) ;

p a i r y e l l o w p a i r ( s o l i d y e l l o w , ” ye l l ow ” ) ;p a i r g r e e n p a i r ( s o l i d g r e e n , ” g r e e n ” ) ;p a i r o r a n g e p a i r ( s o l i d o r a n g e , ” o r an g e ” ) ;a l l c o l o r f i n d e r s . p u s h b a c k ( y e l l o w p a i r ) ;a l l c o l o r f i n d e r s . p u s h b a c k ( g r e e n p a i r ) ;a l l c o l o r f i n d e r s . p u s h b a c k ( o r a n g e p a i r ) ;

346 }

void s e t u p w e b s t r e a m e r ( ) {s t r e a m e r = new m j p g s t r e a m e r ( Camera : : WIDTH, Camera : : HEIGHT) ;

/∗ t h e c o l o r f i n d e r t h a t has i t s r u n t i m e s e t t i n g s m o d i f i a b l e v i a h t t p i n t e r f a c e ∗ /i f ( f i n d e r p r e f e r e n c e == ” g r e e n ” ) c o l o r f i n d e r i n i t ( i n i , g r e e n f i n d e r ) ;e l s e i f ( f i n d e r p r e f e r e n c e == ” ye l l o w ” ) c o l o r f i n d e r i n i t ( i n i , y e l l o w f i n d e r ) ;e l s e i f ( f i n d e r p r e f e r e n c e == ” o r a ng e ” ) c o l o r f i n d e r i n i t ( i n i , o r a n g e f i n d e r ) ;e l s e c o l o r f i n d e r i n i t ( i n i , g r e e n f i n d e r ) ;

356 }

void s e t u p j o y s t i c k i f c o n n e c t e d ( ) {t r y{

J o y s t i c k ∗ j = new J o y s t i c k ( 0 ) ;j o y s t i c k c o n n e c t e d = t rue ;j o y = ∗ j ;s d l i n t e r f a c e s i n i t ( ) ;j o y . u p d a t e ( ) ;p r i n t f ( ” I n i t i a l i z e d J o y s t i c k .\ n ” ) ;

366 }catch ( s t d : : r u n t i m e e r r o r ) {

j o y s t i c k c o n n e c t e d = f a l s e ;c o u t MoveByAngle ( 0 , 4 5 ) ;s t o p w a l k i n g ( ) ;

}

/∗ d oub l e s o c k e t s e t u p ∗ /void c o n n e c t t o a p r i l t a g ( ) {

char ∗ l o c a l a d d r = ( char ∗) ” l o c a l h o s t ” ;396 char ∗ e a s y p o r t = ( char ∗) ” 9090 ” ;

28

char ∗ r e s p o n s e p o r t = ( char ∗) ” 9091 ” ;c e r r m J o i n t . Se tEnableHeadOnly ( true , t rue ) ;Walking : : G e t I n s t a n c e ( )−>m J o i n t . Se tEnableBodyWithoutHead ( true , t rue ) ;MotionManager : : G e t I n s t a n c e ( )−>S e t E n a b l e ( t rue ) ;u n d e r m a n u a l c o n t r o l = t rue ;

416 u n d e r a c t i o n c o n t r o l = f a l s e ;LinuxMot ionTimer : : S t a r t ( ) ;

}

void a c t i o n c o n t r o l ( ) {A ct i on : : G e t I n s t a n c e ( )−>m J o i n t . Se tEnableBody ( true , t rue ) ;MotionManager : : G e t I n s t a n c e ( )−>S e t E n a b l e ( t rue ) ;LinuxMot ionTimer : : S t a r t ( ) ;u n d e r a c t i o n c o n t r o l = t rue ;u n d e r m a n u a l c o n t r o l = f a l s e ;

426 }

void e x i t w i t h e r r o r ( s t r i n g e r r o r ){c e r r

476 }

/∗ s e t some ( n o t a l l ) g l o b a l v a r i a b l e s ∗ /void s e t g l o b a l v a r i a b l e s ( po : : v a r i a b l e s m a p ∗vm) {

t a g s o c k e t o n = (∗vm) [ ” t a g s o c k e t o n ” ] . as() ;p y t h o n p l o t t i n g = (∗vm) [ ” p y t h o n p l o t t i n g ” ] . as() ;r e m o t e p r o c e s s i n g = (∗vm) [ ” r e m o t e p r o c e s s i n g ” ] . as() ;p l o t f i l e n a m e = (∗vm) [ ” p l o t f i l e n a m e ” ] . as() ;p y t h o n p l o t t i n g e x e c u t a b l e = (∗vm) [ ” p y t h o n p l o t t i n g e x e c u t a b l e ” ] . as() ;o u t p u t b e a c o n d a t a = (∗vm) [ ” o u t p u t b e a c o n d a t a ” ] . as() ;

486 o u t p u t t e r m i n a l s t a t e = (∗vm) [ ” o u t p u t t e r m i n a l s t a t e ” ] . as() ;a u t o n o m o u s n a v i g a t i o n = (∗vm) [ ” a u t o n o m o u s n a v i g a t i o n ” ] . as() ;u s e g o a l q u e u e = (∗vm) [ ” u s e g o a l q u e u e ” ] . as() ;d e b u g j o y s t i c k = (∗vm) [ ” d e b u g j o y s t i c k ” ] . as() ;d e b u g t a g = (∗vm) [ ” d e b u g t a g ” ] . as() ;d e b u g c o n t r o l l e r = (∗vm) [ ” d e b u g c o n t r o l l e r ” ] . as() ;u s e c o l o r b e a c o n s = (∗vm) [ ” u s e c o l o r b e a c o n s ” ] . as() ;f i n d e r p r e f e r e n c e = (∗vm) [ ” w e b f i n d e r ” ] . as() ;b e a c o n c o l l i s i o n a d v i s e d i s t = (∗vm) [ ” b e a c o n c o l l i s i o n a d v i s e d i s t ” ] . as() ;g o a l [ 0 ] = (∗vm) [ ” i n i t i a l g o a l ” ] . as() [ 0 ] ;

496 g o a l [ 1 ] = (∗vm) [ ” i n i t i a l g o a l ” ] . as() [ 1 ] ;g o a l q u e u e u n p a r s e d = (∗vm) [ ” g o a l q u e u e u n p a r s e d ” ] . as() ;v e l o c i t y t o c o n t r o l v e l o c i t y = (∗vm) [ ” v e l o c i t y t o c o n t r o l v e l o c i t y ” ] . as() ;t u r n t o c o n t r o l t u r n = (∗vm) [ ” t u r n t o c o n t r o l t u r n ” ] . as() ;

}

void p a r s e a n d b u i l d g o a l q u e u e ( ) {v e c t o r c u r g o a l ;c u r g o a l . p u s h b a c k ( g o a l [ 0 ] ) ;c u r g o a l . p u s h b a c k ( g o a l [ 1 ] ) ;

506 g o a l q u e u e . push ( c u r g o a l ) ;f o r ( unsigned i n t i =0 ; i < g o a l q u e u e u n p a r s e d . s i z e ( ) ; i ++) {

v e c t o r n e x t g o a l ;n e x t g o a l . p u s h b a c k ( g o a l q u e u e u n p a r s e d [ i ∗2 ] ) ;n e x t g o a l . p u s h b a c k ( g o a l q u e u e u n p a r s e d [ i ∗2+1]) ;

/ / f o r some re as on t h e vm v e c t o r has a bunch o f e x t r a t h i n g s t h a t/ / are r i d i c u l o u s l y s m a l l (1 e ˆ−33)i f ( abs ( n e x t g o a l [ 0 ] ) < . 00000001 && n e x t g o a l [ 0 ] != 0 ) break ;g o a l q u e u e . push ( n e x t g o a l ) ;

516 / / c o u t

p a i r t a g 6 l o c ( expand vec2 (vm[ ” t a g 6 l o c ” ] . as() ) ) ;556 p a i r t a g 7 l o c ( expand vec2 (vm[ ” t a g 7 l o c ” ] . as() ) ) ;

p a i r t a g 8 l o c ( expand vec2 (vm[ ” t a g 8 l o c ” ] . as() ) ) ;p a i r t a g 9 l o c ( expand vec2 (vm[ ” t a g 9 l o c ” ] . as() ) ) ;p a i r t a g 1 0 l o c ( expand vec2 (vm[ ” t a g 1 0 l o c ” ] . as() ) ) ;p a i r t a g 1 0 0 l o c ( expand vec2 (vm[ ” t a g 1 0 0 l o c ” ] . as() ) ) ;

(∗ b e a c o n c o v a r i a n c e s ) [ ” o r an g e ” ] = o r a n g e c o v ;(∗ b e a c o n c o v a r i a n c e s ) [ ” g r e e n ” ] = g r e e n c o v ;(∗ b e a c o n c o v a r i a n c e s ) [ ” ye l l o w ” ] = y e l l o w c o v ;(∗ b e a c o n c o v a r i a n c e s ) [ ” t a g 0 ” ] = t a g 0 c o v ;

566 (∗ b e a c o n c o v a r i a n c e s ) [ ” t a g 1 ” ] = t a g 1 c o v ;(∗ b e a c o n c o v a r i a n c e s ) [ ” t a g 2 ” ] = t a g 2 c o v ;(∗ b e a c o n c o v a r i a n c e s ) [ ” t a g 3 ” ] = t a g 3 c o v ;(∗ b e a c o n c o v a r i a n c e s ) [ ” t a g 4 ” ] = t a g 4 c o v ;(∗ b e a c o n c o v a r i a n c e s ) [ ” t a g 5 ” ] = t a g 5 c o v ;(∗ b e a c o n c o v a r i a n c e s ) [ ” t a g 6 ” ] = t a g 6 c o v ;(∗ b e a c o n c o v a r i a n c e s ) [ ” t a g 7 ” ] = t a g 7 c o v ;(∗ b e a c o n c o v a r i a n c e s ) [ ” t a g 8 ” ] = t a g 8 c o v ;(∗ b e a c o n c o v a r i a n c e s ) [ ” t a g 9 ” ] = t a g 9 c o v ;(∗ b e a c o n c o v a r i a n c e s ) [ ” t a g 1 0 ” ] = t a g 1 0 c o v ;

576 (∗ b e a c o n c o v a r i a n c e s ) [ ” t a g 1 0 0 ” ] = t a g 1 0 0 c o v ;

(∗ t r u e b e a c o n l o c a t i o n s ) [ ” ye l l o w ” ] = y e l l o w l o c ;(∗ t r u e b e a c o n l o c a t i o n s ) [ ” g r e e n ” ] = g r e e n l o c ;(∗ t r u e b e a c o n l o c a t i o n s ) [ ” o r a ng e ” ] = o r a n g e l o c ;(∗ t r u e b e a c o n l o c a t i o n s ) [ ” t a g 0 ” ]= t a g 0 l o c ;(∗ t r u e b e a c o n l o c a t i o n s ) [ ” t a g 1 ” ]= t a g 1 l o c ;(∗ t r u e b e a c o n l o c a t i o n s ) [ ” t a g 2 ” ]= t a g 2 l o c ;(∗ t r u e b e a c o n l o c a t i o n s ) [ ” t a g 3 ” ]= t a g 3 l o c ;(∗ t r u e b e a c o n l o c a t i o n s ) [ ” t a g 4 ” ]= t a g 4 l o c ;

586 (∗ t r u e b e a c o n l o c a t i o n s ) [ ” t a g 5 ” ]= t a g 5 l o c ;(∗ t r u e b e a c o n l o c a t i o n s ) [ ” t a g 6 ” ]= t a g 6 l o c ;(∗ t r u e b e a c o n l o c a t i o n s ) [ ” t a g 7 ” ]= t a g 7 l o c ;(∗ t r u e b e a c o n l o c a t i o n s ) [ ” t a g 8 ” ]= t a g 8 l o c ;(∗ t r u e b e a c o n l o c a t i o n s ) [ ” t a g 9 ” ]= t a g 9 l o c ;(∗ t r u e b e a c o n l o c a t i o n s ) [ ” t a g 1 0 ” ]= t a g 1 0 l o c ;(∗ t r u e b e a c o n l o c a t i o n s ) [ ” t a g 1 0 0 ” ]= t a g 1 0 0 l o c ;

}

Initialization.cpp

6.3. GlobalVariables.h.# i f n d e f GLOBAL VARIABLES H# d e f i n e GLOBAL VARIABLES H

us ing namespace cv ;us ing namespace s t d ;

6 namespace po = b o o s t : : p r o g r a m o p t i o n s ;

c l o c k t s t a r t ;c l o c k t d i f f ;f l o a t msec ;

# d e f i n e expand vec9 ( a ) a [ 0 ] , a [ 1 ] , a [ 2 ] , a [ 3 ] , a [ 4 ] , a [ 5 ] , a [ 6 ] , a [ 7 ] , a [ 8 ]# d e f i n e expand vec6 ( a ) a [ 0 ] , a [ 1 ] , a [ 2 ] , a [ 3 ] , a [ 4 ] , a [ 5 ]# d e f i n e expand vec5 ( a ) a [ 0 ] , a [ 1 ] , a [ 2 ] , a [ 3 ] , a [ 4 ]# d e f i n e expand vec4 ( a ) a [ 0 ] , a [ 1 ] , a [ 2 ] , a [ 3 ]

16 # d e f i n e expand vec3 ( a ) a [ 0 ] , a [ 1 ] , a [ 2 ]# d e f i n e expand vec2 ( a ) a [ 0 ] , a [ 1 ]# d e f i n e t i m i n g ( a ) s t a r t = c l o c k ( ) ; a d i f f = c l o c k ( ) − s t a r t ; msec = d i f f ∗ 1000 / CLOCKS PER SEC ; c o u t

36 bool r e a d y t o s e n d i m a g e = t rue ;bool r e a d t a g h o m o g r a p h y = f a l s e ;bool t a g s o c k e t o n = t rue ;bool t a g h a s b e e n r e a d = t rue ;bool p l a y i n g a u d i o = f a l s e ;bool j o y s t i c k c o n n e c t e d = f a l s e ;bool u n d e r m a n u a l c o n t r o l = f a l s e ;bool u n d e r a c t i o n c o n t r o l = f a l s e ;bool w a l k i n g f o r w a r d s = f a l s e ;bool l e f t b a c k b u t t o n d o w n = f a l s e ;

46 bool r e m o t e p r o c e s s i n g = f a l s e ;bool i s w a l k i n g = f a l s e ;

s t r i n g p l o t f i l e n a m e , p y t h o n p l o t t i n g e x e c u t a b l e , f i n d e r p r e f e r e n c e ;f s t r e a m p l o t w r i t i n g s t r e a m ;i n t sockfd , t a g s o c k e t f d , r e s p o n s e s o c k e t f d ;f l o a t g o a l [ 2 ] ;queue g o a l q u e u e ;v e c t o r g o a l q u e u e u n p a r s e d ;

56 C o l o r F i n d e r ∗ o r a n g e f i n d e r ,∗ g r e e n f i n d e r ,∗ y e l l o w f i n d e r ;m j p g s t r e a m e r∗ s t r e a m e r ;m i n I n i∗ i n i ;

/∗ CM730 b a s i c hardware o b j e c t . R e q u i r e d . ∗ /LinuxCM730 l inux cm730 (U2D DEV NAME) ;CM730 cm730(& l inux cm730 ) ;

/∗ Map o f a l l c o n f i g u r a t i o n s e t t i n g s read i n from f i l e by I n i t i a l i z a t i o n . cpp ∗ /po : : v a r i a b l e s m a p vm ;

66/∗ Load w a l k i n g i n i s e t t i n g s , a p p l y o n l y once s t a n d i n g ∗ /m i n I n i∗ f o r w a r d i n i = new m i n I n i (FORWARD FILE PATH) ;m i n I n i∗ b a c k w a r d i n i = new m i n I n i (BACKWARD FILE PATH) ;m i n I n i∗ d e f a u l t i n i = new m i n I n i ( INI FILE PATH ) ;c o n s t char∗ cascade name =

” / u s r / l o c a l / s h a r e / OpenCV / h a a r c a s c a d e s / h a a r c a s c a d e f r o n t a l f a c e d e f a u l t . xml ” ;

c o n s t f l o a t p i = 3 .14159265358 ;76 c o n s t f l o a t m a x t r a n s l a t i o n i n c h e s s e c = 4 . 6 3 8 1 2∗1 . 8 ;

c o n s t f l o a t m a x r o t a t i o n r a d s s e c = 21 .6867∗1 .8∗ p i / 1 8 0 ;c o n s t f l o a t w h i l e l o o p m e a n t i m e = 0 . 1 2 ; / / avg t i m e o f w h i l e loop i n s e c .c o n s t f l o a t m a x t r a n s l a t i o n i n c h e s i t e r a t i o n = m a x t r a n s l a t i o n i n c h e s s e c ∗w h i l e l o o p m e a n t i m e ;c o n s t f l o a t m a x r o t a t i o n r a d s i t e r a t i o n = m a x r o t a t i o n r a d s s e c ∗w h i l e l o o p m e a n t i m e ;c o n s t f l o a t i n c h e s t o m = 0 . 0 2 5 4 ;

f l o a t b e a c o n c o l l i s i o n a d v i s e d i s t = . 0 5 ;

f l o a t r e d [ 3 ] = {255 ,0 ,0} ;86 f l o a t g r e e n [ 3 ] = {0 ,255 ,0} ;

f l o a t b l u e [ 3 ] = {0 ,0 ,255} ;f l o a t o ra ng e [ 3 ] = {255 ,69 , 0} ;f l o a t ye l l ow [ 3 ] = {255 ,255 ,0} ;f l o a t w h i t e [ 3 ] = {255 ,255 ,255} ;

/∗ Camera c a l i b r a t i o n m a t r i x ∗ /f l o a t k [ 3 ] [ 3 ] = {{267 .7 , 0 , 159 .9} , {0 , 2 6 3 . 2 , 121 .4} , {0 , 0 , 1}} ;Mat K = Mat ( 3 , 3 , CV 32F , k ) ;Mat K inv = K. i n v ( ) ;

96/∗ S c a l e f a c t o r s f o r how much t h e j o y s t i c k moves t h e r o b o t ∗ /f l o a t t r a n s l a t i o n s c a l i n g = 1 2 ;f l o a t r o t a t i o n s c a l i n g = 2 0 ;

/∗ Maps d e s i r e d v e l o c i t y ( mot ion model ’ s ) t o c o n t r o l v e l o c i t y ∗ //∗ D e c r e a s i n g makes c o n t r o l s m a l l e r , which make t h e mot ion∗ have l e s s i mpa c t on t h e r o b o t s s t a t e . Beware , however , as t h e∗ c o n t r o l s t h a t we t h i n k we ’ re s e n d i n g t o t h e r o b o t can ’ t

106 ∗ be g r e a t e r than what t h e r o b o t can do .

∗ Va lu es o f 60 f o r t h e t u r n t o c o n t r o l t u r n∗ and .5236 rads ( max ) f o r t h e mot ion model∗ work p r e t t y w e l l . ∗ /

f l o a t v e l o c i t y t o c o n t r o l v e l o c i t y = 140 ;f l o a t t u r n t o c o n t r o l t u r n = 6 0 ;J o y s t i c k j o y ;

32

v e c t o r beacon LUT ;116

f l o a t m a x j o i n t a n g l e [ J o i n t D a t a : : NUMBER OF JOINTS ] ;f l o a t m i n j o i n t a n g l e [ J o i n t D a t a : : NUMBER OF JOINTS ] ;# e n d i f

GlobalVariables.h

6.4. TagPositionWriter.java.1 package a p r i l . t a g ;

import j a v a . awt .∗ ;import j a v a . awt . e v e n t .∗ ;import j a v a . awt . image .∗ ;import j a v a . i o .∗ ;import j a v a . u t i l .∗ ;import j a v a x . swing .∗ ;import j a v a x . image io .∗ ;import j a v a . n e t .∗ ;

11import a p r i l . jma t .∗ ;import a p r i l . jma t . geom .∗ ;

import a p r i l . v i s .∗ ;import a p r i l . jcam .∗ ;

import a p r i l . u t i l .∗ ;

p u b l i c c l a s s T a g P o s i t i o n W r i t e r21 {

ImageSource i s ;

TagFamily t f ;T a g D e t e c t o r d e t e c t o r ;

/ / usage : TagTes t [ t a g f a m i l y c l a s s ]p u b l i c s t a t i c vo id main ( S t r i n g a r g s [ ] ) {

T a g P o s i t i o n W r i t e r p o s i t i o n w r i t e r = n u l l ;p o s i t i o n w r i t e r = new T a g P o s i t i o n W r i t e r ( ) ;

31 }

p u b l i c T a g P o s i t i o n W r i t e r ( ) {new RunThread ( ) . s t a r t ( ) ;

}

c l a s s RunThread ex tends Thread{

p u b l i c vo id run ( ){

41 S t r i n g i n p u t d i r = ” f i l e : / / / da rwin / Linux / p r o j e c t / E90 repo / t inyman / j a v a i m a g e s / image . png ” ;ImageSource i s = n u l l ;TagFamily t f = new Tag36h11 ( ) ;d e t e c t o r = new T a g D e t e c t o r ( t f ) ;

t r y {A r r a y L i s t u r l s = new A r r a y L i s t() ;u r l s . add ( i n p u t d i r ) ;S t r i n g u r l = n u l l ;i f ( u r l s . s i z e ( ) ==1)

51 u r l = u r l s . g e t ( 0 ) ;}ca tch ( E x c e p t i o n ex ) {

System . o u t . p r i n t f ( ” E x c e p t i o n : ”+ex ) ;}

S o c k e t A d d r e s s s o c k e t a d d r e s s = n u l l ;S e r v e r S o c k e t s e r v e r s o c k e t = n u l l ;P r i n t W r i t e r s o c k e t w r i t e r = n u l l ;I n p u t S t r e a m s o c k e t r e a d e r = n u l l ;

61 D a t a I n p u t S t r e a m r e s p o n s e r e a d e r = n u l l ;S e r v e r S o c k e t r e s p o n s e s o c k e t = n u l l ;P r i n t W r i t e r r e s p o n s e w r i t e r = n u l l ;

So ck e t sock = n u l l ;So ck e t r e s p o n s e s o c k = n u l l ;t r y {

s e r v e r s o c k e t = new S e r v e r S o c k e t ( 9 0 9 0 , 0 , I n e t A d d r e s s . getByName ( n u l l ) ) ;

33

r e s p o n s e s o c k e t = new S e r v e r S o c k e t ( 9 0 9 1 , 0 , I n e t A d d r e s s . getByName ( n u l l ) ) ;System . o u t . p r i n t f ( ” S e t up s o c k e t s e r v e r s . A c c e p t i n g c o n n e c t i o n s\n ” ) ;

71 r e s p o n s e s o c k e t . s e t R e u s e A d d r e s s ( t rue ) ;s e r v e r s o c k e t . s e t R e u s e A d d r e s s ( t rue ) ;

sock = s e r v e r s o c k e t . a c c e p t ( ) ;r e s p o n s e s o c k = r e s p o n s e s o c k e t . a c c e p t ( ) ;

s o c k e t w r i t e r = new P r i n t W r i t e r ( sock . g e t O u t p u t S t r e a m ( ) ) ;r e s p o n s e w r i t e r = new P r i n t W r i t e r ( r e s p o n s e s o c k . g e t O u t p u t S t r e a m ( ) ) ;s o c k e t r e a d e r = sock . g e t I n p u t S t r e a m ( ) ;r e s p o n s e r e a d e r = new D a t a I n p u t S t r e a m ( r e s p o n s e s o c k . g e t I n p u t S t r e a m ( ) ) ;

81s o c k e t w r i t e r . f l u s h ( ) ;r e s p o n s e w r i t e r . f l u s h ( ) ;

}ca tch ( E x c e p t i o n e ) {

System . o u t . p r i n t f ( ” e x c e p t i o n : ”+e ) ;}

d e t e c t o r = new T a g D e t e c t o r ( t f ) ;

91 i n t l o o p c o u n t = 0 ;whi le ( t rue ) {

l o o p c o u n t ++;i n t p a c k e t s i z e = 691200;

i n t b y t e s t o r e a d , b y t e s r e a d , b y t e s l e f t t o r e a d ;byte f u l l i m a g e [ ] = new byte [ p a c k e t s i z e ] ;t r y {

b y t e s t o r e a d = r e s p o n s e r e a d e r . r e a d I n t ( ) ;/ / Sys tem . o u t . p r i n t f (” e x p e c t i n g t o read %d b y t e s t o t a l \n ” , b y t e s t o r e a d ) ;

101b y t e s l e f t t o r e a d = b y t e s t o r e a d ;b y t e s r e a d = 0 ;whi le ( b y t e s r e a d < b y t e s t o r e a d ) {

byte p a r t i a l i m a g e [ ] = new byte [ b y t e s l e f t t o r e a d ] ;i n t c u r r e n t r e a d l e n g t h = s o c k e t r e a d e r . r e a d ( p a r t i a l i m a g e , 0 , b y t e s l e f t t o r e a d ) ;b y t e s l e f t t o r e a d = b y t e s t o r e a d − b y t e s r e a d ;System . a r r a y c o p y ( p a r t i a l i m a g e , 0 , f u l l i m a g e , b y t e s r e a d , c u r r e n t r e a d l e n g t h ) ;b y t e s r e a d += c u r r e n t r e a d l e n g t h ;

}111 t r y {

S t r i n g msg = S t r i n g . f o r m a t ( ” Rece ived %d b y t e s o f image d a t a\n ” , b y t e s r e a d ) ;/ / Sys tem . o u t . p r i n t f (” Send ing c o n f i r m a t i o n r e c e i p t \n ”) ;r e s p o n s e w r i t e r . w r i t e ( msg ) ;r e s p o n s e w r i t e r . f l u s h ( ) ;

}

catch ( E x c e p t i o n e ) {System . o u t . p r i n t f ( ” r e s p o n d e r e r r o r : ”+e ) ;

}121

}catch ( IOExcep t ion e ) {

System . o u t . p r i n t f ( ” s o c k e t r e a d e r r o r : ”+e ) ;}

Buf fe r ed Image im = ImageConver t . conver tToImage ( ”RGB” , 3 2 0 , 2 4 0 , f u l l i m a g e ) ;131

/∗ t r y {ImageIO . w r i t e ( im , ” png ” , o u t p u t f i l e ) ;

}c a t c h ( I O E x c e p t i o n i o e ) {

Sys tem . o u t . p r i n t f (” i o e x c e p t i o n : ”+ i o e ) ;}∗ /

t f . s e t E r r o r R e c o v e r y B i t s ( 1 ) ;A r r a y L i s t d e t e c t i o n s = d e t e c t o r . p r o c e s s ( im , new double [ ] {im . ge tWid th ( ) / 2 . 0 , im .

g e t H e i g h t ( ) / 2 . 0} ) ;141

f o r ( T a g D e t e c t i o n d : d e t e c t i o n s ) {double p0 [ ] = d . i n t e r p o l a t e (−1,−1) ;double p1 [ ] = d . i n t e r p o l a t e (1 ,−1) ;double p2 [ ] = d . i n t e r p o l a t e ( 1 , 1 ) ;double p3 [ ] = d . i n t e r p o l a t e (−1 ,1) ;

34

double ymax = Math . max ( Math . max ( p0 [ 1 ] , p1 [ 1 ] ) , Math . max ( p2 [ 1 ] , p3 [ 1 ] ) ) ;

/ / You need t o a d j u s t t h e t a g s i z e ( measured151 / / a c r o s s t h e whole t a g i n m e t e r s and t h e f o c a l

/ / l e n g t h .double t a g s i z e m = 0 . 1 6 ;/ / do ub l e t a g s i z e m = 0 . 1 9 ;double f = 2 6 5 . 4 5 ;double a s p e c t = 752 .0 / 4 8 0 . 0 ;double M[ ] [ ] = CameraUt i l . homographyToPose ( f , f , t a g s i z e m , d . homography ) ;S t r i n g t a g i d = S t r i n g . f o r m a t ( ” t a g i d = %d\n ” , d . i d ) ;S t r i n g homog = S t r i n g . f o r m a t ( ”%g %g %g %g\n %g %g %g %g\n %g %g %g %g\n %g %g %g %g\n\n ” ,

M[ 0 ] [ 0 ] ,M[ 0 ] [ 1 ] ,M[ 0 ] [ 2 ] ,M[ 0 ] [ 3 ] ,161 M[ 1 ] [ 0 ] ,M[ 1 ] [ 1 ] ,M[ 1 ] [ 2 ] ,M[ 1 ] [ 3 ] ,

M[ 2 ] [ 0 ] ,M[ 2 ] [ 1 ] ,M[ 2 ] [ 2 ] ,M[ 2 ] [ 3 ] ,M[ 3 ] [ 0 ] ,M[ 3 ] [ 1 ] ,M[ 3 ] [ 2 ] ,M[ 3 ] [ 3 ] ) ;

t r y {s o c k e t w r i t e r . w r i t e ( t a g i d ) ;s o c k e t w r i t e r . w r i t e ( homog ) ;s o c k e t w r i t e r . f l u s h ( ) ;

}catch ( E x c e p t i o n e ) {System . o u t . p r i n t f ( ”EXCEPTION WRITER FAILED”+e ) ;}

}171

}}

}}

TagPositionWriter.java

REFERENCES

[1] AprilTag: A robust and flexible visual fiducial systemEdwin Olson,University of Michigan, 2010.

[2] Probabilitic RoboticsThrun, Burgard, and FoxThe MIT Press, August, 2005

35

1. Introduction2. Theory3. Approach3.1. Sensory Input3.2. Maintaining Near-Real Time Operation3.3. Flow Diagram of Program3.4. Autonomous Controller

4. Results5. Future Work6. Appendices6.1. main.cpp6.2. Initialization.cpp6.3. GlobalVariables.h6.4. TagPositionWriter.java

References