## jin_ming_2006 / autoSweep .gitee-modal { width: 500px !important; }

Explore and code with more than 6 million developers，Free private repositories ！：）
simulate.py 45.44 KB
#!/usr/local/bin/python
#-*-coding:utf-8-*-

from  Tkinter import *
import networkx as nx

DIRECTION_DELTA = {"E":(1,0),"W":(-1,0),"N":(0,-1),"S":(0,1)}
ROBOT_SIZE = 5
GAP = 2
COLOR_COVERED = "#FFDA89"
COLOR_CLEANED = "#FFFFFF"
COLOR_BOUNDARY = "#FEFFFF"
COLOR_DOOR = "#FDFFFF"

def calculateDistance(start,end):
return abs(start[0] - end[0])+abs(start[1] - end[1])

def getConverseDirection(direction):
if(direction == "E"):
return "W"
if(direction == "W"):
return "E"
if(direction == "N"):
return "S"
if(direction == "S"):
return "N"

def get1StepCleanedRegion(robotPosition,robotDirection, pixelNum = 1):
if(robotDirection == "W"):
edgeStartX = robotPosition[0] - int(ROBOT_SIZE/2) - pixelNum
edgeEndX = robotPosition[0] - int(ROBOT_SIZE/2) -1
edgeStartY = robotPosition[1] - int(ROBOT_SIZE/2)
edgeEndY = robotPosition[1] + int(ROBOT_SIZE/2)
if(robotDirection == "E"):
edgeStartX = robotPosition[0] + int(ROBOT_SIZE/2) +1
edgeEndX = robotPosition[0] + int(ROBOT_SIZE/2) +pixelNum
edgeStartY = robotPosition[1] - int(ROBOT_SIZE/2)
edgeEndY = robotPosition[1] + int(ROBOT_SIZE/2)
if(robotDirection == "S"):
edgeStartX = robotPosition[0] - int(ROBOT_SIZE/2)
edgeEndX = robotPosition[0] + int(ROBOT_SIZE/2)
edgeStartY = robotPosition[1] + int(ROBOT_SIZE/2) +1
edgeEndY = robotPosition[1] +int(ROBOT_SIZE/2) +pixelNum
if(robotDirection == "N"):
edgeStartX = robotPosition[0] - int(ROBOT_SIZE/2)
edgeEndX = robotPosition[0] + int(ROBOT_SIZE/2)
edgeStartY = robotPosition[1] - int(ROBOT_SIZE/2) -pixelNum
edgeEndY = robotPosition[1] - int(ROBOT_SIZE/2) -1
return (edgeStartX,edgeEndX,edgeStartY,edgeEndY)

class Map(nx.Graph,Canvas):

def __init__(self,parent, iniBackground):
nx.Graph.__init__(self)
Canvas.__init__(self,parent,bd=3,relief=RIDGE,width=500, height=300)
self.im = PhotoImage(file= iniBackground)
self.create_image(ROBOT_SIZE,ROBOT_SIZE,anchor=NW, image= self.im)
self.pack(side=RIGHT)
self.uncoveredReg =[]
self.uncoveredRoom = []
self.currentRoom = None
self.coveredReg =[]
self.currentReg = None
self.currentX = int(ROBOT_SIZE/2) +GAP  # robot center coordinate because robot size is 5*5
self.currentY = int(ROBOT_SIZE/2) +GAP
self.index=0
self.NODE_TYPE_SHAPE_MAP = \
{'corner':self.__drawCornerNode, 'obstacle':self.__drawObstacleNode,'joint':self.__drawJointNode}
self.EDGE_TYPE_SHAPE_MAP = \
{'free':self.__drawFreeEdge, 'upObtacle':self.__drawUpObtacleEdge,'downObtacle':self.__drawDownObtacleEdge}

def moveUpdate(self,robotDirection):
(edgeStartX,edgeEndX,edgeStartY,edgeEndY) = get1StepCleanedRegion((self.currentX,self.currentY),robotDirection)
for i in range(edgeStartX, edgeEndX+1):
for j in range(edgeStartY, edgeEndY+1):
if(self.im.get(i,j)=="220 220 220"):
self.im.put(COLOR_CLEANED,(i,j))

self.currentX = self.currentX + DIRECTION_DELTA[robotDirection][0];
self.currentY = self.currentY + DIRECTION_DELTA[robotDirection][1]

def ifRoomFinished(self):
return not self.uncoveredReg

def ifFinished(self):
return not self.uncoveredRoom

def setCurrentPos(self,x,y):
self.currentX = x
self.currentY = y
edgeStartX = x - int(ROBOT_SIZE/2)
edgeEndX = x + int(ROBOT_SIZE/2)
edgeStartY = y - int(ROBOT_SIZE/2)
edgeEndY = y + int(ROBOT_SIZE/2)

for i in range(edgeStartX, edgeEndX+1):
for j in range(edgeStartY, edgeEndY+1):
self.im.put(COLOR_CLEANED,(i,j))

def ifBump(self,robotDirection):
robotPosition = (self.currentX,self.currentY)
(edgeStartX, edgeEndX, edgeStartY, edgeEndY) = get1StepCleanedRegion(robotPosition,robotDirection)

