Ros by Example Fuerte Volume 1

8/9/2019 Ros by Example Fuerte Volume 1

1/246


2/246


3/246

ROS By Example A Do-It-Yourself Guide to the

Robot Operating System

O!"#E $

A P I R OBOT PRODUCTION

R. P ATRICK GOEBEL

Version 1.02

For ROS Fuerte


4/246


5/246

ROS BY EXAMPLE . Co !ri"#t $ 2012 %! R. P&tri'( Goe%e)A)) Ri"#ts Reser*e+. No &rt o, t#is -or( &! %e re ro+u'e+ or tr&ns itte+ in &n! ,oror %! &n! e&ns/ e)e'troni' or e'#&ni'&)/ in')u+in" #oto'o !in"/ re'or+in"/ or %! &n!in,or &tion stor&"e or retrie*&) s!ste / -it#out t#e rior -ritten er ission o, t#e'o !ri"#t o-ner &n+ t#e u%)is#er.

ISBN 30003 411052

LEGO 6 is & tr&+e &r( o, t#e LEGO "rou -#i'# +oes not s onsor/ &ut#ori7e/ oren+orse t#is %oo(.

Ot#er ro+u'ts &n+ 'o &n! n& es entione+ #erein &! %e t#e tr&+e &r(s o, t#eirres e'ti*e o-ners. R&t#er t#&n use & tr&+e &r( s! %o) -it# e*er! o''urren'e o, &tr&+e &r( n& e/ -e &re usin" t#e n& es on)! in &n e+itori&) ,&s#ion &n+ to t#e %ene,ito, t#e tr&+e &r( o-ner/ -it# no intention o, in,rin"e ent o, t#e tr&+e &r(.

In,or &tion 'ont&ine+ in t#is -or( 8P& er%&'( or eBoo(9 #&s %een o%t&ine+ ,rosour'es %e)ie*e+ to %e re)i&%)e. :o-e*er/ t#e &ut#or +oes not "u&r&ntee t#e &''ur&'! or'o )eteness o, &n! in,or &tion 'ont&ine+ in it/ &n+ t#e &ut#or s#&)) not %e res onsi%)e,or &n! errors/ o issions/ )osses/ or +& &"es '&use+ or &))e"e+ to %e '&use+ +ire't)! orin+ire't)! %! t#e in,or &tion u%)is#e+ #erein. T#is -or( is u%)is#e+ -it# t#eun+erst&n+in" t#&t t#e &ut#or is su )!in" in,or &tion %ut is not &tte tin" to ren+er

ro,ession&) ser*i'es. T#is ro+u't &) ost 'ert&in)! 'ont&ins errors. It is !ourres onsi%i)it! to e;& ine/ *eri,!/ test/ &n+ +eter ine t#e & ro ri&teness o, use/ orre


6/246


7/246


8/246


9/246


10/246


11/246

Main Chapter Headings

"# P$%po& o' t(i& Boo)#########################################################

*# R a+ an, Si $+at , Ro.ot######################################################

/# Op %ating Sy&t & an, ROS V %&ion##########################################

1# R 2i 3ing t( ROS Ba&i4####################################################

0# In&ta++ing t( %o&-.y- 6a p+ Co, ###############################################

7# In&ta++ing t( A%.oti6 Si $+ato%###############################################

5# Cont%o++ing a Mo.i+ Ba& ###################################################

9# Na2igation: Pat( P+anning an, SLAM#############################################

8# Sp 4( R 4ognition an, Synt( &i################################################

";# Ro.ot Vi&ion################################################################

""# Co .ining Vi&ion an, Ba& Cont%o+#############################################

"*# Dyna i6 + S %2o& an, ROS####################################################

"/# ############################################################


12/246


13/246

Contents

P% 'a4 2ii

P%int , 2& PDF V %&ion& o' t( Boo) i6

"# P$%po& o' t(i& Boo) "

*# R a+ an, Si $+at , Ro.ot& /

2.1 G&7e%o/ St&"e/ &n+ t#e Ar%oti Si u)&tor 4

2.2 Intro+u'in" t#e Turt)eBot/ &;-e)) &n+ Pi Ro%ot

/# Op %ating Sy&t & an, ROS V %&ion& 0

4.1 Inst&))in" U%untu Linu;

4.2 Gettin" St&rte+ -it# Linu;

1# R 2i 3ing t( ROS Ba&i4& 5

.1 Inst&))in" ROS

.2 Inst&))in" rosinst&))

.4 Cre&tin" Jour Person&) ROS Dire'tor! . ?or(in" t#rou"# t#e O,,i'i&) ROS Tutori&)s 3

. RVi7 T#e ROS Visu&)i7&tion Too) 5

. Usin" ROS P&r& eters in !our Pro"r& s 10

. Usin" +!n& i' re'on,i"ure to set ROS P&r& eters 10

.3 Net-or(in" Bet-een & Ro%ot &n+ & Des(to Co uter 11

.5 ROS Re'& 14

.10 T#e O,,i'i&) ROS C#e&t S#eet 14

.11 ?#&t is & ROS A )i'&tionH 14

.12 Inst&))in" P&'(&"es -it# SVN/ Git/ &n+ er'uri&) 1

.12.1 SVN 1

.12.2 Git 1


14/246


15/246

.4.1 E;& )e T-ist ess&"es 44

.4.2 onitorin" Ro%ot otion usin" RVi7 4

. C&)i%r&tin" Jour Ro%ot=s O+o etr! 4

. .1 Line&r C&)i%r&tion 4

. .2 An"u)&r C&)i%r&tion 43

. Sen+in" T-ist ess&"es to & Re&) Ro%ot 45

. Pu%)is#in" T-ist ess&"es ,ro & ROS No+e 0

. .1 Esti &tin" Dist&n'e &n+ Rot&tion Usin" Ti e &n+ S ee+ 0

. .2 Ti e+ Out &n+ B&'( in t#e Ar%oti Si u)&tor 1

. .4 T#e Ti e+ Out &n+ B&'( S'ri t 2

. . Ti e+ Out &n+ B&'( usin" & Re&) Ro%ot . Are ?e T#ere JetH Goin" t#e Dist&n'e -it# O+o etr! 3

.3 Out &n+ B&'( Usin" O+o etr! 0

.3.1 O+o etr! B&se+ Out &n+ B&'( in t#e Ar%oti Si u)&tor 1

.3.2 O+o etr! B&se+ Out &n+ B&'( Usin" & Re&) Ro%ot 2

.3.4 T#e O+o etr! B&se+ Out &n+ B&'( S'ri t 4

.3. T#e o+o To i' *ersus t#e o+o Fr& e 5

.5 N&*i"&tin" & S


16/246


17/246

8# Sp 4( R 4ognition an, Synt( &i& """

5.1 Inst&))in" Po'(etS #in; ,or S ee'# Re'o"nition 111

5.2 Testin" t#e Po'(etS #in; Re'o"ni7er 112

5.4 Cre&tin" & Vo'&%u)&r! 11

5. A Voi'e Contro) N&*i"&tion S'ri t 11

5. .1 Testin" Voi'e Contro) in t#e Ar%oti Si u)&tor 120

5. .2 Usin" Voi'e Contro) -it# & Re&) Ro%ot 121

5. Inst&))in" &n+ Testin" Festi*&) Te;t to S ee'# 122

5. .1 Usin" Te;t to S ee'# -it#in & ROS No+e 12

5. .2 Testin" t#e t&)(%&'(. ! s'ri t 12

";# Ro.ot Vi&ion "*5

10.1 O enCV/ O enNI &n+ PCL 12

10.2 A Note &%out C& er& Reso)utions 123

10.4 Inst&))in" &n+ Testin" t#e ROS C& er& Dri*ers 123

10.4.1 Inst&))in" t#e O enNI Dri*ers 123

10.4.2 Inst&))in" ?e%'& Dri*ers 123

10.4.4 Testin" !our Kine't or tion C& er& 125 10.4. Testin" !our USB ?e%'& 140

10. Inst&))in" O enCV on U%untu Linu; 140

10. ROS to O enCV T#e '* %ri+"e P&'(&"e 142

10. T#e ros2o en'*2. ! Uti)it! 14

10. Pro'essin" Re'or+e+ Vi+eo 145

10.3 O enCV T#e O en Sour'e Co uter Vision Li%r&r! 1 0

10.3.1 F&'e Dete'tion 1 0

10.3.2 Ke! oint Dete'tion usin" Goo+Fe&turesToTr&'( 1

10.3.4 Tr&'(in" Ke! oints usin" O ti'&) F)o- 1 2

10.3. Bui)+in" & Better F&'e Tr&'(er 1 3

10.3. D!n& i'&))! A++in" &n+ Dro in" Ke! oints 1 1


18/246


19/246

12. .2 Settin" Ser*o S ee+ 202

12. .4 Contro))in" Ser*o Tor


20/246


21/246


22/246

• Ro%ot *ision/ in')u+in" ,&'e +ete'tion &n+ 'o)or tr&'(in" usin" O enCV/s(e)eton tr&'(in" usin" O enNI/ &n+ & %rie, intro+u'tion to PCL ,or 4D *ision

ro'essin".

• Co %inin" ro%ot *ision -it# & o%i)e %&se to 're&te t-o & )i'&tions/ one ,ortr&'(in" ,&'es &n+ 'o)ore+ o%>e'ts/ &n+ &not#er ,or ,o))o-in" & erson &s t#e!o*e &%out t#e roo .

• Contro))in" & &n &n+ ti)t '& er& usin" D!n& i;e) ser*os to tr&'( & o*in"o%>e't.

Gi*en t#e %re&+t# &n+ +e t# o, t#e ROS ,r& e-or(/ -e ust ne'ess&ri)! )e&*e out & ,e-to i's in t#is intro+u'tor! *o)u e. In &rti'u)&r/ -e -i)) not 'o*er t#e G&7e%osi u)&tor/ &r n&*i"&tion/ t&%)eto o%>e't &ni u)&tion/ "ener&) o%>e't +ete'tion /'re&tin" !our o-n URDF ro%ot o+e) / ro%ot+i&"nosti's / or t#e use o, t&s( &n&"erssu'# &s S AC: . Nonet#e)ess/ &s !ou '&n see ,ro t#e )ist &%o*e/ -e sti)) #&*e u'# to+o

Purpose of this Book *

http://www.ros.org/wiki/gazebohttp://www.ros.org/wiki/arm_navigationhttp://ros.org/wiki/pr2_tabletop_manipulation_appshttp://ecto.willowgarage.com/http://ecto.willowgarage.com/http://www.ros.org/wiki/urdf/Tutorialshttp://www.ros.org/wiki/diagnosticshttp://www.ros.org/wiki/executive_smachhttp://www.ros.org/wiki/executive_smachhttp://www.ros.org/wiki/gazebohttp://www.ros.org/wiki/arm_navigationhttp://ros.org/wiki/pr2_tabletop_manipulation_appshttp://ros.org/wiki/pr2_tabletop_manipulation_appshttp://ecto.willowgarage.com/http://www.ros.org/wiki/urdf/Tutorialshttp://www.ros.org/wiki/diagnosticshttp://www.ros.org/wiki/executive_smach


23/246

*# R EAL AND S IMULATED R OBOTS?#i)e ROS '&n %e use+ -it# & )&r"e *&riet! o, #&r+-&re/ !ou +on=t nee+ &n &'tu&) ro%otto "et st&rte+. ROS in')u+es &'(&"es to run & nu %er o, ro%ots in si u)&tion so t#&t!ou '&n test !our ro"r& s %e,ore *enturin" into t#e re&) -or)+.

*#" =a@ .o: Stag : an, t( A%.otiX Si $+ato%T#ere &re & nu %er o, -&!s to si u)&te & ro%ot in ROS. T#e ost so #isti'&te+ usesG&7e%o/ & ,u)) ,e&ture+ 4 D #!si's si u)&tor t#&t re +&tes ROS %ut no- inte"r&tesni'e)! -it# it. T#e se'on+ uses St&"e/ & si )er 2 D si u)&tor t#&t '&n &n&"e u)ti )ero%ots &n+ *&rious sensors su'# &s )&ser s'&nners. T#e t#ir+ uses i'#&e) Fer"uson=s&r%oti; !t#on &'(&"e t#&t '&n run & ,&(e si u)&tion o, & +i,,erenti&) +ri*e ro%ot %ut-it#out sensor ,ee+%&'( or &n! #!si's. ?e -i)) use t#is )&st et#o+ &s it is t#esi )est to set u &n+ -e +on=t nee+ #!si's ,or our ur oses. O, 'ourse/ ,ee) ,ree toe; )ore G&7e%o &n+ St&"e %ut %e re &re+ to s en+ & )itt)e ti e -or(in" on t#e +et&i)s.A)so/ G&7e%o in &rti'u)&r +e &n+s & ,&ir %it o, CPU o-er.

E*en i, !ou +o #&*e !our o-n ro%ot/ it is & "oo+ i+e& to run so e o, t#e e;& )es in t#is %oo( in t#e si u)&tor ,irst. On'e !ou &re #& ! -it# t#e resu)ts/ !ou '&n tr! it on !ourro%ot.

!eal an" #imulate" !obots /

