def LoadMap(self):
self.shape = Shape()
self.shapes = []
self.loadState = 0
self.lawn = 'lawn'
self.obstacle = 'obstacle'
self.mower = 'mower'
self.station = 'station'
f = open(
"/home/pi/catkin_ws/src/intelli_mower/src/IntelliMowerAlgorithmRoS/src/map_one",
'r')
for line in f:
if line.strip() == str(self.lawn):
self.loadState = 3
elif line.strip() == str(self.obstacle):
self.loadState = 4
newShape = Shape()
self.shapes.append(newShape)
elif line.strip() == str(self.mower):
self.loadState = 1
elif line.strip() == str(self.station):
self.loadState = 2
else:
if self.loadState == 1:
cords = line.split()
self.mowerP = Point(float(cords[0]), float(cords[1]))
self.mowerP.borderId = int(cords[2])
elif self.loadState == 2:
cords = line.split()
self.stationP = Point(float(cords[0]), float(cords[1]))
self.stationP.borderId = int(cords[2])
# load outer border
elif self.loadState == 3:
cords = line.split()
p = Point(float(cords[0]), float(cords[1]))
p.borderId = int(cords[2])
self.shape.AddBorderPoint(p)
# load inner border
elif self.loadState == 4:
cords = line.split()
p = Point(float(cords[0]), float(cords[1]))
p.borderId = int(cords[2])
self.shapes[-1].AddBorderPoint(p)
f.close()
def SetState(self, state):
self.calcDone = 0
self.state = state
# print( "IntelliMowerState:", state )
# add new obstacle everytime state is set to 2
if self.state == 2:
newObstacle = Shape()
self.lawn.innerBorders.append(newObstacle)
self.lawn.innerBorders[-1].id = len(self.lawn.innerBorders)
def Reset(self):
self.sweepAngle = 0
self.calcDone = 0
self.lawnExtremePoints = []
self.boundingBox = []
self.sweepLines = []
self.collisionPoints = []
self.collisionPairs = []
self.innerBorders = []
self.outerBorder = Shape()
self.mowerPoint = None
self.stationPoint = None
def Reset(self):
self.isInitialized = 0
self.currentPos = Point(0, 0)
self.currentStationPos = None
self.distToNextPos = 0
self.pathingComplete = 0
self.pathState = 0
self.pathList = []
self.undetectedPaths = []
self.innerBorders = []
self.outerBorder = Shape()
self.collisionPairs = []
self.decreasingSide = []
self.increasingSide = []
def __init__(self, sweepOffset):
self.sweepAngle = 0
self.sweepOffset = sweepOffset
self.calcDone = 0
self.collisionPointAdjustX = 5
self.minAllowedPairDist = 15
self.rotation = 0.0
self.extremePoints = []
self.boundingBox = []
self.centerPoint = Point(0, 0)
self.sweepLines = []
self.collisionGroups = []
self.collisionPairs = []
self.innerBorders = []
self.outerBorder = Shape()
self.mowerPoint = None
self.stationPoint = None
self.mLoader = MapLoader()
def __init__(self):
self.bPThresh = 1 # border point threshhold
self.iPThresh = 0.5
self.fDThresh = 400 # free destination distance threshold
self.distToPairThresh = 0 # distance to pair threshold
self.isInitialized = 0
self.pathingComplete = 0
self.currentPos = Point(0, 0)
self.currentStationPos = None
self.distToNextPos = 0
self.pathState = 0
self.copyPairs = []
self.undetectedPaths = []
self.numPairPoints = 0
self.innerBorders = []
self.outerBorder = Shape()
self.collisionPairs = []
self.pathList = []
self.decreasingSide = []
self.increasingSide = []
class MapLoader():
shape = Shape()
shapes = []
loadState = 0
def __init__(self):
self.mowerP = None
self.stationP = None
self.shape = Shape()
self.shapes = []
self.loadState = 0
#
def LoadMap(self):
self.shape = Shape()
self.shapes = []
self.loadState = 0
self.lawn = 'lawn'
self.obstacle = 'obstacle'
self.mower = 'mower'
self.station = 'station'
f = open("map_one", 'r')
for line in f:
if line.strip() == str(self.lawn):
self.loadState = 3
elif line.strip() == str(self.obstacle):
self.loadState = 4
newShape = Shape()
self.shapes.append(newShape)
elif line.strip() == str(self.mower):
self.loadState = 1
elif line.strip() == str(self.station):
self.loadState = 2
else:
if self.loadState == 1:
cords = line.split()
self.mowerP = Point(float(cords[0]), float(cords[1]))
self.mowerP.borderId = int(cords[2])
elif self.loadState == 2:
cords = line.split()
self.stationP = Point(float(cords[0]), float(cords[1]))
self.stationP.borderId = int(cords[2])
# load outer border
elif self.loadState == 3:
cords = line.split()
p = Point(float(cords[0]), float(cords[1]))
p.borderId = int(cords[2])
self.shape.AddBorderPoint(p)
# load inner border
elif self.loadState == 4:
cords = line.split()
p = Point(float(cords[0]), float(cords[1]))
p.borderId = int(cords[2])
self.shapes[-1].AddBorderPoint(p)
f.close()
#
def SaveMap(self, lawn, obstacles, mower=None, station=None):
s = lawn
obstacles = obstacles
f = open("map_one", 'w')
if mower:
f.write("%s\n" % 'mower')
f.write("%s" % mower.x)
f.write(' ')
f.write("%s" % mower.y)
f.write(' ')
f.write("%s\n" % 0)
if station:
f.write("%s\n" % 'station')
f.write("%s" % station.x)
f.write(' ')
f.write("%s" % station.y)
f.write(' ')
f.write("%s\n" % 0)
f.write("%s\n" % 'lawn')
for point in s.pointList:
f.write("%s" % point.x)
f.write(' ')
f.write("%s" % point.y)
f.write(' ')
f.write("%s\n" % point.borderId)
for shape in obstacles:
f.write("%s\n" % 'obstacle')
for point in shape.pointList:
f.write("%s" % point.x)
f.write(' ')
f.write("%s" % point.y)
f.write(' ')
f.write("%s\n" % point.borderId)
f.close()
def __init__(self):
self.mowerP = None
self.stationP = None
self.shape = Shape()
self.shapes = []
self.loadState = 0
class Pathfinder:
isInitialized = 0
bPThresh = 0
iPThresh = 0
distToPairThresh = 0
currentPos = Point(0, 0)
distToNextPos = 0
pathState = 0
copyPairs = []
undetectedPaths = []
innerBorders = []
outerBorder = Shape()
collisionPairs = []
pathList = []
borderPaths = [] #used only by thre UI to show the traveled borders
def __init__(self):
self.bPThresh = 1 # border point threshhold
self.iPThresh = 0.5
self.fDThresh = 400 # free destination distance threshold
self.distToPairThresh = 0 # distance to pair threshold
self.isInitialized = 0
self.pathingComplete = 0
self.currentPos = Point(0, 0)
self.currentStationPos = None
self.distToNextPos = 0
self.pathState = 0
self.copyPairs = []
self.undetectedPaths = []
self.numPairPoints = 0
self.innerBorders = []
self.outerBorder = Shape()
self.collisionPairs = []
self.pathList = []
self.borderPaths = []
self.decreasingSide = []
self.increasingSide = []
#
def Init( self, outerBorder, innerBorders, collisionPairs ):
self.outerBorder = copy.deepcopy(outerBorder)
self.innerBorders = copy.deepcopy(innerBorders)
self.undetectedPaths = copy.deepcopy(collisionPairs)
self.numPairPoints = len(self.undetectedPaths)
self.isInitialized = 1
# NOTE! is only called from IntelliMower object to set an initial position!
def UpdatePos(self, x, y):
p = Point(x, y)
self.currentPos = p
self.pathList = []
self.pathList.append(p)
def UpdateStationPos(self, x, y):
p = Point(x, y)
p.id = "ChargingStation"
self.currentStationPos = p
# find closest point in a given list to a given point
def ClosestPoint(self, pList, p):
i = 0
closestIndex = 0
currDist = 0
#select a current distance not equal to zero distance
while i < len(pList):
currDist = p.DistToPoint(pList[i].x, pList[i].y)
if pList[i].x != p.x or pList[i].y != p.y:
closestIndex = i
break
i += 1
i = closestIndex + 1
while i < len( pList ):
nextDist = p.DistToPoint(pList[i].x, pList[i].y)
if currDist > nextDist:
currDist = nextDist
closestIndex = i
i += 1
return pList[closestIndex]
#
def ClosestPointFromAllLists(self, pos ):
closestPoints = []
if len(self.undetectedPaths):
closestPoints.append( self.ClosestPoint(self.undetectedPaths, pos) )
if len(self.decreasingSide):
closestPoints.append( self.ClosestPoint(self.decreasingSide, pos) )
if len(self.increasingSide):
closestPoints.append( self.ClosestPoint(self.increasingSide, pos) )
i = 1
clsIndex = 0
dist = pos.DistToPoint( closestPoints[0].x, closestPoints[0].y )
while i < len(closestPoints):
newDist = pos.DistToPoint( closestPoints[i].x, closestPoints[i].y )
if dist >= newDist:
dist = newDist
clsIndex = i
i += 1
return closestPoints[clsIndex]
#
def PairPoint(self, points, point):
for p in points:
if point.id == p.id:
return p
print 'ERROR! in function PairPoint: No pair point found!'
#
def ClosestIntersectionPointToDst( self, p1, p2 ):
line = Line(p1, p2)
intersectionPointList = []
# check intersecting outer borders on line to closest pair point
for l in self.outerBorder.lineList:
p = line.GetIntersectionPoint(l)
if p:
# print 'Found OB intersection!'
intersectionPointList.append(p)
# check intersecting inner borders on line to closest pair point
for shape in self.innerBorders:
for l in shape.lineList:
p = line.GetIntersectionPoint(l)
if p:
# print 'Found IB intersection!'
# print p.borderId
intersectionPointList.append(p)
# if any intersections found return the closest
if len(intersectionPointList) > 0:
# print 'Returning Closest Intersection point!'
return self.ClosestPoint(intersectionPointList, p2)
else:
return None
#
def ClosestIntersectionPointOnBorder( self, p1, p2, border ):
line = Line(p1, p2)
intersectionPointList = []
# check intersecting outer borders on line to closest pair point
if border == 0:
for l in self.outerBorder.lineList:
p = line.GetIntersectionPoint(l)
if p:
# print 'Found OB intersection!'
intersectionPointList.append(p)
elif border > 0:
# check intersecting inner borders on line to closest pair point
shape = self.innerBorders[border - 1]
for l in shape.lineList:
p = line.GetIntersectionPoint(l)
if p:
#print 'Found IB intersection!'
#print p.intersectingLine.p1.borderId
intersectionPointList.append(p)
# if any intersections found return the closest
if len(intersectionPointList) > 0:
# print 'Returning Closest Intersection point!'
return self.ClosestPoint(intersectionPointList, p2)
else:
return None
#
def ClosestIntersectionPoint( self, p1, p2 ):
line = Line(p2, p1)
intersectionPointList = []
# check intersecting outer borders on line to closest pair point
for l in self.outerBorder.lineList:
p = line.GetIntersectionPoint(l)
if p:
# print 'Found OB intersection!'
intersectionPointList.append( p )
# check intersecting inner borders on line to closest pair point
for shape in self.innerBorders:
for l in shape.lineList:
p = line.GetIntersectionPoint( l )
if p:
#print 'Found IB intersection!'
#print p.intersectingLine.p1.borderId
intersectionPointList.append( p )
# if any intersections found return the closest
if len(intersectionPointList) > 0:
# print 'Returning Closest Intersection point!'
return self.ClosestPoint( intersectionPointList, p1 )
else:
return None
# if we are standing on a border choose a intersection point not on this border
def AltClosestIntersectionPoint( self, p1, p2 ):
border = p1.borderId
print "AltClosestIntersectionPoint"
#print ("BorderId:", border)
line = Line( p2, p1 )
intersectionPointList = []
# check intersecting outer borders on line to closest pair point
for l in self.outerBorder.lineList:
p = line.GetIntersectionPoint( l )
if p and p.intersectingLine.p1.borderId != border:
# print 'Found OB intersection!'
intersectionPointList.append( p )
# check intersecting inner borders on line to closest pair point
for shape in self.innerBorders:
for l in shape.lineList:
p = line.GetIntersectionPoint( l )
if p and p.intersectingLine.p1.borderId != border:
#print 'Found IB intersection!'
#print ("intersection has border Id:", p.intersectingLine.p1.borderId )
intersectionPointList.append( p )
# if any intersections found return the closest
if len(intersectionPointList) > 0:
# print 'Returning Closest Intersection point!'
return self.ClosestPoint( intersectionPointList, p1 )
else:
return None
# Note this is bugged because of mirroring probably
# Fixme! this function should be rewritten to fix the mirroring problem
def ClosestPointWithLoS( self, points, currentPos ):
if len(points) < 1:
# print "List is empty"
return None
pointsCopy = copy.deepcopy(points)
while len(pointsCopy) > 0:
closestPos = self.ClosestPoint(pointsCopy, currentPos)
intersectionP = self.ClosestIntersectionPoint( currentPos, closestPos )
if intersectionP == None:
return closestPos
# if intersection point and closest point on line is the same we do have free LoS
elif intersectionP.x == closestPos.x and intersectionP.y == closestPos.y:
return closestPos
elif intersectionP.x == currentPos.x and intersectionP.y == currentPos.y:
interP = self.ClosestIntersectionPointToDst( currentPos, closestPos )
if interP.x == currentPos.x and interP.y == currentPos.y:
print 'Warning in ClosestPointWithLoS: standing on intersection point!'
return closestPos
pointsCopy.pop( pointsCopy.index(closestPos) )
print "Error in ClosestPointWithLoS: No closest point with LoS found"
return None
#
def DeletePointInList(self, points, p ):
i = 0
while i < len(points):
if points[i].x == p.x and points[i].y == p.y:
points.pop(i)
i -= 1
i += 1
#
def CalculateVisiblePaths( self, points, incSide, decSide, currPos, num ):
# we are not standing on a pair point try to go to closest visible
if currPos.id == 0:
print 'Warning CalculateVisiblePaths: Not standing on pair point!'
return
# split up the mow lines in two lists depending
# on which side of the mower they are on
i = 0
while i < num and len(points) > 0:
closestPos = self.ClosestPointWithLoS(points, currPos)
if closestPos == None:
i += 1
continue
elif closestPos.id < currPos.id:
decSide.append(closestPos)
self.DeletePointInList(points, closestPos)
pairPoint = self.PairPoint(points, closestPos)
decSide.append(pairPoint)
self.DeletePointInList(points, pairPoint)
elif closestPos.id > currPos.id:
incSide.append(closestPos)
self.DeletePointInList(points, closestPos)
pairPoint = self.PairPoint(points, closestPos)
incSide.append(pairPoint)
self.DeletePointInList(points, pairPoint)
i += 1
#
def PickActiveSide( self, increasingSide, decreasingSide, currentPos ):
# pick the closest mow line for which there exist the smallest
# number of lines to mow on that side of the mower
if len(increasingSide) == 0 and len(decreasingSide) == 0:
print "Error! Can't pick active side both sides are empty"
return None
elif len(increasingSide) == 0:
return decreasingSide
elif len(decreasingSide) == 0:
return increasingSide
else:
incClosest = self.ClosestPointWithLoS(increasingSide,
currentPos)
decClosest = self.ClosestPointWithLoS(decreasingSide,
currentPos)
if incClosest:
incDist = currentPos.DistToPoint(incClosest.x,
incClosest.y)
else:
return decreasingSide
if decClosest:
decDist = currentPos.DistToPoint(decClosest.x,
decClosest.y)
else:
return increasingSide
if incDist < decDist:
return increasingSide
else:
return decreasingSide
#
def GetBorderPath( self, borderList, startP, endP):
if len(borderList) <= 0:
print 'Error borderList is empty!'
return
if borderList[0].borderId != startP.intersectingLine.p1.borderId \
and borderList[0].borderId != endP.intersectingLine.p1.borderId:
print 'Error border id mismatch!'
return
if startP.intersectingLine.p1.borderId != endP.intersectingLine.p1.borderId:
print 'Error border Id startP and endP not the same!'
return
# start point vertices
bp1 = startP.intersectingLine.p1
bp2 = startP.intersectingLine.p2
# end point vertices
ebp1 = endP.intersectingLine.p1
ebp2 = endP.intersectingLine.p2
# positive traverse
pTL1 = []
pTL2 = []
shortestPTL = []
i = borderList.index(bp1)
j = borderList.index(bp2)
count = 0
print ("bp1 index:", i )
print ("bp2 index:", j )
while True:
# print "hej"
pTL1.append(borderList[i])
pTL2.append(borderList[j])
if borderList[i].x == ebp1.x and borderList[i].y == ebp1.y \
or borderList[i].x == ebp2.x and borderList[i].y == ebp2.y:
shortestPTL = pTL1
break
if borderList[j].x == ebp1.x and borderList[j].y == ebp1.y \
or borderList[j].x == ebp2.x and borderList[j].y == ebp2.y:
shortestPTL = pTL2
break
i += 1
j += 1
count += 1
if i == len(borderList): # if we overflow set the index to the beginning of list
i = 0
if j == len(borderList):
j = 0
# if we count over one length of border without founding a free LoS we have to jump to next obstacle
if count == len(borderList):
break
# negative traverse
nTL1 = []
nTL2 = []
shortestNTL = []
j = borderList.index(bp1)
i = borderList.index(bp2)
count = 0
while True:
nTL1.append(borderList[i])
nTL2.append(borderList[j])
if borderList[i].x == ebp1.x and borderList[i].y == ebp1.y \
or borderList[i].x == ebp2.x and borderList[i].y \
== ebp2.y:
shortestNTL = nTL1
break
if borderList[i].x == ebp1.x and borderList[i].y == ebp1.y \
or borderList[i].x == ebp2.x and borderList[i].y \
== ebp2.y:
shortestNTL = nTL2
break
i -= 1
j -= 1
count += 1
if i == 0: # if we overflow set the index to the end of list
i = len(borderList) - 1
if j == 0:
j = len(borderList) - 1
#if we count over one length of border without founding a free LoS we have to jump to next obstacle
if count == len(borderList):
break
if len(shortestNTL) < 1 and len(shortestPTL) < 1:
print 'Error border lists are faulty'
return None
elif len(shortestNTL) < 1:
return shortestPTL
elif len(shortestPTL) < 1:
return shortestNTL
elif len(shortestPTL) > len(shortestNTL):
return shortestNTL
elif len(shortestPTL) < len(shortestNTL):
return shortestPTL
elif len(shortestPTL) == len(shortestNTL):
print 'Both border paths are the same lenght!'
print ('shortestPTL:', len(shortestPTL))
print ('shortestNTL:', len(shortestNTL))
return shortestPTL
#
def MowBorderPath( self, borderList, startP ):
if len(borderList) <= 0:
print 'Error borderList is empty!'
return
if borderList[0].borderId != startP.borderId:
print 'Error border id mismatch!'
return
pList = []
i = 0
count = -1
while i < len( borderList ):
if borderList[i].x == startP.x and borderList[i].y == startP.y:
print ( "Found start P index i: ", i )
break
i += 1
while True:
# print "hej"
pList.append(borderList[i])
if count > 0 and borderList[i].x == startP.x and borderList[i].y == startP.y:
break
i += 1
count += 1
if i == len( borderList ): # if we overflow set the index to the beginning of list
i = 0
# if we count over one length of border without founding a free LoS we have to jump to next obstacle
if count == len( borderList ):
break
if len( pList ) < 1:
print 'Error border lists are faulty'
return None
return pList
# follows border until we get free LoS to a pair
# point or to the next border intersection point
def TraverseBorder( self, startP, endP ):
# print ("startP:", startP.x, startP.y )
# print ("endP:", endP.x, endP.y )
# print ("startP border id:", startP.intersectingLine.p1.borderId )
# print ("endP border id:", endP.intersectingLine.p1.borderId )
traversedPath = []
if startP.intersectingLine.p1.borderId == 0:
print 'Following lawn border!'
traversedPath = self.GetBorderPath( self.outerBorder.pointList, startP, endP )
i = 0
while i < len(traversedPath):
self.borderPaths.append(traversedPath)
i+=1
elif startP.intersectingLine.p1.borderId > 0:
print 'Following obstacle border!'
traversedPath = self.GetBorderPath( self.innerBorders[ startP.intersectingLine.p1.borderId - 1 ].pointList, startP, endP )
completedPath = []
completedPath.append(startP) # we add start intersection point to pathList first before inserting the planned border paths
i = 0
# we check if we see any free LoS points on the way to end point
while i < len( traversedPath ):
# print "Num undetected points left:", len( self.undetectedPaths )
# print "Num points in ActiveSide:", len( pairPoints )
p = traversedPath[i]
completedPath.append(p)
p.x += 4
undetectedPos = self.ClosestPointWithLoS( self.undetectedPaths, p )
incPos = self.ClosestPointWithLoS( self.increasingSide, p )
decPos = self.ClosestPointWithLoS( self.decreasingSide, p )
if undetectedPos:
print 'Found closer undetected point!'
completedPath.append(undetectedPos)
self.DeletePointInList(self.undetectedPaths,
undetectedPos)
break
elif incPos:
print 'Found closer LoS point!'
completedPath.append(incPos)
self.DeletePointInList(self.increasingSide, incPos)
break
elif decPos:
print 'Found closer LoS point!'
completedPath.append(decPos)
self.DeletePointInList(self.decreasingSide, decPos)
break
p.x -= 4 * 2
undetectedPos = self.ClosestPointWithLoS(self.undetectedPaths, p)
incPos = self.ClosestPointWithLoS(self.increasingSide, p)
decPos = self.ClosestPointWithLoS(self.decreasingSide, p)
if undetectedPos:
print 'Found closer undetected point!'
completedPath.append(undetectedPos)
self.DeletePointInList(self.undetectedPaths,
undetectedPos)
break
elif incPos:
print 'Found closer LoS point!'
completedPath.append(incPos)
self.DeletePointInList(self.increasingSide, incPos)
break
elif decPos:
print 'Found closer LoS point!'
completedPath.append(decPos)
self.DeletePointInList(self.decreasingSide, decPos)
break
p.x += 4
i += 1
i = 0
while i < len(completedPath):
self.GoTo(completedPath[i])
i += 1
#
def GoTo( self, p ):
self.currentPos = p
if p.id: # if route pair id update pair id and turn on path state 1
print ("p id: " , p.id)
self.pathState = 1
print "Path State set to 1"
self.pathList.append( p )
#
def GoToNextPair(self):
# find next route pair and set it to next position
for point in self.increasingSide:
if self.currentPos.id == point.id:
# insert planned next position
self.currentPos = point
self.pathList.append(point)
# remove traveled position
self.increasingSide.pop(self.increasingSide.index(point))
return
for point in self.decreasingSide:
if self.currentPos.id == point.id:
# insert planned next position
self.currentPos = point
self.pathList.append(point)
# remove traveled position
self.decreasingSide.pop(self.decreasingSide.index(point))
return
for point in self.undetectedPaths:
if self.currentPos.id == point.id:
# insert planned next position
self.currentPos = point
self.pathList.append(point)
# remove traveled position
self.undetectedPaths.pop(self.undetectedPaths.index(point))
return
print 'ERROR! No pair point found!'
#
def GoToClosestPairPoint( self ):
print 'Moving to closest pair point!'
pos = self.ClosestPointWithLoS( self.undetectedPaths, self.currentPos )
if pos == None:
print "Can't move to closest pair point none visible! FIXME! Check if we are standing on a border point!"
if len( self.undetectedPaths ) or len( self.increasingSide ) or len( self.decreasingSide ):
# we get closest pair point
closestP = self.ClosestPointFromAllLists( self.currentPos )
# we set the closest intersection point
startP = self.AltClosestIntersectionPoint( self.currentPos, closestP )
#we set the last intersection point on the obstacle border we wish to traverse
endP = self.ClosestIntersectionPointOnBorder( startP, closestP, startP.intersectingLine.p1.borderId )
# start traverse
self.TraverseBorder( startP, endP )
#self.pathState = 0
# check if we are standing on a border line
return
self.GoTo(pos)
self.DeletePointInList( self.undetectedPaths, pos)
self.pathState = 1
#
def GoToNextPath( self, increasingSide, decreasingSide, currentPos ):
activeSide = self.PickActiveSide( increasingSide, decreasingSide, currentPos )
# we are not standing on a pair point try to go to closest visible
if activeSide == None:
if len(self.undetectedPaths) != 0:
print 'no visible pairs available and inc and dec lists are empty. Following border to closest undected pair point! FIXME'
# try checking for close visible path's
print 'Looking for closer visible paths'
self.CalculateVisiblePaths(self.undetectedPaths,
self.increasingSide, self.decreasingSide,
self.currentPos, 8)
activeSide = self.PickActiveSide(increasingSide,
decreasingSide, currentPos)
print 'Updated points in decreasing side memory: ', \
len(self.decreasingSide)
print 'Updated points in increasing side memory: ', \
len(self.increasingSide)
print 'Num undetected points left:', \
len(self.undetectedPaths)
pos = None
if activeSide:
pos = self.ClosestPointWithLoS( activeSide, currentPos )
# we did not find any close visible paths after re calculation follow border
if pos == None:
print 'No visible paths available following border to closest pair point!'
closestP = self.ClosestPoint( self.undetectedPaths, currentPos )
if closestP == None:
return
startP = self.ClosestIntersectionPoint( currentPos, closestP )
if startP == None:
return
endP = self.ClosestIntersectionPointOnBorder( startP, closestP, startP.intersectingLine.p1.borderId )
self.TraverseBorder( startP, endP )
return
print 'No paths available!'
return
pos = self.ClosestPointWithLoS(activeSide, currentPos)
if pos == None:
# try checking for close visible path's
print 'Looking for closer visible paths'
self.CalculateVisiblePaths(self.undetectedPaths,
self.increasingSide, self.decreasingSide,
self.currentPos, 8)
activeSide = self.PickActiveSide(increasingSide,
decreasingSide, currentPos)
print 'Updated points in decreasing side memory: ', \
len(self.decreasingSide)
print 'Updated points in increasing side memory: ', \
len(self.increasingSide)
print 'Num undetected points left:', \
len(self.undetectedPaths)
pos = self.ClosestPointWithLoS(activeSide, currentPos)
# we did not find any close visible paths after re calculation follow border
if pos == None:
print 'No visible paths available following border to closest pair point!'
closestP = self.ClosestPoint( activeSide, currentPos )
startP = self.ClosestIntersectionPoint( currentPos, closestP )
endP = self.ClosestIntersectionPointOnBorder( startP, closestP, startP.intersectingLine.p1.borderId )
self.TraverseBorder( startP, endP )
return
self.GoTo( pos )
self.DeletePointInList( activeSide, pos )
# Only mows outer border right now
def MowBorders( self ):
#outerBorderPoints = copy.deepcopy( self.outerBorder.pointList )
#print len( outerBorderPoints )
dst = self.ClosestPointWithLoS( self.outerBorder.pointList, self.currentPos )
#self.GoTo( dst )
tPath = self.MowBorderPath( self.outerBorder.pointList, dst )
print len( self.outerBorder.pointList )
print len(tPath)
i = 0
while i < len( tPath ):
self.GoTo( tPath[i] )
i += 1
#self.GoTo( dst )
#self.TraverseBorder( startP, startP )
self.pathState = 2
#
def FindNextPath( self ):
if self.isInitialized != 1:
return
if self.pathState == 3:
print "Docked with charging station"
self.pathingComplete = 1
if self.pathState == 2:
print 'Returning to Charging Station'
if self.currentStationPos:
self.undetectedPaths = []
self.increasingSide = []
self.decreasingSide = []
#self.increasingSide.append(self.currentStationPos)
#self.decreasingSide.append(self.currentStationPos)
self.undetectedPaths.append(self.currentStationPos)
closestPos = self.ClosestPointWithLoS(self.undetectedPaths, self.currentPos)
if closestPos != None:
self.GoTo(closestPos)
else:
self.GoToNextPath( self.increasingSide, \
self.decreasingSide, self.currentPos)
self.pathState = 3
return
print "Couldn't go to charging station"
self.pathingComplete = 1
# if pos is on a pair point go to next pair point
# we are assuming next route pair point is always visible
if self.pathState == 0:
if len( self.pathList ) >= self.numPairPoints \
and len( self.undetectedPaths ) == 0 \
and len( self.decreasingSide ) == 0 \
and len( self.increasingSide ) == 0:
# all routes have ben mowed cut the borders now!
print 'Lawn route Completed time to mow borders!'
self.MowBorders()
self.pathState = 2
elif self.currentPos.id == 0:
self.GoToClosestPairPoint()
else:
print 'Total number of pair points: ', \
self.numPairPoints
print 'Num undetected points left:', \
len(self.undetectedPaths)
print 'Traversed pair points: ', len(self.pathList)
print 'Points in decreasing side memory: ', \
len(self.decreasingSide)
print 'Points in increasing side memory: ', \
len(self.increasingSide)
if len(self.increasingSide) < 1 \
and len(self.decreasingSide) < 1:
self.CalculateVisiblePaths(self.undetectedPaths,
self.increasingSide, self.decreasingSide,
self.currentPos, 2)
print 'Calculating Visible Paths........................'
print 'num undetected points left:', \
len(self.undetectedPaths)
print 'Num detected decreasing Side points in memory:', \
len(self.decreasingSide)
print 'Num detected increasing side points in memory:', \
len(self.increasingSide)
self.GoToNextPath(self.increasingSide,
self.decreasingSide, self.currentPos)
elif self.pathState == 1:
#print 'Going To Next Pair Point!'
self.GoToNextPair()
self.pathState = 0
return
#
def Reset(self):
self.isInitialized = 0
self.currentPos = Point(0, 0)
self.currentStationPos = None
self.distToNextPos = 0
self.pathingComplete = 0
self.pathState = 0
self.pathList = []
self.borderPaths = []
self.undetectedPaths = []
self.innerBorders = []
self.outerBorder = Shape()
self.collisionPairs = []
self.decreasingSide = []
self.increasingSide = []
from Geometry import Shape, Point
from ActiveShapeModelsBetter import ASMB
import functools
import timeit
s = Shape([Point(200, 300), Point(150, 200), Point(130, 500)])
def add(x, y):
return x + y
timeit.timeit('map( str , s.shapePoints )')
timeit.timeit('[str(x) for x in s.shapePoints ]')
map(add, s.shapePoints)
map(functools.partial(Point.rotate, [[-1, 1], [1, 1]]), s.shapePoints)
class Map:
sweepAngle = 0
sweepOffset = 0
calcDone = 0
extremePoints = []
boundingBox = []
centerPoint = Point(0, 0)
sweepLines = []
collisionGroups = []
collisionPairs = []
outerBorder = Shape()
innerBorders = []
mLoader = MapLoader()
#
def __init__(self, sweepOffset):
self.sweepAngle = 0
self.sweepOffset = sweepOffset
self.calcDone = 0
self.collisionPointAdjustX = 5
self.minAllowedPairDist = 15
self.rotation = 0.0
self.extremePoints = []
self.boundingBox = []
self.centerPoint = Point(0, 0)
self.sweepLines = []
self.collisionGroups = []
self.collisionPairs = []
self.innerBorders = []
self.outerBorder = Shape()
self.mowerPoint = None
self.stationPoint = None
self.mLoader = MapLoader()
#
def FindExtremePoints(self):
mostNorth = self.outerBorder.pointList[0]
for point in self.outerBorder.pointList:
if mostNorth.y > point.y:
mostNorth = point
mostSouth = self.outerBorder.pointList[0]
for point in self.outerBorder.pointList:
if mostSouth.y < point.y:
mostSouth = point
mostEast = self.outerBorder.pointList[0]
for point in self.outerBorder.pointList:
if mostEast.x < point.x:
mostEast = point
mostWest = self.outerBorder.pointList[0]
for point in self.outerBorder.pointList:
if mostWest.x > point.x:
mostWest = point
mostNorth.y -= 0
mostSouth.y += 0
mostEast.x += 0
mostWest.x -= 0
self.extremePoints = []
self.extremePoints.append(mostNorth)
self.extremePoints.append(mostSouth)
self.extremePoints.append(mostEast)
self.extremePoints.append(mostWest)
print 'Positions relative window: '
print('mostNorth', self.extremePoints[0].x, self.extremePoints[0].y)
print('mostSouth', self.extremePoints[1].x, self.extremePoints[1].y)
print('mostEast', self.extremePoints[2].x, self.extremePoints[2].y)
print('mostWest', self.extremePoints[3].x, self.extremePoints[3].y)
#
def CreateBoundingBox(self):
self.boundingBox = []
self.boundingBox.append(Point(0, 0))
self.boundingBox.append(Point(0, 0))
self.boundingBox.append(Point(0, 0))
self.boundingBox.append(Point(0, 0))
self.boundingBox[0].y = self.extremePoints[0].y # most north
self.boundingBox[0].x = self.extremePoints[3].x # most west
self.boundingBox[1].y = self.extremePoints[0].y # most north
self.boundingBox[1].x = self.extremePoints[2].x # most east
self.boundingBox[2].y = self.extremePoints[1].y # most south
self.boundingBox[2].x = self.extremePoints[2].x # most east
self.boundingBox[3].y = self.extremePoints[1].y # most south
self.boundingBox[3].x = self.extremePoints[3].x # most west
#
def LineSweep(self):
self.sweepLines = []
self.collisionGroups = []
distX = self.boundingBox[0].DistToPoint(self.boundingBox[1].x,
self.boundingBox[1].y)
distY = self.boundingBox[0].DistToPoint(self.boundingBox[3].x,
self.boundingBox[3].y)
print('Distance X:', distX)
print('Distance Y:', distY)
if self.sweepAngle == 0 or self.sweepAngle == 180:
# traverse down on y-axis
y = 0
count = 0
trigger = self.sweepOffset
while True:
if y >= distY:
break
# "Fire line ray"
if y >= trigger:
p1 = Point(self.boundingBox[0].x,
self.boundingBox[0].y + y)
p2 = Point(self.boundingBox[0].x + distX,
self.boundingBox[0].y + y)
l = Line(p1, p2)
collisionsOnSameLine = []
# check line intersections and collect intersect points from lawn boundarys
for line in self.outerBorder.lineList:
point = l.GetIntersectionPoint(line)
if point:
collisionsOnSameLine.append(point)
# check collision with obstacles
if len(self.innerBorders) > 0:
for shape in self.innerBorders:
# check line intersections and collect intersect points from lawn boundarys
for line in shape.lineList:
point = l.GetIntersectionPoint(line)
if point:
collisionsOnSameLine.append(point)
#prevPoint = point
self.SortPoints(collisionsOnSameLine)
self.VertexPointAdjust(collisionsOnSameLine)
#
if len(collisionsOnSameLine) % 2 != 0:
print("Error! odd number of collisions on line!")
print("num collisions on this line",
len(collisionsOnSameLine))
for point in collisionsOnSameLine:
print point.x
if len(collisionsOnSameLine) > 0:
self.collisionGroups.append(collisionsOnSameLine)
trigger += self.sweepOffset
count += 1
self.sweepLines.append(l)
y += 1
count = 0
for points in self.collisionGroups:
for point in points:
count += 1
print("Number of collision points: ", count)
return
# Sorts the points in a list from left closest to the y-axis line
def SortPoints(self, inPoints):
i = 0
sortedPoints = []
points = copy.deepcopy(inPoints)
while True:
if len(points) == 0:
break
j = 1
# x = 0 but y on the same y as the point
newDist = points[0].DistToPoint(0, points[0].y)
dist = 0
currPoint = points[0]
while True:
if j >= len(points):
index = points.index(currPoint)
sortedPoints.append(points.pop(index))
#print ("saved new Dist:", newDist)
break
dist = points[j].DistToPoint(0, points[j].y)
if newDist > dist:
newDist = dist
currPoint = points[j]
j += 1
i += 1
points = sortedPoints
# Sorts the collision points on the same line from left closest to y-axis
def SortCollisionPoints(self):
i = 0
while True:
if i >= len(self.collisionGroups):
break
j = 0
points = self.collisionGroups[i]
sortedPoints = []
while True:
if len(points) == 0:
break
k = 1
# x = 0 but y on the same y as the point
newDist = points[0].DistToPoint(0, points[0].y)
dist = 0
currPoint = points[0]
while True:
if k >= len(points):
index = points.index(currPoint)
sortedPoints.append(points.pop(index))
#print ("saved new Dist:", newDist)
break
dist = points[k].DistToPoint(0, points[k].y)
if newDist > dist:
newDist = dist
currPoint = points[k]
k += 1
j += 1
self.collisionGroups[i] = sortedPoints
i += 1
#
def SplitVertexAndGetCenterPoint(self, p1, p2):
l1 = p1.intersectingLine
l2 = p2.intersectingLine
yDirection1 = None
yDirection2 = None
pEnd1 = p1.intersectingLine.p1
if pEnd1.x == p1.x and pEnd1.y == p1.y:
pEnd1 = p1.intersectingLine.p2
pEnd2 = p2.intersectingLine.p1
if pEnd2.x == p2.x and pEnd2.y == p2.y:
pEnd2 = p2.intersectingLine.p2
print("pEnd1:", pEnd1.x, pEnd1.y)
print("pEnd2:", pEnd2.x, pEnd2.y)
if p1.y > pEnd1.y:
yDirection1 = -1
elif p1.y < pEnd1.y:
yDirection1 = 1
elif p1.y == pEnd1.y:
print "WARNING! p1 vertex y values the same"
return p1
if p2.y > pEnd2.y:
yDirection2 = -1
elif p2.y < pEnd2.y:
yDirection2 = 1
if p2.y == pEnd2.y:
print "WARNING! p2 vertex y values the same"
if yDirection1 == yDirection2:
return None
return p1
#
def VertexPointAdjust(self, inPoints):
if len(inPoints) <= 0:
return
#print "BEGIN VERTEX ADJUST"
i = 0
while True:
if i >= len(inPoints):
break
currPoint = inPoints[i]
j = i + 1
while True:
if j >= len(inPoints):
break
if currPoint.x == inPoints[j].x and currPoint.y == inPoints[
j].y:
print "Vertex Found"
print(currPoint.x, inPoints[j].x)
if self.SplitVertexAndGetCenterPoint(
currPoint, inPoints[j]):
print "removing 1 points"
inPoints.pop(j)
j -= 1
else:
print "removing 2 points"
inPoints.pop(j)
inPoints.pop(i)
j -= 1
i -= 1
j += 1
i += 1
#
def RemoveClosePairs(self, shapeList, minAllowedDist):
# remove pairs that are to close to each other. Set distance to max mower width
for points in shapeList:
i = 0
while True:
if i >= len(self.collisionGroups):
break
j = 0
points = self.collisionGroups[i]
while True:
if j >= len(points):
break
dist = points[j].DistToPoint(points[j + 1].x,
points[j + 1].y)
if dist < minAllowedDist:
points.pop(j + 1)
points.pop(j)
j -= 2
j += 2
i += 1
#
def PairAndAdjustCollisionPoints(self):
if len(self.collisionGroups) <= 0:
return
self.collisionPairs = []
newId = 1
for points in self.collisionGroups:
i = 0
while True:
if i >= len(points):
break
#pair and adjust collision points on x-axis
points[i].id = newId
points[i].x += self.collisionPointAdjustX
points[i + 1].id = newId
points[i + 1].x -= self.collisionPointAdjustX
self.collisionPairs.append(points[i])
self.collisionPairs.append(points[i + 1])
newId += 1
i += 2
print("Number of Pair Id's: ", newId - 1)
print("First Pair id: ", 1)
print("Last Pair id: ", newId)
#
def ScaleBorder(self, pList, k):
length = len(pList)
if length > 0:
i = 0
while True:
if i >= length:
break
mat2 = Mat2()
pList[i] = mat2.Scale(pList[i], k)
i += 1
#
def RotateBorder(self, pList, degree):
length = len(pList)
if length > 0:
i = 0
while True:
if i >= length:
break
mat2 = Mat2()
pList[i] = mat2.Rotate(pList[i], degree)
i += 1
#
def TranslateBorder(self, pList, dst):
length = len(pList)
if length > 0:
i = 0
while True:
if i >= length:
break
mat2 = Mat2()
pList[i] = mat2.Translate(pList[i], dst)
i += 1
#
def Calculate(self):
print "Begin Map Calculation!......................"
if len(self.outerBorder.pointList) <= 0:
return
#
self.centerPoint.y = 400 * -1
self.centerPoint.x = 400 * -1
self.TranslateBorder(self.outerBorder.pointList, self.centerPoint)
self.RotateBorder(self.outerBorder.pointList, self.rotation)
#self.ScaleBorder( self.outerBorder.pointList, 5 )
#
self.centerPoint.y = 400 * 1
self.centerPoint.x = 400 * 1
self.TranslateBorder(self.outerBorder.pointList, self.centerPoint)
self.FindExtremePoints()
self.CreateBoundingBox()
self.LineSweep()
self.SortCollisionPoints()
minAllowedDist = self.minAllowedPairDist * 3
self.RemoveClosePairs(self.collisionGroups, minAllowedDist)
self.PairAndAdjustCollisionPoints()
#
self.centerPoint.y = 400 * -1
self.centerPoint.x = 400 * -1
self.TranslateBorder(self.outerBorder.pointList, self.centerPoint)
self.TranslateBorder(self.collisionPairs, self.centerPoint)
self.RotateBorder(self.outerBorder.pointList, 360 - self.rotation)
self.RotateBorder(self.collisionPairs, 360 - self.rotation)
#self.ScaleBorder( self.outerBorder.pointList, 1.052631579 )
#
self.centerPoint.y = 400 * 1
self.centerPoint.x = 400 * 1
self.TranslateBorder(self.outerBorder.pointList, self.centerPoint)
self.TranslateBorder(self.collisionPairs, self.centerPoint)
#self.CreateBoundingBox()
self.calcDone = 1
print "Map Calculation......................Complete!"
#
def LoadMap(self):
self.mLoader.LoadMap()
self.outerBorder = []
self.innerBorders = []
self.outerBorder = self.mLoader.shape
self.innerBorders = self.mLoader.shapes
self.mowerPoint = self.mLoader.mowerP
self.stationPoint = self.mLoader.stationP
#
def SaveMap(self, mower=None, station=None):
if mower and station:
self.mLoader.SaveMap(self.outerBorder, self.innerBorders, mower,
station)
elif mower:
self.mLoader.SaveMap(self.outerBorder, self.innerBorders, mower)
elif station:
self.mLoader.SaveMap(self.outerBorder, self.innerBorders, None,
station)
else:
self.mLoader.SaveMap(self.outerBorder, self.innerBorders)
#
def Reset(self):
self.sweepAngle = 0
self.calcDone = 0
self.lawnExtremePoints = []
self.boundingBox = []
self.sweepLines = []
self.collisionPoints = []
self.collisionPairs = []
self.innerBorders = []
self.outerBorder = Shape()
self.mowerPoint = None
self.stationPoint = None