for i in range(edgeStartX, edgeEndX+1):
for j in range(edgeStartY, edgeEndY+1):
if(self.im.get(i,j)=="253 255 255"):
return True
return False

def buildVirtalWall(self,start, end,currentDirection):
if start[1] > end[1]:
upLimit = start[1]
downLimit = end[1]
else:
upLimit = end[1]
downLimit = start[1]

for i in range(start[0],end[0]+1):
for j in range(downLimit,upLimit+1):
self.im.put(COLOR_DOOR,(i,j))

self.currentRoom = {'pos':start, 'direction':getConverseDirection(currentDirection)}
self.uncoveredRoom.append({'pos':start, 'direction':currentDirection})

def ifCloseToVirtualWall(self,direction):
currentPos = (self.currentX, self.currentY)
(edgeStartX, edgeEndX, edgeStartY, edgeEndY) = get1StepCleanedRegion(currentPos, direction, pixelNum = ROBOT_SIZE)

for i in range(edgeStartX, edgeEndX+1):
for j in range(edgeStartY, edgeEndY+1):

if(self.im.get(i,j)=="253 255 255"):
return True
return False

def setCurrentRoom(self,room):
self.currentRoom = room
self.uncoveredRoom.remove(room)

def setCurrentRegion(self,region):
self.currentReg = region
self.__delUncoveredReg(region)

def getRouteToNextRoom(self):
currentPos = (self.currentX, self.currentY)

shortestDistance = 999
nextRoom = None

for item in self.uncoveredRoom:

distance= calculateDistance(currentPos, item['pos'])

if (distance < shortestDistance):
shortestDistance = distance
nextRoom = item
if shortestDistance==0:
break

path = self.__getPath(currentPos, nextRoom['pos'])

return {'path':path, 'targetRoom':nextRoom, 'targetRegion':{}}

# route = {path: [position, position], sweepDirection:"N"}
def getRouteToNextRegionBoundary(self):
currentPos = (self.currentX, self.currentY)

shortestDistance = 999
nextRegPoint = None
nextReg = None

for item in self.uncoveredReg:
if item['up']:
distance, point= self.__getDistanceFromPoint2Edge(currentPos, item['up'])
else:
distance,point = self.__getDistanceFromPoint2Edge(currentPos, item['down'])
if (distance < shortestDistance):
shortestDistance = distance
nextRegPoint = point
nextReg = item
if shortestDistance==0:
break

path = self.__getPath(currentPos, nextRegPoint)

return {'path':path, 'targetRegion':nextReg,'targetRoom':{}}

self.NODE_TYPE_SHAPE_MAP[type](position)
return position

self[start][end]['weight'] = calculateDistance(start,end)
self.EDGE_TYPE_SHAPE_MAP[type](start,end)

def updateRegion(self,nodeList):
upBoundary,downBoundary = self.__segmentRegionByBoundary(nodeList)

self.__updateUncoveredReg(upBoundary, downBoundary)

if not self.currentReg:
self.currentReg = self.uncoveredReg.pop()

return

self.combineCurrentBoundary()

def __getDistanceFromPoint2Edge(self,point, edge):
distance1 = calculateDistance(point, edge[0])
distance2 = calculateDistance(point, edge[1])

if distance1<distance2:
return distance1, edge[0]
else:
return distance2, edge[1]

def __getPath(self, start, end):
return self.__briefGetPath(start,end)
# return self.__aGetPath(start,end)
# return self.__dijkstraGetPath(start,end)

# a*算法
def __aGetPath(self, start, end):
opened = {}
closed = {}

closed[start] = {'parent':None, 'C':0, 'H': calculateDistance(start,end)}

point = start

while(point != end):
neighbourList = self.__getNeighbour(point)
for neighbourPoint in neighbourList:
if self.__ifBlock(neighbourPoint):
continue
if closed.has_key(neighbourPoint):
continue
c = closed[point]['C'] +1
h = calculateDistance(neighbourPoint, end)

if opened.has_key(neighbourPoint):
if (opened[neighbourPoint]['C'] + opened[neighbourPoint]['H']) > (c+h):
opened[neighbourPoint]['C'] = c
opened[neighbourPoint]['H'] = h
opened[neighbourPoint]['parent'] = point
else:
opened[neighbourPoint] = {}
opened[neighbourPoint]['C'] = c
opened[neighbourPoint]['H'] = h
opened[neighbourPoint]['parent'] = point

minF = 999
for item in opened:
F = opened[item]['C'] +opened[item]['H']
if minF >F:
minF = F
point = item

closed[point] ={}
closed[point] = opened[point]
del opened[point]

path =[]
point = end
while point:
path.insert(0,point)
point = closed[point]['parent']

return path

def __dijkstraGetPath(self,start,end):
closed = {}
opened ={}

opened[start] = {'parent':None}

while not (end in opened):
backupOpen ={}
for openitem in opened:
neighbourList = self.__getNeighbour(openitem)
for neighbourPoint in neighbourList:
if self.__ifBlock(neighbourPoint):
continue
if closed.has_key(neighbourPoint):
continue
if opened.has_key(neighbourPoint):
continue
backupOpen[neighbourPoint] =  {'parent':openitem}
closed[openitem] = opened[openitem]
opened = backupOpen

path = [end]
point = opened[end]['parent']

while point:
path.insert(0,point)
point = closed[point]['parent']