http://www.ros.org/wiki/gazebohttp://www.ros.org/wiki/stagehttp://ros.org/wiki/arbotix_pythonhttp://ros.org/wiki/arbotix_pythonhttp://www.ros.org/wiki/gazebohttp://www.ros.org/wiki/stagehttp://ros.org/wiki/arbotix_python


24/246

*#* Int%o,$4ing t( T$%t+ Bot: Ma63 ++ an, Pi Ro.otFor t#e ur oses o, t#is %oo(/ -e nee+ & ro%ot t#&t -e '&n &t )e&st run in si u)&tion totest our 'o+e. T#e ros-by-example re ositor! in')u+es su ort ,or t-o test ro%ots/

t#e ?i))o- G&r&"e Turt)eBot 're&te+ %! e)onee ?ise&n+Tu))! Foote / &n+ t#e &ut#or=s o-n #o e %ui)t ro%ot

'&))e+ Pi Ro%ot.

Pi -&s ins ire+ %! i'#&e) Fer"uson=s &;-e)) -#i'# inturn -&s o+e)e+ &,ter Geor"i& Te'#=s EL E ro%ot. I, !ou#&*e & URDF o+e) o, !our o-n ro%ot/ !ou '&n use itinste&+ o, one o, t#ese. In &n! e*ent/ ost o, t#e 'o+e -e+e*e)o -i)) run on &) ost &n! ro%ot t#&t su orts t#e

%&si' ROS ess&"e inter,&'es.

!eal an" #imulate" !obots 1

Pi Ro.ot

Ma63 ++

http://www.ros.org/wiki/TurtleBothttp://www.meloneewise.com/?page_id=33http://www.meloneewise.com/?page_id=33http://www.willowgarage.com/pages/people/tully-footehttp://www.pirobot.org/http://www.showusyoursensors.com/http://www.ros.org/wiki/maxwellhttp://www.ros.org/news/2010/03/robots-using-ros-georgia-techs-assistive-robots.htmlhttp://www.ros.org/news/2010/03/robots-using-ros-georgia-techs-assistive-robots.htmlhttp://www.turtlebot.com/http://www.ros.org/wiki/TurtleBothttp://www.meloneewise.com/?page_id=33http://www.willowgarage.com/pages/people/tully-footehttp://www.pirobot.org/http://www.showusyoursensors.com/http://www.ros.org/wiki/maxwellhttp://www.ros.org/news/2010/03/robots-using-ros-georgia-techs-assistive-robots.html


25/246

/# O PERATIN= SYSTEMS AND ROS V ERSIONSROS '&n run on *&rious ,)&*ors o, Linu;/ &'OS &n+ &rti&))! on i'roso,t?in+o-s. :o-e*er/ t#e e&siest -&! to "et st&rte+ is to use U%untu Linu; &s t#is is t#eOS o,,i'i&))! su orte+ %! ?i))o- G&r&"e. Furt#er ore/ U%untu is ,ree &n+ e&s! toinst&)) &)on"si+e ot#er o er&tin" s!ste s. In '&se !ou &re not &)re&+! usin" U%untu/ -e-i)) )ist & ,e- ointers on inst&))in" it in t#e ne;t se'tion.

ROS '&n %e run on &n!t#in" ,ro & su er'o uter to & Be&")e%o&r+. But u'# o, t#e'o+e -e -i)) +e*e)o -i)) re


26/246

)i(e)! "et %o""e+ +o-n -#en tr!in" to run "r& #i's intensi*e ro"r& s )i(e RViz . 8In,&'t/ t#e! i"#t not run &t &)).9

:ere &re & ,e- )in(s to "et !ou st&rte+

• Inst&))in" U%untu 12.0 on & ?in+o-s 'o uter

• Inst&))in" U%untu 11.10 on & ?in+o-s 'o uter . :o-e*er/ %e &-&re t#&t t#e@+o-n)o&+@ )in(s on t#is &"e -i)) t&(e !ou %&'( to t#e 12.0 *ersion so #ere ist#e )in( to t#e 11.10 +o-n)o&+ &"e +ire't )in( to t#e 11.10 +o-n)o&+ &"e .Jou -i)) ro%&%)! -&nt to inst&)) t#e Des(to CD r&t#er t#&n t#e Ser*er CD.

• Usin" ?u%i Jou '&n &)so tr! t#e wubi.exe inst&))er ,oun+ &t t#e *er! en+ o,t#e ,i)e )ist on t#e +o-n)o&+ &"e )in(e+ to &%o*e. T#is inst&))er runs un+er?in+o-s &n+ -i)) inst&)) U%untu &s i, it -ere >ust &not#er ?in+o-s & )i'&tion.T#is e&ns !ou '&n uninst&)) it &t & )&ter ti e usin" A++ Re o*e Pro"r& s8?in+o-s P9 or Pro"r& s &n+ Fe&tures. T#is is t#e e&siest -&! to "et U%untu

u &n+ runnin" &)on"si+e ?in+o-s %ut t#ere &re & ,e- '&*e&ts 819 t#e U%untu+is( si7e &! %e )i ite+/ 829 !ou -i)) not %e &%)e to :i%ern&te t#e &'#ine ,ro-it#in Linu;/ t#ou"# !ou -i)) sti)) %e &%)e to ut it in St&n+%!/ &n+ 849 Linu;-i)) not run


27/246

1# R EVIE


28/246

NOTE For & ore &+*&n'e+ et#o+ o, 're&tin" &n+ &n&"in" ROS -or(s &'es/ seet#e +o'u ent&tion ,or t#e rosws too). T#e si )er ro'e+ure +es'ri%e+ %e)o- -i))su,,i'e ,or t#e ur oses o, t#is %oo(. 8A o)o"ies to t#e ROS urists 9

I, !ou #&*en=t &)re&+! +one so/ 're&te !our erson&) ROS +ire'tor! usin" t#e 'o &n+

$ mkdir /ros_workspa!e

-#ere is t#e Linu; s#ort#&n+ ,or !our #o e +ire'tor! &n+ -e use t#e $ si"n toin+i'&te t#e st&rt o, t#e Linu; 'o &n+ )ine. 8It is not &rt o, t#e 'o &n+ itse), &n+ so!ou +o not #&*e to t! e it.9 Jou '&n 're&te !our erson&) ROS +ire'tor! &n!-#ere !ou)i(e %ut >ust re e %er to su%stitute !our o-n &t# -#ene*er -e re,er to/ros_workspa!e in t#is %oo(.

On'e !ou #&*e 're&te+ !our erson&) ROS +ire'tor!/ &++ it to !ourR"#_ %&'%()_ %*+ en*iron ent *&ri&%)e %! e+itin" !our .bashr! ,i)e 8note t#e)e&+in" erio+ in t#e ,i)en& e9. For e;& )e/ i, !ou use t#e gedit te;t e+itor/ run t#e'o &n+

$ gedit /.bashr!

T#en &++ t#e ,o))o-in" to t#e en+ o, t#e ,i)e

export R"#_ %&'%()_ %*+, /ros_workspa!e $R"#_ %&'%()_ %*+

8Re e %er/ su%stitute !our o-n +ire'tor! ,or /ros_workspa!e i, !ou 're&te+ itso e-#ere e)se.9 S&*e !our '#&n"es/ e;it t#e e+itor/ &n+ run t#e 'o &n+

$ sour!e /.bashr!

Jou on)! #&*e to +o t#is on'e in &n! o en ter in&) ne- ter in&) -in+o-s -i)) i'(u t#e ne- &t# &uto &ti'&))!. ?it# !our erson&) ROS +ire'tor! & e&rin" ,irst in t#eROS PACKAGE PAT: *&ri&%)e &s s#o-n &%o*e/ not on)! -i)) ROS i'( u &'(&"es!ou &++ to !our erson&) ROS +ire'tor!/ it -i)) &)so ,in+ t#e ,irst. T#is e&ns t#&t i,!ou #&*e & )&ter e; eri ent&) *ersion o, one o, t#e st&n+&r+ ROS &'(&"es t#&t &)so)i*es in t#e /opt/ros/release +ire'tor!/ !our e; eri ent&) *ersion -i)) %e use+inste&+.

1#1


29/246

T#e o,,i'i&) ROS Be"inner Tutori&)s &re su er%)! -ritten &n+ #&*e %een teste+ %! &n!ne- ROS users. It is t#ere,ore essenti&) to %e"in !our intro+u'tion to ROS %! runnin"t#rou"# t#ese tutori&)s in se


30/246


31/246


32/246

'o &n+ &%o*e to ,in+ t#e &++ress ,or t#e #&r+ )ine. 8I, !ou run t#e if!onfig'o &n+ on its o-n/ !ou '&n see &)) t#e +et&i)s ,or &)) net-or( inter,&'es.9

:ere=s #o- t#e setu ro'ess -ou)+ "o i, !our +es(to 'o uter #&s IP &++ress152.1 3.1.2 &n+ !our ro%ot=s 'o uter #&s IP &++ress 152.1 3.1.4. ?e -i)) run t#eros!ore ro'ess on t#e +es(to 'o uter.

St&rtin" on !our +es(to &'#ine/ run t#e ,o))o-in". Re e %er to su%stitute !our o-nIP &++resses -#ere & ro ri&te

$ export R"#_0%#*)R_UR1,http //596.5:;.5.6 55>55$ export R"#_1 ,596.5:;.5.6$ ros!ore

Ne;t/ o en &not#er ter in&) -in+o- &n+ )o" into !our ro%ot=s 'o uter usin" ssh .First s!n'#roni7e t#e ')o'( -it# t#e &ster. T#en set t#e R"#_0%#*)R_UR1 &n+R"#_1 en*iron ent *&ri&%)es

$ ssh 596.5:;.5.>$ sudo ntpdate -b 596.5:;.5.6$ export R"#_0%#*)R_UR1,http //596.5:;.5.6 55>55$ export R"#_1 ,596.5:;.5.>

I, &)) "oes -e))/ !ou s#ou)+ %e &%)e to see t#e /rosout &n+/rosout_agg to i's on %ot# &'#ines

$ rostopi! list

/rosout/rosout_agg

Jou '&n no- run !our ro%ot=s st&rtu )&un'# ,i)e8s9 in t#e s& e ssh -in+o- t#&t !ouuse+ to )o" in *i& !our +es(to .

To run RViz on t#e +es(to / o en &not#er ter in&)/ set t#e en*iron ent *&ri&%)es &s -e+i+ ,or t#e &ster &%o*e/ t#en st&rt u RViz

$ export R"#_0%#*)R_UR1,http //596.5:;.5.6 55>55$ export R"#_1 ,596.5:;.5.6$ rosrun r4iz r4iz

I, !our ro%ot &n+ +es(to &)-&!s #&*e t#e s& e IP &++resses on !our )o'&) net-or(/ !ou'&n &++ t#e & ro ri&te export 'o &n+s to t#e /.bashr! ,i)e on e&'# &'#ine sot#&t !ou +on=t #&*e to run t#e e*er! ti e !ou o en & ne- ter in&).

!e&ie'ing the !$# Basics "*


33/246

1#8 ROS R 4apSin'e it i"#t #&*e %een &-#i)e sin'e !ou +i+ t#e Be"inner &n+t, Tutori&)s/ #ere is &

%rie, re'& o, t#e ri &r! ROS 'on'e ts. T#e 'ore entit! in ROS is '&))e+ & no+e. Ano+e is "ener&))! & s &)) ro"r& -ritten in P!t#on or C t#&t e;e'utes so ere)&ti*e)! si )e t&s( or ro'ess. No+es '&n %e st&rte+ &n+ sto e+ in+e en+ent)! o,

one &not#er &n+ t#e! 'o uni'&te %! &ssin" ess&"es. A no+e '&n u%)is# ess&"eson 'ert&in to i's or ro*i+e ser*i'es to ot#er no+es.

For e;& )e/ & u%)is#er no+e i"#t re ort +&t& ,ro sensors &tt&'#e+ to !our ro%ot=si'ro'ontro))er. A ess&"e on t#e /head_sonar to i' -it# & *&)ue o, 0. -ou)+ e&nt#&t t#e sensor is 'urrent)! +ete'tin" &n o%>e't 0. eters &-&!. 8Re e %er t#&t ROSuses eters ,or +ist&n'e &n+ r&+i&ns ,or &n"u)&r e&sure ents.9 An! no+e t#&t -&nts to(no- t#e re&+in" ,ro t#is sensor nee+ on)! su%s'ri%e to t#e /head_sonar to i'. To&(e use o, t#ese *&)ues/ t#e su%s'ri%er no+e +e,ines & '&))%&'( ,un'tion t#&t "etse;e'ute+ -#ene*er & ne- ess&"e &rri*es on t#e su%s'ri%e+ to i'. :o- o,ten t#is#& ens +e en+s on t#e r&te &t -#i'# t#e u%)is#er no+e u +&tes its ess&"es.

A no+e '&n &)so +e,ine one or ore ser*i'es. A ROS ser*i'e ro+u'es so e %e#&*ior orsen+s %&'( & re )! -#en sent & re


34/246

