1 /*
2 F i l e n a m e : Pbotsim . c c
3 Author : N. Naidoo
4 Date : 2 0 / 0 9 / 2 0 1 4 5 R e v i s i o n : 4 e
6 D e s c r i p t i o n : C l i e n t a p p l i c a t i o n program f o r t h e P e o p l e B o t
7 */
8 #i n c l u d e <i o s t r e a m>
9 #i n c l u d e <s t d i o . h>
10 #i n c l u d e <s t d l i b . h>
11 #i n c l u d e <math . h>
12 #i n c l u d e <t i m e . h>
13 #i n c l u d e <u t i l i t i e s . h>
14 #i n c l u d e <c o n f i g . h>
15 #i n c l u d e <s t r i n g . h>
16 #i n c l u d e <l i b p l a y e r c ++/p l a y e r c ++.h>
17 #i n c l u d e <c t y p e . h>
18 #i n c l u d e <e r r n o . h>
19 #i n c l u d e ”svm . h”
20 #i n c l u d e ”svm−t r a i n . h”
21 #i n c l u d e ”svm−p r e d i c t . h”
22 #i n c l u d e ” a g e n t . h”
23 //#i n c l u d e <u n i s t d . h>
24 #i n c l u d e <s y s / t y p e s . h>
25 #i n c l u d e <s y s / s o c k e t . h>
26 #i n c l u d e <n e t i n e t / i n . h>
27 #i n c l u d e <a r p a / i n e t . h>
28
29 // g l o b a l v a r i a b l e s
30 d o u b l e xg , yg , yawg ; // g o a l l o c a t i o n s 31 c h a r s t r 1 [ 5 0 ] ;
32 i n t amode , a g o a l , a i d , a p a s s ; // amode i s t h e s t a t e number 33 i n t svmode , t o u t , ago , a c l r ; // go i f ago =1 , e l s e s t o p
34 i n t imode ;
35 c h a r s e n d l i n e [MAXLINE] , r e c v l i n e [MAXLINE] , i n b u f [MAXLINE ] ; 36 i n t s o c k f d ;
37 b o o l goalcmd= f a l s e ;
38 s t r u c t s o c k a d d r i n s e r v a d d r ; 39
40 b o o l o p e n s o c k e t ( )
41 {
42 b o o l tempb=t r u e ;
43 // C r e a t e a s o c k e t f o r t h e c l i e n t
44 // I f s o c k f d<0 t h e r e was an e r r o r i n t h e c r e a t i o n o f t h e s o c k e t 45 i f ( ( s o c k f d = s o c k e t ( AF INET , SOCK STREAM, 0 ) ) <0)
46 {
47 p e r r o r (” Problem i n c r e a t i n g t h e s o c k e t ”) ; 48 tempb= f a l s e ;
49 }
50 // C r e a t i o n o f t h e s o c k e t
51 memset(& s e r v a d d r , 0 , s i z e o f( s e r v a d d r ) ) ; 52 s e r v a d d r . s i n f a m i l y = AF INET ;
53 s e r v a d d r . s i n a d d r . s a d d r= i n e t a d d r (” 1 9 2 . 1 6 8 . 4 3 . 1 7 1 ”) ; // 1 9 2 . 1 6 8 . 4 3 . 1 2 5 54 s e r v a d d r . s i n p o r t = h t o n s (SERV PORT) ; // c o n v e r t t o b i g−e n d i a n o r d e r 55 // C o n n e c t i o n o f t h e c l i e n t t o t h e s o c k e t
56 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)
57 {
58 p e r r o r (” Problem i n c o n n e c t i n g t o t h e s e r v e r ”) ; 59 tempb= f a l s e ;
60 }
61 i f ( tempb==t r u e ) r e t u r n t r u e ; 62 e l s e r e t u r n f a l s e ;
63 }
64
65 v o i d u p d a t e p k t ( ) // update s e n d p a c k e t t o s e r v e r
66 {
67 s p r i n t f ( s t r 1 ,”%s%d%s%d%s%d%s%d%s ”,”#c l i e n t ; a i d=”, a i d ,” ; amode=”, amode ,” ; a g o a l=”, a g o a l ,” ; a p a s s=”, a p a s s ,” ; end#”) ;
68 // c l e a r t h e s e n d and r e c e i v e b u f f e r s : 69 memset(& s e n d l i n e [ 0 ] , 0 , s i z e o f( s e n d l i n e ) ) ; 70 memset(& r e c v l i n e [ 0 ] , 0 , s i z e o f( r e c v l i n e ) ) ; 71 memset(& i n b u f [ 0 ] , 0 , s i z e o f( i n b u f ) ) ; 72 // s e t t h e s e n d b u f f e r :
73 s n p r i n t f ( s e n d l i n e , MAXLINE,”%s ”, s t r 1 ) ;
74 }
75
76 i n t g e t a i d ( ) // g e t a g e n t i d
77 {
78 c h a r *p a r s t r=” a i d ”;
79 s n p r i n t f ( r e c v l i n e , MAXLINE,”%s ”, i n b u f ) ; // r e s t o r e r e c b u f from i n b u f 80 i n t tempi=GetParam ( r e c v l i n e , p a r s t r ) ;
81 s t d : : c o u t << ” a i d : ” << tempi << s t d : : e n d l ; 82 i f ( tempi==a i d )
83 {
84 r e t u r n ( 1 ) ; // s u c c e s s
85 }
86 e l s e
87 {
88 r e t u r n ( 0 ) ; // f a i l u r e
89 }
90 }
91
92 i n t getsvm ( ) // g e t svm mode
93 {
94 c h a r *p a r s t r=”svm”;
95 s n p r i n t f ( r e c v l i n e , MAXLINE,”%s ”, i n b u f ) ; // r e s t o r e r e c b u f from i n b u f 96 i n t tempi=GetParam ( r e c v l i n e , p a r s t r ) ;
97 s t d : : c o u t << ” svmode : ” << tempi << s t d : : e n d l ; 98 i f ( ( tempi>=0)&&(tempi<=2) ) // c h e c k svmode
99 {
100 svmode=tempi ;
101 r e t u r n ( 1 ) ; // s u c c e s s
102 }
103 e l s e
104 {
105 r e t u r n ( 0 ) ; // f a i l u r e
106 }
107 }
108
109 i n t g e t a g o ( ) // g e t ago
110 {
111 c h a r *p a r s t r=” ago ”;
112 s n p r i n t f ( r e c v l i n e , MAXLINE,”%s ”, i n b u f ) ; // r e s t o r e r e c b u f from i n b u f 113 i n t tempi=GetParam ( r e c v l i n e , p a r s t r ) ;
114 s t d : : c o u t << ” ago : ” << tempi << s t d : : e n d l ; 115 i f ( ( tempi==0)| |( tempi==1) ) // c h e c k ago
116 {
117 ago=tempi ;
118 r e t u r n ( 1 ) ; // s u c c e s s
119 }
120 e l s e
121 {
122 r e t u r n ( 0 ) ; // f a i l u r e
123 }
124 }
125
126 i n t g e t a c l r ( ) // g e t a c l r
127 {
128 c h a r *p a r s t r=” a c l r ”;
129 s n p r i n t f ( r e c v l i n e , MAXLINE,”%s ”, i n b u f ) ; // r e s t o r e r e c b u f from i n b u f 130 i n t tempi=GetParam ( r e c v l i n e , p a r s t r ) ;
131 s t d : : c o u t << ” a c l r : ” << tempi << s t d : : e n d l ; 132 i f ( ( tempi==0)| |( tempi==1) ) // c h e c k a c l r
133 {
134 a c l r=tempi ;
135 r e t u r n ( 1 ) ; // s u c c e s s
136 }
137 e l s e
138 {
139 r e t u r n ( 0 ) ; // f a i l u r e
140 }
141 }
142
143 i n t g e t t o u t ( ) // g e t t o u t
144 {
145 c h a r *p a r s t r=” t o u t ”;
146 s n p r i n t f ( r e c v l i n e , MAXLINE,”%s ”, i n b u f ) ; // r e s t o r e r e c b u f from i n b u f 147 i n t tempi=GetParam ( r e c v l i n e , p a r s t r ) ;
148 s t d : : c o u t << ” t o u t : ” << tempi << s t d : : e n d l ; 149 i f ( tempi>0) // c h e c k t o u t
150 {
151 t o u t=tempi ;
152 r e t u r n ( 1 ) ; // s u c c e s s
153 }
154 e l s e
155 {
156 r e t u r n ( 0 ) ; // f a i l u r e
157 }
158 }
159
160 i n t g e t b u f d a t ( ) // g e t b u f f e r d a t a
161 {
162 c h a r *p a r s t r=” d a t a ”;
163 s n p r i n t f ( r e c v l i n e , MAXLINE,”%s ”, i n b u f ) ; // r e s t o r e r e c b u f from i n b u f 164 GetData ( r e c v l i n e , p a r s t r ) ;// s t o r e d a t a [ 0 ] s h o u l d have t h e l o c a t i o n now ! 165 i n t tempi=a t o i (& s t o r e d a t a [ 0 ] ) ;
166 s t d : : c o u t << ” t a r g e t : ” << tempi << s t d : : e n d l ;
167 i f ( ( tempi>=0)&&(tempi<=Lnum) ) // c h e c k l o c a t i o n number
168 {
169 a g o a l=tempi ; // s e t a g e n t g o a l 170 r e t u r n ( 1 ) ; // s u c c e s s
171 }
172 e l s e
173 {
174 r e t u r n ( 0 ) ; // f a i l u r e
175 }
176 }
177
178 // S e t x−y g o a l :
179 v o i d S e t G o a l (d o u b l e x1 , d o u b l e y1 , d o u b l e yaw1 )
180 {
181 xg=x1 ;
182 yg=y1 ;
183 yawg=yaw1 ; 184 goalcmd=t r u e ;
185 }
186
187 //*******************b e g i n main program***********************
188 i n t main (i n t a r g c , c h a r *a r g v [ ] )
189 {
190 imode=a t o i ( a r g v [ a r g c−1 ] ) ; // 0 : gapnav o n l y ; any o t h e r number : gapnav and svm/ s e r v e r comms
191 s t d : : c o u t << ” imode : ” << imode << s t d : : e n d l ;
192 i f ( a r g c ==2)//&&((imode>=l e a r n c )&&(imode<=t r n p r e d c ) ) )
193 {
194 u s i n g namespace P l a y e r C c ;
195 P l a y e r C l i e n t r o b o t (” l o c a l h o s t ”) ; 196 S i m u l a t i o n P r o x y s i m p r o x y (& r o b o t ) ; 197 P o s i t i o n 2 d P r o x y pp0(& r o b o t , 1 ) ; 198 // RangerProxy rp (& r o b o t , 0 ) ; 199 L a s e r P r o x y l p (& r o b o t , 1 ) ; 200
201 pp0 . S e t M o t o r E n a b l e ( t r u e ) ; 202
203 // v a r i a b l e l i s t
204 i n t e l a p s e c , p r e v s e c , t s e c ;
205 i n t w a i t i m e ; // w a i t t i m e i n s e c o n d s 206 i n t w a i t c o u n t ; // w a i t t i m e c o u n t e r
207 // i n t s v m l o c ; // g o a l l o c a t i o n f o r a g e n t d e t e r m i n e d by svm a l g o r i t h m 208 d o u b l e xc , yc , yawc ; // c u r r e n t x−y and yaw l o c a t i o n s o f a g e n t 209 d o u b l e Home [ 2 ] ; // x−y l o c a t i o n s o f home p o s i t i o n
210 d o u b l e S r c [ 3 ] [ 2 ] ; // x−y l o c a t i o n s o f s o u r c e p o s i t i o n s 211 d o u b l e Dest [ 3 ] [ 2 ] ; // x−y l o c a t i o n s o f d e s t i n a t i o n p o s i t i o n s
212 b o o l w a i t= f a l s e ; // t r u e i f r o b o t must w a i t b e f o r e p r o c e e d i n g t o n e x t s t a t e
213 b o o l s v m f l a g ; //=1 i f we have an o u t p u t from svm a l g o r i t h m 214 b o o l svmready ; //=1 i f r e a d y t o run svm a l g o r i t h m
215 b o o l s e r v e r c o m , s e r v e r o k ; // s e r v e r c o m : i n i t i a t e comms w i t h s e r v e r ; s e r v e r o k : good p a c k e t r e p l y from s e r v e r
216 b o o l s e n d f l a g ; //=1 when we want t o s e n d p a c k e t s t o t h e s e r v e r 217 t i m e t s t a r t = t i m e ( 0 ) ;
218 // gap−nav v a r i a b l e s :
219 d o u b l e s e n s d a t a [ 1 0 0 ] ; // i n t e g r a t e d s e n s o r data , 8 s e c t o r s
220 d o u b l e ThetaArr [ secnum ] [ 2 ] ; // c h o s e n t h e t a r a n g e s f o r r o u t i n g : t h e t a 1 , t h e t a 2
221 d o u b l e v=vmin ;
222 d o u b l e w=wmin ;
223 d o u b l e v i n , win ; // l i n e a r and a n g u l a r v e l o c i t y u s e r i n p u t s 224 d o u b l e xw , yw ; // x−y w a y p o i n t s
225 d o u b l e yawn ; // n o r m a l i s e d yaw 226 d o u b l e d g o a l ; // d i s t a n c e t o g o a l
227 d o u b l e r d i s t=maxObsDist ;
228 d o u b l e w d e l t a , g d e l t a ; // wa ypoin t and g o a l a n g l e d i f f 229 d o u b l e xt , yt , t h e t a , t h e t a t ; // x&y temp v a r s
230 d o u b l e xmin , ymin ;
231 d o u b l e w d i r ; //w d i r e c t i o n
232 d o u b l e wgap ; //w f o r moving i n t i g h t s p a c e s 233 d o u b l e a r e a l i m ;
234 d o u b l e r1 , r 2 ;
235 i n t l a s e r c o u n t ; // l a s e r s c a n number 236 i n t c e l l n u m ;
237 i n t n a v t i m e r =0; // n a v i g a t i o n t i m e r
238 i n t stopnum =0; // number o f e s t o p s
239 i n t revnum =0; // number o f r e v e r s e s
240 i n t stnum =1; // s t a t e machine number
241 b o o l e s t o p= f a l s e ; 242 b o o l r e v r o b o t= f a l s e ; 243 b o o l o n e s h o t=t r u e ; 244 b o o l t u r n i n g= f a l s e ; 245 b o o l navrun= f a l s e ;
246 b o o l slowdown= f a l s e ; // s l o w down r o b o t f l a g 247 b o o l r t i g h t= f a l s e ; // t i g h t s p a c e f l a g
248 b o o l o b s a v o i d= f a l s e ; // o b s t a c l e a v o i d a n c e f l a g 249 b o o l w a y c a l c= f a l s e ; // way point c a l c done f l a g 250 b o o l f r o n t c l r= f a l s e ; // r o b o t f r o n t c l e a r t o move 251 u i n t e w a i t s e c =0;
252 u i n t r e v s e c =0;
253 u i n t f w d s e c =0;
254 l o n g t o t s e c =0; // t o t a l no . o f s e c o n d s s i n c e s t a r t 255 // end v a r i a b l e l i s t
256
257 // I n i t i a l i s a t i o n s
258 // i n i t i a l i s e sample c o u n t e r
259 t s e c =0;
260 a i d=a i d ;
261 amode =0; // wake up
262 a g o a l =0; //home
263 ago =0; // n o t r e a d y t o go 264 a c l r =0; // n o t c l e a r t o p r o c e e d
265 a p a s s =0; // n o t w a i t i n g t o go t o b u f f e r s o u r c e / d e s t i n a t i o n 266 w a i t i m e=htime ;
267 w a i t c o u n t =0;
268 // s v m l o c =0; //home l o c a t i o n
269 s v m f l a g= f a l s e ; //we dont have an o u t p u t from svm a l g o r i t h m 270 svmready= f a l s e ; // n o t r e a d y t o run svm a l g o r i t h m
271 s e r v e r c o m= f a l s e ; // n o t r e a d y t o communicate w i t h s e r v e r 272 s e r v e r o k= f a l s e ; // no good r e p l y y e t from s e r v e r
273 s e n d f l a g= f a l s e ; // dont s e n d d a t a t o s e r v e r 274 Home [ 0 ] = hx ;
275 Home [ 1 ] = hy ; 276 S r c [ 0 ] [ 0 ] = s 1 x ; 277 S r c [ 0 ] [ 1 ] = s 1 y ; 278 S r c [ 1 ] [ 0 ] = s 2 x ; 279 S r c [ 1 ] [ 1 ] = s 2 y ; 280 S r c [ 2 ] [ 0 ] = s 3 x ; 281 S r c [ 2 ] [ 1 ] = s 3 y ; 282 Dest [ 0 ] [ 0 ] = d1x ; 283 Dest [ 0 ] [ 1 ] = d1y ; 284 Dest [ 1 ] [ 0 ] = d2x ; 285 Dest [ 1 ] [ 1 ] = d2y ; 286 Dest [ 2 ] [ 0 ] = d3x ; 287 Dest [ 2 ] [ 1 ] = d3y ;
288 u p d a t e p k t ( ) ; // i n i t i a l i s e s e n d b u f f e r p a c k e t
289 i f ( imode==0)
290 {
291 xg=d2x ;//Home [ 0 ] ;
292 yg=d2y ;//Home [ 1 ] ;
293 goalcmd=t r u e ;
294 }
295 // end i n i t i a l i s a t i o n s 296
297 w h i l e( 1 ) // main l o o p : gapnav and / o r svm
298 {
299 // g e t t h e e l a p s e d s e c o n d s from s t a r t 300 e l a p s e c = d i f f t i m e ( t i m e ( 0 ) , s t a r t ) ; 301 i f ( e l a p s e c != p r e v s e c )
302 {
303 // t i c k = t r u e ;
304 i f ( imode ! = 0 ) //svm−s e r v e r
305 {
306 i f ( w a i t==t r u e )
307 {
308 w a i t c o u n t ++;
309 i f ( w a i t c o u n t>=t o u t ) w a i t= f a l s e ; // n o t w a i t i n g any l o n g e r
310 }
311 e l s e w a i t c o u n t =0;
312 }
313 i f ( e w a i t s e c>0) e w a i t s e c−−; // d e c r e m e n t e s t o p t i m e r i f n e c c e s s a r y 314 i f ( r e v s e c>0) r e v s e c−−; // d e c r e m e n t r e v e r s e t i m e r i f n e c c e s s a r y 315 i f ( f w d s e c>0) f w d s e c−−; // d e c r e m e n t f o r w a r d t i m e r i f n e c c e s s a r y 316 n a v t i m e r ++; // i n c r e m e n t n a v i g a t i o n t i m e r
317
318 i f ( w a i t==t r u e ) s t d : : c o u t << ” w a i t i n g ! ! ! ” << s t d : : e n d l ; 319
320 t o t s e c ++; // i n c r e m e n t t o t a l s e c o n d s 321 // s t o r e x−y l o c a t i o n s :
322 x y s t o r e ( t o t s e c , xc , yc ) ; 323
324 s t d : : c o u t << ”x , y , yaw , yawn : ” << xc << ” , ” << yc << ” , ” << yawc <<
” , ” << yawn << s t d : : e n d l ;
325 s t d : : c o u t << ” r d i s t , v , w : ” << r d i s t << ” , ” << v << ” , ” << w <<
s t d : : e n d l ;
326 s t d : : c o u t << ” xg , yg : ” << xg << ” , ” << yg << s t d : : e n d l ; 327 s t d : : c o u t << ”xw , yw : ” << xw << ” , ” << yw << s t d : : e n d l ; 328 s t d : : c o u t << ” r1 , r 2 : ” << r 1 << ” , ” << r 2 << s t d : : e n d l ;
329 s t d : : c o u t << ”xmin , ymin , Area Lim : ” << xmin << ” , ” << ymin << ” , ”
<< a r e a l i m << s t d : : e n d l ;
330 s t d : : c o u t << ” g d e l t a , t h e t a , d g o a l : ” << g d e l t a << ” , ” <<t h e t a << ” ,
” << d g o a l << s t d : : e n d l ;
331 s t d : : c o u t << ” stnum : ” << stnum << s t d : : e n d l ;
332 f o r (i n t i =0; i<secnum ; i ++)
333 {
334 s t d : : c o u t << ” S e c t o r Area ” << i << ” : ” << s e n s d a t a [ i ] << s t d : : e n d l ;
335 }
336 f o r (i n t i =0; i<l a s e r c o u n t ; i ++)
337 {
338 // s t d : : c o u t << ” LasArr : ” << i << ” : ” << l p [ i ] << s t d : : e n d l ;
339 }
340 i f ( e s t o p ) s t d : : c o u t << ” Robot i n E−STOP ! ! ! ” << s t d : : e n d l ; 341 p r e v s e c = e l a p s e c ;
342 }
343 // e l s e t i c k = f a l s e ; 344
345 r o b o t . Read ( ) ; // r e a d from t h e p r o x i e s
346 xc=pp0 . GetXPos ( )+hx ; // g e t c u r r e n t x−l o c a t i o n o f r o b o t 347 yc=pp0 . GetYPos ( )+hy ; // g e t c u r r e n t y−l o c a t i o n o f r o b o t
348 yawc=pp0 . GetYaw ( )+hyaw ;
349
350 //************************Begin GapNav**************************
351 i f ( ( l p . I s V a l i d ( )==t r u e ) && ( goalcmd==t r u e ) )
352 {
353 // r o b o t . Read ( ) ; // r e a d from t h e p r o x i e s 354 // n o r m a l i s e yawc t o [ 0 ; 2 p i ]
355 i f ( yawc<0) yawn = ( 2*M PI ) + yawc ;
356 e l s e yawn = yawc ;
357
358 l a s e r c o u n t = l p . GetCount ( ) ; 359
360 // c l e a r s e n s o r d a t a
361 f o r (i n t i =0; i<secnum ; i ++)
362 {
363 s e n s d a t a [ i ] = 0 ;
364 }
365
366 // g e t xmin and ymin :
367 xmin = 1 0 0 . 0 ;
368 ymin = 1 0 0 . 0 ;
369 f o r (i n t i =0; i<l a s e r c o u n t ; i ++)
370 {
371 d o u b l e a n g t=DTOR( a n g o f f s e t )+DTOR( i*p i t c h a n g ) ;
372 i f ( ( angt>( 0 . 3*M PI ) )&&(angt<( 0 . 8*M PI ) ) ) x t=l p [ i ] ; // x t=f a b s ( l p [ i ]*s i n ( a n g t ) ) ;
373 i f ( ( ( angt>=0)&&(angt<=(0.3*M PI ) ) ) | | ( ( angt>=(0.8*M PI ) )&&(
angt<=M PI ) ) ) y t=l p [ i ] ; // y t=f a b s ( l p [ i ]*c o s ( a n g t ) ) ;
374 i f ( xt<xmin ) xmin=x t ;
375 i f ( yt<ymin ) ymin=y t ;
376 }
377
378 // c h e c k e s t o p c o n d i t i o n
379 i f ( ( ( xmin<e s t o p D i s t )| |( ymin<e s t o p D i s t ) )&&(e s t o p==f a l s e )&&( r e v r o b o t
==f a l s e ) )
380 {
381 e s t o p=t r u e ;
382 o b s a v o i d=t r u e ; // h a n d l e e s t o p i n o b s a v o i d r o u t i n e 383 e w a i t s e c=e w a i t s p ;
384 stopnum++; // i n c r e m e n t number o f s t o p s
385 }
386
387 // c h e c k i f r o b o t must s l o w down
388 i f ( ( xmin>e s t o p D i s t )&&(xmin<minObsDist ) ) slowdown=t r u e ;
389 e l s e slowdown= f a l s e ;
390
391 // c h e c k i f r o b o t must w a i t
392 i f ( w a i t==t r u e ) o b s a v o i d=t r u e ; // h a n d l e e s t o p i n o b s a v o i d r o u t i n e 393
394 // i n t e g r a t e l a s e r d a t a
395 i n t k =1;
396 d o u b l e l p t ; // temp l a s e r v a r
397 f o r (i n t i =0; i<l a s e r c o u n t ; i ++)
398 {
399 i f ( l p [ i ]>r d i s t ) l p t=r d i s t ;
400 e l s e l p t=l p [ i ] ;
401 // l p t=l p [ i ] ;
402 s e n s d a t a [ k−1] += (DTOR( p i t c h a n g / 2 . 0 ) )*s q r d ( l p t ) ; // a r e a =( t h e t a / 2 )
*Rˆ 2 .
403 i f ( i== i n t( ( l a s e r c o u n t / secnum )*k ) ) k++;
404 } // end f o r
405
406 // c h e c k i f r o b o t f r o n t c l e a r :
407 a r e a l i m =(DTOR( ( r a n g e s p a n / secnum ) / 2 ) )*s q r d ( maxObsDist ) ; // Area=(
t h e t a / 2 )*Rˆ2
408 i f ( ( ( s e n s d a t a [ secnum / 2 ] )>( p f a c*a r e a l i m ) ) &&(( s e n s d a t a [ ( secnum / 2 )
−1])>( p f a c*a r e a l i m ) ) ) f r o n t c l r=t r u e ; 409 e l s e f r o n t c l r= f a l s e ;
410
411 // o b s t a c l e a v o i d a n c e r o u t i n e
412 //−−−−−−−−−−−−−−−−−−−−−−−−−−
413 i f ( o b s a v o i d==t r u e )
414 {
415 // t u r n i n g :
416 i f ( ( t u r n i n g==t r u e )&&(e s t o p==f a l s e )&&( r e v r o b o t==f a l s e ) ) // t u r n i n g r e q u i r e d ?
417 {
418 w d e l t a=A n g D i f f ( xw , xc , yw , yc , yawc ) ; // g e t a n g l e d i f f between r o b o t and waypo int
419 w d i r=GetDirn ( w d e l t a ) ; // g e t d i r n t o t u r n cw o r acw 420 // c h e c k i f r o b o t i n t i g h t s p a c e and c a l c wgap :
421 i f ( ( xmin<(1*Rw) ) | | ( ymin<(1*Rw) ) )
422 {
423 // b e g i n i t e r a t i o n t o f i n d r1 , wgap :
424 b o o l w e x i t= f a l s e ;
425 r 1=xmin ;//−e s t o p D i s t ;
426 w h i l e ( ( r1>0)&&(w e x i t==f a l s e ) )
427 {
428 r1−=0.05;
429 r 2=s q r t ( s q r d ( r 1 )+s q r d (Rw) ) ;
430 i f ( ( r2−r 1 )<ymin )
431 {
432 wgap = ( 0 . 5*vmax ) / r 1 ;
433 i f ( wgap>wmax) wgap=wmax ;
434 w e x i t=t r u e ;
435 }
436 }
437 i f ( r1<=0) wgap=wmin ;
438 w=w d i r*wgap ;
439 }
440 e l s e w=w d i r*0 . 5*wmax ; // r o b o t n o t i n t i g h t s p a c e
441 v =0.5*vmax ;
442 i f ( f a b s ( w d e l t a )<=a n g t o l ) t u r n i n g= f a l s e ;
443 }
444 e l s e // t u r n i n g==f a l s e o r e s t o p==t r u e
445 {
446 i f ( r e v r o b o t==f a l s e )
447 {
448 // s t o p r o b o t :
449 v=vmin ;
450 w=wmin ;
451 }
452 }
453 // slowdown :
454 // i f ( slowdown==t r u e ) v =0.5*vmax ; // h a l v e t h e s p e e d
455 // e l s e v=vmax ;
456
457 // e s t o p :
458 i f ( e s t o p==t r u e )
459 {
460 i f ( ( r e v r o b o t==f a l s e )&&(stopnum>1) )
461 {
462 r e v r o b o t=t r u e ;
463 r e v s e c=r e v s p ; // s t a r t r e v e r s e t i m e r
464 e s t o p= f a l s e ;
465 v=−1*vmax ;
466 w=wmin ;
467 }
468 e l s e i f ( e w a i t s e c<=0) e s t o p= f a l s e ;// c l e a r e s t o p f l a g
469 }
470 // r e v e r s e :
471 i f ( r e v r o b o t==t r u e )
472 {
473 i f ( ( r e v s e c<=0)&&(stopnum>0) )
474 {
475 r e v r o b o t= f a l s e ;
476 stopnum =0;
477 // s t o p r o b o t :
478 v=vmin ;
479 w=wmin ;
480 }
481 }
482 // w a i t i n g :
483 i f ( w a i t==t r u e )
484 {
485 v=vmin ;
486 w=wmin ;
487 }
488
489 i f ( ( t u r n i n g==f a l s e )&&(e s t o p==f a l s e )&&(r e v r o b o t==f a l s e )&&(w a i t==
f a l s e ) ) o b s a v o i d= f a l s e ; // e x i t r o u t i n e 490 } // end i f ( o b s a v o i d==t r u e )
491
492 // S t a t e #1: P o s i t i o n r o b o t t o w a r d s g o a l
493 //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
494 i f ( stnum==1)
495 {
496 g d e l t a=A n g D i f f ( xg , xc , yg , yc , yawc ) ;
497 i f ( ( f a b s ( g d e l t a )>a n g t o l )&&(o b s a v o i d==f a l s e ) ) // need t o t u r n t o w a r d s g o a l
498 {
499 xw=xg ;
500 yw=yg ;
501 t u r n i n g=t r u e ;
502 o b s a v o i d=t r u e ;
503 }
504 i f ( f a b s ( g d e l t a )<=a n g t o l ) // i n d i r e c t i o n o f g o a l − p o s i t i o n i n g a l m o s t done
505 {
506 // w a i t f o r o b s a v o i d r o u t i n e t o c o m p l e t e
507 i f ( o b s a v o i d==f a l s e ) stnum =2; // p o s i t i o n i n g done
508 }
509 }
510
511 // S t a t e #2: P o s i t i o n i n g done
512 //−−−−−−−−−−−−−−−−−−−−−−−−−
513 i f ( stnum==2)
514 {
515 i f ( o b s a v o i d==f a l s e )
516 {
517 i f ( f r o n t c l r==t r u e ) stnum =4; // c l e a r path t o g o a l
518 e l s e stnum =3; // r e a d y t o p l a n path
519 }
520 }
521
522 // S t a t e #3: Plan path u s i n g g a p s
523 //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
524 i f ( stnum==3)
525 {
526 d g o a l = d c a l c ( xg , xc , yg , yc ) ;
527 g d e l t a=A n g D i f f ( xg , xc , yg , yc , yawc ) ; 528 i f ( o b s a v o i d==f a l s e )
529 {
530 i f ( ( d g o a l>g o a l t o l ) && ( d g o a l<(4*g o a l t o l ) ) ) // a r e we c l o s e t o g o a l ?
531 {
532 i f ( f a b s ( g d e l t a )<=a n g t o l ) stnum =4; // c l e a r path t o g o a l
533 e l s e stnum =1; // p o s i t o n r o b o t t o w a r d s g o a l
534 }
535 e l s e
536 {
537 i f ( ( d g o a l<=g o a l t o l )&&( f a b s ( g d e l t a )<=a n g t o l ) ) stnum =5; // g o a l r e a c h e d
538 i f ( ( d g o a l<=g o a l t o l )&&( f a b s ( g d e l t a )>a n g t o l ) ) stnum =1; //
p o s i t o n r o b o t t o w a r d s g o a l
539 e l s e // f a r away from g o a l − p l a n p a t h s
540 {
541 i f ( ( w a y c a l c==t r u e )&&(n a v t i m e r<n a v f r e q ) )
542 {
543 w d e l t a=A n g D i f f ( xw , xc , yw , yc , yawc ) ;
544 i f ( ( f a b s ( w d e l t a )>a n g t o l )&&(o b s a v o i d==f a l s e ) ) // need t o t u r n t o w a r d s w aypoin t
545 {
546 t u r n i n g=t r u e ;
547 o b s a v o i d=t r u e ;
548 }
549 i f ( ( f a b s ( w d e l t a )<=a n g t o l )&&(o b s a v o i d==f a l s e ) ) // t r a v e l s t r a i g h t t o w a r d s way point
550 {
551 w=wmin ;
552 v=vmax ;
553 }
554 }
555 e l s e
556 {
557 i f ( n a v t i m e r>=n a v f r e q )
558 {
559 w a y c a l c= f a l s e ;
560 navrun=t r u e ; // c a l c u l a t e w a y p o i n t s i n nav r o u t i n e 561 n a v t i m e r =0; // r e s e t t i m e r f o r n e x t i t e r a t i o n
562 }
563 }
564 }
565 }
566 }
567 }
568
569 // S t a t e #4: C l e a r path t o g o a l
570 //−−−−−−−−−−−−−−−−−−−−−−−−−−−
571 i f ( stnum==4)
572 {
573 d g o a l = d c a l c ( xg , xc , yg , yc ) ;
574 g d e l t a = A n g D i f f ( xg , xc , yg , yc , yawc ) ; 575 i f ( o b s a v o i d==f a l s e )
576 {
577 w=wmin ;
578 v=vmax ;
579 i f ( ( d g o a l<=g o a l t o l )&&( f a b s ( g d e l t a )<=a n g t o l ) ) stnum =5; // g o a l r e a c h e d
580 i f ( ( f r o n t c l r==f a l s e )| |( f a b s ( g d e l t a )>a n g t o l ) ) stnum =3; // n o t p o s i t i o n e d t o g o a l o r no c l e a r path t o g o a l
581 }
582 }
583
584 // S t a t e #5: Goal r e a c h e d
585 //−−−−−−−−−−−−−−−−−−−−−
586 i f ( stnum==5)
587 {
588 // s t o p r o b o t :
589 v=vmin ;
590 w=wmin ;
591 s t d : : c o u t << ” Goal r e a c h e d ! ” << s t d : : e n d l ; 592 // f o r t e s t p u r p o s e s :
593 // xg=s 3 x ;
594 // yg=s 3 y ;
595 stnum =1;
596 goalcmd= f a l s e ;
597 }
598
599 // Path p l a n n i n g / n a v i g a t i o n r o u t i n e :
600 //−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
601 i f ( navrun==t r u e )
602 {
603 navrun= f a l s e ;
604 w a y c a l c=t r u e ;
605 x t =1000;
606 y t =1000;
607 r d i s t=maxObsDist ;
608 a r e a l i m =(DTOR( ( r a n g e s p a n / secnum ) / 2 ) )*s q r d ( r d i s t ) ; // Area=( t h e t a / 2 )*Rˆ2
609 g d e l t a=A n g D i f f ( xg , xc , yg , yc , yawc ) ; 610 d g o a l = d c a l c ( xg , xc , yg , yc ) ;
611
612 i f ( xmin<r d i s t )
613 {
614 i n t s i d x =0; // c l e a r s e c t o r i n d e x
615 i n t c s e c t o r =0; // c l e a r c h o s e n s e c t o r i n d e x
616 i n t s c o u n t ; // s e c t o r c o u n t e r
617 w h i l e ( s i d x<secnum )
618 {
619 s c o u n t =0;
620 i f ( s e n s d a t a [ s i d x ]>( p f a c*a r e a l i m ) )
621 {
622 // i f ( ( ( s i d x>1)&&(s i d x<6) )&&(xmin<RL) ) s c o u n t =0;
623 s c o u n t ++;
624 f o r (i n t i =( s i d x +1) ; i<secnum ; i ++)
625 {
626 // i f ( ( ( s i d x>1)&&(s i d x<6) )&&(xmin<RL) ) i=secnum ;
627 // e l s e
628 //{
629 i f ( s e n s d a t a [ i ]>( p f a c*a r e a l i m ) ) s c o u n t ++;
630 e l s e i=secnum ;
631 //}
632 }
633 i f ( ( maxObsDist*s c o u n t*DTOR( r a n g e s p a n / secnum ) )>=(2.5*Rw) ) // ( S=R*t h e t a ) > 1 . 5*Rw?
634 {
635 ThetaArr [ c s e c t o r ] [ 0 ] = yawn + ( ( s i d x−(secnum / 2 ) )*DTOR(
r a n g e s p a n / secnum ) ) ; // t h e t a 1 ( s t a r t )
636 ThetaArr [ c s e c t o r ] [ 1 ] = yawn + ( ( s c o u n t−(secnum / 2 ) )*
DTOR( r a n g e s p a n / secnum ) ) ; // t h e t a 2 ( end )
637 // N o r m a l i s e t o [ 0 ; 2 p i ]
638 i f ( ThetaArr [ c s e c t o r ] [ 0 ]<0 ) ThetaArr [ c s e c t o r ] [ 0 ] + = ( 2*
M PI ) ;
639 i f ( ThetaArr [ c s e c t o r ] [ 0 ]>( 2*M PI ) ) ThetaArr [ c s e c t o r ] [ 0 ]−= ( 2*M PI ) ;
640 i f ( ThetaArr [ c s e c t o r ] [ 1 ]<0 ) ThetaArr [ c s e c t o r ] [ 1 ] + = ( 2* M PI ) ;
641 i f ( ThetaArr [ c s e c t o r ] [ 1 ]>( 2*M PI ) ) ThetaArr [ c s e c t o r ] [ 1 ]−= ( 2*M PI ) ;
642 // add i n s a f e t y d i s t a n c e :
643 ThetaArr [ c s e c t o r ] [ 0 ] + = s a f e D i s t ;
644 ThetaArr [ c s e c t o r ] [ 1 ]−= s a f e D i s t ;
645 c s e c t o r ++;
646 }
647 s i d x+=s c o u n t ;
648 s t d : : c o u t << ” s i d x : ” << s i d x << s t d : : e n d l ;
649 }
650 e l s e s i d x ++;
651 } // end w h i l e
652 f o r (i n t i =0; i<c s e c t o r ; i ++)
653 {
654 s t d : : c o u t << ” C S e c t o r ” << i << ” : ” << ThetaArr [ i ] [ 0 ] <<
” , ” << ThetaArr [ i ] [ 1 ] << s t d : : e n d l ;
655 }
656 //now d e t e r m i n e t h e b e s t t h e t a and w a y p o i n t s :
657 i n t j =0;
658 i n t tnum ;
659 b o o l gpath= f a l s e ; // path t o g o a l f l a g
660 f o r (i n t i =0; i<c s e c t o r ; i ++)
661 {
662 tnum=BestThetaGoal ( xc , yc , xg , yg , r d i s t , ThetaArr [ i ] [ 0 ] , ThetaArr [ i ] [ 1 ] ) ;
663 i f ( tnum==2) // path t o g o a l
664 {
665 gpath=t r u e ;
666 xw=xg ;
667 yw=yg ;
668 }
669 e l s e
670 {
671 i f ( tnum==0) ThetaArr [ i ] [ 0 ] = ThetaArr [ i ] [ 0 ] ;//−s a f e D i s t ;
672 e l s e ThetaArr [ i ] [ 0 ] = ThetaArr [ i ] [ 1 ] ;//+s a f e D i s t ;
673 j ++;
674 }
675 i f ( gpath==t r u e ) b r e a k; // e x i t f o r l o o p
676 }
677 // c l e a r path t o g o a l n o t found , s o i t e r a t e t o g e t b e s t o v e r a l l t h e t a :
678 i f ( gpath==f a l s e )
679 {
680 t h e t a t=ThetaArr [ 0 ] [ 0 ] ;
681 f o r (i n t i =1; i<j ; i ++)
682 {
683 tnum=B e s t T h e t a P a i r ( xc , yc , xg , yg , r d i s t , t h e t a t , ThetaArr [ i ] [ 0 ] ) ;
684 i f ( tnum ! = 0 ) t h e t a t=ThetaArr [ i ] [ 0 ] ; // r e p l a c e new b e s t t h e t a
685 }
686 // might have t o tweak p o i n t h e r e s o t h a t s i d e s o f r o b o t dont c o l l i d e w i t h o b s t a c l e !
687 xw = xc + ( r d i s t*c o s ( t h e t a t ) ) ;
688 yw = yc + ( r d i s t*s i n ( t h e t a t ) ) ;
689 }
690 }
691 e l s e
692 {
693 xw=xg ;
694 yw=yg ;
695 }
696 } // end i f ( navrun==t r u e )
697
698 pp0 . S e t S p e e d ( v , w) ;
699 }// end ( l p . I s V a l i d ( )==t r u e )
700 //************************End GapNav**************************
701
702 //******************Begin SVM and s e r v e r comms********************
703 i f ( imode ! = 0 ) //svm and s e r v e r comms
704 {
705 //***********Agent i n t e r f a c e************
706 // communicate w i t h t h e s e r v e r 707 i f ( s e r v e r c o m==t r u e )
708 {
709 s t d : : c o u t << ”<<<<Agent I n t e r f a c e>>>>” << s t d : : e n d l ; 710 s e r v e r c o m= f a l s e ; // r e s e t f l a g
711 // c h e c k s e r v e r mes sa ge h e r e ( f o r #s e r v e r , end#) and o n l y p r o c e e d i f me ss age i s good ! ! !
712
713 // L e v e l 0 r e q u e s t ( a g e n t a t home o r d e s t i n a t i o n ) : 714 i f ( ( amode==athome )| |( amode==a t d e s t ) )
715 {
716 i f ( g e t a i d ( ) ==1)
717 {
718 i f ( getsvm ( ) ==1)
719 {
720 i f ( g e t a g o ( ) ==1)
721 {
722 i f ( g e t a c l r ( ) ==1)
723 {
724 i f ( g e t t o u t ( ) ==1)
725 {
726 i f ( g e t b u f d a t ( ) ==1)
727 {
728 s t d : : c o u t << ”<<<<S e r v e r p a c k e t i s good !!!>>>>” <<
s t d : : e n d l ;
729 s e r v e r o k=t r u e ; // s e r v e r s t r i n g i s good
730 svmready=t r u e ; // r e a d y t o run svm a l g o r i t h m
731 }
732 e l s e s e r v e r o k= f a l s e ;
733 }
734 e l s e s e r v e r o k= f a l s e ;
735 }
736 e l s e s e r v e r o k= f a l s e ;
737 }
738 e l s e s e r v e r o k= f a l s e ;
739 }
740 e l s e s e r v e r o k= f a l s e ;
741 }
742 e l s e s e r v e r o k= f a l s e ;
743 } // l e v e l 0
744
745 // L e v e l 1 r e q u e s t ( a g e n t g o i n g t o s o u r c e o r d e s t i n a t i o n ) : 746 i f ( ( amode==g o s r c )| |( amode==g o d e s t ) )
747 {
748 i f ( g e t a i d ( ) ==1)
749 {
750 i f ( g e t a c l r ( ) ==1)
751 {
752 i f ( g e t t o u t ( ) ==1)
753 {
754 s t d : : c o u t << ”<<<<S e r v e r p a c k e t i s good !!!>>>>” << s t d : : e n d l ;
755 s e r v e r o k=t r u e ; // s e r v e r s t r i n g i s good
756 }
757 e l s e s e r v e r o k= f a l s e ;
758 }
759 e l s e s e r v e r o k= f a l s e ;
760 }
761 e l s e s e r v e r o k= f a l s e ;
762 } // l e v e l 1
763
764 // L e v e l 2 r e q u e s t ( a g e n t g o i n g a t s o u r c e ) :
765 i f ( amode==a t s r c )
766 {
767 i f ( g e t a i d ( ) ==1)
768 {
769 i f ( g e t a g o ( ) ==1)
770 {
771 i f ( g e t t o u t ( ) ==1)
772 {
773 s t d : : c o u t << ”<<<<S e r v e r p a c k e t i s good !!!>>>>” << s t d : : e n d l ;
774 s e r v e r o k=t r u e ; // s e r v e r s t r i n g i s good
775 }
776 e l s e s e r v e r o k= f a l s e ;
777 }
778 e l s e s e r v e r o k= f a l s e ;
779 }
780 e l s e s e r v e r o k= f a l s e ;
781 } // l e v e l 2
782
783 }// end i f ( s e r v e r c o m==t r u e ) 784
785 // s e n d p a c k e t t o s e r v e r
786 i f ( ( w a i t==f a l s e )&&( s e n d f l a g==t r u e ) )
787 {
788 i f ( o p e n s o c k e t ( )==t r u e ) // c o n n e c t e d t o s e r v e r
789 {
790 s e n d f l a g= f a l s e ; // r e s e t f l a g
791 u p d a t e p k t ( ) ; // update p a c k e t b e f o r e s e n d t o s e r v e r 792 s t d : : c o u t << ” S e n d i n g s t r i n g t o s e r v e r . . . ” << s t d : : e n d l ; 793 s e n d ( s o c k f d , s e n d l i n e , s t r l e n ( s e n d l i n e ) , 0 ) ;
794 i f ( r e c v ( s o c k f d , i n b u f , MAXLINE, 0 ) == 0 )
795 {
796 // e r r o r : s e r v e r t e r m i n a t e d p r e m a t u r e l y 797 p e r r o r (”The s e r v e r t e r m i n a t e d p r e m a t u r e l y ”) ; 798 c l o s e ( s o c k f d ) ; // c l o s e t h e c o n n e c t i o n
799 }
800 e l s e
801 {
802 p r i n t f (”%s ”, ” S t r i n g r e c e i v e d from t h e s e r v e r : ”) ;
803 p u t s ( i n b u f ) ;
804 s e r v e r c o m=t r u e ; // r e a d y t o a n a l y s e p a c k e t r e c e i v e d from s e r v e r 805 c l o s e ( s o c k f d ) ; // c l o s e t h e c o n n e c t i o n
806 }
807 }
808 e l s e
809 {
810 t o u t=htime ;
811 w a i t=t r u e ; // w a i t f o r t o u t s e c o n d s b e f o r e t r y i n g t o c o n n e c t t o s e r v e r a g a i n
812 }
813 }
814 //********End Agent i n t e r f a c e**********
815 816
817 //**********L e a r n i n g component************
818 i f ( svmready==t r u e ) // t h i s f l a g i s s e t a f t e r c h e c k i n g p a c k e t q u a l i t y from s e r v e r !
819 {
820 s t d : : c o u t << ”<<<<L e a r n i n g component>>>>” << s t d : : e n d l ; 821 svmready= f a l s e ; // c l e a r f l a g
822 c h a r *f t r a i n=” t r a i n ”;
823 c h a r *fmod=” model ”;
824 c h a r *f i n=” t e s t ”;
825 c h a r *f o u t=” p r e d i c t ”;
826 s w i t c h ( svmode )
827 {
828 c a s e l e a r n c :// l e a r n i n g mode
829 s t r b u f ( svmode ) ; // append d a t a s t r i n g t o t r a i n f i l e
830 s v m f l a g=t r u e ;
831 b r e a k;
832 c a s e t r n p r e d c :// t r a i n−p r e d i c t i o n mode
833 s t r b u f ( svmode ) ; // w r i t e d a t a s t r i n g t o t e s t f i l e 834 t r a i n m a i n ( f t r a i n , fmod ) ; // g e n e r a t e t h e model f i l e
835 p r e d i c t m a i n ( f i n , fmod , f o u t ) ; // p r e d i c t t a r g e t l o c a t i o n and o u t p u t t o ” p r e d i c t ” f i l e
836 // g e t t h e t a r g e t l o c a t i o n from t h e ” p r e d i c t ” f i l e and e v a l u a t e l o c a t i o n
837 i f ( g e t l o c ( )<=0) a g o a l =0;
838 e l s e
839 {
840 i f ( ( Lmask & i n t( pow ( 2 , g e t l o c ( )−1) ) ) > 0 ) a g o a l=g e t l o c ( ) ;
841 e l s e a g o a l =0;
842 }
843 s v m f l a g=t r u e ;
844 b r e a k;
845 d e f a u l t:// do n o t h i n g
846 s v m f l a g= f a l s e ;
847 b r e a k;
848 }
849 }
850 //********End L e a r n i n g component**********
851 852
853 //*********** C e n t r a l P r o c e s s ************
854 // r o b o t s o u r c e and d e s t i n a t i o n path p l a n n i n g 855
856 // r o b o t . Read ( ) ; // r e a d from t h e p r o x i e s 857 i f ( l p . I s V a l i d ( )==t r u e )
858 {
859 s w i t c h ( amode )
860 {
861 c a s e 0 : // a g e n t j u s t woke up
862 amode =1; // n e x t s t a t e
863 b r e a k;
864 c a s e 1 : // go home
865 S e t G o a l (Home [ 0 ] , Home [ 1 ] , 0 ) ;
866 i f ( d c a l c ( xc , Home [ 0 ] , yc , Home [ 1 ] )<d t h r e s h )
867 {
868 amode =2; // n e x t s t a t e
869 w a i t=t r u e ; // s e t w a i t f l a g
870 t o u t=htime ; // t i m e o u t=htime
871 s e n d f l a g=t r u e ; // r e a d y t o s e n d p a c k e t t o s e r v e r
872 }
873 b r e a k;
874 c a s e 2 : // a t home
875 i f ( ( w a i t==f a l s e )&&( s e n d f l a g==f a l s e ) )
876 {
877 i f ( ( s v m f l a g==t r u e )&&(ago==1)&&( s e r v e r o k==t r u e ) )
878 {
879 s v m f l a g= f a l s e ;
880 s e r v e r o k= f a l s e ; // r e s e t f l a g
881 // i f ( a g o a l ==0) amode =1; // go home
882 i f ( a g o a l>0) amode =3; // go t o s o u r c e
883 ago =0; // c l e a r go f l a g
884 s e n d f l a g=t r u e ; // r e a d y t o s e n d p a c k e t t o s e r v e r
885 }
886 e l s e
887 {
888 w a i t=t r u e ; // w a i t t i m e o u t
889 s e n d f l a g=t r u e ; // r e a d y t o s e n d p a c k e t t o s e r v e r
890 }
891 }
892 b r e a k;
893 c a s e 3 : // go t o s o u r c e
894 i f ( d c a l c ( xc , S r c [ a g o a l−1 ] [ 0 ] , yc , S r c [ a g o a l−1 ] [ 1 ] )>d n e a r ) // f a r away from s o u r c e
895 {
896 S e t G o a l ( S r c [ a g o a l−1 ] [ 0 ] , S r c [ a g o a l−1 ] [ 1 ] , 0 ) ; // c o n t i n u e t o s o u r c e
897 }
898 e l s e
899 {
900 i f ( ( d c a l c ( xc , S r c [ a g o a l−1 ] [ 0 ] , yc , S r c [ a g o a l−1 ] [ 1 ] )<=d n e a r )&&(
d c a l c ( xc , S r c [ a g o a l−1 ] [ 0 ] , yc , S r c [ a g o a l−1 ] [ 1 ] )>=d t h r e s h )&&(w a i t==f a l s e )
&&( s e n d f l a g==f a l s e ) ) // n e a r s o u r c e
901 {
902 a p a s s =1; // s e t p a s s f l a g s o t h a t a g e n t w a i t s 903 i f ( ( a c l r ==1)&&( s e r v e r o k==t r u e ) )
904 {
905 S e t G o a l ( S r c [ a g o a l−1 ] [ 0 ] , S r c [ a g o a l−1 ] [ 1 ] , 0 ) ; // c o n t i n u e t o s o u r c e
906 }
907 e l s e
908 {
909 // s t a y h e r e and w a i t f o r t o u t s e c o n d s
910 w a i t=t r u e ;
911 s e n d f l a g=t r u e ;
912 }
913 }
914 e l s e
915 {
916 i f ( d c a l c ( xc , S r c [ a g o a l −1 ] [ 0 ] , yc , S r c [ a g o a l −1 ] [ 1 ] )<d t h r e s h ) //
r o b o t i s a t s o u r c e − g o t o n e x t s t a t e
917 {
918 amode =4; // n e x t s t a t e
919 a p a s s =0; // c l e a r a g e n t p a s s f l a g
920 }
921 }
922 }
923 b r e a k;
924 c a s e 4 : // a t s o u r c e
925 i f ( ( w a i t==f a l s e )&&( s e n d f l a g==f a l s e ) )
926 {
927 i f ( ( ago==1)&&( s e r v e r o k==t r u e ) )
928 {
929 s e r v e r o k= f a l s e ; // r e s e t f l a g
930 amode =5; // n e x t s t a t e
931 ago =0; // c l e a r go f l a g
932 s e n d f l a g=t r u e ; // r e a d y t o s e n d p a c k e t t o s e r v e r
933 }
934 e l s e
935 {
936 w a i t=t r u e ; // w a i t t i m e o u t
937 s e n d f l a g=t r u e ; // r e a d y t o s e n d p a c k e t t o s e r v e r
938 }
939 }
940 b r e a k;
941 c a s e 5 : // go t o d e s t i n a t i o n
942 i f ( d c a l c ( xc , Dest [ a g o a l−1 ] [ 0 ] , yc , Dest [ a g o a l−1 ] [ 1 ] )>d n e a r ) // f a r away from d e s t i n a t i o n
943 {
944 S e t G o a l ( Dest [ a g o a l−1 ] [ 0 ] , Dest [ a g o a l−1 ] [ 1 ] , 0 ) ; // c o n t i n u e t o d e s t i n a t i o n
945 }
946 e l s e
947 {
948 i f ( ( d c a l c ( xc , Dest [ a g o a l −1 ] [ 0 ] , yc , Dest [ a g o a l−1 ] [ 1 ] )<=d n e a r )&&(
d c a l c ( xc , Dest [ a g o a l−1 ] [ 0 ] , yc , Dest [ a g o a l−1 ] [ 1 ] )>=d t h r e s h )&&(w a i t==f a l s e )&&( s e n d f l a g==f a l s e ) ) // n e a r d e s t i n a t i o n
949 {
950 a p a s s =1; // s e t p a s s f l a g s o t h a t a g e n t w a i t s 951 i f ( ( a c l r ==1)&&( s e r v e r o k==t r u e ) )
952 {
953 S e t G o a l ( Dest [ a g o a l−1 ] [ 0 ] , Dest [ a g o a l−1 ] [ 1 ] , 0 ) ; // c o n t i n u e t o d e s t i n a t i o n
954 }
955 e l s e
956 {
957 // s t a y h e r e and w a i t f o r t o u t s e c o n d s
958 w a i t=t r u e ;
959 s e n d f l a g=t r u e ;
960 }
961 }
962 e l s e
963 {
964 i f ( d c a l c ( xc , Dest [ a g o a l−1 ] [ 0 ] , yc , Dest [ a g o a l−1 ] [ 1 ] )<d t h r e s h ) // a t d e s t i n a t i o n
965 {
966 amode =6; // n e x t s t a t e
967 a p a s s =0; // c l e a r a g e n t p a s s f l a g
968 }
969 }
970 }
971 b r e a k;
972 c a s e 6 : // a t d e s t i n a t i o n
973 i f ( ( w a i t==f a l s e )&&( s e n d f l a g==f a l s e ) )
974 {
975 i f ( ( s v m f l a g==t r u e )&&(ago==1)&&( s e r v e r o k==t r u e ) )
976 {
977 s v m f l a g= f a l s e ;
978 s e r v e r o k= f a l s e ; // r e s e t f l a g
979 i f ( a g o a l ==0) amode =1; // go home
980 i f ( a g o a l>0) amode =3; // go t o s o u r c e
981 ago =0; // c l e a r go f l a g
982 s e n d f l a g=t r u e ; // r e a d y t o s e n d p a c k e t t o s e r v e r
983 }
984 e l s e
985 {
986 w a i t=t r u e ; // w a i t t i m e o u t
987 s e n d f l a g=t r u e ; // r e a d y t o s e n d p a c k e t t o s e r v e r
988 }
989 }
990 b r e a k;
991 d e f a u l t:
992 amode =0;
993 b r e a k;
994 }// end s w i t c h
995 }// end i f ( l p . I s V a l i d ( )==t r u e ) 996
997 // end s o u r c e and d e s t i n a t i o n path p l a n n i n g 998 //*********** End C e n t r a l P r o c e s s ************
999
1000 }// end i f ( imode ! = 0 )
1001 //******************End SVM and s e r v e r comms********************
1002
1003 }// end w h i l e ( 1 ) − main l o o p
1004 }
1005 e l s e
1006 {
1007 p r i n t f (” I n c o r r e c t i n p u t p a r a m e t e r s\n”) ; 1008 e x i t ( 1 ) ;
1009 }// end i f−e l s e 1010 r e t u r n 0 ; 1011 }// main
A.5 PeopleBot real-world program: Player .cfg file
1 d r i v e r
2 (
3 name ” p 2 o s ”
4 p r o v i d e s [ ” odometry : : : p o s i t i o n 2 d : 0 ” ” s o n a r : 0 ” ] 5 p o r t ” / dev / t t y S 0 ”
6 )