return path

# 简洁算法
def __briefGetPath(self,start,end):
path = []
current = start

path.append(start)

while (current != end):
neighbourList = self.__getNeighbour(current)
F = 999
nextPoint = None
for neighbourPoint in neighbourList:
if self.__ifBlock(neighbourPoint):
continue
if neighbourPoint in path:
continue

weight = calculateDistance(neighbourPoint, end)

if( weight< F):
F = weight
nextPoint = neighbourPoint
current = nextPoint

path.append(current)

return path

def ifTouchBoundary(self,direction):
robotPosition = (self.currentX, self.currentY)
(edgeStartX, edgeEndX, edgeStartY, edgeEndY) = get1StepCleanedRegion(robotPosition,direction)

for i in range(edgeStartX, edgeEndX+1):
for j in range(edgeStartY, edgeEndY+1):
if(self.im.get(i,j)=="254 255 255"):
return True
return False

def __ifBlock(self,point):
edgeStartX = point[0] - int(ROBOT_SIZE/2)
edgeEndX = point[0] + int(ROBOT_SIZE/2)
edgeStartY = point[1] - int(ROBOT_SIZE/2)
edgeEndY = point[1] + int(ROBOT_SIZE/2)

# if ((edgeStartX <GAP) or (edgeEndX> Env.WIDTH-GAP) or (edgeStartY <GAP) or (edgeEndY>Env.HEIGHT-GAP)):
#     return True

for i in range(edgeStartX, edgeEndX+1):
for j in range(edgeStartY, edgeEndY+1):
if(self.im.get(i,j)=="220 220 220"):
return True
return False

def __calculateF(self, point, start, end):
G = calculateDistance(point, start)
H = calculateDistance(point, end)
return H

def __getNeighbour(self,current):
neighbourList =[]
neighbourList.append((current[0]+1, current[1]))
neighbourList.append((current[0], current[1]-1))
neighbourList.append((current[0]-1, current[1]))
neighbourList.append((current[0], current[1]+1))

return neighbourList

def __updateUncoveredReg(self,upBoundary,downBoundary):
for item in upBoundary:

for item in downBoundary:

self.__buildVirtualBoundary(region)
self.uncoveredReg.append(region)

def __delUncoveredReg(self, region):
self.__buildVirtualBoundary(region,color=COLOR_CLEANED)
self.uncoveredReg.remove(region)

def __buildVirtualBoundary(self,region,color=COLOR_BOUNDARY):
edge = None
if(region['up']):
edge = region['up']
edge = ((edge[0][0],edge[0][1]+2), (edge[1][0],edge[1][1]+2))
else:
edge = region['down']
edge = ((edge[0][0],edge[0][1]-2), (edge[1][0],edge[1][1]-2))

for i in range(edge[0][0], edge[1][0]+1):
for j in range(edge[0][1], edge[1][1]+1):
self.im.put(color,(i,j))

def combineCurrentBoundary(self):
if not self.currentReg:
return
if not self.currentReg['up']:
region = self.__findClosedRegion(self.currentReg['down'], 'up')
self.currentReg['up'] = region['up']
self.__delUncoveredReg(region)
elif not self.currentReg['down']:
region = self.__findClosedRegion(self.currentReg['up'], 'down')

self.currentReg['down'] = region['down']
self.__delUncoveredReg(region)

self.__drawFreeEdge(self.currentReg['up'][0], self.currentReg['down'][0])
self.__drawFreeEdge(self.currentReg['up'][1], self.currentReg['down'][1])
self.coveredReg.append(self.currentReg)

def __findClosedRegion(self,edge, direction):
anotherRegion = ()
for item in self.uncoveredReg:
if not item[direction]:
continue

if (direction == 'up') and (item[direction][0][1] >= edge[0][1]):
continue
if (direction == 'down') and (item[direction][0][1] <= edge[0][1]):
continue
if not anotherRegion:
anotherRegion = item
continue

if calculateDistance(edge[0],item[direction][0])< calculateDistance(edge[0],anotherRegion[direction][0]):
anotherRegion = item

return anotherRegion

def __drawCornerNode(self,position):
pos = self.__convertPos(position)
self.create_rectangle(pos[0]-1.5,pos[1]-1.5,pos[0]+1.5,pos[1]+1.5,\
fill="red",outline="red")

def __drawObstacleNode(self,position):
pos = self.__convertPos(position)
self.create_oval(pos[0]-1.5,pos[1]-1.5,pos[0]+1.5,pos[1]+1.5,\
outline="blue")
def __drawJointNode(self,position):
pos = self.__convertPos(position)
self.create_oval(pos[0]-1.5,pos[1]-1.5,pos[0]+1.5,pos[1]+1.5,\
fill="black")
def __drawFreeEdge(self,start,end):
newStart = self.__convertPos(start)
newEnd = self.__convertPos(end)
self.create_line(newStart[0], newStart[1],newEnd[0],newEnd[1])

def __drawUpObtacleEdge(self,start,end):
newStart = self.__convertPos(start)
newEnd = self.__convertPos(end)
self.create_line(newStart[0], newStart[1],newEnd[0],newEnd[1],dash=True, fill="red")

