Here
From Rsewiki
Drivebase.rule code
<?xml version="1.0" ?>
<rule name="initDrive" run="true">
<init> global.drive.mapDest[2] = 0.0 # destination in map coordinates global.drive.utmDest[2] = 0.0 # destination in UTM coordinates global.drive.odoDest[2] = 0.0 # destination in ODO coordinates global.drive.lastMapDest[2] = 0.0 # last destination in map coordinates global.drive.lastUtmDest[2] = 0.0 # last destination in UTM coordinates global.drive.lastOdoDest[2] = 0.0 # last destination in ODO coordinates global.drive.refCoord = -1 # 0=odo, 1=UTM, 2=map global.drive.newDest = 0 global.drive.pause = false global.drive.waitHere = true # waits in a state with low RPM global.drive.gettingClose = false global.drive.skipDestination = false global.drive.compasHeading = false # heading in compas degrees if true (else math) global.drive.radians = true # heading in radians if true, else in degrees (math heading only) global.drive.continueDist = 1 # continues when closer than this from next node global.drive.holdLine = true # try hold line from last waypoint, else go direct global.drive.engineRPM= 1900; global.mission.name= 'none' global.mission.leg= 0 odoPose.tripTimeB= 0 odoPose.tripB= 0 global.drive.distToTgt = 0 global.drive.fromPose[2] = 0.0 # last used source pose print("------------ rule drivebase version 2.747") </init>
</rule>
<rule name="drive" if="true">
<description> Main drive rules </description> <init> # define drive local variables destUpdated = false dest[2] = 0 odoDest[2] = 0 destOK = false relPose[2] = 0 tripStart = now() lastManDist = 100.0 # target distance at last plan holdLineDist = 11 # aiming pose position when holding line lastOdoDest[2] = 0 newDest = false renewDrive = false #distToTgt = 0 #fromPose[2] = 0 # #define rule to update map target in odometry coordinates <rule name="updateMap" if="global.drive.refCoord == 2 and global.drive.newDest"> relPose = mapToPose(mapPose.pose, global.drive.mapDest) odoDest = poseToMap(odoPose.pose, relPose) relPose = mapToPose(mapPose.pose, global.drive.lastMapDest) lastOdoDest = poseToMap(odoPose.pose, relPose) newDest = true </rule> # #define rule to update utm target in odometry coordinates <rule name="updateUtm" if="global.drive.refCoord == 1 and global.drive.newDest"> relPose = mapToPose(utmPose.pose, global.drive.utmDest) odoDest = poseToMap(odoPose.pose, relPose) relPose = mapToPose(utmPose.pose, global.drive.lastUtmDest) lastOdoDest = poseToMap(odoPose.pose, relPose) newDest = true </rule> # #define rule to use destination odometry coordinates <rule name="updateOdo" if="global.drive.refCoord == 0 and global.drive.newDest"> lastOdoDest = odoDest odoDest = global.drive.odoDest global.drive.newDest = false; newDest = true </rule> # # should destination be used directly, or are we following a line <rule name="aim4dest" if="newDest and not global.drive.holdLine"> # use of next destination is OK dest = odoDest; destOK = true lastManDist = 100 newDest = false; # print("aim4dest direct dest " dest) </rule> # # should destination be used directly, or are we following a line <rule name="aim4line" if="global.drive.holdLine and (newDest or lastManDist < holdLineDist)"> # use of next destination is not OK <init> dtgt = 0 srcDist = 0 </init> dest = odoDest; dtgt = dist2d(dest, odoPose.pose) if (dtgt > holdLineDist + 500) # disabled <block> # use a shorter distance than the real destination srcDist = holdLineDist + 5 + dist2d(lastOdoDest, odoPose.pose) dest = poseOnLine(lastOdoDest, odoDest, srcDist) # print("aim4line target " dtgt "m, new dest " dest) </block> # else # print("aim4line target " dtgt "m, direct dest " dest) destOK = true if (newDest) lastManDist = 100 newDest = false; </rule> # # stop the robot for a pause <rule name="stopAndWait" if="smrctl.directWait or global.drive.pause or global.drive.waitHere"> print("Stopping ...") odoPose.tripB = 0 odoPose.tripTimeB = 0 smr.send("flushcmds") smr.do('setvar "hakoenginespeed" 800') if (global.drive.waitHere and destOK and odopose.vel > 0.3) <block> # drive towards the destination and stop smr.do("drive " dest[0] " " dest[1] " " dest[2] ' "rad" @v0.2 : ($targetdist < 0.0) | ($odovelocity < 0.3)') </block> smr.send("idle") print("StopAndWait: stopped at pose " odopose.pose) wait() : not if() print("StopAndWait: resume after pause") smr.do('setvar "hakoenginespeed" ' global.drive.engineRPM) </rule> # # test if robot is close to destination <rule name="closeTest" if="destOK"> <init> ds = 0 rel[2] = 1 # target relative pose x,y,h </init> # get destination relative to current pose rel = mapToPose(odoPose.pose, odoDest) ds = rulestate.sampletime * smr.speed + 0.1 + global.drive.continueDist # close if x is less that a sample distance, and total distance is small too global.drive.gettingClose = ds > rel[0] and (3.0 > hypot(rel[1], rel[0]) or abs(rel[0]) > abs(rel[1])) destOK = not global.drive.gettingClose </rule> # <rule name="replayRenewDrive" if="not renewDrive"> # renew drive command if driven more than 1 meter # should only be needed in replay mode wih no live MRC to event trigger this renewal <init> d = 0.0 </init> d = dist2d(global.drive.fromPose, odoPose.pose) renewDrive = d > 1 if (renewDrive) print("Rule::replayRenewDrive: Driven 1m, so forced renewal of drive command") </rule> # advance a further distance <rule name="advance" if="destOK and lastManDist > smrctl.manPlanDist and not global.drive.pause and smr.connected"> odoPose.tripB = 0 odoPose.tripTimeB = 0 global.drive.fromPose = odoPose.pose renewDrive = false print("Drive advance from " odopose.pose " towards " dest " (leg " global.mission.leg ")") tripStart = now() lastManDist = dist2d(dest, odoPose.pose) drivePos.odo(dest[0], dest[1], dest[2]) : renewDrive or not destOK or global.drive.pause or not smr.connected print("Drive advance ended leg at odo pose " odopose.pose " (leg " global.mission.leg ")") </rule> # </init> # stay alert here wait() : false
</rule>
<rule name="driveUTM">
<parameters x="0" y="0" h="0" /> global.drive.lastUtmDest = global.drive.utmDest global.drive.utmDest[0] = x global.drive.utmDest[1] = y if (global.drive.compasHeading == true) global.drive.utmDest[2] = limittopi((90 - h) * pi / 180.0) else if (global.drive.radians) global.drive.utmDest[2] = h else global.drive.utmDest[2] = limittopi(h * pi / 180.0) global.drive.refCoord = 1 global.drive.gettingClose = false global.drive.skipDestination = false global.drive.waitHere = false; global.mission.leg = global.mission.leg + 1 print("Leg " global.mission.leg " UTM target pose: x:" x ", y:" y ", h:" h) global.drive.newDest = true wait() : global.drive.gettingClose or global.drive.skipDestination <post> global.drive.newDest = false #global.drive.holdLine=false </post>
</rule>
<rule name="driveMap">
<parameters x="0" y="0" h="0" /> global.drive.lastMapDest = global.drive.mapDest global.drive.mapDest[0] = x global.drive.mapDest[1] = y if (global.drive.compasHeading) global.drive.mapDest[2] = limittopi((90 - h) * pi / 180.0) else if (global.drive.radians) global.drive.mapDest[2] = h else global.drive.mapDest[2] = limittopi(h * pi / 180.0) global.drive.refCoord = 2 global.drive.gettingClose = false global.drive.skipDestination = false global.drive.waitHere = false; global.mission.leg = global.mission.leg + 1 print("Leg " global.mission.leg " map target pose: x:" x ", y:" y ", h:" h) global.drive.newDest = true wait() : global.drive.gettingClose or global.drive.skipDestination <post> global.drive.newDest = false #global.drive.holdLine=false </post>
</rule>
<rule name="driveNode">
<parameters node="" hdg="0"/> <init> nextNode[2] = 0.0; </init> nextNode[0] = mapbase.node(node, 'x') nextNode[1] = mapbase.node(node, 'y') nextNode[2] = mapbase.node(node, 'th') + hdg driveMap(nextNode[0], nextNode[1], nextNode[2])
</rule>
<rule name="driveOdo">
<parameters x="0" y="0" h="0" /> global.drive.lastOdoDest = global.drive.odoDest global.drive.odoDest[0] = x global.drive.odoDest[1] = y if (global.drive.compasHeading) global.drive.odoDest[2] = limittopi((90 - h) * pi / 180.0) else if (global.drive.radians) global.drive.odoDest[2] = h else global.drive.odoDest[2] = limittopi(h * pi / 180.0) global.drive.refCoord = 0 global.drive.gettingClose = false global.drive.skipDestination = false global.drive.waitHere = false; global.mission.leg = global.mission.leg + 1 print("Leg " global.mission.leg " odo target pose: x:" x ", y:" y ", h:" h) global.drive.newDest = true wait() : global.drive.gettingClose or global.drive.skipDestination <post> print("driveodo continues, as gettingClose=" global.drive.gettingClose ", and skip=" global.drive.skipDestination) global.drive.newDest = false #global.drive.holdLine=false </post>
</rule>
<rule name="driveWait">
# stop and wait are language keywords # stop here for some seconds <parameters stoptime="1.0"/> print("********** stop and wait for " stoptime " sec") global.drive.waitHere = true; wait(stoptime) : false print("********** resumes drive")
</rule>
<rule name="missionStart">
<parameters missionName="'noname'"/> global.mission.name=missionName global.mission.leg=0 odoPose.tripTimeB= 0 odoPose.tripB= 0 print("Mission " global.mission.name " started.")
</rule>
<rule name="missionEnd">
global.drive.waitHere=true wait(2) : false print("Mission " global.mission.name " stopped after " global.mission.leg " legs.") print("Mission took " odoPose.tripTimeB " and drove " odoPose.tripB "m.")
</rule>