class BrainTestNavigator(Brain):
def setup(self):
#################################
# We start with a map built from a to-scale image of the floor-plan
# of the location of the robot. Then we localize in that 'perfect'
# representation of the space. (The location of the 'real world'
# marker in the map was determined manually by placing the marker
# on an easily identifiable feature of the image, like a pillar.)
#################################
# cropped map (only the downstairs lab area)
################################
self.nav = Navigator(cols=26, # approx 40cm each
rows=38, # approx 40cm each
widthMM=float(10383), # map width in mm
heightMM=float(15184), # map height in mm
# lab-map-crop-scaled.png has 202x308 pixels
# map scale information:
# xScale 0.0514 (m/pixel), yScale 0.0493 (m/pixel)
image='map-images/lab-map-crop-scaled.png',
gridResize = 2)
self.nav.placeMarker(row=29,col=6,name="A",angle=0, redraw=1)
self.nav.placeMarker(row=19,col=13,name="B",angle=3.14, redraw=1)
self.nav.setGoal(row=4,col=22)
################################
# full map (entire floor of the downstairs lab area)
################################
# self.nav = Navigator(cols=64, # approx 40cm each
# rows=124, # approx 40cm
# widthMM=float(25724), # map width in mm
# heightMM=float(49537), # map height in mm
## lab-map-scaled.png has 500x1004 pixels
## map scale information:
## width 152 pixels = 782cm
## height 227 pixels = 1120cm
# image='map-images/lab-map-scaled.png',
# imageResize = 0.90)
# self.nav.placeMarker(row=30,col=6,name="A",angle=0, redraw=1)
# self.nav.placeMarker(row=19,col=14,name="B",angle=3.14, redraw=1)
# self.nav.setGoal(row=4,col=22)
################################
# then we use update the robot's location
self.nav.updateRobotLocation(self.robot)
try:
self.pathList = self.nav.findPath()
except:
pass
def step(self):
# then we use update the robot's location
self.nav.updateRobotLocation(self.robot)
# if we have already arrived at the next subgoal we remove it and
# move on to the next.
if (self.pathList[0] == (self.nav.getCurrentRow(),self.nav.getCurrentCol())):
print "I reached old subgoal",self.pathList[0]
del self.pathList[0]
print "current Location",self.nav.getCurrentRow(),self.nav.getCurrentCol()
print "next subgoal",self.pathList[0]
translation, rotate = self.nav.determineMove(self.pathList[0])
self.robot.move(translation,rotate)