def __drawDownObtacleEdge(self,start,end):
newStart = self.__convertPos(start)
newEnd = self.__convertPos(end)
self.create_line(newStart[0], newStart[1],newEnd[0],newEnd[1],dash=True, fill="blue")

def __convertPos(self,position):
return (position[0]+5, position[1]+5)

def __segmentRegionByBoundary(self,nodeList):
upRegionBoundary = []
downRegionBoundary = []

up = ()
down=()

length = len(nodeList)
for i in range(length-1):
edge = self.get_edge_data(nodeList[i], nodeList[i+1])
if(edge['type'] == 'free') or (edge['type']=="upObtacle"):
if not up:
up = (nodeList[i], nodeList[i+1])
else:
if (up[1] == nodeList[i]):
up = (up[0], nodeList[i+1])
else:
upRegionBoundary.append(up)
up = (nodeList[i], nodeList[i+1])
if(edge['type'] == 'free') or (edge['type']=="downObtacle"):
if not down:
down = (nodeList[i], nodeList[i+1])
else:
if (down[1] == nodeList[i]):
down = (down[0], nodeList[i+1])
else:
downRegionBoundary.append(down)
down = (nodeList[i], nodeList[i+1])

if up:
upRegionBoundary.append(up)

if down:
downRegionBoundary.append(down)

return upRegionBoundary,downRegionBoundary

#控制行走策略， 通过在走每一步前， 安放检查点， 改变机器人的 行走方向，达到控制的目的
class MoveCtl():
def __init__(self, robot):
self.robot = robot

# return 是否真正移动
def decideNextMove(self):
return True

def endControl(self):
self.robot.checkProgress()

class FindCornerCtl(MoveCtl):
def __init__(self,robot,stepDirection):
MoveCtl.__init__(self, robot)
self.robot.currentDirection = stepDirection
self.stepNum =0

def decideNextMove(self):
if(self.stepNum < ROBOT_SIZE):
self.stepNum = self.stepNum +1
return True
else:
self.robot.currentDirection = 'N'

ifBump = self.robot.ifBump()

if(ifBump):
self.robot.moveCtl = ZigzagMoveCtl(self.robot,'S')
return False

return True

class ZigzagMoveCtl(MoveCtl):
global ROBOT_SIZE
global GAP
def __init__(self, robot, sweepDirection, ifAlterSliceAtFirst = False, firstSliceDirection="W"):
MoveCtl.__init__(self, robot)
self.alterDistance = 0
self.alterMaxDistance = ROBOT_SIZE
self.sweepDirection = sweepDirection
self.lastSliceDirection = firstSliceDirection
self.currentSlice = {}
self.ifLastSlice = False
self.robot.stateText = "弓形清扫"

if (ifAlterSliceAtFirst):
self.robot.currentDirection = sweepDirection
self.__act = self.__processAlterSlice
else:
self.__act = self.__processSlice
self.robot.currentDirection = firstSliceDirection

def __ifInCornerAtStart(self):
range = self.robot.getRangeData()

if(range[getConverseDirection(self.robot.currentDirection)]> GAP):
return False
else:
return True

def decideNextMove(self):

return self.__act()

def __processSlice(self):
ifBump = self.robot.ifBump()
if(ifBump):
if(self.ifLastSlice):
self.robot.map.combineCurrentBoundary()
assert(self.robot.ifCurrentRegionFinished())
self.endControl()
return False

self.robot.currentDirection = getConverseDirection(self.robot.currentDirection)
self.lastSliceDirection = self.robot.currentDirection
self.__act = self.__processSlice

self.__handleSliceBegin()

else:
self.__handleSliceEnd()
self.__act = self.__processAlterSlice
range = self.__getRangeData()
self.alterMaxDistance = 3
else:
self.alterMaxDistance = ROBOT_SIZE
self.alterDistance = 0
self.lastSliceDirection = self.robot.currentDirection
self.robot.currentDirection = self.sweepDirection
return False
if(self.__ifLeftRightDoorFound()):

self.__buildVirtalWall()
self.__handleSliceEnd()
self.alterMaxDistance = ROBOT_SIZE
self.lastSliceDirection = self.robot.currentDirection
self.robot.currentDirection = getConverseDirection(self.lastSliceDirection)
self.__act = self.__processAlterSlice
return False

self.__handleSliceInProgress()
return True

def __buildVirtalWall(self):
point = self.currentSlice['doorPoint'][-1]['pos']
start = point
if(self.sweepDirection == "S"):
end = (start[0], start[1] + ROBOT_SIZE*10)
else:
end = (start[0], start[1] - ROBOT_SIZE*10)
self.robot.map.buildVirtalWall(start,end,self.robot.currentDirection)

def __ifLeftRightDoorFound(self):
if (not self.currentSlice):
return False
if (not self.currentSlice['doorPoint']):
return False

if len(self.currentSlice['doorPoint'])<2:
return False

if(calculateDistance(self.currentSlice['doorPoint'][0]['pos'],self.currentSlice['start'])<ROBOT_SIZE):
self.currentSlice['doorPoint'] =[]
return False

corridorWidth = abs(self.currentSlice['doorPoint'][1]['pos'][0] - self.currentSlice['doorPoint'][0]['pos'][0])

if (corridorWidth<ROBOT_SIZE*3) and (corridorWidth>ROBOT_SIZE):
return True