sin")e )&r"e ro"r& t#&t 'ontro)s t#e ro%ot=s %e#&*ior. oreo*er/ t#e ro"r& -i))usu&))! t&)( +ire't)! to t#e #&r+-&re/ or &t )e&st/ to & )i%r&r! s e'i,i'&))! +esi"ne+ ,or t#e#&r+-&re !ou &re usin".

?#en usin" ROS/ t#e ,irst ste is to +i*i+e u t#e +esire+ %e#&*ior into in+e en+ent,un'tions t#&t '&n %e #&n+)e+ %! se &r&te no+es. For e;& )e/ i, !our ro%ot uses &

-e%'& or & +e t# '& er& )i(e & Kine't or tion Pro/ one no+e -i)) 'onne't to t#e'& er& &n+ si )! u%)is# t#e i &"e &n+ or +e t# +&t& so t#&t ot#er no+es '&n use it. I,!our ro%ot uses & o%i)e %&se/ & %&se 'ontro))er no+e -i)) )isten ,or otion 'o &n+son so e to i' &n+ 'ontro) t#e ro%ot=s otors to o*e t#e ro%ot &''or+in")!. T#eseno+es '&n %e use+ -it#out o+i,i'&tion in &n! +i,,erent & )i'&tions -#ene*er t#e+esire+ %e#&*ior re


35/246

1#"* In&ta++ing Pa4)ag & 3it( SVN: =it: an, M %4$%ia+On'e in &-#i)e t#e ROS &'(&"e !ou nee+ -on=t %e &*&i)&%)e &s & De%i&n &'(&"e &n+!ou -i)) nee+ to inst&)) it ,ro sour'e. T#ere &re t#ree &>or sour'e 'ontro) s!ste s

o u)&r -it# 'o+e +e*e)o ers SVN/ Git &n+ e'uri&). T#e t! e o, s!ste use+ %! t#e+e*e)o er +eter ines #o- !ou inst&)) t#e sour'e. To &(e sure !ou &re re&+! ,or &))

t#ree s!ste s/ run t#e ,o))o-in" inst&)) 'o &n+ on !our U%untu &'#ine$ sudo apt-get install git sub4ersion mer!urial

For &)) t#ree s!ste s/ t#ere &re t-o o er&tions !ou -i)) use ost o, t#e ti e. T#e ,irsto er&tion &))o-s !ou to '#e'( out t#e so,t-&re ,or t#e ,irst ti e/ -#i)e t#e se'on+ isuse+ ,or "ettin" u +&tes t#&t i"#t %e &*&i)&%)e )&ter on. Sin'e t#ese 'o &n+s &re+i,,erent ,or &)) t#ree s!ste s/ )et=s )oo( &t e&'# in turn.

4.12.1 SVN

Let=s &ssu e t#&t t#e SVN sour'e !ou -ou)+ )i(e to '#e'( out is )o'&te+ &t(ttp! % po&ito%y &2n pa4)ag na . To +o t#e initi&) '#e'(out &n+ %ui)+ t#e &'(&"ein !our erson&) ROS +ire'tor!/ run t#e ,o))o-in" 'o &n+s. 8I, ne'ess&r!/ '#&n"e t#e,irst 'o &n+ to re,)e't t#e &'tu&) )o'&tion o, !our erson&) ROS +ire'tor!.9

$ !d /ros_workspa!e$ s4n !he!kout http //repository/s4n/pa!kage_name$ !d pa!kage_name$ rosmake

To u +&te t#e &'(&"e )&ter on/ run t#e 'o &n+s

$ ros!d pa!kage_name$ s4n update$ rosmake --pre-!lean

4.12.2 Git

Let=s &ssu e t#&t t#e Git sour'e !ou -ou)+ )i(e to '#e'( out is )o'&te+ &tgit! % po&ito%y pa4)ag na . To +o t#e initi&) '#e'(out &n+ %ui)+ t#e &'(&"e in!our erson&) ROS +ire'tor!/ run t#e ,o))o-in" 'o &n+s. 8I, ne'ess&r!/ '#&n"e t#e,irst 'o &n+ to re,)e't t#e &'tu&) )o'&tion o, !our erson&) ROS +ire'tor!.9

$ !d /ros_workspa!e$ git !lone git //repository/pa!kage_name$ !d pa!kage_name$ rosmake

To u +&te t#e &'(&"e )&ter on/ run t#e 'o &n+s

!e&ie'ing the !$# Basics "0


36/246

$ ros!d pa!kage_name$ git pull$ rosmake --pre-!lean

4.12.3 Mercurial

Let=s &ssu e t#&t t#e er'uri&) sour'e !ou=+ )i(e to '#e'( out is )o'&te+ &t(ttp! % po&ito%y pa4)ag na . To +o t#e initi&) '#e'(out &n+ %ui)+ t#e &'(&"e in!our erson&) ROS +ire'tor!/ run t#e ,o))o-in" 'o &n+s. 8I, ne'ess&r!/ '#&n"e t#e,irst 'o &n+ to re,)e't t#e &'tu&) )o'&tion o, !our erson&) ROS +ire'tor!.9

$ !d /ros_workspa!e$ hg !lone http //repository/pa!kage_name$ !d pa!kage_name$ rosmake

8In '&se !ou &re -on+erin" -#! er'uri&) uses hg ,or its &in 'o &n+ n& e/ :" ist#e s! %o) ,or t#e e)e ent er'ur! on t#e Perio+i' T&%)e in '#e istr!.9 To u +&te t#e

&'(&"e )&ter on/ run t#e 'o &n+s$ ros!d pa!kage_name$ hg update$ rosmake --pre-!lean

1#"/ R o2ing Pa4)ag & '%o yo$% P %&ona+ ROS Di% 4to%yTo re o*e & &'(&"e or st&'( inst&))e+ in !our erson&) ROS +ire'tor!/ si )! re o*et#e entire &'(&"e or st&'( +ire'tor! or o*e it to & +i,,erent )o'&tion outsi+e o, !ourR"#_ %&'%()_ %*+ . For e;& )e/ to re o*e & &'(&"e '&))e+ my_ros_pa!kage,ro !our /ros_workspa!e +ire'tor!/ run t#e 'o &n+s

$ !d /ros_workspa!e$ ?rm -rf my_ros_pa!kage

8T#e %&'(s)&s# es'& es &n! &)i&ses !ou i"#t #&*e 're&te+ ,or t#e Linu; rm 'o &n+.9Jou '&n test t#&t t#e &'(&"e #&s %een re o*e+ usin" t#e ros!d 'o &n+

$ ros!d my_ros_pa!kage

-#i'# s#ou)+ ro+u'e t#e out ut

ros!d @o su!h pa!kage Amy_ros_pa!kageA

A)tern&ti*e)!/ 're&te & +ire'tor! outsi+e o, !our ROS &'(&"e &t# &n+ o*e t#eun-&nte+ &'(&"e into t#&t +ire'tor!. I, !ou -&nt to re &'ti*&te t#e &'(&"e &t & )&terti e/ >ust o*e it %&'( into !our erson&) ROS +ire'tor!.



37/246


38/246

$ roslo!ate uri skeleton_markers

-#i'# resu)ts in t#e out ut

http //pi-robot-ros-pkg.google!ode.!om/s4n/trunk/skeleton_markers

In t#is '&se/ -e see t#&t t#e re ositor! uses SVN/ so to inst&)) t#e &'(&"e -e -ou)+ run$ !d /ros_workspa!e$ s4n !o http //pi-robot-ros-?

pkg.google!ode.!om/s4n/trunk/skeleton_markers$ rosmake skeleton_markers

8Note Jou -ou)+ not nor &))! t! e t#e %&'(s)&s# '#&r&'ter 8Q9 on t#e se'on+ 'o &n+)ine &%o*e. It is in')u+e+ #ere to @es'& e@ t#e )ine %re&( -#i'# -i)) not ot#er-ise 'o !&n+ &ste 'orre't)! i, usin" t#e PDF *ersion o, t#is %oo(. Jou -i)) see ot#er inst&n'eso, t#e Q '#&r&'ter t#rou"#out t#e %oo( -#ere & 'o &n+ -r& s &'ross ore t#&n one)ine.9

Fin&))!/ &n e;& )e t#&t use er'uri&) 8 hg 9 is t#e rosBa4a st&'(. Runnin" roslo!ateto ,in+ t#e rosBa4a st&'( !ie)+s

$ roslo!ate uri rosBa4a

https //rosBa4a.google!ode.!om/hg

T#e hg &t t#e en+ o, t#e URL &(es us sus e't t#&t t#is is ro%&%)! & er'uri&)re ositor!. To %e sure/ use t#e roslo!ate 4!s 'o &n+

$ roslo!ate 4!s rosBa4a

-#i'# !ie)+s t#e out ut

hg

&n+ -e see t#&t t#is is in+ee+ & er'uri&) re ositor!. To inst&)) t#e sour'e -e -ou)+run

$ !d /ros_workspa!e

$ hg !lone https //rosBa4a.google!ode.!om/hg rosBa4a_!ore$ !d rosBa4a_!ore$ rosmake

Note #o- -e &++ t#e +ire'tor! n& e rosBa4a_!ore &t t#e en+ o, t#e hg !lone'o &n+. Ot#er-ise t#e resu)tin" +ire'tor! -ou)+ %e n& e+ t#e r&t#er non+es'ri t@hg @ sin'e t#&t is t#e )&st &rt o, t#e URL.



39/246


40/246

!e&ie'ing the !$# Basics *;


41/246

0# INSTALLIN= THE ROS -BY -EXAMPLE C ODE

0#" In&ta++ing t( P% % ?$i&it &Be,ore inst&))in" t#e ros-by-example 'o+e itse),/ it -i)) s&*e so e ti e i, -e inst&))ost o, t#e &++ition&) ROS &'(&"es -e -i)) nee+ )&ter. 8Instru'tions -i)) &)so %e

ro*i+e+ ,or inst&))in" in+i*i+u&) &'(&"es &s nee+e+ t#rou"#out t#e %oo(.9 Si )!'o ! &n+ &ste t#e ,o))o-in" 'o &n+ 8-it#out t#e si"n9 into & ter in&) -in+o- toinst&)) t#e De%i&n &'(&"es -e -i)) nee+ 8t#e Q '#&r&'ters &(e t#e ,o))o-in" )ines )oo()i(e one )ine to Linu; -#en +oin" & 'o ! &n+ &ste9

$ sudo apt-get install ros-fuerte-turtlebotC ros-fuerte-!ontrol ?ros-fuerte-openni-kine!t ros-fuerte-laser-dri4ers ?ros-fuerte-audio-!ommon ros-fuerte-Boysti!k-dri4ers ?

ros-fuerte-oro!os-kinemati!s-dynami!s ?ros-fuerte-dynamixel-motor gstreamer8.58-po!ketsphinx ?python-setuptools ros-fuerte-open!46 ros-fuerte-4ision-open!4

?e &)so nee+ one re re


42/246


43/246

0#/ In&ta++ing t( Pi Ro.ot URDF Fi+ &I, !ou -&nt to run Pi Ro%ot in si u)&tion/ inst&)) t#e pi_robot_des!ription &'(&"e-it# t#e ,o))o-in" 'o &n+s

$ !d /ros_workspa!e$ s4n !o Froslo!ate uri pi_robot_des!riptionF

:ere -e use t#e Linu; @%&'(ti'(@ o er&tor to 'o %ine t#e s4n 'o &n+ 8@'o@ is s#ort,or '#e'(out9 -it# t#e out ut ,ro t#e roslo!ate 'o &n+.

0#1 A.o$t t( Co, Li&ting& in t(i& Boo) For t#e %ene,it o, t#ose -#o -ou)+ )i(e to -or( ,ro & rinte+ *ersion o, t#e %oo(inste&+ o, or in &++ition to t#e PDF *ersion/ ost o, t#e s& )e ro"r& s &re +is )&!e+in t#eir entiret! &s -e)) &s %ro(en +o-n )ine %! )ine ,or &n&)!sis. At t#e to o, e&'#s& )e s'ri t is & )in( to t#e on)ine *ersion o, t#e ,i)e &s ,oun+ in t#e ros-by-examplere ositor!. I, !ou &re re&+in" t#e PDF *ersion o, t#e %oo(/ ')i'(in" on t#e )in( -i))

%rin" !ou to & ni'e)! ,or &tte+ &n+ 'o)or 'o+e+ *ersion o, t#e ro"r& . Be &-&ret#ou"# t#&t t#e )ine nu %ers in t#e on)ine *ersion -i)) not &t'# t#ose in t#e %oo( sin'et#e rinte+ *ersion )e&*es out so e e;tr&neous 'o ents to s&*e s &'e &n+ &(e t#in"se&sier to re&+. O, 'ourse/ on'e !ou #&*e +o-n)o&+e+ t#e 'o+e ,ro t#e ros-by-exampl re ositor!/ !ou '&n &)so %rin" u !our )o'&) 'o ! o, t#e s& e ,i)es in !our,&*orite e+itor.

As -it# ost ro"r& in" 'o+e/ t#ere is usu&))! ore t#&n one -&! to so)*e & ro%)e .T#e s& )e ro"r& s +es'ri%e+ #ere &re no e;'e tion &n+ i, !ou '&n t#in( o, & %etter

-&! to &''o )is# t#e s& e "o&)/ "re&t. T#e 'o+e in')u+e+ in t#e ros-by-examplere ositor! is e&nt on)! &s & "ui+e &n+ 'ert&in)! +oes not re resent t#e %est or on)! -&!to &''o )is# & "i*en t&s( usin" ROS.

(nstalling the ros by e)ample *o"e */


44/246

(nstalling the ros by e)ample *o"e *1


