introduc)on to ros · • the publish/subscribe paern is very useful to broadcast informaon...
TRANSCRIPT
![Page 1: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/1.jpg)
Introduc)ontoROS
NarcísPalomerasUniversitatdeGirona
![Page 2: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/2.jpg)
Howtoprogramarobot?
• Differenthardware• Differentuses
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 2
![Page 3: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/3.jpg)
Layersofarobot
S.O.(Linux/Windows/OSX/...)
Hardware
Middleware/Framework/...
ControlArchitecture
Hardware
ControlArchitecture
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 3
![Page 4: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/4.jpg)
GenericMiddlewares,Frameworks,...
GostaiURBI
MOOS
OROCOS
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 4
![Page 5: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/5.jpg)
Commonfeatures• Simplifythecrea9onandtheaddi9onofnewcomponentsbeingas
uninvasiveaspossiblewhenbuildingthecomponents,andminimizingthecouplingwiththesystemandtherestofthecomponents.
• Providesimplemechanismsforcommunica9onbetweenthecomponents.Allowingthedistribu)onofcomponentsamongthenodesofanetwork.[client/service,publish/subscribe,blackboard,...]
• Enablecomponentloggingfeatures.[logfiles]• Allowenablinganddisablingcomponentsduringtheini)aliza)onand
run-)me.[ini)aliza)onscrip...]• Allowcomponentstosaveinforma9onrelatedtoitsconfigura9onin
apersistentway.[configfiles,databases...]• Provideextratoolstosimplifysoawaredevelopmentandtes)ng.
[visualizers,simulators...]
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 5
![Page 6: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/6.jpg)
ROSBasics
• Simplifiesthecrea)onandtheaddi9onofnewcomponents• Providessimplemechanismsforcommunica9onbetweenthe
componentsinanetwork• Providesloggingfeaturestosavepreciousinforma)oninasafe
andpersistentway• Providesextratools,suchasvisualizers,simulatorsandmuchmore• Opensource:UsesasapreferredlicenseBSD
www.ros.org
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 6
![Page 7: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/7.jpg)
HowtoinstallROS• ROScurrentlysupportsUbuntuLinuxandhasexperimental
supportforotherOSs,suchasDebian,ArchLinuxorOSX• ROSisreallyeasytoinstall.Theinstalla9onguidefromthe
webiscomplete:hdp://wiki.ros.org/jade/Installa)on/Ubuntu
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 7
![Page 8: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/8.jpg)
HowtolearnROS• Therearemanytutorialsinthewebthatsimplifytheprocess
oflearningROS:
• Thetutorialshavebeenorganizedinalogicandprogressiveway.Thereisalsoanindica)onoftheexpecteddifficultyofeverytutorial
• Ifyouhaveanyproblemorques)onregardingROS:
hdp://wiki.ros.org/ROS/Tutorials
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 8
![Page 9: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/9.jpg)
BasicconceptsofROSI• Node:anodeisanexecutablethatusesROStocommunicate
withothernodes.• Master(roscore):itistheROSprocessthatenablesthe
communica9onbetweenalltheothernodes(i.e.ithelpsthenodestofindeachother(kindofnameserver)).
• Messages:messagesareROSdatatypesusedtocommunicateinforma)on.
• Topic:nodescanpublishmessagestoatopicaswellassubscribetoatopictoreceivemessages.Itislikeadatastream.
• Service:aserviceisusedwhenarequest/reply(RPC)communica)onisneeded.
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 9
![Page 10: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/10.jpg)
BasicconceptsofROSII
• Parameterserver:theparameterserverisashared,mul)variatedic9onarythatisaccessiblevianetworkAPIs.
• Launchfiles:roslaunchisatooltorunmul9pleROSnodeslocallyorremotelyviaSSHaswellasloadparameterstotheparamserver.
• Package:apackageisagroupofnodes,messages,servicesandconfigura)onfiles.
• Metapackage:ametapackageisagroupofrelatedpackages.
• Workspace:theworkspaceisthemaindirectoryinwhichallourpackagesandmetapackagesmustbelocated.
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 10
![Page 11: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/11.jpg)
Communica)ons:Publish/Subscribe
Node1
Node3
Node2
ROSMaster
Asynchronous,many-to-manyandone-waytransport.
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 11
![Page 12: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/12.jpg)
Communica)ons:Publish/Subscribe
Node1
Node3
Node2
ROSMaster
Asynchronous,many-to-manyandone-waytransport.
Communicatestopictopublish
andtype
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 12
![Page 13: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/13.jpg)
Communica)ons:Publish/Subscribe
Node1
Node3
Node2
ROSMaster
Asynchronous,many-to-manyandone-waytransport.
topic_id(type)
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 13
![Page 14: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/14.jpg)
Communica)ons:Publish/Subscribe
Node1
Node3
Node2
ROSMaster
Asynchronous,many-to-manyandone-waytransport.
topic_id(type)
Communicatestopictosubscibe
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 14
![Page 15: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/15.jpg)
Communica)ons:Publish/Subscribe
Node1
Node3
Node2
ROSMaster
Asynchronous,many-to-manyandone-waytransport.
topic_id(type)
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 15
![Page 16: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/16.jpg)
Communica)ons:Publish/Subscribe
Node1
Node3
Node2
ROSMaster
Asynchronous,many-to-manyandone-waytransport.
Msg
topic_id(type)
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 16
![Page 17: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/17.jpg)
Communica)ons:Publish/Subscribe
Node1
Node3
Node2
ROSMaster
Asynchronous,many-to-manyandone-waytransport.
Msg
topic_id(type)
Msg
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 17
![Page 18: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/18.jpg)
Communica)ons:Publish/Subscribe
Node1
Node3
Node2
ROSMaster
Asynchronous,many-to-manyandone-waytransport.
topic_id(type)
Node4
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 18
![Page 19: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/19.jpg)
Communica)ons:Publish/Subscribe
Node1
Node3
Node2
ROSMaster
Asynchronous,many-to-manyandone-waytransport.
topic_id(type)
Node4Announceservice
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 19
![Page 20: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/20.jpg)
Communica)ons:Publish/Subscribe
Node1
Node3
Node2
ROSMaster
Asynchronous,many-to-manyandone-waytransport.
topic_id(type)
Node4
Communicatestopictosubscibe
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 20
![Page 21: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/21.jpg)
Communica)ons:Publish/Subscribe
Node1
Node3
Node2
ROSMaster
Asynchronous,many-to-manyandone-waytransport.
topic_id(type)
Node4
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 21
![Page 22: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/22.jpg)
Communica)ons:Client/Service
Node1(server)
ROSMaster
Node2(client)
Synchronous,one-to-manyNarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 22
![Page 23: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/23.jpg)
Communica)ons:Client/Service
Node1(server)
ROSMaster
Node2(client)
Announceservice
Synchronous,one-to-manyNarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 23
![Page 24: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/24.jpg)
Communica)ons:Client/Service
Node1(server)
ROSMaster
Node2(client)
Synchronous,one-to-many
ServiceNarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 24
![Page 25: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/25.jpg)
Communica)ons:Client/Service
Node1(server)
ROSMaster
Node2(client)
Requestservice
Synchronous,one-to-many
ServiceNarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 25
![Page 26: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/26.jpg)
Communica)ons:Client/Service
Node1(server)
ROSMaster
Node2(client)
Request
Synchronous,one-to-many
ServiceNarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 26
![Page 27: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/27.jpg)
Communica)ons:Client/Service
Node1(server)
ROSMaster
Node2(client)
Response
Synchronous,one-to-many
ServiceNarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 27
![Page 28: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/28.jpg)
HowtocompileourROSproject
• ROSsupportsC++andPythonprogramminglanguages:
• TocompileourROScodeweusethecatkinbuildsystem.To
definehowcatkinhastocompileourprojectwecandefinethedependenciesbetweendifferentpackagesinthefilesCMakeLists.txtandpackages.xml
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 28
![Page 29: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/29.jpg)
ROSbasiccommands
• Exercise:TestthefollowingROScommands
rosparamrosmsgrossrvrosbagcatkin_make
roscorerosrunroslaunchrosnoderostopicrosservice
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa)a 29
![Page 30: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/30.jpg)
ROSintroduc-on--handson--
NarcísPalomeras
UniversitatdeGirona
![Page 31: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/31.jpg)
Crea-ngnodesandpackages
• ThebasicbuildingblocksofaRobotOpera-veSystem(ROS)applica-onarethenodes.EachnodeisanindependentprocessthatcommunicateswiththeothersbyseveralwaysprovidedbytheROSframework.Nodesareorganizedinpackagesandpackageshavetobeinsideaworkspace.Then,weneedtocreatefirstourworkspaceandthenthepackagethatwillcontainthenodesthatwewillcreate.
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa-a 2
![Page 32: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/32.jpg)
ROSworkspace
• IfyouhavejustinstalledROSyouneedtosourceitssetup.# source /opt/ros/<distro>/setup.bash
• Tocreateacatkinworkspacedo:$ mkdir -p ~/catkin_ws/src$ cd ~/catkin_ws/src$ catkin_init_workspace$ cd ~/catkin_ws/$ catkin_make
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa-a 3
![Page 33: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/33.jpg)
ROSpackages
• TonavigatethroughROSpackagesuseroscd.Itallowsyoutochangedirectorydirectlytoapackageorastack.# roscd [locationname[/subdir]]
• Forapackagetobeconsideredacatkinpackageitmustmeetafewrequirements:
my_package/ CMakeLists.txt package.xml
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa-a 4
![Page 34: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/34.jpg)
ROSpackages
• Tocreateanewpackage,navigatetoyourworkspaceandthenusethecatkin_create_pkgu-lity.$ cd ~/catkin_ws/src$ catkin_create_pkg beginner_tutorials std_msgs rospy roscpp
• Tobuildthenewcreatedpackagedo:$ cd ~/catkin_ws$ catkin_make$ . ~/catkin_ws/devel/setup.bash$ rospack profile
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa-a 5
![Page 35: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/35.jpg)
UnderstandingROStopics
• OneofthemechanismsthatROSnodesusetocommunicateamongthemiscalledpublish/subscribe.Apublishernodepublishesamessage(withaspecifictype)intoatopic(akindofaddress).Asubscribernodeissubscribedtoatopicandevery-methatanynodepublishesamessagetothistopicacallbackcontainingthemessageisexecutedinsidethesubscribernode.
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa-a 6
![Page 36: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/36.jpg)
UnderstandingROStopics
• Thepublish/subscribepa]ernisveryusefultobroadcastinforma-onasynchronously.Thismeansthatonceanodereceivessomeinforma-on(e.g.,acompasssensorreadsthecurrentheadingfromaserialbus)itcansendthisdatatoseveralnodeswithouthavingtowaitthatallthenodesthataresubscribedtothistopichavereceivedtheinforma-on.Severalnodescanpublishtothesametopicandanodecanpublishorbesubscribedtoseveraltopics.
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa-a 7
![Page 37: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/37.jpg)
Wri-ngapublishernode
• Createasrcfolderinsideyourbeginner_tutorialspackage.Andcreateafilenamedtalker.pyinit.
$ cd ~/catkin_ws/src/beginner_tutorials $ mkdir src // if not exists$ cd src$ touch talker.py$ chmod +x talker.py
• Copythefollowingcodetoit:
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa-a 8
![Page 38: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/38.jpg)
#!/usr/bin/env pythonimport rospyfrom std_msgs.msg import String
def talker(): pub = rospy.Publisher('chatter', String, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): hello_str = "hello world %s" % rospy.get_time() rospy.loginfo(hello_str) pub.publish(hello_str) rate.sleep()
if __name__ == '__main__': try: talker() except rospy.ROSInterruptException: pass
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa-a 9
![Page 39: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/39.jpg)
Wri-ngasubscribernode
• Createafilenamedlistener.pyinthebeginner_tutorials/srcfolder.
$ cd ~/catkin_ws/src/beginner_tutorials/src$ touch listener.py$ chmod +x listener.py
• Copythefollowingcodetoit:
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa-a 10
![Page 40: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/40.jpg)
#!/usr/bin/env pythonimport rospyfrom std_msgs.msg import String
def callback(data): rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data) def listener(): rospy.init_node('listener', anonymous=True rospy.Subscriber("chatter", String, callback)
# spin() simply keeps python from exiting until this # node is stopped rospy.spin()
if __name__ == '__main__': listener()
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa-a 11
![Page 41: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/41.jpg)
Buildandrun• Tobuildyourcodedo:$ cd ~/catkin_ws$ catkin_make
• Torunitexecutethesethreelinesinthreedifferentterminals:
$ roscore------------------------------------------------$ rosrun beginner_tutorials talker.py------------------------------------------------$ rosrun beginner_tutorials listener.py
• Toseethepublis/subscriberconnec-ondo:$ rosru rqt_graph rqt_graph
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa-a 12
![Page 42: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/42.jpg)
UnderstandingROSservices• AnothermechanismsthatROSnodesuseto
communicateamongthemistheclient/servermodel.Aclientnodecallsaserviceprovidedbyaservernode.Whenaservernodeislauncheditannouncestheservicesthatitprovidesandifanyoftheirservicesiscalled,acallbackcontainingtheservicerequestisexecutedinsidetheservernode.Oncetheserverfinalizesitsexecu-onitreturnsaserviceresponsetotheclientnode.
• Theclient/servicemodelisveryusefultosynchronouslyrequestapar-cularcomputa-on/ac-onfromonenodetoanother.Examplescanbe:gotoawaypoint,enableasensor,disableacontroller,...
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa-a 13
![Page 43: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/43.jpg)
Crea-ngaROSsrv
• An.srvfiledescribesaservice.Itiscomposedoftwoparts:arequestandaresponse.Servicefilesarestoredinthepackage_name/srvfolder.
• Thefieldtypesyoucanuseare:– int8,int16,int32,int64(plusuint*)– float32,float64– string– -me,dura-on– Header– othermsgfiles– variable-lengtharray[]andfixed-lengtharray[C]
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa-a 14
![Page 44: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/44.jpg)
Crea-ngaROSsrv
$ roscd beginner_tutorials$ mkdir srv$ cd srv
• CreateafilenamedAddTwoInts.srvwiththefollowingcontent:
int64 aint64 b---int64 sum
• Createasrvfolderinsidethebeginner_tutorialsfolder
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa-a 15
![Page 45: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/45.jpg)
Crea-ngaROSsrv• Openthefilepackage.xmlandaddthesetwolines:<build_depend>message_generation</build_depend><run_depend>message_runtime</run_depend>
• OpenthefileCMakeList.txtandadd/uncomment:find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation)...add_service_files( FILES AddTwoInts.srv)...generate_messages( DEPENDENCIES std_msgs)NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa-a 16
![Page 46: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/46.jpg)
Wri-ngadd_two_ints_server.py
#!/usr/bin/env python
from beginner_tutorials.srv import *import rospy
def handle_add_two_ints(req): print "Returning “, req.a, “+”, req.b,” = “, (req.a + req.b) return AddTwoIntsResponse(req.a + req.b)
def add_two_ints_server(): rospy.init_node('add_two_ints_server') s = rospy.Service('add_two_ints', AddTwoInts, handle_add_two_ints) print "Ready to add two ints." rospy.spin()
if __name__ == "__main__": add_two_ints_server()
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa-a 17
![Page 47: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/47.jpg)
#!/usr/bin/env python
import sysimport rospyfrom beginner_tutorials.srv import *
def add_two_ints_client(x, y): rospy.wait_for_service('add_two_ints') try: add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts) resp1 = add_two_ints(x, y) return resp1.sum except rospy.ServiceException, e: print "Service call failed: %s"%e
def usage(): return "%s [x y]"%sys.argv[0]
if __name__ == "__main__": if len(sys.argv) == 3: x = int(sys.argv[1]) y = int(sys.argv[2]) else: print usage() sys.exit(1) print "Requesting %s+%s"%(x, y) print "%s + %s = %s"%(x, y, add_two_ints_client(x, y))
Wri-ngadd_two_ints_client.py
18
![Page 48: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/48.jpg)
Buildandrun
• Buildtheservicemessageasusual:
$ cd ~/catkin_ws$ catkin_make
• Runitasbefore:$ roscore------------------------------------------------$ rosrun beginner_tutorials talker.py------------------------------------------------$ rosrun beginner_tutorials listener.py
• Makethescriptsexecutable:$ chmod +x add_two_ints_client.py$ chmod +x add_two_ints_server.py
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa-a 19
![Page 49: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/49.jpg)
ROSu-li-es
• ROSprovidesseveralu-li-esthatareveryhelpfultodevelop,test,debug...yourrobotsojware:
• rqt_console• rqt_k_tree• rqt_graph• rqt_bag
• rosmsg• rossrv• rosbag• rosparam
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa-a 20
![Page 50: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/50.jpg)
COLA2
ComponentOrientedLayered-basedArchitectureforAutonomy
![Page 51: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/51.jpg)
COLA2So:warearchitecture
Naviga&onsensors
Percep&onsensors
Actuators
Localiza&on Control
HighLevelBehaviors(Guidance,Mapping,pathplanning...)
Safety
Missioncontrol(taskplanning,statemachine,...)
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-CroaJa 2
![Page 52: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/52.jpg)
DVL depthsensor GPS USBLIMU thrusters
localizaJonfilter
camera
visualdetector
thrusterallocator
velocitycontroller
posecontroller
Requestpose/twist
HighLevelControlLocaliza&on Guidance&control
DetecJons
Currentpose/twist
Safety
ArchitecturecoreRequestwaypoint
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-CroaJa 3
![Page 53: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/53.jpg)
LocalizaJonschema
DVL depthsensor GPS USBL
localizaJonfilter
IMU
[uvw] [z] [xy] [xyz]
camera visualdetectorimg
[lxilyilzilφilθilψi]
[φθψpqr]
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-CroaJa 4
![Page 54: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/54.jpg)
ControlschemalocalizaJonfilter
velocitycontroller
thrusterallocator
[φθψpqr][xyz]
posecontroller
[uvw]
[u’v’w’r’]
[X’Y’Z’N’]
Requestpose
Requesttwist
Thrustersetpoints
[u’v’w’r’]
[x’y’z’ψ’]
Requesttwist
[X’Y’Z’N’]
+
+
HighLevelCon
trol
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-CroaJa 5
![Page 55: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/55.jpg)
KalmanFilterfornaviga0on--introduc0on--
NarcísPalomeras-UniveritatdeGironabasedonGregWelchandGaryBishop“Anintroduc0onto
theKalmanFilter”slidesinSIGGRAPH2001
![Page 56: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/56.jpg)
WhatisaKalmanFilter?
• Justsomeappliedmath.• Alinearsystem:f(a+b)=f(a)+f(b).• Noisydatainhopefullylessnoisyout.• Butdelayisthepriceforfiltering...
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 2
![Page 57: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/57.jpg)
Whatisitusedfor?
• Trackingmissiles• Trackingheads/hands/drums0cks• Extrac0nglipmo0onfromvideo• Fi]ngBezierpatchestopointdata• Lotsofcomputervisionapplica0ons• Economics• Naviga&on
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 3
![Page 58: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/58.jpg)
Asimpleexample• AliceandBobmeasureatensionwiththeirvol0meters.The
nominaltensionis10vbutAlicevol0meter,thathasavarianceof2v,measures9.85vwhileBobvol0meter,thathasavarianceof5v,measures11.07.
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 4
![Page 59: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/59.jpg)
Asimpleexample
• Canwecombinebothmeasures?Yes!• Tocombinethemweuse:
– zcombined=zalice+K(zbob–zalice)– K=variancealice/(variancealice+variancebob)– variancecombined=1/(1/variancealice+1/variancebob)
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 5
![Page 60: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/60.jpg)
Asimpleexample
• Canwecombinebothmeasures?Yes!• Tocombinethemweuse:
– zcombined=zalice+K(zbob–zalice)– K=variancealice/(variancealice+variancebob)– variancecombined=1/(1/variancealice+1/variancebob)
Gain Innova&on
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 6
![Page 61: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/61.jpg)
Asimpleexample
• Canwecombinebothmeasures?Yes!• Tocombinethemweuse:
– zcombined=zalice+K(zbob–zalice)– K=variancealice/(variancealice+variancebob)– variancecombined=1/(1/variancealice+1/variancebob)
• Doingthemaths...– K:2/(2+5)=0.29– z:9.85+0.29*(11.07–9.85)=10.2– v:1/(1/2+1/5)=1.43
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 7
![Page 62: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/62.jpg)
• zcombined=10.2• variancecombined=1.43
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 8
![Page 63: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/63.jpg)
Supposethatwemeasuremo0on
• Notallthedifferenceiserror• Somemaybemo0on• KFcanincludeamo0onmodel• Es0matevelocityandposi0on
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 9
![Page 64: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/64.jpg)
ProcessModel
• TheModeldescribeshowthestatechangesover0me
• Thestateforthefirstexamplewasscalar– Theprocessmodelwas“nothingchanges”
• Forasimplemo0onexample:– Stateisa2-vector[posi0on,velocity]– posi0onn+1=posi0onn+velocityn*0me– velocityn+1=velocityn
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 10
![Page 65: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/65.jpg)
TwoSteps:PredictàCorrect
• KFoperatesby– Predic0ngthenewstateanditsuncertainty– Correc0ngwiththenewmeasurement
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 11
![Page 66: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/66.jpg)
Mo0onexample
• Wewanttofilterthemo0onofapointthatmovesintheplane.
• Wehaveanoisymeasure(x,y)everysecond.• Thevelocity(dx/dt,dy/dt)ismorelessconstantwithsmallvaria0ons.
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 12
![Page 67: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/67.jpg)
Prepara0on
• Definestatevector:– xK=[pxpyvxvy]
where:
px=xposi0onpy=yposi0onvx=dx/dtvy=dy/dt
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 13
![Page 68: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/68.jpg)
Prepara0on
• Definestatevector:– xK=[pxpyvxvy]
• Definemodel– pxK=pxK-1+vxK-1*dt– pyK=pyK-1+vyK-1*dt– vxK=vxK-1– vyK=vyK-1
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 14
![Page 69: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/69.jpg)
Prepara0on
• Definestatevector:– xK=[pxpyvxvy]T
• Definemodel– pxK=pxK-1+vxK-1*dt– pyK=pyK-1+vyK-1*dt– vxK=vxK-1– vyK=vyK-1
DefineAmatrix:
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 15
![Page 70: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/70.jpg)
ProcessModel
Statetransi0on
A
Statexk
Statexk-1
NoiseWk-1
pxKpyKvxKvyK
pxK-1pyK-1vxK-1vyK-1
~pxK-1~pyK-1~vxK-1~vyK-1
= +
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 16
![Page 71: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/71.jpg)
MeasurementModel
• Definemeasure: zK=[zxzy]
• DefineHmatrix:
H=
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 17
![Page 72: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/72.jpg)
MeasurementModel
MeasurementzK
MeasurementmatrixH
StatexK
NoisevK
zxzy
pxKpyKvxKvyK
~zx~zy
= +10000100
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 18
![Page 73: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/73.jpg)
• ProcessNoiseCovariance:
• MeasurementNoiseCovariance:
Preparenoisematrices
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 19
![Page 74: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/74.jpg)
Predict
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 20
![Page 75: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/75.jpg)
Correct
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 21
![Page 76: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/76.jpg)
Summary
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 22
![Page 77: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/77.jpg)
Results
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa0a 23
![Page 78: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/78.jpg)
ExtendedKalmanFilterforlocaliza3onandSLAM
COLA2implementa3on
![Page 79: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/79.jpg)
Mo3va3on
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 2
![Page 80: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/80.jpg)
Mo3va3on
Chain
Panel
VehiclePose
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 3
![Page 81: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/81.jpg)
RobotLocaliza3on
RobotPoseMap
RobotLocaliza3on
Environment
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 4
![Page 82: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/82.jpg)
Mapping
RobotPoseMap
Landmarkdetec3on
Environment
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 5
![Page 83: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/83.jpg)
SimultaneousLocaliza3onAndMapping
RobotPoseMap
RobotLocaliza3on
Landmarkdetec3on
Environment
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 6
![Page 84: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/84.jpg)
Modular&genericNaviga3onSensors
navdata
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 7
![Page 85: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/85.jpg)
Modular&genericNaviga3onSensors
Naviga3onadapter
navdata
imudataposi3onupdate
velocityupdate
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 8
![Page 86: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/86.jpg)
Naviga3onadapter
• Transformunitstometricsystem• Right-handedreferenceframe• Transformsensormeasurementsfrom{S}to{B}.
Posi%onpB=pS−RPY·tSPpB=PpS+JpB·PRPY·JTpBVelocity:vB=rotS·vS−(wB⊗tS)PvB=rotS·PvS·rotTS+
J(wB⊗t)·PwB·JT(wB⊗t)Girona500AUV
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 9
![Page 87: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/87.jpg)
Modular&genericNaviga3onSensors
Naviga3onadapter
navdata
imudataposi3onupdate
velocityupdate
EKF
odometry
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 10
![Page 88: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/88.jpg)
EKFLocaliza3on
imudata posi3onupdate
velocityupdate
EKF
odometry
Predic3on
Correc3on
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 11
![Page 89: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/89.jpg)
EKFlocaliza3on:Predic3on
• Es3matesvehicleposi3on[xyz]andlinearvelocity[uvw].
Statevectorxk=[x,y,z,u,v,w]
Model• Constantvelocitymodel
NoiseZero-meanwhiteGaussianaccelera3onnoise
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 12
![Page 90: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/90.jpg)
EKFlocaliza3on:Predic3on
A=Jacobianmatrix
off(x)
dxK-1 dyK-1 dzK-1 duK-1 dvK-1 dwK-1
xK 1 0 0 ... ... ...
yK 0 1 0 ... ... ...
zK 0 0 1 ... ... ...
uK 0 0 0 1 0 0
vK 0 0 0 0 1 0
wK 0 0 0 0 0 1
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 13
![Page 91: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/91.jpg)
EKFlocaliza3on:Predic3on
A=Eigen::MatrixXd::Iden3ty(6,6);A(0,3)=cos(pitch)*cos(yaw)*t;A(0,4)=-cos(roll)*sin(yaw)*t+sin(roll)*sin(pitch)*cos(yaw)*t;A(0,5)=sin(roll)*sin(yaw)*t+cos(roll)*sin(pitch)*cos(yaw)*t;A(1,3)=cos(pitch)*sin(yaw)*t;A(1,4)=cos(roll)*cos(yaw)*t+sin(roll)*sin(pitch)*sin(yaw)*t;A(1,5)=-sin(roll)*cos(yaw)*t+cos(roll)*sin(pitch)*sin(yaw)*t;A(2,3)=-sin(pitch)*t;A(2,4)=sin(roll)*cos(pitch)*t;A(2,5)=cos(roll)*cos(pitch)*t;
Jacobianmatrixoff(x)
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 14
![Page 92: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/92.jpg)
• InsteadofxK=A·xK-1andPK=A*PK-1*AT+Q
• wehavexK=f(xK-1)andPK=A*PK-1*AT+QwhereA=J(x)
EKFlocaliza3on:Predic3on
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 15
![Page 93: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/93.jpg)
EKFlocaliza3on:Correc3on
• Twolinearmeasurementupdatesareappliedinthefilter
poseupdate
zk=[xyz];H=[I3×303×]
velocityupdatezk=[uvw];H=[03×3I3×]
measurementmodel
zk=Hxk+sk
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 16
![Page 94: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/94.jpg)
Modular&generic
percep3ondata
Percep3onSensorsNaviga3on
Sensors
Naviga3onadapter
navdata
imudataposi3onupdate
velocityupdate
EKF
odometryNarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 17
![Page 95: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/95.jpg)
Modular&generic
Landmarkdetec3on
percep3ondata
landmarkupdate
Percep3onSensorsNaviga3on
Sensors
Naviga3onadapter
navdata
imudataposi3onupdate
velocityupdate
EKF
odometryNarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 18
![Page 96: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/96.jpg)
Vision-basedlandmarkdetectors
Camera
ARToolkit
imgdata
landmarkupdate
Feature-BasedMatching
• Computelandmarkspose,bycomparingtheimagesfromthecameraagainstanaprioriknowntemplate.
• Twoproposedmethods:ARToolkit&feature-basedmatching
templates
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 19
![Page 97: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/97.jpg)
Vision-basedlandmarkdetectors
Template20x20 cm
Camera imageARToolKit• Greyscalemonocularcamera• Lowerresources• Needtoaddextramarkers• Badcovariancees3ma3on:R=(I3×3·d2)·c
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 20
![Page 98: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/98.jpg)
Vision-basedlandmarkdetectors
Feature-basedMatching• Greyscalemonocularcamera
• UsesORBfeaturedetector
• Noneedofextramarkers
• Morecomplexbutreal-3me
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 21
![Page 99: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/99.jpg)
Modular&genericNaviga3onSensors
Landmarkdetec3on
Naviga3onadapter
percep3ondatanavdata
imudata landmarkupdate
posi3onupdate
velocityupdate
EKF-SLAM
odometry map
Percep3onSensors
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 22
![Page 100: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/100.jpg)
EKF-SLAMLocaliza3on
imudata posi3onupdate
velocityupdate
EKF-SLAM
odometry
Predic3on
Correc3on
landmarkupdate
map
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 23
![Page 101: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/101.jpg)
EKF-SLAMalgorithm:Predic3on
• Es3matesvehicleposi3on[xyz]andlinearvelocity[uvw],andmapstheposi3onoflandmarks[lxilyilzi]iden3fiedbyavisualdetector.
Statevectorxk=[x,y,z,u,v,w,lx0,ly0,lz0...,lxn,lyn,lzn]
Model• Constantvelocityforthevehicle• Sta3cposi3onforthelandmarks
NoiseZero-meanwhiteGaussianaccelera3onnoise
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 24
![Page 102: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/102.jpg)
EKFlocaliza3on:Model
Jacobianmatrixoff(x)
dxK-1 dyK-1 dzK-1 duK-1 dvK-1 ...xK-1 ...
yK-1 ...
zK-1 ...
uK-1 1
vK-1 1
... ...NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 25
![Page 103: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/103.jpg)
EKF-SLAMnaviga3on:Correc3on
• Threelinearmeasurementupdatesareappliedinthefilter
poseupdate
zk=[xyz];H=[I3×303×303×3n]
velocityupdatezk=[uvw];H=[03×3I3×303×3n]
landmarkupdate
zk=[lxlylz];H=[−RotT03×3...RotT...]
measurementmodel
zk=Hxk+sk
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 26
![Page 104: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/104.jpg)
Filterini3aliza3on
Landmarkdetected
Computequeuecovariance
landmarkinmap?
size(queue)>N?
yes
no
Performupdate
Addlandmarkposetoqueue
Addlandmarktomap
var(queue)<value?
yes
yes
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 27
![Page 105: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/105.jpg)
Experimentssetup
• Vehiclesensors:IMU,DVL,depthsensorandcamera.
• Watertankof16x8x5m• Twolandmarks• Ground-truthposteratthe
bouomofthewatertank
Girona500AUVwasteleoperatedforabout10minutes.VisualdetectoralgorithmandEKF-SLAMwererunningon-boardthevehicle.
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 28
![Page 106: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/106.jpg)
Groundtruth
• Toobtaines3matesofabsolutecameraposes,togetherwiththeiruncertainty,cameraimagesareregisteredtotheoriginalimageoftheposter.
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 29
![Page 107: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/107.jpg)
Trajectoryresults
DeadreckoningVs.Ground-truth(dashed)
EKF-SLAMVs.Ground-truth(dashed)
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 30
![Page 108: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/108.jpg)
ErrorcomparisonHellingerdistanceinthees3matedtrajectoriesfromthedeadreckoningandEKF-SLAM
Standarddevia3onforaxisXover3mebetweendeadreckoning,
EKF-SLAMandtheground-truth.
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa3a 31
![Page 109: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/109.jpg)
COLA2controlscheme
NarcísPalomerasUniversitatdeGirona
![Page 110: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/110.jpg)
COLA2controlschemelocaliza;onfilter
velocitycontroller
thrusterallocator
[φθψpqr][xyz]
posecontroller
[uvw]
[u’v’w’r’]
[X’Y’Z’N’]
Requestpose
Requesttwist
Thrustersetpoints
[u’v’w’r’]
[x’y’z’ψ’]
Requestwrench
[X’Y’Z’N’]
+
+
HighLevelCon
trolers
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa;a 2
![Page 111: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/111.jpg)
Lowlevelcontroller:posi;oncontrol[φθψpqr][xyz]
posecontroller
[u’v’w’r’]
Requestpose
[x’y’z’ψ’]
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa;a 3
![Page 112: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/112.jpg)
Lowlevelcontroller:velocitycontrol
velocitycontroller
[φθψpqr]
[uvw]
[u’v’w’r’]
[X’Y’Z’N’]
Requesttwist
[u’v’w’r’]+
τn=A*v’2+B*v’+C
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa;a 4
![Page 113: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/113.jpg)
Lowlevelcontroller:thrusterallocator
thrusterallocator
[X’Y’Z’N’]
Thrustersetpoints
Requestwrench
[X’Y’Z’N’]+
[th1th2th3th4th5]=[X’Y’X’N’]*
th1 th2 th3 th4 th5X 1 1 0 0 0
Y 0 0 0 0 1
Z 0 0 1 1 0
N 0.4 -0.4 0 0 0
Thrusteralloca;onmatrix
+Thrustermodeltotransformforceperthrustertosetpoint
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa;a 5
![Page 114: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/114.jpg)
Combinerequests
1. Allrequestsareaccumulatedinaqueue2. Ifthequeuealreadycontainsapreviousrequest
fromthesamenode,theoldoneiserased3. Requestsarecombinedat10Hz(lowcontroller
frequenzy):a. Requestsolderthan0.15saredeletedb. Remainingrequestsaresortedbypriorityc. Requestsarecombined
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa;a 6
![Page 115: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/115.jpg)
Combinerequestsv u w p q r
value 0.5 0 0 0 0 0.3
disabled_axis F T T T T F
req:teleopera;onpriority:409me:...
v u w p q r
value 0 0 0.2 0 0 0
disabled_axis T T F T T T
req:al;tude_controlpriority:109me:...
v u w p q r
value 0.5 0 0.2 0 0 0.3
disabled_axis F T F T T F
req:teleopera;onpriority:409me:...
+
=
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa;a 7
![Page 116: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/116.jpg)
Combinerequestsv u w p q r
value -1. 0 0.4 0 0 0
disabled_axis F T F T T T
req:teleopera;onpriority:409me:...
v u w p q r
value 0.4 0 0.2 0 0 0.1
disabled_axis F T F T T F
req:gotopriority:109me:...
v u w p q r
value 0.5 0 0.4 0 0 0
disabled_axis F T F T T T
req:teleopera;onpriority:409me:...
+
=
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa;a 8
![Page 117: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/117.jpg)
HighLevelControllers
• Movetoposi;on[xyzΨ]àposereq[xyzΨ]• Gotowaypoint[xyz]àposereq[zΨ]&twistreq[u]• Lineofsightwithcrosstrackingerror[xyz]• Followtrajectory{[x0y0z0],...[xnynzn]}• Al;tudecontrolleràposereq[z]• ...
NarcísPalomeras-UniversitatdeGirona EXCELLABUST2016-Croa;a 9
![Page 118: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/118.jpg)
1. Software installation
It is required to have a computer with UBUNTU 14.04 LTS and ROS Jade already installed to not loose time preparing the system. To follow the course it will be also necessary to install the UWSim simulator and the COLA2 software architecture: * To install UBUNTU 14.04 http://releases.ubuntu.com/14.04/ * To install ROS http://wiki.ros.org/ROS/Installation * To install UWSim: sudo apt-get install ros-jade-uwsim Once done, run UWSim for the first time. It will download some extra content. roscore & rosrun uwsim uwsim * To install dependencies and additional libraries: sudo apt-get install ros-jade-rosbridge-server ros-jade-octomap-ros ros-jade-ompl * To install COLA2 architecture:
Once your system is ready (UBUNTU and ROS installed), you have to create a catkin workspace (http://wiki.ros.org/ROS/Tutorials/InstallingandConfiguringROSEnvironment) to install the packages that form the COLA2 control architecture. Next, install the follow packages: cd ~/catkin_ws/src/ git clone https://bitbucket.org/udg_cirs/auv_msgs.git git clone https://bitbucket.org/udg_cirs/cola2_core.git git clone https://bitbucket.org/udg_cirs/cola2_s2.git
* To install Mapping and Path Planning modules:
cd ~/catkin_ws/src/ git clone https://bitbucket.org/udg_cirs/excellabust_croatia_16.git
To check that everything has been correctly installed do:
cd ~/catkin_ws/ catkin_make
2. How to run mapping and planning modules
After installing all the dependencies and repositories previously mentioned, there are two test scenarios to visualize, simulate and test different functionalities that are part of the software architecture. The first scenario resembles a natural-like environment composed of seamounts that form an underwater canyon. The second one is a 3-dimensional (3D) model of a breakwater structure composed of a series of concrete blocks. In order to simulate the SPARUS-II AUV in either scenario, it is necessary to execute the corresponding launch file: roslaunch croatia16_planning start_s2_croatia16_test_canyon.launch # (canyon) or roslaunch croatia16_planning start_s2_croatia16_test_blocks.launch # (breakwater structure) One of those functionalities that can be tested is the path-planning module, which uses the Open Motion Planning Library (OMPL) to solve start-to-goal queries. In order to use it, once the test 1
scenario and the AUV have been loaded (UWSim and RViz), there is a configuration file that
1 http://ompl.kavrakilab.org/
![Page 119: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/119.jpg)
includes different parameters for the path planner, including the Configuration Space (C-space) boundaries, the start and goal positions (query), the planner used to solve the query (sample-based methods), the computation time dedicated to attempt to solve the query, the depth (constant) at which the AUV navigates and the AUV used (Girona500 or SPARUS-II). To modify such configuration file: rosed croatia16_planning tests_canyon_croatia16.yaml # (canyon) or rosed croatia16_planning tests_blocks_croatia16.yaml # (breakwater structure) Once the path-planning parameters have been defined, it is necessary to run the planner to generate the mission file, which contains the collision-free path that solves the start-to-goal query. The planner uses a mapping module which provides an Octomap , which works as a 3D 2
representation (map) of the environment. For this demonstration, we assume all the environment as explored, thus a complete map is available a priori, as visualized in Rviz. To solve query using such map, it is necessary to run the corresponding launch file in a different terminal: roslaunch croatia16_planning croatia16_test_canyon_planning.launch # (canyon) or roslaunch croatia16_planning croatia16_test_blocks_planning.launch # (breakwater structure) Doing so, the planner will attempt to solve the query and will visualize a solution if there is one. Now, in order to send the AUV to conduct the mission and follow the resulting path, it is required first to load the mission and then to tell the pilot (software architecture) to follow the mission, which is specified as a series of waypoints. To do this, execute in a different terminal the following commands: # To load the mission (replace path_to_catkin_ws according to your local workspace installation rosservice call /cola2_control/load_trajectory "mystring: '/path_to_catkin_ws/src/excellabust_croatia_16/croatia16_planning/missions/mission.yaml'" #To execute the mission rosservice call /cola2_control/enable_trajectory
3. Exercises
Once the basic planning module has been tested, there is a series of exercises to extend its functionality.
3.1. Understanding the planning module
The planning module uses OMPL to solve a start-to-goal query. The basic interface to solve such type of queries is coded and documented in offline_planner_R2.cpp. This file contains the class OfflinePlannerR2, in which Offline means that the environment is assumed as explored and the path is calculated offline (i.e., before conducting the mission) and R2 3
refers to the dimension of the C-Space (RxR, for x and y coordinates). The class constructor
2 https://octomap.github.io/ 3 For an example of online mapping and planning using Octomaps and OMPL take a look to our work http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=7139336&tag=1, https://www.youtube.com/watch?v=A5-8LTBxbHQ
![Page 120: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/120.jpg)
basically defines the ROS publishers needed to visualize the resulting path and load the parameters from the configuration file (e.g. tests_canyon_croatia16.yaml).
Apart from the constructor and the methods that visualize the path and create the mission file, there is another important method planWithSimpleSetup(), which attempts to solve the start-to-goal query using a SimpleSetup (a class that defines and solves a path-planning problem). planWithSimpleSetup() defines the C-Space (RxR), creates an instance of the desired planner (RRT, RRT*, EST, PRM, etc., specified in the configuration file) and then creates an instance of the state validity checker. This latter is a user-defined set of routines (state_validity_checker_octomap_fcl_R2.cpp/hpp) that verifies whether a configuration is valid or not, which in the simplest case, as the one used in our example, means that the state (configuration or position) is collision-free. After creating the state validity checker, the start and goal positions (loaded from the configuration file in the constructor) are provided to the SimpleSetup. The final step is to attempt to solve the query for a specified time (also given in the configuration file).
3.2. Subscribing to the vehicle’s odometry
There are some parts of the code that are commented and incomplete. The first of them is a subscriber to the vehicle’s odometry. Subscribe to /cola2_navigation/nav_sts and store the vehicle x and y positions in order to continuously have updated information of vehicle’s position.
3.3. Creating a service to solve a query
The provided code attempts to solve the query once the instance of the planner has been created, as observed in the main procedure offline_planner.planWithSimpleSetup(). Create (complete) the service /planning/solve and its respective callback in order to solve the query only when such service is called.
3.4. Creating a service to solve a query from current position
Now that a subscriber is obtaining updated information of vehicle’s position, create (complete) the service /planning/solve_from_curr_pos and its respective callback in order to solve the query using the current position as the start position. The goal will remain as the one specified in the configuration. (Note: in order to modified the query start position provide start_state_[0] and start_state_[1] with the appropriate values).
3.5. Creating a service to solve a query from current position to a given goal
Now create (complete) the service /planning/solve_from_curr_pos_to_giv_goal and its respective callback in order to solve the query using the current position as the start position and a given goal. For this it is necessary to create the message SetGoal.srv that permits a service to receive the goal specified as a x and y values. (Note: in order to modify the query start and goal positions provide start_state_[0], start_state_[1], goal_state_[0], goal_state_[1] with the appropriate values. Also do not forget to modify the CMakeLists to compile the message and include the header in the source file).
3.6. Creating a service to solve a query from a given start to a given goal
Now create (complete) the service /planning/solve_from_giv_start_to_giv_goal and its respective callback in order to solve the query using given start and goal positions. For this it is necessary to create the message SetStartGoal.srv that permits a service to receive the
![Page 121: Introduc)on to ROS · • The publish/subscribe paern is very useful to broadcast informaon asynchronously. This means that once a node receives some informaon (e.g., a compass sensor](https://reader033.vdocuments.mx/reader033/viewer/2022050501/5f9368c2c83ab63b8f7d1e6f/html5/thumbnails/121.jpg)
start and goal specified as a start_x, start_y, goal_x and goal_y values. (Note: in order to modify the query start and goal positions provide start_state_[0], start_state_[1], goal_state_[0], goal_state_[1] with the appropriate values. Also do not forget to modify the CMakeLists to compile the message and include the header in the source file).