self.currentSlice['doorPoint'] =[]
return False

def __processAlterSlice(self):
if(self.robot.ifTouchBoundary()):
self.ifLastSlice = True
self.robot.currentDirection = getConverseDirection(self.lastSliceDirection)
self.__act = self.__processSlice

return False

ifBump = self.robot.ifBump()
if(ifBump):
if(self.alterDistance>0):
self.robot.currentDirection = getConverseDirection(self.lastSliceDirection)
self.__act = self.__processSlice

self.__handleSliceBegin()
else:
self.robot.currentDirection = getConverseDirection(self.lastSliceDirection)
self.backDistance = 0
self.__act = self.__processBackOneRobot
return False

if(self.alterDistance == self.alterMaxDistance):
self.robot.currentDirection = getConverseDirection(self.lastSliceDirection)
self.__act = self.__processSlice
if(self.__ifBackEmpty()):

else:

self.__handleSliceBegin()
return False
self.alterDistance +=1
return True

def __ifBackEmpty(self):

range = self.robot.getRangeData()
backSpace = range[self.lastSliceDirection]
if (backSpace > ROBOT_SIZE*3):
if(self.robot.map.ifCloseToVirtualWall(self.lastSliceDirection)):
return False
return True

return False

def __processBackOneRobot(self):
ifBump = self.robot.ifBump()
if ifBump:
print "very very interesting"
self.robot.currentDirection = self.sweepDirection
self.__act = self.__processAlterSlice
return False

if(self.backDistance == ROBOT_SIZE):
self.robot.currentDirection = self.sweepDirection
self.__act = self.__processAlterSlice
return False
self.backDistance +=1
return True

def __handleSliceInProgress(self):
if(not self.currentSlice):
return None
self.__checkCriticality()

def __handleSliceBegin(self):

self.currentSlice["start"] = self.robot.getCurrentPos()
self.currentSlice["end"] = (999,999)
self.currentSlice['direction'] = self.robot.currentDirection

self.currentSlice['criticality'] = []
self.currentSlice['doorPoint'] = []

self.__checkCriticality()

def __handleSliceEnd(self):
self.currentSlice['end'] = self.robot.getCurrentPos()

self.__checkCriticality()

self.__filterEnd()

#普通slice， 没有临界点
if(len(self.currentSlice['criticality']) ==2) and \
(self.currentSlice['criticality'][0]['rangeStatus'] == (False,False)):
return

self.robot.buildBoundary(self.currentSlice['criticality'])
if(self.robot.ifCurrentRegionFinished()):
self.endControl()

#避免球形，斜边，凹形slice边界造成短距离的障碍物误判
def __filterEnd(self):
if (len(self.currentSlice['criticality']) > 2):
startNode = self.currentSlice['criticality'][0]
secondNode = self.currentSlice['criticality'][1]
startDistance = calculateDistance(startNode['pos'], secondNode['pos'])
if(startNode['rangeStatus'] != (False,False) and secondNode['rangeStatus'] != (False,False)):
self.currentSlice['criticality'][0]['rangeStatus'] = (False,False)
if (startDistance < (ROBOT_SIZE)):
startNode['rangeStatus'] = secondNode['rangeStatus']
self.currentSlice['criticality'].remove(secondNode)

if (len(self.currentSlice['criticality']) > 2):
endNode = self.currentSlice['criticality'][-1]
secondEndNode = self.currentSlice['criticality'][-2]
thirdEndNode = self.currentSlice['criticality'][-3]
endDistance = calculateDistance(endNode['pos'], secondEndNode['pos'])
if(thirdEndNode['rangeStatus'] != (False,False) and secondEndNode['rangeStatus'] != (False,False)):
self.currentSlice['criticality'][-1]['rangeStatus'] = (False,False)
self.currentSlice['criticality'][-2]['rangeStatus'] = (False,False)
if (endDistance < (ROBOT_SIZE)):
endNode['rangeStatus'] = secondEndNode['rangeStatus']
self.currentSlice['criticality'].remove(secondEndNode)

if (len(self.currentSlice['criticality']) == 2):
if(self.currentSlice['criticality'][0]['rangeStatus'] != self.currentSlice['criticality'][1]['rangeStatus']):
self.currentSlice['criticality'][0]['rangeStatus'] = (False,False)
self.currentSlice['criticality'][1]['rangeStatus'] = (False,False)

def __getLastPos(self,currentPos):
currentDirection = self.robot.currentDirection
if(currentDirection == "E"):
return(currentPos[0]-1, currentPos[1])
if(currentDirection == "W"):
return(currentPos[0]+1, currentPos[1])
if(currentDirection == "N"):
return(currentPos[0], currentPos[1]+1)
if(currentDirection == "S"):
return(currentPos[0], currentPos[1]-1)

def __checkCriticality(self):
currentPosition = self.robot.getCurrentPos()

range = self.__getRangeData()

ifUpClose = (range['up'] < ROBOT_SIZE+2)
ifBelowClose = (range['down'] < ROBOT_SIZE+2)

if(ifUpClose and ifBelowClose):
if(range['up']>range['down']):
ifUpClose = False
else:
ifBelowClose = False
# ifForwardClose = (range['head'] < ROBOT_SIZE+GAP+1)
upDownSpace = range['up'] + range['down']
ifUpDownClose = (upDownSpace<ROBOT_SIZE*10)

