RobertOates
UniversityofNottingham
February7,2006
Abstract
ThisguideprovidesabasicgroundingintheuseofARIAandintroducessomeofthearchitectureusedinstandard,ARIA-basedprograms.Itincludesadiscussiononthestandardtechniquesassoci-atedwiththeimplementationofprogramsforcontrollingthePioneerrangeofrobots.ThisdocumentisnotasubstituteforreadingtheARIAdocumentation,butdealswiththepracticalissuesassociatedwithgettingaplatformworkinginthequickestpossibletime.
Contents
1Introduction
2TestinganInstallation3TheARIAArchitecture
3.1ArRobot-TheRobotClass...................3.2SensorsandActuators......................3.3
Actions,ActionGroupsandModes...............3.3.1ArAction.........................3.3.2ArActionGroup......................3.3.3ArMode..........................3.3.4ArActionDesired......
...............4CompilinganARIA-EnabledProgram4.1UsingWindows..........................4.2UsingLinux...............
.............4.2.1g++............................4.2.2Eclipse..............
.............
1
334445557777888
5UsefulARIACommands
5.1InitialisingtheRobot...................
5.1.1ArgumentParsingandRobotCommunications5.1.2KeyHandling...................5.1.3BehaviourModes.................5.1.4GenericSensorInitialisation...........5.1.5LaserInitialisation................5.2GeneralMovement....................5.3AcquiringSensorData..................5.45DOFArmControl....................5.5Miscellaneous.......................
........................................
9991010111112121313
6Troubleshooting146.1LinuxOnly............................14AExampleProgram-basedgram
A.1RobotTest.cpp......A.2ArActionWallFollow.h.A.3ArActionGroupFollow.hA.4ArModeWallFollow.h..
ontheoriginalARIAdemopro-....................................................................................1515192929
2
Table1:ThePrograms/PackagesRequiredtoUseARIASuccessfullyProgram/PackageVersionUsedAtDescriptiontheTimeofWriting
ARIA2.4.1ThesourcecodeandlibrariesrequiredtocreatenewARIAprogramsandsomedemoapplications.
Mapper3-Basic1.2.1Aprogramforgenerating
environmentsfortherobottonavigate
MobileSim0.2Thestage-basedsimulatorfromActivMedia
1Introduction
ThisguideisdesignedtogetpeopleupandrunningwiththeActivMediaRoboticsInterfaceforApplications,(ARIA)inthefastestpossibletime.Tounderstandit,youwillneedsomeknowledgeofC++.Itisassumedthatyouareworkingwithamachinethathastheprograms/packageslistedinTable1downloadedandinstalled.
2TestinganInstallation
Ifyouhaveonlyjustinstalledtherelevantprogramsandwanttocheckthattheinstallationwasasuccess,orifyousimplywanttoseeanexampleofARIAinaction,followthestepsoutlinedinthissection.Ifnot,simplyskipaheadtosection3.
StartbyrunningtheMobileSimprogram,(viatheStartmenuinWin-dowsorviathecommandMobileSim&fromaLinuxconsole)andselectthe”NoMap”optionwhenprompted.Thisshouldpresentyouwithalarge,blankgridwitharobot,(thereddot!)inthemiddle.Youcanzoominandoutusingthewheelonawheelmouse;movethemapcentrebyrightclickinganddragging;clickanddragtherobotintonewpositions;andchangetherobot’sorientationbyrightclickinganddraggingtherobot.Ifyounowruntheprogram”demo”(viatheStartmenuinWindowsorviathecommand
3
./demofromaLinuxconsole),youshouldbeabletocontroltherobotin”teleop”mode,(thedemoprogramwillneedfocus)andallowtherobottowanderin”wander”mode.
Youcanalsotrygeneratingyourownmapusing”Mapper3-Basic”.The”line”toolwillconstructwallsthatwilleffectthesimulatedrobot’ssonarandlaser-rangefinder.Youcanverifythisbyloadingyourmapandputtingthedemoprograminto”wander”mode.
3TheARIAArchitecture
TobeabletowriteARIA-basedprograms,itisfirstnecessarytounderstandthearchitecturetheARIAlibrarywaswrittentosupport.YouarenotforcedtowriteARIAprogramsinthisway.Ifyouwish,youcansimplycreatearobotclassandcalltherelevantcommandsfordirectlymanipulatingmovement,(seesection5.2)however,therearedistinctadvantagestousingthestandardarchitecture,(notleast,theveryfactthatawell-definedcodestructuremakesco-operativeprojectsconsiderablyeasier.)
ThissectionwilldiscusssomeoftheindividualclassesthataremadeavailablebyARIA,however,foracompletereferenceforallARIAclasses,see[1].
3.1ArRobot-TheRobotClass
This,unsurprisingly,isthemainARIAclass.Itencapsulatesallofthepri-maryinformationabouttherobotbeingcontrolled.Onitsown,aninstan-tiationofthisclasswillrepresentastandardpioneerbase,withnosensors,andonlythemotorsasactuators.
3.2SensorsandActuators
Toelaborateonthemainrobotclass,onemustaddtheadditionalsensorsandactuatorstoit.Basic,rangedsensors,suchasthelaserandthesonar,canbeaddedbyinstantiatingtherelevantclasses,(ArSickandArSonarDevicerespectively)andaddingthemusingArRobot’saddRangeDevicemethod,passingapointertotheinstantiatedsensorclassasinput.Seesections5.1.5and5.1.4fordetailsoninitialisingthelaser.Itisofnotethatthebumpersaretreatedasarangeddevicewhichreportsobjectsaseitherbeingundetectedorasexistingattheedgeoftherobot’sradius.
Thereisausefulvirtualrangeddevicethatcanbeaddedtotherobotcalledthe”ForbiddenRangeDevice”.Thisdevice,(ArForbiddenRangeDe-4
vice),canbeusedtoprovidedistancereadingsfromareasmarkedontherobot’smapas”forbidden”.Youcanadd”forbidden”linestoavirtualmapusingMapper3-Basic.Thiscanbeusedtocreatevirtualbarriersbetweentherobotandhazardsthatmightotherwisebedifficulttoperceiveusingrangedsensors,suchasstairwells.
Thebehavioursofothersensorsandactuatorsshouldbelookedupin[1].Bewarnedthatsensors(withtheexceptionof”rangedsensors”),andactua-tors,generallyattachtotherobot(i.e.ArDevice.setRobot(&robot)),asop-posedtotherobotattachingtothem,(i.e.robot.addRangeDevice(&sonar))asisthecasewithrangedsensors.
3.3Actions,ActionGroupsandModes
ThebehaviourofarobotisencapsulatedbyclassesthatinheritfromtheArAction,ArActionGroupandArModeclasses.3.3.1
ArAction
TheArActionclassrepresentsanindividualactionthattherobotcantakeinresponsetoitscircumstances.Pre-definedactionswithintheArialibraryin-cludeArAvoidFront,ArAvoidSideandArActionGoto,which,unsurprisingly,avoidobstaclesinfrontoftherobot,avoidobstaclestothesideoftherobot,andgotoagivenpositionrespectively.
ThecrucialmethodtooverridewithinArActionisArAction::fire().Thismethodreceiveswhattherobotcurrentlywantstodoasaninput,intheformofanArActionDesiredobjectandreturnsasecondArActionDesiredobjectrepresentingwhatthisArActionobjectwouldprefertodo.NULLcanbereturnediftheactiondoesnotwishtochangewhattherobotisdoing.3.3.2
ArActionGroup
AnArActiongroupisusedtosimplywrapacollection(oneormore)ofArActionsthatcollectivelyimplementabehaviour.Thisismerelyfortheconvenienceofbeingabletosimultaneouslyactivateeachactionwithasinglecalltothegroup’sactivatemethod.WhenaddingArActionsitispossibletoassociateaprioritywiththataction.TheallocationofprioritiestoArActionsallowstheimplementationofasubsumptiontypearchitecture,withactionsbeingfiredfromhighestlevelcommandtolowest,passingtheirdesiredout-putdownthechain.Finally,thelowestlevelultimatelydecideswhethertoimplementthehigh-levelrequestortooverrideit.
5
Figure1:AnoverviewoftherelationshipbetweentheActionimplementa-tionclasses.Italicsmarksclassesthatareinheritedfromthetypespecifiedandboldmarksactualinstantiationsofthetypespecified.
6
TheArActionGroupclassisalsousedbybeingoverridden.Thecon-structoranddatamembertypesusedinthenewclassspecifytheindividualArActiontypesthatitistorepresent.3.3.3
ArMode
AnArModeencapsulatesarobotbehaviour.AprivatedatamemberthatisaninheritedtypefromArActionGroupisusedtodictatethenatureoftheArMode.AnArModecanbeassociatedwithtwokeyboardcharacters(usuallythelower-caseandupper-caseofthesameletter),which,inthestandardARIAprogramarchitecture,willspecifythelettertheuserwillpushtoactivatethatmode.3.3.4
ArActionDesired
AnArActionDesiredobjecthasvariousmethodsforspecifyingachangeinheadingorvelocity,orabsoluteheadingsandvelocities.See[1]foracompletelist.
4
4.1
••••••••
CompilinganARIA-EnabledProgram
UsingWindows
CreateanewWIN32consoleapplication.Opentheprojectpropertiesdialog
(Project⇒[PROJECTNAME]Properties)Select”AllConfigurations”
NavigatetotheC/C++⇒GeneralSection
AddtheARIAincludepathstotheAdditionalIncludeDirectoriesNavigatetotheLinker⇒InputSection
AddtheARIAlibrarytotheAdditionalDependencies
EnsurethatthelocationofAria.dllisstoredinyoursystem’spathvariable
TheseinstructionswerewrittenforVisualStudio.NET2003.
NotethatifyouintendtorunyourprogramonaLinux-basedplatformyoumustavoidusinganyMFC-specificclassesandmakesurethatthedatatypesspecifiedforthe”main”functionareportable.Youcanachievethisusingconditionalcompilation.
#ifdefWIN32
int_tmain(intargc,_TCHAR*argv[])
7
#else
intmain(intargc,char**argv)#endif
WhencompilingforLinux,youwillneedtocommentoutreferencestostdafx.h.SadlytheMicrosoftcompiler’srelianceonstdafx.hmeansthatyoucannotuseconditionalcompilationtoremoveitautomatically.
4.2
4.2.1
UsingLinux
g++
WhencompilinganARIA-enabledprogramunderLinuxusingg++onthecommandlineyouneedtospecifyadditionaloptionstoensurethattheap-propriatelinkstakeplace.Anexamplecommandlinefollows.
g++-Wall-oOutputFile-lAria-ldl-lpthread-L/usr/local/Aria/lib-I/usr/local/Aria/includeInputFile.cpp
Intheexamplecommandlinetheprogramiscompiledusingthefol-lowingoptions:the”Wall”optionprovidesusefulwarningsaboutpotentialcodingproblems;the”oOutputFile”specifiestheoutputexecutablename;the”lAria”specifiesthattheprogramwillrequiretheARIAlibrary;the”ldl”and”lpthread”argumentsspecifyadditionallibrariesrequiredbyARIA;the”L/usr/local/Aria/lib”specifiesthestandardlocationoftheARIAlibrary;the”I/usr/local/Aria/include”specifiesthestandardlocationoftheARIAheaders;thefinalargumentspecifiesthemaininputfilefortheproject.4.2.2
Eclipse
IfyoupreferworkingwithanIDEsuchasEclipseyouwillneedtospecifytheinformationprovidedtog++viathemenuoptions.
TheProject⇒Propertiesmenuoptionwillallowyoutoconfigurealloftheseoptions.Simplyaddtherelevantinformationspecifiedinsection4.2.1viatheGCCC++Compiler⇒DirectoriesandGCCC++Linker⇒Librariesmenuoptions.The”-X”labelsatthetopofeachsectioncorrespondtoa”-X”compileroptionfromsection4.2.1.
8
5
5.1
UsefulARIACommands
InitialisingtheRobot
BeforeonecanusetheARIAlibrary,itisimportantthatitbeinitialised.Thiscanbedonesimplybycalling:Aria::init();
Thisshouldbedoneatthebeginningoftheprogram.Attheendofthepro-gram,insteadofusingthestandardreturntoleavemain,callthefollowing,passingthedesiredreturnvalue.Aria::exit(0);5.1.1
ArgumentParsingandRobotCommunications
TomaintainuniformitybetweenARIA-basedprograms,thelibrarycomeswithastandardargumentparser.Duringtheinitialisationofaprogram,thiscanbeusedtoensurethatalltheconfigurableelementsofanARIAprogram,(robotIPaddressetc)canbepassedinthesamewaytoanyARIAprogram.ArArgumentParser::loadDefaultArguments()willallocatethede-faultsrequiredtoconnecttothelocalhost,(eitheraMobileSimsimulationonthecurrentmachineorarealrobot.)ArArgumentParserparser(&argc,argv);parser.loadDefaultArguments();
Actuallyconnectingtoarobot,requirestheArSimpleConnectorclass.ThiscanreceiveanArArgumentParserclassasaninitialisertoitsconstruc-tor.Thevaluesspecifiedwithinthelatterclassdictatetheconnectionset-tings.
ArSimpleConnectorconnector(&parser);if(!connector.parseArgs()){
//outputsomeerrormessageexit(1);}
ActuallyconnectingtotherobotrequiresanArRobotclass,(noinitiali-sationrequired)tobeattachedtotheconnector.Onceconnected,therobotcanbeplacedintoasynchronousmode,whichensuresthatiftheconnec-tionislost,therobotwillceasetooperate.Thisisrecommendedforsafetyreasons.
9
ArRobotrobot;
if(!connector.connectRobot(&robot)){
//Someerrormessagehereexit(1);}
robot.runAsync(true);
Beforeattemptingtoruntherobot,itisrecommendedthatyouplacethemotorsintoanenabledandsafestate.ThisisachievedviathecommandArRobot::comInt(ArCommands::ENABLE,1).YoucanuseArRobot::lock()andArRobot::unlock()toensurethattheinitialisationcommandisnotin-terferedwithbyotheruserserroneouslyconnectingtotherobot.robot.lock();
robot.comInt(ArCommands::ENABLE,1);robot.unlock();
Thereareothertasksthatbenefitfrombeingperformedwhilsttherobotisinthelockedstate,seesection5.1.35.1.2
KeyHandling
Ifyouwanttobeabletocontroltherobotviatheterminal,youwillrequireameansofprocessingkey-events.IntheARIAarchitecture,thisisdoneviatheARKeyHandlerclass.Akeyhandlershouldbeattachedtoarobotclass,beforeyouattempttoconnecttotherobot.However,youshouldnotattachakeyhandlerifyouwishtoaddtheprogramtothestart-upbehaviouroftheoperatingsystem,asthiscanpotentiallyresultinrequestsbeingsenttotheconsole,beforeitisactuallyallocated,resultinginseriouserrorsonbootup.
AkeyhandlerneedstoberegisteredwiththemainARIAlibrary,andthenattachedtoaspecificrobot.
ArKeyHandlerkeyHandler;
Aria::setKeyHandler(&keyHandler);robot.attachKeyHandler(&keyHandler);
5.1.3
BehaviourModes
Youcanaddanynumberofbehaviourmodestoarobot.Ifakeyhandlerisassignedtotherobot,youcanassignshort-cutkeystoeachbehaviour,
10
andswitchbetweenthematruntime.Itisagoodideatoaddnewmodeswhilsttherobotisinthelockedstate,toavoidotherpeopleerroneouslyconnectingtotherobotwhilstyouareaddingthenewmodes.Thecodeextractwhichfollows,showstwonewbehavioursbeingaddedtoarobot,thestandardwandermode,andthestandardteleopmode.Thewandermodeisactivated,thusmakingitthe”default”behaviouroftherobot.Notethattheletters”w”and”t”areassociatedwiththebehaviours,sothekeyhandlercanbeusedtoswitchbetweenthem.Alsonotethatinthisinstancetherobotobjectis”added”tothebehaviour,ratherthantheotherwayround.
robot.lock();
ArModeWanderwander(&robot,\"wander\’w’,’W’);ArModeTeleopteleop(&robot,\"teleop\’t’,’T’);wander.activate();robot.unlock();
5.1.4
GenericSensorInitialisation
Therearetwomaintypesofsensor,rangedandnon-ranged.Rangedsensors,(sonar,lasersand,withintheARIAenvironment,bumpers),areaddedbycreatinganobjectoftheappropriatetype,andaddingtheobjecttotherobot,usingtheaddRangeDevicecommand.Theexamplebelowillustratesasonarbeingaddedtotherobot.ArSonarDevicesonarDev;
robot.addRangeDevice(&sonarDev);
Non-rangedsensorsareaddedbycreatinganobjectoftheappropriatetype,andaddingtheROBOTtotheOBJECT.Insomecasesthisisdoneaspartoftheclassinitialisation.Belowisthecommandtoaddagyrototherobot.ArAnalogGyrogyro(&robot);5.1.5
LaserInitialisation
Lasersarearangedsensor,soareaddedusingtheaddRangeDevicecommand.However,lasersrequireadditionalinitialisation.Thelasercontrolobject,ArSickisexplicitlyaddedtotheArSimpleConnectorobjectusedbytherobot.Thismeansthatthecontrolschemeusedtooperatethelaserisalsospecifiedseparately.Again,forsafetypurposes,anasynchronouscontroltechniqueisrecommended.Onceattachedtoaconnector,thelasercanthenbeattachedtotherobotandusedinthesamewayasasonarsensor.
11
ArSicksick;
sick.runAsync();
if(!connector.connectLaser(&sick)){
//SomeerrormessageAria::exit(2);}
robot.addRangeDevice(&sick);
5.2GeneralMovement
AswellassettingmovementoptionsintheformofanArActionDesiredfromanArActionitisalsopossibletousemethodsoftheArRobotobjecttomanipulatetheactuatorsdirectly.Commoncommandsinclude:
•ArRobot::setVel(doublevel)-Setstheforwardspeedoftherobot
ArRobot::setVel2(doublevel1,doublevel2)-Setstheindividualspeedsofthemotors
•ArRobot::setHeading(doubleheading)-Setstheangletherobotshouldturnto(degrees)
•ArRobot::setRotVel(doublevel)-SetstherotationalspeedoftherobotForafulllist,see[1].
5.3AcquiringSensorData
Acquiringsensordataislargelyspecifictothetypeofsensor.However,ausefulcommandtouseis:
ArRobot::checkRangeDevicesCurrentPolar(doublestart,doubleend)Thiscommandwillcheckalltherobot’srangedsensorsthatcoverthespec-ifiedrangeofangles,(specifiedindegrees)andreturnthesmallestvalue,(i.e.theclosestobject.)Therearehowever,twoimportantissuestocon-siderwhenusingthiscommand.Firstly,thecommandalwaysassumesacounter-clockwiserotationfromthestartangletotheendangle,(specifiedintherange±180owherepositivenumbersdenoteacounter-clockwiserota-tion)soitisimportanttoensurethattheanglesareprovidedinthecorrect
12
order.Secondlythedistancegivenisthedistancefromthepointassumedtobeinthecentreoftherobot.Toacquiretheabsolutedistancebetweenthedetectedobjectandtherobotitself,youshouldsubtracttheresultofArRobot::getRobotRadius()fromtheanswer.
5.45DOFArmControl
Controllingthe5DOFArmviaARIArequirestheuseoftheArP2Armclass.Thearmmustbeinitialisedpriortouse,andthesafetyinstructionsforthearmrecommendthatthearmbeplacedintothe”home”positionbeforeacti-vation,topreventsudden,potentially-damaging,movementsinthejoints.Inordertoinitialisethearm,theArP2ArmobjectmustbeattachedtoanAr-Robotobject,thathasalreadybeenconnectedandplacedintoasynchronouscommunicationmode.Thefirstthingthatshouldbedoneafterattachingthearmtoarobotistopowerthearmon.ArP2Arm::powerOn()bydefaultinsertsatwosecondwaitafterpowerisappliedtoarm,toallowtheinitialvibrationstosettle.Thiscanbeoverriddenbyexplicitlysettingthe”sleep”flagtofalse,(ArP2Arm::powerOn(false))butthisisnotrecommended.Ex-plicitlyun-initialisingthearmwillmeanthatitwillautomaticallyreturntothehomestatebeforeshuttingdown.Belowisanexampleofthecommandsusedtocontrola5DOFarm.
ArP2ArmmyArm;
//Itisassumedthattherobotisalready//connectedandrunningasynchronouslymyArm.setRobot(&robot);
myArm.init();myArm.powerOn();
//Thiswillmovethefirstjoint,(1-basedindex)
//tothe45degreeposition,usingthedefaultvelocitymyArm.moveTo(1,45,0);
Itisofnotethatifthegripperisheldintheshutpositionformorethan10minutesitcancausedamagetotheservo-mechanismthatdrivesit.Bydefault,ARIAwillkeeptrackofthetimethegripperhasbeenshutforandshouldopenitwhenthedangertimeapproaches.However,thishasnotbeentestedbytheauthoranditisadvisedthatgripperusebeminimised.
5.5Miscellaneous
Whenallinitialisationiscomplete,acalltoArRobot::waitForRunExit()willpreventtheprogramfromexitinguntilaterminationsignalisreceivedfrom
13
therobotcontroller.Ifakeyhandlerisassociatedwiththerobot,youcanforceaterminationsignal,byhittingescape.
6Troubleshooting
Ididn’tunderstandawordofthat-wherecanIgetmorehelp?
Try[1]ortheARIAnewsgroup.Atthetimeofwriting,youcouldfindthelinktotheusergroupshere:http://robots.mobilerobots.com/.
6.1LinuxOnly
WhenIrunmyprogramIgetstrangesegmentationfaults
TherearetwomaincausesforsegmentationfaultsfromtheARIAlibrary.Firstly,tryrebuildingyourcodeonthetargetmachine,ratherthanbuildingitelsewhereandmovingtheexecutable.Thisresolvesmanysegmentationfaultissues.Secondly,theARIAlibraryitselfmaynotbefullyinstalledprop-erly.Tryperformingthefollowingstepsasroottorebuilditfromsource.
cd/usr/local/ARIAmakecleanmake
References
[1]ARIAOverview(html),AvailablewiththestandardinstallofARIA
(index.html)
14
A
ExampleProgram-basedontheoriginalARIAdemoprogram
Thisprogramimplementsaprogramsimilartotheoriginal”demo”program,distributedwiththeArialibrary.Ithastheoriginal”wandermode”andtheoriginal”teleopmode”.Anadditionalcustommodeforfollowingtherighthandwallofapenisalsoaddedusingthestandardarchitecture.
A.1RobotTest.cpp
Thisisthemainfunctionoftheprogram.Itinitialisestherobotandaddsallofthesensorsandbehaviours.Afunction”msg”isimplementedtofacilitatepumpingusefulfeedbacktotheuser.#include\"Aria.h\"#defineLASER//outputheader#include\"stdio.h\"
//Custommode
#include\"armodewallfollow.h\"//Toolazytowrite\\nallthetimevoidmsg(char*opMesg){
printf(opMesg);printf(\"\\n\");
}
intmain(intargc,char*argv[]){
intnumber;
msg(\"Initialisingtherobot\");
//Initiatethelibrary-doingthisatthebeginningand\"aria::exit\"
attheend,seemstostopcertainproblems
//inthedestructorsofsomeoftheobjectsAria::init();
msg(\"Creatinganargumentparser\");
15
//AnencapsulationofthevalidinputargumentstoanARIAprogramArArgumentParserparser(&argc,argv);msg(\"Creatingaconnector\");
//AmeansofconnectingtotherobotArSimpleConnectorconnector(&parser);msg(\"Creatingarobot\");//ThemainrobotclassArRobotrobot;
#ifdefLASER
//alaserincaseoneisusedArSicksick;
#endif
msg(\"Creatingasonardevice\");//CreateasonardeviceArSonarDevicesonarDev;
//Assignthedefaultsrequiredtoconnecttothesimulationmsg(\"Assigningdefaultvalues\");parser.loadDefaultArguments();
//Thiswillparsetheargumentsmsg(\"Parsingdefaultvalues\");if(!connector.parseArgs()){
msg(\"Unabletoidentifydefaultsettings\");scanf(\"%d\exit(1);
}
//akeyhandlersowecandoourkeyhandlingmsg(\"Creatingakeyhandler\");ArKeyHandlerkeyHandler;
msg(\"Attachingkeyhandler\");
//lettheglobalariastuffknowaboutitAria::setKeyHandler(&keyHandler);
16
//attachtotherobot
robot.attachKeyHandler(&keyHandler);msg(\"Youmaypressescapetoexit\");
//Addthesonartotherobotmsg(\"Attachingsonardevice\");robot.addRangeDevice(&sonarDev);
#ifdefLASER
//addthelasertotherobotrobot.addRangeDevice(&sick);
//addagyro,it’llseeifitshouldattachtotherobotornotArAnalogGyrogyro(&robot);
#endif
//Connecttotherobot
msg(\"Connectingtotherobot\");
if(!connector.connectRobot(&robot)){
msg(\"Unabletoconnecttosimulation\");
scanf(\"%d\exit(1);
}
//starttherobotrunning,truesothatifweloseconnectiontherun
stops
msg(\"Runtherobot!\");robot.runAsync(true);
#ifdefLASER
//setupthelaserbeforehandingittothelasermodesick.runAsync();
//connectthelaserifitwasrequestedif(!connector.connectLaser(&sick)){
printf(\"Couldnotconnecttolaser...exiting\\n\");Aria::exit(2);
}
#endif
//Preventanyonefromtamperingwhilstwesetuptherobotmsg(\"locktherobot!\");
17
robot.lock();
//SETUPROBOTHERE
//turnonthemotors
msg(\"Turnonthemotors\");
robot.comInt(ArCommands::ENABLE,1);
ArModeWanderwander(&robot,\"wander\’w’,’W’);ArModeTeleopteleop(&robot,\"teleop\’t’,’T’);
ArModeWallFollowfollow(&robot,\"follow\’f’,’F’);//Thedefaultbehaviourwander.activate();
//turnonthemotors
robot.comInt(ArCommands::ENABLE,1);msg(\"Unlocktherobot\");robot.unlock();
robot.waitForRunExit();Aria::exit(0);
}
18
A.2ArActionWallFollow.h
Thisfileimplementstheclassthatcontrolsthewallfollowingbehaviour.Itistoohigh-leveltobepartofasubsumptionarchitecture,soistheonlyactionwithinthe”wallfollow”actiongroup.Itisusedheremerelyasaproofofconcept.
#ifndefARACTIONWALLFOLLOW#defineARACTIONWALLFOLLOW//Standardariatypedefinitions#include\"ariaTypedefs.h\"
//TheArActionclass,thatthisclasswillinheritfrom#include\"ArAction.h\"
//Thresholdsforbeing\"toonear\"toawall#defineTOO_NEAR_UPPER500#defineNEAR_UPPER1000
#defineTOO_NEAR_UPPER_B500
//Rotationalspeedlimits#defineCLOCK_MAX
-10#defineCLOCK_MID40#defineCOUNTER_MIN0
//Translationalspeedlimits#defineFORWARD_MIN200#defineSTOP-100
200#defineNEAR_UPPER_B
-40#defineCLOCK_MIN
-20#defineCOUNTER_MAX20#defineNO_ROT
100#defineFORWARD_MAX
0#defineBACKWARD_MIN
//Thewallfollowingclass
classArActionWallFollow:publicArAction{public:
//Constructor-dostandardinit,specifyingthepurposeofthe
function
ArActionWallFollow(constchar*name=\"Followstheright-handwall\"):
ArAction(name,\"followstherighthandwall\"){
19
//Theseangleselectionsbasicallymeanthat\"right\"and
\"left\"havefoursensorseach,(twofromthefrontandtwofromtheback
//The\"front\"and\"back\"arealsogivenfoursensorseachFRONT.startAngle=-22;FRONT.endAngle=22;
LEFT.startAngle=22;LEFT.endAngle=112;BACK.startAngle=112;BACK.endAngle=-112;RIGHT.startAngle=-112;RIGHT.endAngle=-22;
//Thissimplyallocatesmemorytothemessagebufferusedfor
givingfeedback
opMessage=(char*)malloc(255*sizeof(char));
}
//Destructor-freethemessagebuffer’sallocatedmemory~ArActionWallFollow(){
free(opMessage);
}
//The\"firing\"routine
ArActionDesired*fire(ArActionDesiredcurrentDesired){
FUZZY_DISTANCEleftDist,rightDist,frontDist,backDist;
//Gatherthesensordata
leftDist=GetReading(LEFT);rightDist=GetReading(RIGHT);frontDist=GetReading(FRONT);backDist=GetReading(BACK);//ResetmyoutputmyDesired.reset();situ=UNDECIDED;
20
//Iftoocloseontwoopposingwalls,juststop-theremay
notberoomtomanouvre
{
TOO_NEAR)){
TOO_NEAR)){
TOO_NEAR)){
if(
((leftDist==TOO_NEAR)&&(rightDist==TOO_NEAR))||
((frontDist==TOO_NEAR)&&(backDist==TOO_NEAR)))
{
myDesired.setRotVel(0);myDesired.setVel(0);situ=TRAPPED;
}else{
//Checkforcornertoocloseconditions
if((frontDist==TOO_NEAR)&&(leftDist==TOO_NEAR))
//front-left
myDesired.setRotVel(CLOCK_MAX);myDesired.setVel(BACKWARD_MIN);situ=CORNER_FL;
}elseif((frontDist==TOO_NEAR)&&(rightDist==
//front-right
myDesired.setRotVel(COUNTER_MAX);myDesired.setVel(BACKWARD_MIN);situ=CORNER_FR;
}elseif((backDist==TOO_NEAR)&&(leftDist==
//backleft
myDesired.setRotVel(CLOCK_MAX);myDesired.setVel(STOP);situ=CORNER_BL;
}elseif((backDist==TOO_NEAR)&&(leftDist==
//back-right
myDesired.setRotVel(COUNTER_MIN);myDesired.setVel(FORWARD_MIN);situ=CORNER_BR;
}else{
//Checkforsinglewalltoocloseconditions
21
collision
ACCEPTABLY_NEAR){
RIGHT_ACCEPTABLE_FRONTCLOSE;
RIGHT_ACCEPTABLE_FRONTFAR;
ACCEPTABLY_NEAR){
if(frontDist==TOO_NEAR){//front
myDesired.setRotVel(COUNTER_MAX);myDesired.setVel(BACKWARD_MIN);situ=FRONT_CLOSE;
}elseif(backDist==TOO_NEAR){
//back
myDesired.setRotVel(NO_ROT);myDesired.setVel(FORWARD_MAX);situ=BACK_CLOSE;
}elseif(leftDist==TOO_NEAR){
//left
myDesired.setRotVel(CLOCK_MAX);myDesired.setVel(STOP);situ=LEFT_CLOSE;
}elseif(rightDist==TOO_NEAR){
//right
myDesired.setRotVel(COUNTER_MIN);myDesired.setVel(FORWARD_MIN);situ=RIGHT_CLOSE;
}else{
//Acceptablesinglewallconditionsif(rightDist==ACCEPTABLY_NEAR){//Acceptablyclosetorightwall
//Checkforimminentfrontwall
if(frontDist==
myDesired.setRotVel(COUNTER_MAX);myDesired.setVel(FORWARD_MIN);situ=
}else{
myDesired.setRotVel(CLOCK_MIN);myDesired.setVel(FORWARD_MAX);situ=
}
}elseif(frontDist==
22
//Acceptablyclosetofront
wall
myDesired.setRotVel(COUNTER_MIN);myDesired.setVel(STOP);situ=
FRONT_ACCEPTABLE;
}elseif(leftDist==
ACCEPTABLY_NEAR){
//Acceptablyclosetoleft
wall
myDesired.setRotVel(CLOCK_MAX);myDesired.setVel(STOP);situ=LEFT_ACCEPTABLE;
}elseif(backDist==
ACCEPTABLY_NEAR){
//elseacceptablycloseto
backwall
myDesired.setRotVel(CLOCK_MID);myDesired.setVel(STOP);situ=BACK_ACCEPTABLE;
}elseif(
(backDist==FAR_AWAY)&&
(frontDist==FAR_AWAY)&&
(leftDist==FAR_AWAY)&&
(rightDist==FAR_AWAY)){
//farfromallwalls
myDesired.setRotVel(CLOCK_MIN);myDesired.setVel(FORWARD_MAX);situ=OPEN_ROAD;
}
}
}
}
if(situ==UNDECIDED){
//notthoughtaboutthisscenariomyDesired.setRotVel(NO_ROT);
23
myDesired.setVel(STOP);situ=NOT_TAUGHT;
}
return(&myDesired);
}
//Fornowjustblindlyreturnthedesiredstructure.Shouldreally
makethatapointer,tofacilitatepassingNULL
ArActionDesired*getDesired(void){return&myDesired;}//Returnsaplaintextexplanationaboutwhat’sgoingon!char*GetExplanation(){
switch(situ){
caseTRAPPED:
opMessage=\"Tooclosetotwoopposingwalls,
stoppingforsafety\";
break;
caseNOT_TAUGHT:
opMessage=\"Iwasnottoldwhattodointhis
situation!\";
break;
caseCORNER_FR:
opMessage=\"Tooclosetofront-rightcorner\";break;caseCORNER_FL:
opMessage=\"Tooclosetofront-leftcorner\";break;caseCORNER_BR:
opMessage=\"Tooclosetoback-rightcorner\";break;caseCORNER_BL:
opMessage=\"Tooclosetoback-leftcorner\";break;caseFRONT_CLOSE:
24
opMessage=\"Tooclosetothefrontwall\";break;
caseBACK_CLOSE:
following\";
following\";
following\";
directionchange\";
opMessage=\"Tooclosetothebackwall\";break;caseLEFT_CLOSE:
opMessage=\"Tooclosetotheleftwall\";break;caseRIGHT_CLOSE:
opMessage=\"Tooclosetotherightwall\";break;caseFRONT_ACCEPTABLE:
opMessage=\"Frontwallisacceptablyclose-break;
caseBACK_ACCEPTABLE:
opMessage=\"Backwallisacceptablyclose-break;
caseLEFT_ACCEPTABLE:
opMessage=\"Leftwallisacceptablyclose-break;
caseRIGHT_ACCEPTABLE_FRONTCLOSE:
opMessage=\"Turningtoavoidiminentwall
break;
caseRIGHT_ACCEPTABLE_FRONTFAR:
opMessage=\"Followingrightwall\";break;caseOPEN_ROAD:
opMessage=\"Searchingforawall\";
25
break;
default:
opMessage=\"Eeek-Idon’tknowwhatI’m
thinking!\";
break;
}
return(opMessage);
}
private:
//Encapsulatestypedefstruct
doubledouble
}DIRECTION;
adirection{
startAngle;endAngle;
//Notreally\"fuzzy\"initstruestsense,butit’lldo!//Justforthepurposesofclassfiyingthedistancestypedefenum{
TOO_NEAR,
ACCEPTABLY_NEAR,FAR_AWAY
}FUZZY_DISTANCE;
//Situationenumerations-seemethod\"getExplanation\"forafull
definitionofeach
typedefenum{
TRAPPED,NOT_TAUGHT,CORNER_FR,CORNER_FL,CORNER_BR,CORNER_BL,FRONT_CLOSE,BACK_CLOSE,LEFT_CLOSE,RIGHT_CLOSE,
FRONT_ACCEPTABLE,BACK_ACCEPTABLE,
26
LEFT_ACCEPTABLE,
RIGHT_ACCEPTABLE_FRONTCLOSE,RIGHT_ACCEPTABLE_FRONTFAR,OPEN_ROAD,UNDECIDED
}SITUATION;
//Thedesiredaction
ArActionDesiredmyDesired;//Therobot’scurrentsituationSITUATIONsitu;char*
opMessage;
//SimplifiescodereadabilityforthemainalgorithmDIRECTIONFRONT;DIRECTIONLEFT;DIRECTIONRIGHT;DIRECTIONBACK;
//Usefulfunctionforpollingthesensorsandreturninga
classificationofwherethenearestobjectinthegivendirectionis
FUZZY_DISTANCEGetReading(DIRECTIONdir){
//ValuetobereturnedFUZZY_DISTANCEretVal;//TherawsensordatadoublerealReading=
(myRobot->checkRangeDevicesCurrentPolar(dir.startAngle,dir.endAngle)-myRobot->getRobotRadius());
//ThresholdsforclassificationdoubletooNearThresh,nearThresh;
//Decidedtousedifferentclassificationsforthebacksensor
data,asitisnotasimportanttoachievingthetask
//Isthistheback?if(
(dir.startAngle==BACK.startAngle)
27
thresholds
}
};#endif
&&
(dir.endAngle==BACK.endAngle))
{
//Usethefront-specificclassificationthresholdstooNearThresh=TOO_NEAR_UPPER_B;nearThresh=NEAR_UPPER_B;
}else{
//UsethegeneralclassificationthresholdstooNearThresh=TOO_NEAR_UPPER;nearThresh=NEAR_UPPER;
}
//Classification
if(realReading }elseif(realReading retVal=ACCEPTABLY_NEAR; }else{ //Readingisabovethe\"near\"thresholdretVal=FAR_AWAY; } return(retVal); 28 A.3ArActionGroupFollow.h Thisimplementstheactiongroupthatimplementsthewallfollowingbe-haviour.Fordemonstrationpurposes,onlytheactiondefinedinA.2isadded.Notethatapriorityof100isgiventotheactionasitisadded.Anaddi-tionalmethodwasimplementedtohandlethepassingofmessagesfromthelow-levelactiontothemainfunction.Thiswassimplyforthepurposesofuser-feedbackanddebugging.The”userTask”methodisutilisedtopumpfeedbackmessagesfromthelow-levelcontroller,aboutwhatstateitisin.#ifndefARACTIONGROUPFOLLOW#defineARACTIONGROUPFOLLOW#include\"Aria.h\"#include\"aractionwallfollow.h\" classArActionGroupFollow:publicArActionGroup{public: ArActionGroupFollow(ArRobot*robot):ArActionGroup(robot){ myAction=newArActionWallFollow(\"RightWallFollow\");addAction(myAction,100); }char*GetStatus(){ return(myAction->GetExplanation()); } protected: ArActionWallFollow*myAction; };#endif A.4ArModeWallFollow.h Thisfileimplementsthe”mode”forwallfollowing.Itsimplyallocatesmem-orytotheactiongroup,andusesthebaseclassactivationcheckertoensurethatitissafetorun. #ifndefARMODEWALLFOLLOW#defineARMODEWALLFOLLOW#include\"Aria.h\"#include\"ArActionGroupFollow.h\"classArModeWallFollow:publicArMode{public: ///Constructor ArModeWallFollow(ArRobot*robot,constchar*name,charkey,char key2): ArMode(robot,name,key,key2), 29 myGroup(robot){ myGroup.deactivate(); } ///Destructor ~ArModeWallFollow(){} voidactivate(void){ if(!baseActivate()) return; myGroup.activateExclusive(); } voiddeactivate(void){ if(!baseDeactivate()) return; myGroup.deactivate(); } voidhelp(void){ ArLog::log(ArLog::Terse,\"Attemptingtofollowtherighthand printf(myGroup.GetStatus()); printf(\"Therobotwillnowattempttofollowtheright-hand // wall\");//wall\\n\"); } voiduserTask(void){ printf(myGroup.GetStatus());printf(\"\\n\"); } protected: ArActionGroupFollowmyGroup;};#endif 30
因篇幅问题不能全部显示，请点此查看更多更全内容