45/246

7# INSTALLIN= THE A RBOTIX S IMULATOR To test our 'o+e on & si u)&te+ ro%ot/ -e -i)) use t#e arbotix_python si u)&tor,oun+ in t#e Ar%oti st&'( %! i'#&e) Fer"uson.

7#" In&ta++ing t( Si $+ato%To inst&)) t#e si u)&tor in !our erson&) ROS +ire'tor!/ run t#e ,o))o-in" 'o &n+s

$ !d /ros_workspa!e$ sudo apt-get install ros-fuerte-!ontrol$ s4n !he!kout http //4anadium-ros-pkg.google!ode.!om/s4n/trunk/arbotix$ rosmake arbotix

NOTE T#e arbotix st&'( is &)so &*&i)&%)e &s & De%i&n &'(&"e. :o-e*er/ -e nee+t#e )&test *ersion ,ro trun( -#i'# ,i;es & s &)) %u" in #o- t#e o+o etr! +&t& is

u%)is#e+.

7#* T &ting t( Si $+ato%To &(e sure e*er!t#in" is -or(in"/ &(e sure ros!ore is runnin"/ t#en )&un'# t#esi u)&te+ Turt)eBot &s ,o))o-s

$ roslaun!h rbx5_bringup fake_turtlebot.laun!h

-#i'# s#ou)+ resu)t in t#e ,o))o-in" st&rtu ess&"es

pro!essGarbotix-5H started with pid GI;9:Hpro!essGrobot_state_publisher-6H started with pid GI;97Hpro!essGodom_left_wheel_broad!aster->H started with pid GI;9;Hpro!essGodom_right_wheel_broad!aster-IH started with pid GI;99HG1@D"H GJall*ime 5>>;:;5>;=.8:;=>9H %rbotiK being simulated.G1@D"H GJall*ime 5>>;:;5>;=.555I96H #tarted Liff&ontrollerbase_!ontroller . (eometry 8.6:m wideM I588.8 ti!ks/m.

To use & o+e) o, Pi Ro%ot inste&+/ run t#e 'o &n+

$ roslaun!h rbx5_bringup fake_pi_robot.laun!h

Ne;t/ %rin" u RViz so -e '&n o%ser*e t#e si u)&te+ ro%ot in &'tion

$ rosrun r4iz r4iz -d Frospa!k find rbx5_na4F/sim_fuerte.4!g

(nstalling the +rboti) #imulator *0


46/246

8Note #o- -e use t#e Linu; %&'(ti'( o er&tor to"et#er -it# t#e rospa!k find'o &n+ to )o'&te t#e rbx5_na4 &'(&"e -it#out #&*in" to t! e t#e entire &t#.9

I, e*er!t#in" -ent ri"#t/ !ou s#ou)+ see t#e Turt)eBot 8or Pi Ro%ot9 in RViz . 8T#e+e,&u)t *ie- is to +o-n. To '#&n"e *ie-s/ ')i'( on t#e Pan +& enu in RViz .9

To test t#e si u)&tion/ o en &not#er ter in&) -in+o- &n+ run t#e ,o))o-in" 'o &n+-#i'# s#ou)+ '&use t#e si u)&te+ ro%ot to o*e in & 'ounter ')o'(-ise 'ir')e