if(ifUpDownClose) and (not self.currentSlice['doorPoint']):
self.currentSlice['doorPoint'].append({'pos':currentPosition, 'upDownCloseStatus':ifUpDownClose})

if (self.currentSlice['doorPoint']) and (ifUpDownClose != self.currentSlice['doorPoint'][-1]['upDownCloseStatus']):
self.currentSlice['doorPoint'].append({'pos':currentPosition, 'upDownCloseStatus':ifUpDownClose})

if(self.currentSlice["start"] == currentPosition) and (not self.currentSlice['criticality']):
self.currentSlice['criticality'].append({'pos':currentPosition, 'rangeStatus':(ifUpClose,ifBelowClose)})
return

if(self.currentSlice['end'] == currentPosition):
self.currentSlice['criticality'].append({'pos':currentPosition, 'rangeStatus':(ifUpClose,ifBelowClose)})
return

if (self.currentSlice['criticality'][-1]['rangeStatus'] != (ifUpClose,ifBelowClose)):
self.currentSlice['criticality'].append({'pos':currentPosition, \
'rangeStatus':(ifUpClose,ifBelowClose)})

return

# return range of slice direction, sweep direction and anti-sweep direction
def __getRangeData(self):
range = self.robot.getRangeData()

up = range['N']
down = range['S']

class NavMoveCtl(MoveCtl):
def __init__(self, robot, route):
MoveCtl.__init__(self, robot)
self.path = route['path']
self.targetRegion = route['targetRegion']
self.targetRoom = route['targetRoom']
self.pathIndex = 1

def decideNextMove(self):
if (self.pathIndex == len(self.path)):
if(self.targetRegion):
self.__startSweep()
elif(self.targetRoom):
self.__findCorner()
return False

currentPos = self.robot.getCurrentPos()
self.robot.currentDirection = self.__getMoveDirection(currentPos, self.path[self.pathIndex])

self.pathIndex+=1
return True

def __findCorner(self):
self.robot.setCurrentRoom(self.targetRoom)
self.robot.moveCtl = FindCornerCtl(self.robot, self.targetRoom['direction'])

def __startSweep(self):
self.robot.setCurrentRegion(self.targetRegion)

sweepDirection = "S"
targetEdge =None
firstSliceDirection = None
if self.targetRegion['down']:
sweepDirection ='N'
targetEdge = self.targetRegion['down']
else:
sweepDirection = 'S'
targetEdge = self.targetRegion['up']
currentPos = self.robot.getCurrentPos()

if currentPos == targetEdge[0]:
firstSliceDirection = "W"
else:
firstSliceDirection = "E"

self.robot.moveCtl = ZigzagMoveCtl(self.robot,sweepDirection, \
ifAlterSliceAtFirst = True, firstSliceDirection=firstSliceDirection)

def __getMoveDirection(self,start,end):
if start[0] < end[0]:
return 'E'

if start[0] > end[0]:
return 'W'
if start[1] < end[1]:
return 'S'

if start[1] > end[1]:
return 'N'

class Robot():
global DIRECTION_DELTA
global ROBOT_SIZE
global GAP
def __init__(self, env,map,statistic):
self.env = env
self.map = map
self.statistic = statistic
self.velocity = 0
self.state = "move"
self.map.setCurrentPos(int(ROBOT_SIZE/2) +GAP,int(ROBOT_SIZE/2) +GAP )
self.env.clearInitRobot(self.getCurrentPos())
self.appearance= env.create_rectangle(7,7,12,12, fill="red")
self.currentDirection = "E"
self.stateText = "弓形清扫"
self.moveCtl = ZigzagMoveCtl(self,"S")
self.move()

def checkProgress(self):
if not self.map.ifRoomFinished():
self.__navToNextRegion()
return

if not self.map.ifFinished():
self.__navToNextRoom()
return

self.stateText = "清扫结束"
self.stopMove()
return

def __navToNextRegion(self):
# route = {'path':path, 'targetRegion':edge}
route = self.map.getRouteToNextRegionBoundary()
self.stateText = "导航最近未清扫区域"
self.moveCtl = NavMoveCtl(self, route)

def __navToNextRoom(self):
# route = {'path':path, 'targetRegion':edge}
route = self.map.getRouteToNextRoom()
self.stateText = "导航最近房间"
self.moveCtl = NavMoveCtl(self, route)

def setCurrentRegion(self,region):
self.map.setCurrentRegion(region)

def setCurrentRoom(self,room):
self.map.setCurrentRoom(room)

def stopMove(self):
self.state  = "stop"

def resumeMove(self):
self.state  = "move"

def buildBoundary(self, criticality):
length = len(criticality)
nodeList =[]

for i in range(length):
if(i==0 or i==(length-1)):
if criticality[i]['rangeStatus'] == (False,False):
else:
else:

for i in range(length-1):
if criticality[i]['rangeStatus'] == (False,False):
elif criticality[i]['rangeStatus'] == (True,False):
else:

if(self.currentDirection == "W"):
nodeList.reverse()

self.map.updateRegion(nodeList)

def ifCurrentRegionFinished(self):
currentReg = self.map.currentReg
return (currentReg['up'] and currentReg['down'])

