def drawMatrixRow(self,rowNumber):
for k in range(0,2):
position = PositionClass()
position.y = rowNumber
for position.x in range(0,self.xSize):
if(position.x is not 0):
if(self.isWallExist(position,Direction.MAP_LEFT)):
sys.stdout.write("|")
if(k==1 and self.isWallExist(position,Direction.MAP_BOTTOM)):
sys.stdout.write("___")
elif(k==1 and position.y==0):
sys.stdout.write("___")
else:
sys.stdout.write(" ")
else:
if(k==1 and self.isWallExist(position,Direction.MAP_BOTTOM)): sys.stdout.write("____")
elif(k==1 and position.y==0): sys.stdout.write("____")
else: sys.stdout.write(" ")
else:
sys.stdout.write("|")
if(k==1 and self.isWallExist(position,Direction.MAP_BOTTOM)):
sys.stdout.write("___")
elif(k==1 and position.y==0):
sys.stdout.write("___")
else:
sys.stdout.write(" ")
if(position.x==self.xSize-1):
sys.stdout.write("|")
print("")
def removeWallsInContiguousField(self,position,direction):
self.removeWall(position,direction)
positionForContiguousField = PositionClass()
positionForContiguousField.copyFrom(position)
directionForContiguousField = Direction.MAP_TOP
if(direction == Direction.MAP_TOP):
directionForContiguousField = Direction.MAP_BOTTOM
positionForContiguousField.y = positionForContiguousField.y + 1
elif(direction == Direction.MAP_BOTTOM):
directionForContiguousField = Direction.MAP_TOP
positionForContiguousField.y = positionForContiguousField.y - 1
elif(direction == Direction.MAP_RIGHT):
directionForContiguousField = Direction.MAP_LEFT
positionForContiguousField.x = positionForContiguousField.x + 1
elif(direction == Direction.MAP_LEFT):
directionForContiguousField = Direction.MAP_RIGHT
positionForContiguousField.x = positionForContiguousField.x - 1
try:
self.removeWall(positionForContiguousField,directionForContiguousField)
except Exception as e:
#print("Remove wall: %s" % e)
pass
def __init__(self):
#GPIO.setmode(GPIO.BCM)
#inicjalizacja silnikow
#self.motors = MotorControl(23,12,22,13)
#inicjalizacja sensorow
##
#wykorzystwane tylko do testowania algorytmu potem do usuniecia
self.simulationMap = SimulationMapMatrixClass(10, 10)
self.position = PositionClass()
self.moves = 0
self.map = MapMatrixClass(1, 1)
def checkPositionsForFindPath(self,position,fromPosition,toPosition):
positionToCheck = PositionClass()
i = position.course
for j in range(0,4):
positionToCheck.copyFrom(self.preparePositionToCheck(i,position))
directionToCheck = self.prepareDirectionToCheck(i)
if(self.checkRangeOfIndex(positionToCheck)):
if(not self.currentMap.isWallExist(position,directionToCheck)):
if(self.pathMap[positionToCheck.x][positionToCheck.y] == 0):
self.pathMap[positionToCheck.x][positionToCheck.y] = self.actualValue + 1
self.changeAmount = self.changeAmount + 1
if(fromPosition == positionToCheck):
return True
i = i + 90
return False
def checkPositions(self,position):
positionToCheck = PositionClass()
i = position.course
for j in range(0,4):
positionToCheck.copyFrom(self.preparePositionToCheck(i,position))
directionToCheck = self.prepareDirectionToCheck(i)
if(not self.currentMap.isWallExist(position,directionToCheck)):
if(self.pathMap[positionToCheck.x][positionToCheck.y] == 0):
self.pathMap[positionToCheck.x][positionToCheck.y] = self.actualValue + 1
self.changeAmount = self.changeAmount + 1
if(not self.currentMap.wasVisited(positionToCheck)):
return positionToCheck
i = i + 90
positionToCheck.x = -1
return positionToCheck
def findNearestNoVisitedSpot(self,robotPosition):
self.pathMap.fill(0)
self.pathMap[robotPosition.x][robotPosition.y] = 1
self.actualValue = 1
foundPosition = PositionClass()
while(True):
self.changeAmount = 0
position = PositionClass()
for position.x in range(0,self.currentMap.xSize):
for position.y in range(0,self.currentMap.ySize):
if(self.pathMap[position.x][position.y] == self.actualValue):
foundPosition.copyFrom(self.checkPositions(position))
if(foundPosition.x != -1):
return foundPosition
self.actualValue = self.actualValue + 1
if(self.changeAmount == 0):
raise Exception("Not found anymore no visited spot")
def preparePositionToCheck(self,iteration,position):
positionToCheck = PositionClass()
positionToCheck.copyFrom(position)
if((iteration/90) % 4 == 0):
positionToCheck.x = positionToCheck.x - 1
elif((iteration/90) % 4 == 1):
positionToCheck.y = positionToCheck.y + 1
elif((iteration/90) % 4 == 2):
positionToCheck.x = positionToCheck.x + 1
elif((iteration/90) % 4 == 3):
positionToCheck.y = positionToCheck.y - 1
return positionToCheck
def randomlyRemoveTheWalls(self):
position = PositionClass()
for position.x in range(0, self.xSize):
for position.y in range(0, self.ySize):
if (random.randint(0, 1) == 1):
self.removeWallsInContiguousField(position,
Direction.MAP_TOP)
if (random.randint(0, 1) == 1):
self.removeWallsInContiguousField(position,
Direction.MAP_RIGHT)
if (random.randint(0, 1) == 1):
self.removeWallsInContiguousField(position,
Direction.MAP_BOTTOM)
if (random.randint(0, 1) == 1):
self.removeWallsInContiguousField(position,
Direction.MAP_LEFT)
def generateMapEdge(self):
position = PositionClass()
for position.x in range(0, self.xSize):
position.y = self.ySize - 1
self.setWall(position, Direction.MAP_TOP)
position.y = 0
self.setWall(position, Direction.MAP_BOTTOM)
for position.y in range(0, self.ySize):
position.x = 0
self.setWall(position, Direction.MAP_LEFT)
position.x = self.xSize - 1
self.setWall(position, Direction.MAP_RIGHT)
def findPathTo(self,fromPosition,toPosition):
if(self.currentMap.isNotAvailable(toPosition)):
raise Exception("There is no way to the given point")
else:
fromPosition.course = 0
toPosition.course = 0
self.pathMap.fill(0)
self.pathMap[toPosition.x][toPosition.y] = 1
self.actualValue = 1
while (True):
self.changeAmount = 0
position = PositionClass()
for position.x in range(0,self.currentMap.xSize):
for position.y in range(0,self.currentMap.ySize):
if(self.pathMap[position.x][position.y] == self.actualValue):
if (self.checkPositionsForFindPath(position,fromPosition,toPosition)):
return self.pathMap
self.actualValue = self.actualValue + 1
if(self.changeAmount == 0):
raise Exception("There is no way to the given point")
return self.pathMap
def completeMapAfterMapping(self): position = PositionClass() for position.x in range(0,self.xSize): for position.y in range(0,self.ySize): if(not self.wasVisited(position)): self.setNotAvailable(position) positionToCheck = PositionClass() positionToCheck.copyFrom(position) positionToCheck.x = position.x - 1 if(position.x != 0 and self.isWallExist(positionToCheck,Direction.MAP_RIGHT)): self.setWall(position,Direction.MAP_LEFT) positionToCheck.copyFrom(position) positionToCheck.x = position.x + 1 if(position.x != self.xSize-1 and self.isWallExist(positionToCheck,Direction.MAP_LEFT)): self.setWall(position,Direction.MAP_RIGHT) positionToCheck.copyFrom(position) positionToCheck.y = position.y - 1 if(position.y != 0 and self.isWallExist(positionToCheck,Direction.MAP_TOP)): self.setWall(position,Direction.MAP_BOTTOM) positionToCheck.copyFrom(position) positionToCheck.y = position.y + 1 if(position.y != self.ySize-1 and self.isWallExist(positionToCheck,Direction.MAP_BOTTOM)): self.setWall(position,Direction.MAP_TOP)
def findWayToNearestNoVisitedSpot2(self,robotPosition):
foundNoVisitedSpot = PositionClass()
foundNoVisitedSpot.copyFrom(self.findNearestNoVisitedSpot2(robotPosition))
return self.findPathTo(robotPosition,foundNoVisitedSpot)
class RobotClass:
def __init__(self):
#GPIO.setmode(GPIO.BCM)
#inicjalizacja silnikow
#self.motors = MotorControl(23,12,22,13)
#inicjalizacja sensorow
##
#wykorzystwane tylko do testowania algorytmu potem do usuniecia
self.simulationMap = SimulationMapMatrixClass(10, 10)
self.position = PositionClass()
self.moves = 0
self.map = MapMatrixClass(1, 1)
def checkSensor(self):
#do testow, pozniej do zamienia przez odczyty z czujnikow ktory bedzie trzeba uzaleznic od wartosci self.position.course
if self.simulationMap.isWallExist(self.position, Direction.MAP_TOP):
self.map.setWallsInContiguousField(self.position,
Direction.MAP_TOP)
elif self.map.addNewRowIfNeeded(self.position.y, Direction.MAP_TOP):
pass
if self.simulationMap.isWallExist(self.position, Direction.MAP_RIGHT):
self.map.setWallsInContiguousField(self.position,
Direction.MAP_RIGHT)
elif self.map.addNewColumnIfNeeded(self.position.x,
Direction.MAP_RIGHT):
pass
if self.simulationMap.isWallExist(self.position, Direction.MAP_BOTTOM):
self.map.setWallsInContiguousField(self.position,
Direction.MAP_BOTTOM)
elif self.map.addNewRowIfNeeded(self.position.y, Direction.MAP_BOTTOM):
self.position.y = self.position.y + 1
if self.simulationMap.isWallExist(self.position, Direction.MAP_LEFT):
self.map.setWallsInContiguousField(self.position,
Direction.MAP_LEFT)
elif self.map.addNewColumnIfNeeded(self.position.x,
Direction.MAP_LEFT):
self.position.x = self.position.x + 1
self.map.setVisited(self.position)
def goForward(self):
#motors.ForwardOneField
self.position.goOneFieldForward()
def goRight(self):
#motors.turnRight()
self.position.addToCourse(90)
self.goForward()
def goBack(self):
#motors.turnBack()
self.position.addToCourse(180)
self.goForward()
def goLeft(self):
#motors.turnLeft()
self.position.addToCourse(270)
self.goForward()
def goByPath(self, pathMap):
while (pathMap[self.position.x][self.position.y] != 1):
actualValue = pathMap[self.position.x][self.position.y]
if (self.position.x != self.map.xSize - 1
and pathMap[self.position.x + 1][self.position.y]
== actualValue - 1):
if self.position.course == 0:
self.goRight()
elif self.position.course == 90:
self.goForward()
elif self.position.course == 180:
self.goLeft()
elif self.position.course == 270:
self.goBack()
elif (self.position.x != 0
and pathMap[self.position.x - 1][self.position.y]
== actualValue - 1):
if self.position.course == 0:
self.goLeft()
elif self.position.course == 90:
self.goBack()
elif self.position.course == 180:
self.goRight()
elif self.position.course == 270:
self.goForward()
elif (self.position.y != self.map.ySize - 1
and pathMap[self.position.x][self.position.y + 1]
== actualValue - 1):
if self.position.course == 0:
self.goForward()
elif self.position.course == 90:
self.goLeft()
elif self.position.course == 180:
self.goBack()
elif self.position.course == 270:
self.goRight()
elif (self.position.y != 0
and pathMap[self.position.x][self.position.y - 1]
== actualValue - 1):
if self.position.course == 0:
self.goBack()
elif self.position.course == 90:
self.goRight()
elif self.position.course == 180:
self.goForward()
elif self.position.course == 270:
self.goLeft()
self.moves = self.moves + 1