$ rostopi! pub -r 58 /!md_4el geometry_msgs/*wist ANlinear Nx 8.6M y8M z 8OM angular Nx 8M y 8M z 8.=OOA

T#e *ie- in RViz s#ou)+ )oo( so et#in" )i(e t#is

To sto t#e rot&tion/ t! e &trl-& in t#e s& e ter in&) -in+o-/ t#en u%)is# t#e e t!*wist ess&"e

$ rostopi! pub -5 /!md_4el geometry_msgs/*wist ANOA



47/246

7#/ R$nning t( Si $+ato% 3it( Yo$% O3n Ro.otI, !ou #&*e & URDF o+e) o, !our o-n ro%ot/ !ou '&n run it in t#e Ar%oti si u)&torinste&+ o, t#e Turt)eBot or Pi Ro%ot. First/ &(e & 'o ! o, t#e ,&(e Turt)eBot )&un'#,i)e

$ ros!d rbx5_bringup/laun!h$ !p fake_turtlebot.laun!h fake_my_robot.laun!h

T#en %rin" u !our )&un'# ,i)e in !our ,&*orite e+itor. At ,irst it -i)) )oo( )i(e t#is

Plaun!hQ P -- Eoad the URLD/Ka!ro model of our robot --Q Parg name,3urdf_file3 default,3$ find xa!ro /xa!ro.py A$ findturtlebot_des!ription /urdf/turtlebot.urdf.xa!roA3 /Q

Pparam name,3robot_des!ription3 !ommand,3$ arg urdf_file 3 /Q

Pnode name,3arbotix3 pkg,3arbotix_python3 type,3dri4er.py3 output,3s!reen3Q Prosparam file,3$ find rbx5_bringup /!onfig/fake_turtlebot_arbotix.yaml3!ommand,3load3 /Q Pparam name,3sim3 4alue,3true3/Q P/nodeQ

Pnode name,3robot_state_publisher3 pkg,3robot_state_publisher3type,3state_publisher3Q Pparam name,3publish_freSuen!y3 type,3double3 4alue,368.83 /Q P/nodeQ

P -- Je need a stati! transforms for the wheels --Q Pnode pkg,3tf3 type,3stati!_transform_publisher3name,3odom_left_wheel_broad!aster3 args,38 8 8 8 8 8 /base_link /left_wheel_link5883 /Q Pnode pkg,3tf3 type,3stati!_transform_publisher3

name,3odom_right_wheel_broad!aster3 args,38 8 8 8 8 8 /base_link/right_wheel_link 5883 /QP/laun!hQ

As !ou '&n see/ t#e URDF o+e) is )o&+e+ ne&r t#e to o, t#e ,i)e. Si )! re )&'e t#e &'(&"e &n+ &t# n& es to oint to !our o-n URDF &'ro ,i)e. Jou '&n )e&*e ost o,t#e rest o, t#e )&un'# ,i)e t#e s& e. :o-e*er/ !ou ro%&%)! +on=t nee+ t#e st&ti'tr&ns,or s &t t#e en+ o, t#e ,i)e -#i'# &re t#ere ,or t#e Turt)eBot -#ee)s. So !ou '&n+e)ete t#ose )ines. T#e resu)t -ou)+ )oo( so et#in" )i(e t#is

Plaun!hQ P -- Eoad the URLD/Ka!ro model of our robot --Q Parg name,3urdf_file3 default,3$ find xa!ro /xa!ro.py A$ findT"UR_ %&'%()_@%0) /T"UR_URLD_ %*+A3 /Q

Pparam name,3robot_des!ription3 !ommand,3$ arg urdf_file 3 /Q

Pnode name,3arbotix3 pkg,3arbotix_python3 type,3dri4er.py3 output,3s!reen3Q Prosparam file,3$ find rbx5_bringup /!onfig/fake_turtlebot_arbotix.yaml3!ommand,3load3 /Q Pparam name,3sim3 4alue,3true3/Q


http://www.ros.org/wiki/urdf/Tutorialshttp://www.ros.org/wiki/urdf/Tutorials


48/246

P/nodeQ

Pnode name,3robot_state_publisher3 pkg,3robot_state_publisher3type,3state_publisher3Q Pparam name,3publish_freSuen!y3 type,3double3 4alue,368.83 /Q P/nodeQP/laun!hQ

I, !our ro%ot #&s &n &r or & &n &n+ ti)t #e&+/ !ou '&n st&rt -it# t#efake_pi_robot.laun!h ,i)e &s & te )&te.



49/246

5# CONTROLLIN= A M OBILE B ASEIn t#is '#& ter -e -i)) )e&rn #o- to 'ontro) & o%i)e %&se t#&t uses& &ir o, +i,,erenti&) +ri*e -#ee)s &n+ & &ssi*e '&ster -#ee) ,or

%&)&n'e. ROS '&n &)so %e use+ to 'ontro) &n o ni +ire'tion&) %&se&s -e)) &s ,)!in" ro%ots or un+er-&ter *e#i')es %ut & )&n+ %&se++i,,erenti&) +ri*e ro%ot is & "oo+ )&'e to st&rt.

5#" Unit& an, Coo%,inat Sy&t &Be,ore -e '&n sen+ o*e ent 'o &n+s to our ro%ot/ -e nee+ tore*ie- t#e e&sure ent units &n+ 'oor+in&te s!ste 'on*entions use+ in ROS.

?#en -or(in" -it# re,eren'e ,r& es/ (ee in in+ t#&t ROSuses & ri"#t #&n+ 'on*ention ,or orientin" t#e 'oor+in&te &;es&s s#o-n on )e,t. T#e in+e; &n+ i++)e,in"ers oint &)on" t#e ositi*e ; &n+ ! &;es&n+ t#e t#u % oints in t#e +ire'tion o, t#e

ositi*e 7 &;is. T#e +ire'tion o, & rot&tion&%out &n &;is is +e,ine+ %! t#e ri"#t #&n+ru)e s#o-n on t#e ri"#t i, !ou oint !ourt#u % in t#e ositi*e +ire'tion o, &n! &;is/

!our ,in"ers 'ur) in t#e +ire'tion o, & ositi*e rot&tion. For & o%i)e ro%ot usin" ROS/t#e ; &;is oints ,or-&r+/ t#e ! &;is oints to t#e )e,t &n+ t#e 7 &;is oints u -&r+.

Un+er t#e ri"#t #&n+ ru)e/ & ositi*e rot&tion o, t#e ro%ot &%out t#e 7 &;is is'ounter')o'(-ise -#i)e & ne"&ti*e rot&tion is ')o'(-ise.

Re e %er &)so t#&t ROS uses t#e etri' s!ste so t#&t )ine&r *e)o'ities &re &)-&!ss e'i,ie+ in eters er se'on+ 8 s9 &n+ &n"u)&r *e)o'ities &re "i*en in r&+i&ns erse'on+ 8r&+ s9. A )ine&r *e)o'it! o, 0. s is &'tu&))!


50/246

'.2.1 Motors( Wheels( an )nco ers

ost +i,,erenti&) +ri*e ro%ots runnin" ROS use en'o+ers on t#e +ri*e otors or -#ee)s.An en'o+er re"isters & 'ert&in nu %er o, ti'(s 8usu&))! #un+re+s or e*en t#ous&n+s9 erre*o)ution o, t#e 'orres on+in" -#ee). Kno-in" t#e +i& eter o, t#e -#ee)s &n+ t#e+ist&n'e %et-een t#e / en'o+er ti'(s '&n %e 'on*erte+ to t#e +ist&n'e tr&*e)e+ in eters

or t#e &n")e rot&te+ in r&+i&ns. To 'o ute s ee+/ t#ese *&)ues &re si )! +i*i+e+ %!t#e ti e inter*&) %et-een e&sure ents.

T#is intern&) otion +&t& is 'o))e'ti*e)! (no-n &s o,o t%y &n+ ROS &(es #e&*! useo, it &s -e s#&)) see. It #e) s i, !our ro%ot #&s &''ur&te &n+ re)i&%)e en'o+ers %ut -#ee)+&t& '&n %e &u" ente+ usin" ot#er sour'es. For e;& )e/ t#e Turt)eBot uses & sin")e&;is "!ro to ro*i+e &n &++ition&) e&sure o, t#e ro%ot=s rot&tion&) otion sin'e t#eiRo%ot Cre&te=s en'o+ers &re not&%)! in&''ur&te +urin" rot&tions.

It is i ort&nt to (ee in in+ t#&t no &tter #o- &n! sour'es o, o+o etr! +&t& &reuse+/ t#e actual osition &n+ s ee+ o, t#e ro%ot in t#e -or)+ '&n 8&n+ ro%&%)! -i))9+i,,er ,ro t#e *&)ues re orte+ %! t#e o+o etr!. T#e +e"ree o, +is're &n'! -i)) *&r!+e en+in" on t#e en*iron ent&) 'on+itions &n+ t#e re)i&%i)it! o, t#e o+o etr! sour'es.

'.2.2 Motor Controllers an &ri*ers

At t#e )o-est )e*e) o, otion 'ontro) -e nee+ & +ri*er ,or t#e ro%ot=s otor 'ontro))ert#&t '&n turn t#e +ri*e -#ee)s &t & +esire+ s ee+/ usu&))! usin" intern&) units su'# &sen'o+er ti'(s er se'on+ or & er'ent&"e o, &; s ee+. ?it# t#e e;'e tion o, t#e?i))o- G&r&"e PR2 &n+ Turt)eBot/ t#e 'ore ROS &'(&"es +o not in')u+e +ri*ers ,ors e'i,i' otor 'ontro))ers. :o-e*er/ & nu %er o, t#ir+ &rt! ROS +e*e)o ers #&*e

u%)is#e+ +ri*ers ,or so e o, t#e ore o u)&r 'ontro))ers &n+ or ro%ots su'# &s t#e

Ar%oti / Seri&)i7er / LEGO6 N T &n+ Ro*io . 8For & ore 'o )ete )ist o, su orte+ )&t,or s/ see Ro%ots Usin" ROS .9

'.2.3 +he ROS !ase Controller

At t#e ne;t )e*e) o, &%str&'tion/ t#e +esire+ s ee+ o, t#e ro%ot is s e'i,ie+ in re&) -or)+units su'# &s eters &n+ r&+i&ns er se'on+. It is &)so 'o on to e )o! so e ,or o,PID 'ontro). PID st&n+s ,or @Pro ortion&) Inte"r&) Deri*&ti*e@ &n+ is so n& e+ %e'&uset#e 'ontro) &)"orit# 'orre'ts t#e -#ee) *e)o'ities %&se+ not on)! on t#e +i,,eren'e8 ro ortion&)9 error %et-een t#e &'tu&) &n+ +esire+ *e)o'it!/ %ut &)so on t#e +eri*&ti*e&n+ inte"r&) o*er ti e. Jou '&n )e&rn ore &%out PID 'ontro) on ?i(i e+i& . For our

ur oses/ -e si )! nee+ to (no- t#&t t#e 'ontro))er -i)) +o its %est to o*e t#e ro%otin t#e -&! -e #&*e re


51/246

t#e ro%ot. A nu %er o, %&se 'ontro))ers '&n &)so %e si u)&te+ in G&7e%o in')u+in" t#eTurt)eBot / PR2 &n+Err&ti' .

T#e %&se 'ontro))er no+e t! i'&))! u%)is#es o+o etr! +&t& on t#e /odom to i' &n+)istens ,or otion 'o &n+s on t#e /!md_4el to i'. At t#e s& e ti e/ t#e 'ontro))erno+e t! i'&))! 8%ut not &)-&!s9 u%)is#es & tr&ns,or ,ro t#e /odom ,r& e to t#e %&se

,r& e eit#er /base_link or /base_footprint . ?e s&! @not &)-&!s@ %e'&useso e ro%ots )i(e t#e Turt)eBot/ uses t#e robot_pose_ekf &'(&"e to 'o %ine -#ee)o+o etr! &n+ "!ro +&t& to "et & ore &''ur&te esti &te o, t#e ro%ot=s osition &n+orient&tion. In t#is '&se/ it is t#e robot_pose_ekf no+e t#&t u%)is#es t#e tr&ns,or,ro /odom to /base_footprint . 8T#e robot_pose_ekf &'(&"e i )e ents &nE;ten+e+ K&) &n Fi)ter &s !ou '&n re&+ &%out on t#e ?i(i &"e )in(e+ to &%o*e.9

On'e -e #&*e & %&se 'ontro))er ,or our ro%ot/ ROS ro*i+es t#e too)s -e nee+ to issueotion 'o &n+s eit#er ,ro t#e 'o &n+ )ine or %! usin" ot#er ROS no+es to

u%)is# t#ese 'o &n+s %&se+ on & #i"#er )e*e) )&n. At t#is )e*e)/ it +oes not &tter-#&t #&r+-&re -e &re usin" ,or our %&se 'ontro))er our ro"r& in" '&n ,o'us ure)!on t#e +esire+ )ine&r &n+ &n"u)&r *e)o'ities in re&) -or)+ units &n+ &n! 'o+e -e -rites#ou)+ -or( on &n! %&se 'ontro))er -it# & ROS inter,&'e.

'.2.4 ,rame-!ase Motion using the mo*e /ase ROS 0ackage

At t#e ne;t )e*e) o, &%str&'tion/ ROS ro*i+es t#e mo4e_base &'(&"e t#&t &))o-s usto s e'i,! & t&r"et osition &n+ orient&tion o, t#e ro%ot -it# res e't to so e ,r& e o,re,eren'e mo4e_base -i)) t#en &tte t to o*e t#e ro%ot to t#e "o&) -#i)e &*oi+in"o%st&')es. T#e mo4e_base &'(&"e is & *er! so #isti'&te+ &t# )&nner &n+ 'o %ineso+o etr! +&t& -it# %ot# )o'&) &n+ ")o%&) 'ost & s -#en se)e'tin" & &t# ,or t#e ro%ot

to ,o))o-. It &)so 'ontro)s t#e )ine&r &n+ &n"u)&r *e)o'ities &n+ &''e)er&tions&uto &ti'&))! %&se+ on t#e ini u &n+ &;i u *&)ues -e set in t#e 'on,i"ur&tion,i)es.

'.2. S M using the gma ing an amcl ROS 0ackages

At &n e*en #i"#er )e*e)/ ROS en&%)es our ro%ot to 're&te & & o, its en*iron ent usin"t#e SLA gmapping &'(&"e. T#e & in" ro'ess -or(s %est usin" & )&ser s'&nner

%ut '&n &)so %e +one usin" & Kine't or Asus tion +e t# '& er& to ro*i+e & si u)&te+)&ser s'&n. I, !ou o-n & Turt)eBot/ t#e Turt)eBot st&'( in')u+es &)) t#e too)s !ou nee+ to+o SLA .

On'e & & o, t#e en*iron ent is &*&i)&%)e/ ROS ro*i+es t#e am!l &'(&"e 8&+& ti*eonte C&r)o )o'&)i7&tion9 ,or &uto &ti'&))! )o'&)i7in" t#e ro%ot %&se+ on its 'urrent s'&n&n+ o+o etr! +&t&. T#is &))o-s t#e o er&tor to oint &n+ ')i'( on &n! )o'&tion on & &&n+ t#e ro%ot -i)) ,in+ its -&! t#ere -#i)e &*oi+in" o%st&')es. 8For & su er%

*ontrolling a ,obile Base /"

http://www.ros.org/wiki/turtlebot_simulatorhttp://www.ros.org/wiki/pr2_simulator/Tutorialshttp://www.ros.org/wiki/pr2_simulator/Tutorialshttp://www.ros.org/wiki/pr2_simulator/Tutorialshttp://www.ros.org/wiki/erratic_robothttp://ros.org/wiki/robot_pose_ekfhttp://ros.org/wiki/robot_pose_ekfhttp://www.ros.org/wiki/move_basehttp://www.ros.org/wiki/gmappinghttp://www.ros.org/wiki/gmappinghttp://ros.org/wiki/Robots/TurtleBothttp://www.ros.org/wiki/amclhttp://www.ros.org/wiki/turtlebot_simulatorhttp://www.ros.org/wiki/pr2_simulator/Tutorialshttp://www.ros.org/wiki/erratic_robothttp://ros.org/wiki/robot_pose_ekfhttp://www.ros.org/wiki/move_basehttp://www.ros.org/wiki/gmappinghttp://ros.org/wiki/Robots/TurtleBothttp://www.ros.org/wiki/amcl


52/246

intro+u'tion to t#e &t#e &ti's un+er)!in" SLA / '#e'( out Se%&sti&n T#run=s on)ineArti,i'i&) Inte))i"en'e 'ourse on U+&'it!.9

'.2.5 Semantic Goals

Fin&))!/ &t t#e #i"#est )e*e) o, &%str&'tion/ otion "o&)s &re s e'i,ie+ se &nti'&))! su'#

&s @"o to t#e (it'#en &n+ %rin" e & %eer@/ or si )!/ @%rin" e & %eer@. In t#is '&se/t#e se &nti' "o&) ust %e &rse+ &n+ tr&ns)&te+ into & series o, &'tions. For &'tionsre


53/246

5#/ T3i&ting an, T$%ning 3it( ROSROS uses t#e *wist ess&"e t! e 8see +et&i)s %e)o-9 ,or u%)is#in" otion 'o &n+sto %e use+ %! t#e %&se 'ontro))er. ?#i)e -e 'ou)+ use &) ost &n! n& e ,or & to i'/ it isusu&))! '&))e+ /!md_4el -#i'# is s#ort ,or @'o &n+ *e)o'ities@. T#e %&se 'ontro))erno+e su%s'ri%es to t#e /!md_4el to i' &n+ tr&ns)&tes *wist ess&"es into otorsi"n&)s t#&t &'tu&))! turn t#e -#ee)s.

To see t#e 'o onents o, & *wist ess&"e/ run t#e 'o &n+

$ rosmsg show geometry_msgs/*wist

-#i'# -i)) ro+u'e t#e ,o))o-in" out ut

geometry_msgs/Ve!tor> linearfloat:I xfloat:I yfloat:I z

geometry_msgs/Ve!tor> angularfloat:I xfloat:I yfloat:I z

As !ou '&n see/ t#e *wist ess&"e is 'o ose+ o, t-o su% ess&"es -it# t! eVe!tor> / one ,or t#e ;/ ! &n+ 7 )ine&r *e)o'it! 'o onents &n+ &not#er ,or t#e ;/ ! &n+7 &n"u)&r *e)o'it! 'o onents. Line&r *e)o'ities &re s e'i,ie+ in eters er se'on+ &n+&n"u)&r *e)o'ities &re "i*en in r&+i&ns er se'on+. 81 r&+i&n e


54/246

Noti'e #o- -e +e)ine&te t#e su% ess&"es usin" %r&'es &n+ se &r&te t#e 'o onentn& es ,ro t#eir *&)ues -it# & 'o)on &n+ & s &'e 8t#e s &'e is re


55/246


56/246


No- )et=s tr! & se'on+ e;& )e. First ')e&r t#e o+o etr! &rro-s %! ')i'(in" t#e R & t %utton in RViz . T#e ,o))o-in" &ir o, 'o &n+s 8se &r&te+ %! & se i 'o)on9 -i)) ,irsto*e t#e ro%ot str&i"#t ,or &%out 4 se'on+s 8t#e @ -5 @ o tion e&ns @ u%)is# on'e@9/t#en 'ontinue in+e,inite)! in & 'ounter ')o'(-ise 'ir')e

$ rostopi! pub -5 /!md_4el geometry_msgs/*wist ANlinear Nx 8.6M y 8Mz 8OM angular Nx 8M y 8M z 8OOA rostopi! pub -r 58 /!md_4elgeometry_msgs/*wist ANlinear Nx 8.6M y 8M z 8OM angular Nx 8My 8M z 8.=OOA

To sto t#e ro%ot/ t! e &trl-& in t#e s& e ter in&) -in+o- &n+ u%)is# t#e e t!*wist ess&"e


Be,ore -e tr! out so e *wist ess&"es on & re&) ro%ot/ -e #&*e to s en+ & ,e-o ents t&)(in" &%out '&)i%r&tion.

5#1 Ca+i.%ating Yo$% Ro.ot & O,o t%yI, !ou +on=t #&*e & ro%ot/ !ou '&n s(i t#is se'tion &)to"et#er. I, !ou #&*e & Turt)eBot/

%e sure use to use t#e &uto &te+ '&)i%r&tion routine to set t#e &n"u)&r 'orre'tion ,&'tors,or !our ro%ot. Jou '&n sti)) use t#e ,irst &rt o, t#is se'tion to set t#e )ine&r 'orre'tion,&'tor. Note t#&t in &)) '&ses !ou i"#t #&*e to use +i,,erent '&)i%r&tion &r& eters ,or+i,,erent t! es o, ,)oor sur,&'es e.". '&r et *ersus #&r+-oo+. T#e e&siest -&! to&n&"e t#e +i,,erent &r& eters is to use +i,,erent )&un'# ,i)es ,or e&'# sur,&'e.

I, !ou &re usin" !our o-n 'usto %ui)t ro%ot/ !ou i"#t &)re&+! #&*e !our o-n'&)i%r&tion et#o+. I, so/ !ou '&n s&,e)! s(i t#is se'tion. Ot#er-ise/ re&+ on.

Be,ore runnin" t#e '&)i%r&tion routines/ %e sure to "et t#e Oro'os (ine &ti's &'(&"esusin" t#e 'o &n+

$ sudo apt-get install ros-fuerte-oro!os-kinemati!s-dynami!s

T#e rbx5_na4 &'(&"e in')u+es t-o '&)i%r&tion s'ri ts !alibrate_linear.py &n+!alibrate_angular.py . T#e ,irst &tte ts to o*e t#e ro%ot 1.0 eters ,or-&r+ %!

onitorin" t#e /odom to i' &n+ sto in" -#en t#e re orte+ +ist&n'e is -it#in 1 ' o,t#e t&r"et. Jou '&n &+>ust t#e t&r"et +ist&n'e &n+ o*e ent s ee+ %! e+itin" t#e s'ri tor %! usin" dynami!_re!onfigure . T#e se'on+ s'ri t rot&tes t#e ro%ot 4 0 +e"rees/&"&in %! onitorin" t#e /odom to i'. ?e=)) +es'ri%e #o- to &+>ust !our PID &r& eters

%&se+ on t#e resu)ts in t#e ne;t t-o se'tions.

*ontrolling a ,obile Base /7

http://www.ros.org/wiki/turtlebot_calibration/Tutorials/Calibrate%20Odometry%20and%20Gyrohttp://www.ros.org/wiki/turtlebot_calibration/Tutorials/Calibrate%20Odometry%20and%20Gyrohttp://www.ros.org/wiki/turtlebot_calibration/Tutorials/Calibrate%20Odometry%20and%20Gyro


57/246

'.4.1 inear Cali/ration

First &(e sure !ou #&*e )ent! o, roo in ,ront o, !our ro%ot &t )e&st 2 eters ,or t#e+e,&u)t test +ist&n'e o, 1.0 eters. Usin" & t& e e&sure/ )&! out &t )e&st 1 eter o, t& eon t#e ,)oor &n+ &)i"n t#e st&rtin" en+ o, t#e t& e -it# so e i+enti,i&%)e &r( on !ourro%ot. Rot&te t#e ro%ot so t#&t it is &i e+ &r&))e) to t#e t& e.

Ne;t/ %rin" u u !our ro%ot=s %&se 'ontro))er -it# t#e & ro ri&te )&un'# ,i)e. For &Turt)eBot/ !ou -ou)+ run

$ roslaun!h turtlebot_bringup turtlebot.laun!h

Ne;t/ run dynami!_re!onfigure

$ rosrun dynami!_re!onfigure re!onfigure_gui

Fin&))!/ run t#e )ine&r '&)i%r&tion no+e

$ rosrun rbx5_na4 !alibrate_linear.py

Return to t#e dynami!_re!onfigure -in+o- &n+ se)e't t#e !alibrate_linearno+e. To st&rt t#e test/ '#e'( t#e '#e'(%o; %esi+e start_test . Jour ro%ot s#ou)+o*e ,or-&r+ & ro;i &te)! 1 eter. To "et t#e 'orre'tion ,&'tor/ ,o))o- t#ese ste s

• e&sure t#e &'tu&) +ist&n'e -it# t#e t& e &n+ &(e & note o, it.

• Di*i+e t#e &'tu&) +ist&n'e %! t#e t&r"et +ist&n'e &n+ &(e & note o, t#e r&tio.

• Return to t#e dynami!_re!onfigure GUI &n+multiply t#e

odom_linear_s!ale_!orre!tion *&)ue %! t#e r&tio !ou >ust 'o ute+. Sett#e &r& eter to t#e ne- *&)ue.

• Re e&t t#e test %! o*in" t#e ro%ot %&'( to t#e st&rt o, t#e t& e/ t#en '#e'(in"t#e start_test '#e'(%o; in t#e dynami!_re!onfigure -in+o-.

• Continue re e&tin" t#e test unti) !ou &re s&tis,ie+ -it# t#e resu)t. An &''ur&'!o, 1 ' in 1 eter is ro%&%)! "oo+ enou"#.

?it# !our ,in&) 'orre'tion ,&'tor in #&n+/ !ou nee+ to & )! it to t#e &r& eters o, !ourro%ot=s %&se 'ontro))er usin" t#e & ro ri&te )&un'# ,i)e. For & Turt)eBot/ ,irst t&(e t#e

reciprocal o, t#e nu %er !ou ,oun+ &%o*e. Let=s '&)) t#e resu)tin" nu %er . A++ t#e,o))o-in" )ine to !our turtlebot.laun!h ,i)e

Pparam name,3turtlebot_node/odom_linear_s!ale_!orre!tion3 4alue,3K3/Q

su%stitutin" !our *&)ue -#ere in+i'&te+ %! t#e .



58/246


59/246

• :o e,u))! t#e rot&tion -i)) %e ')oser to 4 0 +e"rees. I, it is sti)) s#ort/ +e're&set#e odom_angular_s!ale_!orre!tion &r& eter & )itt)e &n+ tr! &"&in. I, itrot&tes too ,&r/ in're&se it & )itt)e &n+ tr! &"&in.

• Re e&t unti) !ou &re #& ! -it# t#e resu)t.

?#&t !ou +o -it# !our ,in&) 'orre'tion ,&'tor +e en+s on !our %&se 'ontro))er=s PID &r& eters. For & ro%ot 'ontro))e+ %! t#e Ar%oti %&se 'ontro))er/ e+it !our JA L'on,i"ur&tion ,i)e &n+ '#&n"e t#e base_width &r& eter %! "i&i"ing it %! !our'orre'tion ,&'tor.

As & ,in&) '#e'(/ )&un'# !our ro%ot=s st&rtu ,i)e -it# t#e ne- 'orre'tion ,&'tor. T#enrun t#e !alibration_angular.py s'ri t %ut -it# t#eodom_angular_s!ale_!orre!tion &r& eter set to 1.0. Jour ro%ot s#ou)+ no-rot&te 4 0 +e"rees -it#out ,urt#er 'orre'tion.

5#0 S n,ing Twist M &&ag & to a R a+ Ro.otI, !ou #&*e & Turt)eBot or &n! ot#er ro%ot t#&t )istens on t#e !md_4el to i' ,or otion'o &n+s/ !ou '&n tr! so e *wist ess&"es in t#e re&) -or)+. A)-&!s %e sure to st&rt-it# s &)) *&)ues ,or t#e )ine&r &n+ &n"u)&r s ee+s. Tr! & ure rot&tion ,irst so t#&t !ourro%ot -i)) not su++en)! ,)! &'ross t#e roo &n+ ruin t#e ,urniture.

First o-er on !our ro%ot &n+ )&un'# t#e & ro ri&te st&rtu ,i)es. I, !ou #&*e &Turt)eBot/ !ou -ou)+ run t#e ,o))o-in" on t#e ro%ot=s )& to


Or/ i, !ou #&*e !our o-n )&un'# ,i)e t#&t in')u+es !our '&)i%r&tion &r& eters/ run t#&t,i)e inste&+.

To rot&te t#e ro%ot 'ounter')o'(-ise in )&'e &t 1.0 r&+i&ns er se'on+ 8&%out se'on+s er re*o)ution9/ run t#e 'o &n+

$ rostopi! pub -r 58 /!md_4el geometry_msgs/*wist ANlinear Nx 8M y 8Mz 8OM angular Nx 8M y 8M z 5.8OOA

To sto t#e ro%ot/ t! e &trl-& &n+/ i, ne'ess&r!/ u%)is# t#e e t! *wist ess&"e


8A Turt)eBot s#ou)+ sto on its o-n &,ter t! in" &trl-& t#e ,irst ti e.9



60/246


61/246

T#e s'ri t '&n %e ,oun+ in t#e ,i)e timed_out_and_ba!k.py in t#erbx5_na4/nodes su%+ire'tor!. Be,ore )oo(in" &t t#e 'o+e/ )et=s tr! it in t#e Ar%otisi u)&tor.

'.5.2 +ime Out-an -!ack in the r/oti8 Simulator

I, t#e si u)&tor is not &)re&+! runnin"/ st&rt it -it# t#e 'o &n+


8I, +esire+/ re )&'e t#e fake_turtlebot.laun!h ,i)e -it# t#e one ,or Pi Ro%ot or!our o-n ro%ot. It -on=t &(e & +i,,eren'e to t#e resu)ts.9

I, RViz is not &)re&+! runnin"/ ,ire it u no-


Fin&))!/ run t#e timed_out_and_ba!k.py no+e

$ rosrun rbx5_na4 timed_out_and_ba!k.py

:o e,u))! RViz -i)) s#o- !our ro%ot e;e'utin" &n out &n+ %&'( &neu*er &n+ t#e en+resu)t -i)) )oo( so et#in" )i(e t#e ,o))o-in"

*ontrolling a ,obile Base 1"


62/246

T#e )&r"e &rro-s re resent t#e osition &n+ orient&tion o, t#e ro%ot &t *&rious oints&)on" its tr&>e'tor! &s re orte+ %! t#e ro%ot=s 8,&(e9 intern&) o+o etr!. ?e -i)) )e&rn#o- to &(e use o, t#is o+o etr! in,or &tion in t#e ne;t ,e- se'tions.

So ,&r/ t#in"s )oo( rett! "oo+ in t#e i+e&) si u)&tor. But %e,ore tr!in" it out on & re&)ro%ot/ )et=s )oo( &t t#e 'o+e.

'.5.3 +he +ime Out-an -!ack Scri t

:ere is t#e ,u)) s'ri t ,or t#e ti e+ out &n+ %&'( no+e. A,ter t#e )istin"/ -e -i)) %re&( it+o-n into s &))er ie'es.

Lin) to SVN &o$%4 ! timed_out-and-ba!k.py

1 /usr/bin/en4 python2 import roslib roslib.load_manifest 'rbx1_nav'4 import rospy

from geometry_msgs.msg import *wistfrom math import pi

!lass OutAndBack3 def __init__ self 5 (i4e the node a name10 rospy.init_node 'out_and_back' M anonymous, Dalse1112 #et rospy to exe!tute a shutdown fun!tion when exiting14 rospy.on_shutdown self .shutdown1 1 ublisher to !ontrol the robotAs speed1 self .!md_4el , rospy. ublisher '/cmd_vel' M *wist1

13 +ow fast will we update the robotAs mo4ementW15 rate , =820 21 #et the eSui4alent R"# rate 4ariable22 r , rospy.Rate rate24 2 #et the forward linear speed to 8.6 meters per se!ond2 linear_speed , 8.62 2 #et the tra4el distan!e to 5.8 meters23 goal_distan!e , 5.825 40 +ow long should it take us to get thereW41 linear_duration , goal_distan!e / linear_speed

42 44 #et the rotation speed to 5.8 radians per se!ond4 angular_speed , 5.84 4 #et the rotation angle to i radians 5;8 degrees4 goal_angle , pi43 45 +ow long should it take to rotateW

*ontrolling a ,obile Base 1*

http://code.google.com/p/ros-by-example/source/browse/trunk/rbx_vol_1/rbx1_nav/nodes/timed_out_and_back.pyhttp://code.google.com/p/ros-by-example/source/browse/trunk/rbx_vol_1/rbx1_nav/nodes/timed_out_and_back.py


63/246

0 angular_duration , goal_angle / angular_speed1 2 Eoop through the two legs of the trip4 for i in range 6

1nitialize the mo4ement !ommand mo4e_!md , *wist #et the forward speed

3 mo4e_!md.linear.x , linear_speed5 0 0o4e forward for a time to go 5 meter1 ti!ks , int linear_duration C rate2 4 for t in range ti!ks

self .!md_4el.publish mo4e_!md r.sleep #top the robot before the rotation

3 mo4e_!md , *wist5 self .!md_4el.publish mo4e_!md0 rospy.sleep 5

1 2 @ow rotate left roughly 5;8 degrees4

#et the angular speed mo4e_!md.angular.z , angular_speed

Rotate for a time to go 5;8 degrees3 ti!ks , int goal_angle C rate5 0 for t in range ti!ks 1 self .!md_4el.publish mo4e_!md2 r.sleep4

#top the robot before the next leg

mo4e_!md , *wist self .!md_4el.publish mo4e_!md rospy.sleep 5

3 5 #top the robot30 self .!md_4el.publish *wist31 32 def shutdown self 34 %lways stop the robot when shutting down the node.3 rospy.loginfo "Stopping the robot..."3 self .!md_4el.publish *wist3 rospy.sleep 53 33 if __name__ ,, '__main__'35 try50 "ut%nd


64/246

1 /usr/bin/en4 python2 import roslib roslib.load_manifest 'rbx1_nav'4 import rospy

I, !ou +i+ t#e ROS Be"inner Tutori&)s in P!t#on/ !ou=)) &)re&+! (no- t#&t &)) o, ourROS no+es %e"in -it# t#ese t#ree )ines/ &)t#ou"# t#e n& e o, t#e &ni,est 8 rbx5_na4

in t#is '&se9 -i)) re,)e't t#e &'(&"e t#e no+e %e)on"s to.

from geometry_msgs.msg import *wistfrom math import pi

:ere -e t&(e '&re o, &n! ot#er i orts -e nee+ ,or t#e s'ri t. In t#is '&se/ -e -i)) nee+t#e *wist ess&"e t! e ,ro t#e ROS geometry_msgs &'(&"e &n+ t#e 'onst&nt pi,ro t#e P!t#on math o+u)e. Note t#&t & 'o on sour'e o, i ort errors is to ,or"etto in')u+e t#e ne'ess&r! ROS Pdepend pa!kageQ )ine in !our &'(&"e=smanifest.xml ,i)e. In t#is '&se/ our manfest.xml ,i)e #&s to in')u+e t#e )ine

Pdepend pa!kage,3geometry_msgs3/Q

so t#&t -e '&n i ort *wist ,ro geometry_msgs.msg .

!lass OutAndBack3 def __init__ self

:ere -e %e"in t#e &in %o+! o, our ROS no+e %! +e,inin" it &s & P!t#on ')&ss &)on"-it# t#e st&n+&r+ ')&ss initi&)i7&tion )ine.

5 (i4e the node a name10 rospy.init_node 'out_and_back' M anonymous, Dalse1112 #et rospy to exe!tute a shutdown fun!tion when exiting14 rospy.on_shutdown self .shutdown

E*er! ROS no+e re


65/246

:ere -e +e,ine our ROS u%)is#er ,or sen+in" *wist 'o &n+s to t#e /!md_4elto i'. ?e &)so set t#e r&te &t -#i'# -e -&nt to u +&te t#e ro%ot=s o*e ent 0 ti es

er se'on+ in t#is '&se.

2 #et the forward linear speed to 8.6 meters per se!ond

2 linear_speed , 8.62 2 #et the tra4el distan!e to 5.8 meters23 goal_distan!e , 5.825 40 +ow long should it take us to get thereW41 linear_duration , linear_distan!e / linear_speed

?e initi&)i7e t#e ,or-&r+ s ee+ to & re)&ti*e)! s&,e 0.2 eters er se'on+ &n+ t#e t&r"et+ist&n'e to 1.0 eters. ?e t#en 'o ute #o- )on" t#is s#ou)+ t&(e.

44 #et the rotation speed to 5.8 radians per se!ond4 angular_speed , 5.84 4 #et the rotation angle to i radians 5;8 degrees4 goal_angle , pi43 45 +ow long should it take to rotateW0 angular_duration , angular_distan!e / angular_speed

Si i)&r)!/ -e set t#e rot&tion s ee+ to 1.0 r&+i&ns er se'on+ &n+ t#e t&r"et &n")e to 130+e"rees or Pi r&+i&ns.

2 Eoop through the two legs of the trip4 for i in range 6

1ntialize the mo4ement !ommand mo4e_!md , *wist #et the forward speed

3 mo4e_!md.linear.x , linear_speed5 0 0o4e forward for a time to go the desired distan!e1 ti!ks , int linear_duration C rate2 4 for t in range ti!ks

self .!md_4el.publish mo4e_!md r.sleep

T#is is t#e )oo t#&t &'tu&))! o*es t#e ro%ot one '!')e ,or e&'# o, t#e t-o )e"s.Re'&)) t#&t so e ro%ots re


66/246

2 @ow rotate left roughly 5;8 degrees4

#et the angular speed mo4e_!md.angular.z , angular_speed

Rotate for a time to go 5;8 degrees3 ti!ks , int goal_angle C rate5 0 for t in range ti!ks 1 self .!md_4el.publish mo4e_!md2 r.sleep

In t#e se'on+ &rt o, t#e )oo / -e rot&te t#e ro%ot &t & r&te o, angular_speed r&+i&ns er se'on+ ,or t#e & ro ri&te +ur&tion 8Pi se'on+s in t#is '&se9 -#i'# s#ou)+ !ie)+ 130+e"rees.

5 #top the robot.30 self .!md_4el.publish *wist

?#en t#e ro%ot #&s 'o )ete+ t#e out &n+ %&'( >ourne!/ -e sto it %! u%)is#in" &ne t! *wist ess&"e 8&)) ,ie)+s &re 09.

32 def shutdown self 34 %lways stop the robot when shutting down the node.3 rospy.loginfo "Stopping the robot..."3 self .!md_4el.publish *wist3 rospy.sleep 5

T#is is our s#ut+o-n '&))%&'( ,un'tion. I, t#e s'ri t is ter in&te+ ,or &n! re&son/ -esto t#e ro%ot %! u%)is#in" &n e t! *wist ess&"e.

33 if __name__ ,, A__main__A35 try50 "ut%nd


67/246

First/ ter in&te &n! runnin" si u)&tions. Ne;t/ &(e sure !our ro%ot #&s )ent! o, rooto -or( in &t )e&st 1. eters e&+ o, it &n+ & eter on eit#er si+e. T#en %rin" u!our ro%ot=s st&rtu )&un'# ,i)e8s9. I, !ou #&*e & Turt)eBot/ !ou -ou)+ run t#e ,o))o-in"on t#e ro%ot=s )& to


Or use !our o-n )&un'# ,i)e i, !ou #&*e 're&te+ one to store !our '&)i%r&tion &r& eters.

?e -i)) &)so use &n &u;i)i&r! s'ri t so t#&t -e '&n see t#e Turt)eBot=s 'o %ine+o+o etr! ,r& e in RViz . 8T#is -i)) %e'o e ')e&rer in t#e ne;t se'tion.9 Jou '&n s(it#is i, !ou &re not usin" & Turt)eBot. T#is )&un'# ,i)e s#ou)+ %e run on t#e Turt)eBot=s)& to

$ roslaun!h rbx5_bringup odom_ekf.laun!h

Ne;t -e=re "oin" to 'on,i"ure RViz to +is )&! t#e 'o %ine+ o+o etr! +&t& 8en'o+ers

"!ro9 r&t#er t#&n /odom -#i'# on)! s#o-s t#e en'o+er +&t&. I, !ou &)re&+! #&*e RVizrunnin" ,ro t#e re*ious test/ !ou '&n si )! un '#e'( t#e O,o t%y +is )&! &n+'#e'( t#e Odometry EKF +is )&!/ t#en s(i t#e ,o))o-in" ste .

I, RViz is not &)re&+! u / run it no- -it# t#e ekf_fuerte 'on,i" ,i)e. T#is ,i)e si )! re se)e'ts t#e odom_ekf to i' ,or +is )&!in" t#e 'o %ine+ o+o etr! +&t&

$ rosrun r4iz r4iz -d Frospa!k find rbx5_na4F/ekf_fuerte.4!g

T#e on)! +i,,eren'e %et-een t#is 'on,i"ur&tion &n+ t#e re*ious one is t#&t -e &re no-+is )&!in" t#e 'o %ine+ o+o etr! +&t& on t#e /odom_ekf to i' r&t#er t#&n >ust t#e

-#ee) en'o+er +&t& u%)is#e+ on t#e /odom to i'. Jou '&n '#e'( %ot# +is )&!s i, !ou-&nt to 'o &re t#e t-o.

Fin&))!/ -e run t#e out &n+ %&'( s'ri t >ust )i(e -e +i+ %e,ore. Note t#&t t#e s'ri t itse),+oes not '&re i, -e &re runnin" & si u)&tion or 'ontro))in" & re&) ro%ot. It is si )!

u%)is#in" *wist ess&"es on t#e /!md_4el to i' ,or &n!one -#o '&res to )isten.T#is is one e;& )e o, #o- ROS &))o-s us to &%str&'t &-&! t#e )o-er )e*e)s o, t#eotion 'ontro) #ier&r'#!.

$ rosrun rbx5_na4 timed_out_and_ba!k.py

*ontrolling a ,obile Base 15


68/246

:ere is t#e resu)t ,or ! o-n Turt)eBot -#en o er&tin" on & )o- )! '&r et

As !ou '&n te)) ,ro t#e i'ture/ t#e ro%ot +i+ not en+ u *er! ')ose to t#e st&rtin" osition. First o, &))/ it +i+ not "o ,&r enou"# %e,ore turnin" &roun+ 8t#e "ri+ s


69/246


70/246

+eader headeruint>6 seStime stampstring frame_id

string !hild_frame_idgeometry_msgs/ oseJith&o4arian!e pose

geometry_msgs/ ose pose

geometry_msgs/ oint positionfloat:I xfloat:I yfloat:I z

geometry_msgs/Xuaternion orientationfloat:I xfloat:I yfloat:I zfloat:I w

float:IG>:H !o4arian!egeometry_msgs/*wistJith&o4arian!e twist

geometry_msgs/*wist twistgeometry_msgs/Ve!tor> linear

float:I xfloat:I yfloat:I z

geometry_msgs/Ve!tor> angularfloat:I xfloat:I yfloat:I z

float:IG>:H !o4arian!e

T#e oseJith&o4arian!e su% ess&"e re'or+s t#e osition &n+ orient&tion o, t#ero%ot -#i)e t#e *wistJith&o4arian!e 'o onent "i*es us t#e )ine&r &n+ &n"u)&rs ee+s &s -e #&*e &)re&+! seen. Bot# t#e pose &n+twist '&n %e su )e ente+ -it# &

!o4arian!e &tri; -#i'# e&sures t#e un'ert&int! in t#e *&rious e&sure ents.

T#e +eader &n+!hild_frame_id +e,ine t#e re,eren'e ,r& es -e &re usin" toe&sure +ist&n'es &n+ &n")es. It &)so ro*i+es & ti est& ,or e&'# ess&"e so -e(no- not on)! -#ere -e &re %ut -#en. B! 'on*ention/ o+o etr! e&sure ents in ROSuse /odom &s t#e &rent ,r& e i+ &n+ /base_link 8or /base_footprint 9 &s t#e'#i)+ ,r& e i+. ?#i)e t#e /base_link ,r& e 'orres on+s to & re&) #!si'&) &rt o, t#ero%ot/ t#e /odom ,r& e is +e,ine+ %! t#e tr&ns)&tions &n+ rot&tions en'& su)&te+ in t#eo+o etr! +&t&. T#ese tr&ns,or &tions o*e t#e ro%ot re)&ti*e to t#e /odom ,r& e. I,-e +is )&! t#e ro%ot o+e) in RViz &n+ set t#e ,i;e+ ,r& e to t#e /odom ,r& e/ t#e

ro%ot=s osition -i)) re,)e't -#ere t#e ro%ot @t#in(s@ it is re)&ti*e to its st&rtin" osition.

5#9 O$t an, Ba4) U&ing O,o t%y No- t#&t -e un+erst&n+ #o- o+o etr! in,or &tion is re resente+ in ROS/ -e '&n %eore re'ise &%out o*in" our ro%ot on &n out &n+ %&'( 'ourse. R&t#er t#&n "uessin"

*ontrolling a ,obile Base 0;


71/246

+ist&n'es &n+ &n")es %&se+ on ti e &n+ s ee+/ our ne;t s'ri t -i)) onitor t#e ro%ot=s osition &n+ orient&tion &s re orte+ %! t#e tr&ns,or %et-een t#e /odom &n+/base_link ,r& es.

T#e ne- ,i)e is '&))e+ odom_out_and_ba!k.py in t#e rbx5_na4/nodes +ire'tor!.Be,ore )oo(in" &t t#e 'o+e/ )et=s 'o &re t#e resu)ts %et-een t#e si u)&tor &n+ & re&)ro%ot.

'.9.1 O ometr6-!ase Out an !ack in the r/oti8 Simulator

I, !ou &)re&+! #&*e & si u)&te+ ro%ot runnin"/ ,irst &trl-& out o, t#e si u)&tion so -e'&n st&rt t#e o+o etr! re&+in"s ,ro s'r&t'#. T#en %rin" u t#e si u)&te+ ro%ot &"&in/run RViz / t#en run t#e odom_out_and_ba!k.py s'ri t &s ,o))o-s



$ rosrun rbx5_na4 odom_out_and_ba!k.py

A t! i'&) resu)t is s#o-n %e)o-

*ontrolling a ,obile Base 0"


72/246

As !ou '&n see/ usin" o+o etr! in &n i+e&) si u)&tor -it#out #!si's ro+u'es %&si'&))! er,e't resu)ts. T#is s#ou)+ not %e terri%)! sur risin". So -#&t #& ens -#en -e tr! iton & re&) ro%otH

'.9.2 O ometr6-!ase Out an !ack Using a Real Ro/ot

I, !ou #&*e & Turt)eBot or ot#er ROS 'o &ti%)e ro%ot/ !ou '&n tr! t#e o+o etr! %&se+out &n+ %&'( s'ri t in t#e re&) -or)+.

First &(e sure !ou ter in&te &n! runnin" si u)&tions. T#en %rin" u !our ro%ot=sst&rtu )&un'# ,i)e8s9. For & Turt)eBot !ou -ou)+ run


8Or use !our o-n )&un'# ,i)e i, !ou #&*e 're&te+ one to store !our '&)i%r&tion &r& eters.9

&(e sure !our ro%ot #&s )ent! o, roo to -or( in &t )e&st 1. eters e&+ o, it &n+& eter on eit#er si+e.

I, !ou &re usin" & Turt)eBot/ -e -i)) &)so run t#e odom_ekf.py s'ri t 8in')u+e+ in t#erbx5_bringup &'(&"e9 so -e '&n see t#e Turt)eBot=s 'o %ine+ o+o etr! ,r& e inRViz . Jou '&n s(i t#is i, !ou &re not usin" & Turt)eBot. T#is )&un'# ,i)e s#ou)+ %e runon t#e Turt)eBot=s )& to

$ roslaun!h rbx5_bringup odom_ekf.laun!h

I, !ou &)re&+! #&*eRViz runnin" ,ro t#e re*ious test/ !ou '&n si )! un '#e'( t#e

O,o t%y +is )&! &n+ '#e'( t#e Odometry EKF +is )&!/ t#en s(i t#e ,o))o-in" ste .

I, RViz is not &)re&+! u / run it no- -it# t#e ekf_fuerte 'on,i" ,i)e. T#is ,i)e si )! re se)e'ts t#e odom_ekf to i' ,or +is )&!in" t#e 'o %ine+ o+o etr! +&t&

$ rosrun r4iz r4iz -d Frospa!k find rbx5_na4F/ekf_fuerte.4!g

Fin&))!/ )&un'# t#e o+o etr! %&se+ out &n+ %&'( s'ri t >ust )i(e -e +i+ in si u)&tion

$ rosrun rbx5_na4 odom_out_and_ba!k.py

:ere is t#e resu)t ,or ! o-n Turt)eBot -#en o er&tin" on & )o- )! '&r et

*ontrolling a ,obile Base 0*


73/246


74/246

5 !lass OutAndBack10 def __init__ self 11 (i4e the node a name12 rospy.init_node 'out_and_back' M anonymous, Dalse141 #et rospy to exe!tute a shutdown fun!tion when exiting1 rospy.on_shutdown self .shutdown11 ublisher to !ontrol the robotAs speed13 self .!md_4el , rospy. ublisher '/cmd_vel' M *wist15 20 +ow fast will we update the robotAs mo4ementW21 rate , 6822 24 #et the eSui4alent R"# rate 4ariable2 r , rospy.Rate rate2 2 #et the forward linear speed to 8.6 meters per se!ond2 linear_speed , 8.623 25 #et the tra4el distan!e in meters

40 goal_distan!e , 5.84142 #et the rotation speed in radians per se!ond44 angular_speed , 5.84 4 #et the angular toleran!e in degrees !on4erted to radians4 angular_toleran!e , radians 6.=4 43 #et the rotation angle to i radians 5;8 degrees45 goal_angle , pi01 1nitialize the tf listener2 self .tf_listener , tf.*ransformEistener4

#et the odom frame self .odom_frame , '/odom' Dind out if the robot uses /base_link or /base_footprint

3 try5 self .tf_listener.waitDor*ransform self .odom_frameM'/base_footprint' M rospy.*ime M rospy.Luration 5.80 self .base_frame , '/base_footprint'1 ex!ept tf.)x!eptionM tf.&onne!ti4ity)x!eptionM tf.Eookup)x!eption2 try4 self .tf_listener.waitDor*ransform self .odom_frameM'/base_link' M rospy.*ime M rospy.Luration 5.8

self .base_frame , '/base_link' ex!ept tf.)x!eptionM tf.&onne!ti4ity)x!eptionM

tf.Eookup)x!eption rospy.loginfo "!annot find transform bet een /odom and

/base_link or /base_footprint" rospy.signal_shutdown "tf #xception"

3 5 1nitialize the position 4ariable as a oint type0 position , oint1



75/246

2 Eoop on!e for ea!h leg of the trip4 for i in range 6

1nitialize the mo4ement !ommand mo4e_!md , *wist #et the mo4ement !ommand to forward motion

3 mo4e_!md.linear.x , linear_speed5 0 (et the starting position 4alues1 positionM rotation , self .get_odom2 4 x_start , position.x

y_start , position.y 'eep tra!k of the distan!e tra4eled distan!e , 8

3 5 )nter the loop to mo4e along a side30 while distan!e P goal_distan!e and not rospy.is_shutdown31 ublish the *wist message and sleep 5 !y!le32 self .!md_4el.publish mo4e_!md

34 3 r.sleep3 3 (et the !urrent position3 positionM rotation , self .get_odom33 35 &ompute the )u!lidean distan!e from the start50 distan!e , sSrt pow position.x - x_start M 6 Y 51 pow position.y - y_start M 65254 #top the robot before the rotation5 mo4e_!md , *wist5 self .!md_4el.publish mo4e_!md5 rospy.sleep 5

5 53 #et the mo4ement !ommand to a rotation55 mo4e_!md.angular.z , angular_speed100 101 *ra!k the last angle measured102 last_angle , rotation104 10 *ra!k how far we ha4e turned10 from geometry_msgs.msg import *wistM ointM Xuaternion10 import tf10 from transform_utils import Suat_to_angleM normalize_angleturn_angle , 8103 105 while abs turn_angle Y angular_toleran!e P abs goal_angle and notrospy.is_shutdown110 ublish the *wist message and sleep 5 !y!le111 self .!md_4el.publish mo4e_!md112 r.sleep114 11 (et the !urrent rotation11 positionM rotation , self .get_odom11 11 &ompute the amount of rotation sin!e the last lopp



76/246

113 delta_angle , normalize_angle rotation - last_angle115 120 %dd to the running total121 turn_angle Y, delta_angle122 last_angle , rotation124 12 #top the robot before the next leg12 mo4e_!md , *wist12 self .!md_4el.publish mo4e_!md12 rospy.sleep 5123 125 #top the robot for good140 self .!md_4el.publish *wist141 142 def get_odom self 144 (et the !urrent transform between the odom and base frames14 try14 transM rot , self .tf_listener.lookup*ransform self .odom_frameMself .base_frameM rospy.*ime 814 ex!ept tf.)x!eptionM tf.&onne!ti4ity)x!eptionM tf.Eookup)x!eption14 rospy.loginfo "$% #xception"

143 return1451 0 return oint Ctrans M Suat_to_angle Xuaternion Crot1 1 1 2 def shutdown self 1 4 %lways stop the robot when shutting down the node.1 rospy.loginfo "Stopping the robot..."1 self .!md_4el.publish *wist1 rospy.sleep 51 1 3 if __name__ ,, '__main__'1 5 try1 0 "ut%nd


77/246

4 #et the angular toleran!e in degrees !on4erted to radians4 angular_toleran!e , radians 6.=

:ere -e +e,ine &n angular_toleran!e ,or rot&tions. T#e re&son is t#&t it is *er! e&s!

to o*ers#oot t#e rot&tion -it# & re&) ro%ot &n+ e*en & s)i"#t &n"u)&r error '&n sen+ t#ero%ot ,&r &-&! ,ro t#e ne;t )o'&tion. E iri'&))! it -&s ,oun+ t#&t & to)er&n'e o, &%out2. +e"rees "i*es &''e t&%)e resu)ts.

1 1nitialize the tf listener2 self .tf_listener , tf.*ransformEistener4

#et the odom frame self .odom_frame , '/odom' Dind out if the robot uses /base_link or /base_footprint

3 try5 self .tf_listener.waitDor*ransform self .odom_frameM

'/base_footprint' M rospy.*ime M rospy.Luration 5.80 self .base_frame , '/base_footprint'1 ex!ept tf.)x!eptionM tf.&onne!ti4ity)x!eptionM tf.Eookup)x!eption2 try4 self .tf_listener.waitDor*ransform self .odom_frameM'/base_link' M rospy.*ime M rospy.Luration 5.8

self .base_frame , '/base_link' ex!ept tf.)x!eptionM tf.&onne!ti4ity)x!eptionM

tf.Eookup)x!eption rospy.loginfo "!annot find transform bet een /odom and

/base_link or /base_footprint" rospy.signal_shutdown "tf #xception"

Ne;t -e 're&te & *ransformEistener o%>e't to onitor ,r& e tr&ns,or s. To o%t&int#e ro%ot=s osition &n+ orient&tion/ -e nee+ t#e tr&ns,or %et-een t#e /odom ,r& e&n+ eit#er t#e /base_footprint ,r& e &s use+ %! t#e Turt)eBot or t#e /base_link,r& e &s use+ %! Pi Ro%ot &n+ &;-e)). First -e test ,or t#e /base_footprint ,r& e&n+ i, -e +on=t ,in+ it/ -e test ,or t#e base_link ,r& e. T#e resu)t is store+ in t#eself.base_frame *&ri&%)e to %e use+ )&ter in t#e s'ri t.

4 for i in range 6 1nitialize the mo4ement !ommand mo4e_!md , *wist

As -it# t#e ti e+ out &n+ %&'( s'ri t/ -e )oo t#rou"# t#e t-o )e"s o, t#e tri ,irsto*in" t#e ro%ot 1 eter ,or-&r+/ t#en rot&tin" it 130 +e"rees.

1 positionM rotation , self .get_odom2 4 x_start , position.x

y_start , position.y



78/246

At t#e st&rt o, e&'# )e"/ -e re'or+ t#e st&rtin" osition &n+ orient&tion usin" t#eget_odom ,un'tion. Let=s )oo( &t t#&t ne;t so -e un+erst&n+ #o- it -or(s.

142 def get_odom self 144 (et the !urrent transform between the odom and base frames14 try14 transM rot , self .tf_listener.lookup*ransform self .odom_frameMself .base_frameM rospy.*ime 814 ex!ept tf.)x!eptionM tf.&onne!ti4ity)x!eptionM tf.Eookup)x!eption14 rospy.loginfo "$% #xception"143 return1451 0 return oint Ctrans M Suat_to_angle Xuaternion Crot

T#e get_odom ,un'tion ,irst uses t#e tf_listener o%>e't to )oo( u t#e 'urrenttr&ns,or %et-een t#e o+o etr! &n+ %&se ,r& es. I, t#ere is & ro%)e -it# t#e )oo(u /-e t#ro- &n e;'e tion. Ot#er-ise/ -e return & oint re resent&tion o, t#e tr&ns)&tion&n+ &Xuaternion re resent&tion o, t#e rot&tion. T#e C in ,ront o, t#e trans &n+rot*&ri&%)es is P!t#on=s not&tion ,or &ssin" & )ist o, nu %ers to & ,un'tion &n+ -e use it#ere sin'e trans is & )ist o, ;/ !/ &n+ 7 'oor+in&tes -#i)e rot is & )ist o, ;/ !/ 7 &n+ -ust our )oo to o*e t#e ro%ot ,or-&r+ unti) -e #&*e "one 1.0 eters.

10 *ra!k how far we ha4e turned

10 turn_angle , 810 10 while abs turn_angle Y angular_toleran!e P abs goal_angle and notrospy.is_shutdown103 ublish the *wist message and sleep for 5 !y!le105 self .!md_4el.publish mo4e_!md110 r.sleep111



79/246

112 (et the !urrent rotation114 positionM rotation , self .get_odom11 11 &ompute the amount of rotation sin!e the last lopp11 delta_angle , normalize_angle rotation - last_angle11 113 %dd to the running total115 turn_angle Y, delta_angle120 last_angle , rotation

An+ t#is is our )oo ,or rot&tin" t#rou"# 130 +e"rees -it#in t#e &n"u)&r to)er&n'e -e setne&r t#e %e"innin" o, t#e s'ri t.

'.9.4 +he /odom +o ic *ersus the /odom ,rame

T#e re&+er &! %e -on+erin" -#! -e use+ & *ransformEistener in t#e re*iouss'ri t to &''ess o+o etr! in,or &tion r&t#er t#&n >ust su%s'ri%in" to t#e /odom to i'.T#e re&son is t#&t t#e +&t& u%)is#e+ on t#e/odom to i' is not &)-&!s t#e ,u)) stor!. For

e;& )e/ t#e Turt)eBot uses & sin")e &;is "!ro to ro*i+e &n &++ition&) esti &te o, t#ero%ot=s rot&tion. T#is is 'o %ine+ -it# t#e +&t& ,ro t#e -#ee) en'o+ers %! t#erobot_pose_ekf no+e 8-#i'# is st&rte+ in t#e Turt)eBot )&un'# ,i)e9 to "et & %etteresti &te o, rot&tion t#&n eit#er sour'e &)one.

:o-e*er/ t#e robot_pose_ekf no+e +oes not u%)is# its +&t& %&'( on t#e /odom to i'-#i'# is reser*e+ ,or t#e -#ee) en'o+er +&t&. Inste&+/ it u%)is#es it on t#e/robot_pose_ekf/odom to i'. Furt#er ore/ t#e +&t& is u%)is#e+ not &s &n"dometry ess&"e %ut &s & oseJith&o4arian!e#tamped ess&"e. It "oes#o-e*er/ u%)is# & tr&ns,or ,ro t#e /odom ,r& e to t#e /base_link ,r& e -#i'#

ro*i+es t#e in,or &tion -e nee+. As & resu)t/ it is "ener&))! s&,er to use tf to onitort#e tr&ns,or &tion %et-een t#e /odom &n+/base_link 8or /base_footprint 9,r& es t#&n to re)! on t#e /odom ess&"e to i'.

In t#e rbx5_bringup/nodes +ire'tor! !ou -i)) ,in+ & no+e '&))e+ odom_ekf.py t#&tre u%)is#es t#e oseJith&o4arian!e#tamped ess&"e ,oun+ on t#e/robot_pose_ekf/odom to i' &s &n "dometry t! e ess&"e on t#e to i' '&))e+/odom_ekf . T#is &))o-s us to *ie- %ot# t#e /odom to i' &n+ t#e /odom_ekf to i' inRViz to 'o &re t#e Turt)eBot=s -#ee) %&se+ o+o etr! -it# t#e 'o %ine+ o+o etr!t#&t in')u+es t#e "!ro +&t&.

5#8 Na2igating a S?$a% $&ing O,o t%yAs -e +i+ -it# t#e o+o etr! %&se+ out &n+ %&'( s'ri t/ -e -i)) onitor t#e osition&n+ orient&tion o, t#e ro%ot usin" t#e tf tr&ns,or %et-een t#e /odom &n+/base_link 8or /base_footprint 9 ,r& es. :o-e*er/ t#is ti e -e -i)) &tte t too*e t#e ro%ot in & s


80/246

t#e run/ -e '&n see #o- ')ose t#e ro%ot "ets %&'( to t#e st&rtin" )o'&tion &n+ orient&tion.Let=s st&rt -it# & si u)&tion &n+ t#en tr! it on & re&) ro%ot.

'.:.1 Na*igating a S;uare in the r/oti8 Simulator

I, !ou &)re&+! #&*e & si u)&te+ Turt)eBot or Pi Ro%ot runnin"/ ,irst &trl-& out o, t#e

si u)&tion so -e '&n st&rt t#e o+o etr! re&+in"s ,ro s'r&t'#. T#en %rin" u t#esi u)&te+ ro%ot &"&in/ run RViz / t#en run t#e na4_sSuare.py s'ri t &s ,o))o-s



$ rosrun rbx5_na4 na4_sSuare.py

A t! i'&) resu)t is s#o-n %e)o-

As %e,ore/ t#e o+o etr! &rro-s i))ustr&te t#e #e&+in" o, t#e ro%ot &t *&rious oints &)on"its route. As !ou '&n see/ t#e s


81/246


82/246

As !ou '&n see/ t#e resu)t is not too %&+. In t#e re&) -or)+/ t#e ro%ot en+e+ u 1 '&-&! ,ro t#e st&rtin" oint &n+ &%out 20 +e"rees o,, t#e ori"in&) orient&tion. But o,'ourse/ i, -e

Documents

Ros by Example Fuerte Volume 1