#速度分解
def move(self):
if(self.velocity == 0 ) or (self.state != "move"):
self.env.after(1, self.move)
return

for i in range(self.velocity):
if(self.moveCtl.decideNextMove()):
self.__moveOnePixel()

self.env.after(1, self.move)

def getCurrentPos(self):
return (self.map.currentX, self.map.currentY)

def getRangeData(self):
return self.env.getRangeData(self.getCurrentPos())

def ifBump(self,direction='default'):
if (direction == 'default'):
direction = self.currentDirection
return self.env.ifBump(self.getCurrentPos(), direction) or self.map.ifBump(direction)

def ifTouchBoundary(self):
return self.map.ifTouchBoundary(self.currentDirection)

def setVelocity(self, velocity):
self.velocity = velocity

statistic = self.env.getStatistic()
self.statistic.coverageRate.set(statistic['coverageRate'])
self.statistic.repeatedRate.set(statistic['repeatedRate'])

pos = self.getCurrentPos()
self.statistic.robotPos.set("(%d,%d)" %(pos[0],pos[1]))

self.statistic.robotState.set(self.stateText)

range = self.getRangeData()
if(self.currentDirection == "E"):
self.statistic.left.set(range['N'])
self.statistic.right.set(range['S'])

if(self.currentDirection == "W"):
self.statistic.left.set(range['S'])
self.statistic.right.set(range['N'])

if(self.currentDirection == "N"):
self.statistic.left.set(range['W'])
self.statistic.right.set(range['E'])

if(self.currentDirection == "S"):
self.statistic.left.set(range['E'])
self.statistic.right.set(range['W'])

def __moveOnePixel(self):
self.env.move(self.appearance, self.getCurrentPos(), self.currentDirection)
self.map.moveUpdate(self.currentDirection)

class Env(Canvas):
global DIRECTION_DELTA
global ROBOT_SIZE
global GAP
WIDTH = 500
HEIGHT = 296

def __init__(self, parent, iniBackground):
Canvas.__init__(self,parent,bd=3,relief=RIDGE,width=Env.WIDTH, height=Env.HEIGHT)
self.im = PhotoImage(file= iniBackground)
self.create_image(ROBOT_SIZE,ROBOT_SIZE,anchor=NW, image= self.im)
self.pack()
self.totalArea = 0
self.clearedArea = ROBOT_SIZE*ROBOT_SIZE
self.repeatedArea = 0
self.__calFreeArea()

def clearInitRobot(self, robotPosition):
edgeStartX = robotPosition[0] - int(ROBOT_SIZE/2)
edgeEndX = robotPosition[0] + int(ROBOT_SIZE/2)
edgeStartY = robotPosition[1] - int(ROBOT_SIZE/2)
edgeEndY = robotPosition[1] + int(ROBOT_SIZE/2)

for i in range(edgeStartX, edgeEndX+1):
for j in range(edgeStartY, edgeEndY+1):
self.im.put(COLOR_COVERED,(i,j))
self.clearedArea+=1

def move(self,item,robotPosition, robotDirection):
Canvas.move(self,item,DIRECTION_DELTA[robotDirection][0],DIRECTION_DELTA[robotDirection][1])

(edgeStartX, edgeEndX, edgeStartY, edgeEndY) = get1StepCleanedRegion(robotPosition,robotDirection)

for i in range(edgeStartX, edgeEndX+1):
for j in range(edgeStartY, edgeEndY+1):
assert(self.im.get(i,j)!="0 0 0")
if(self.im.get(i,j)=="255 255 255"):
self.im.put(COLOR_COVERED,(i,j))
self.clearedArea+=1
else:
self.repeatedArea+=1

def __calFreeArea(self):
for i in range(Env.WIDTH):
for j in range(Env.HEIGHT):
r=self.im.get(i,j)
if(r == "255 255 255"):
self.totalArea+=1

# Range detected in (3 ~ 50). for convinience, the distance is calculated from center of robot
def getRangeData(self, robotPosition):
MAX_RANGE = 50
MIN_RANGE = int(ROBOT_SIZE/2)
east = 999
west = 999
north = 999
south = 999

for i in range(MIN_RANGE,MAX_RANGE +1):
if((east==999)and ((robotPosition[0] + i) == Env.WIDTH-1)):
east = i -1

if((east==999) and (self.im.get(robotPosition[0]+i,robotPosition[1])=="0 0 0")):
east = i -1

if((west ==999) and ((robotPosition[0] - i) == 0)):
west = i -1

if((west ==999) and (self.im.get(robotPosition[0]-i,robotPosition[1])=="0 0 0")):
west = i -1

if((south==999) and ((robotPosition[1] + i) == Env.HEIGHT-1)):
south = i -1

if((south==999) and (self.im.get(robotPosition[0],robotPosition[1]+i)=="0 0 0")):
south = i-1

if((north==999) and ((robotPosition[1] - i) == 0)):
north = i-1

if((north==999) and (self.im.get(robotPosition[0],robotPosition[1]-i)=="0 0 0")):
north = i-1

if((east != 999) and (west != 999) and (south != 999) and (north != 999)):
break;

return {'W':west, 'E':east, 'N':north, 'S':south}

def ifBump(self,robotPosition, robotDirection):
(edgeStartX, edgeEndX, edgeStartY, edgeEndY) = get1StepCleanedRegion(robotPosition,robotDirection)

if ((edgeStartX <1) or (edgeEndX> Env.WIDTH-2) or (edgeStartY <1) or (edgeEndY>Env.HEIGHT-2)):
return True

for i in range(edgeStartX, edgeEndX+1):
for j in range(edgeStartY, edgeEndY+1):
if(self.im.get(i,j)=="0 0 0"):
return True
return False

def getStatistic(self):
coverageRate = "%.2f%%" %(self.clearedArea*100/self.totalArea)
repeatedRate = "%.2f%%" %(self.repeatedArea*100/self.clearedArea)

return {'coverageRate':coverageRate, 'repeatedRate':repeatedRate}

class Statistic():
def __init__(self):
self.coverageRate = StringVar()
self.repeatedRate = StringVar()
self.robotPos = StringVar()
self.left = StringVar()
self.right = StringVar()
self.robotState = StringVar()

class App():
def __init__(self, root):
self.statistic = Statistic()
self.master.pack()
self.showSweepView()
self.showGraphView()
self.showCommand()
self.showDashBoard()
self.robot=Robot(self.env,self.map, self.statistic)
self.run()

def __restart(self):
self.master.destroy()
self.master.pack()
self.showSweepView()
self.showGraphView()
self.showCommand()
self.showDashBoard()
self.robot=Robot(self.env,self.map, self.statistic)
self.run()

def showSweepView(self):
sweepFrame.grid(row=0, column = 0)
title = Label(sweepFrame,text="清扫视图",font=("Times", "16", "bold italic"))
title.pack()
self.env = Env(sweepFrame, "./multiRoom.pgm")

def showGraphView(self):
graphFrame.grid(row=0, column = 1)
title = Label(graphFrame,text="地图",font=("Times", "16", "bold italic"))
title.pack()

self.map=Map(graphFrame,"./room_grey.pgm")

def showCommand(self):
scaleFrame.grid(row=1, column = 1,sticky=W+N)
scaleLabel = Label(scaleFrame, text="速度:",font=("Times", "16", "bold italic"))
scaleLabel.grid(row=0, column=0, sticky=E+S)
self.scale = Scale(scaleFrame, orient=HORIZONTAL, from_= 0, to=20, length=400)
self.scale.grid(row=0, column=1)
self.scale.bind("<ButtonRelease-1>", self.updateRobotVelocity)
placeholder = Label(scaleFrame, text="  ", width=10, anchor=E)
placeholder.grid(row = 1, column =2, columnspan=2, sticky= W)

pauseButtion = Button(scaleFrame,text = '暂停',command = self.__pause)
pauseButtion.grid(row=2,column=0,sticky=W+S)
restartButtion = Button(scaleFrame,text = '初始化',command = self.__restart)
restartButtion.grid(row=2,column=1,sticky=W+S)

def __pause(self):
self.scale.set(0)
self.robot.setVelocity(self.scale.get())

def showDashBoard(self):
frame.grid(row=1, column = 0)
dashFrame = LabelFrame(frame, text="统计数据",
dashFrame.pack()

label = Label(dashFrame, text="覆盖率:", width=10, anchor=E)
label.grid(row = 0, column =0, sticky= W)
value = Label(dashFrame, textvariable=self.statistic.coverageRate, width=10,anchor=W)
value.grid(row =0, column=1, sticky=W)

label = Label(dashFrame, text="重复率:", width=10, anchor=E)
label.grid(row = 0, column =2, sticky= W)
value = Label(dashFrame, textvariable=self.statistic.repeatedRate, width=10, anchor=W)
value.grid(row =0, column=3, sticky=W)

placeholder = Label(dashFrame, text="  ", width=10, anchor=E)
placeholder.grid(row = 1, column =2, columnspan=4, sticky= W)

label = Label(dashFrame, text="位置:", width=10, anchor=E)
label.grid(row = 2, column =0, sticky= W)
value = Label(dashFrame, textvariable=self.statistic.robotPos, width=10,anchor=W)
value.grid(row =2, column=1, sticky=W)

label = Label(dashFrame, text="前距离:", width=10, anchor=E)
label.grid(row = 2, column =2, sticky= W)
value.grid(row =2, column=3, sticky=W)

label = Label(dashFrame, text="左距离:", width=10, anchor=E)
label.grid(row = 3, column =0, sticky= W)
value = Label(dashFrame, textvariable=self.statistic.left, width=10, anchor=W)
value.grid(row =3, column=1, sticky=W)

label = Label(dashFrame, text="右距离:", width=10, anchor=E)
label.grid(row = 3, column =2, sticky= W)
value = Label(dashFrame, textvariable=self.statistic.right, width=10, anchor=W)
value.grid(row =3, column=3, sticky=W)

label = Label(dashFrame, text="状态:", width=10, anchor=E)
label.grid(row = 4, column =0, sticky= W)
value = Label(dashFrame, textvariable=self.statistic.robotState, width=20, anchor=W, foreground="blue",font=("Times", "12", "bold italic"))
value.grid(row =4, column=1, sticky=W)

def updateRobotVelocity(self,event):
self.robot.setVelocity(self.scale.get())

def run(self):

root = Tk()
root.title("仿真在线路径规划")
app = App(root)

root.mainloop()