def __init__(self, startMap, robotRadius):
self.gridDist = robotRadius/2;
# finding edges of map
xList = []
yList = []
for obstacle in startMap:
for point in obstacle.getLocationList():
xList.append(point.getX())
yList.append(point.getY())
self.minX = min(xList);
self.maxX = max(xList);
self.minY = min(yList);
self.maxY = max(yList);
self.robotRadius = robotRadius;
self.cspace = [];
self.createCSpace(startMap);
self.createGraph();
# creating the subscriber that listens to the mapUpdater
# TODO: commented out for debugging
#self.mapSub = rospy.Subscriber('mapUpdates', ObstacleMsg, self.handleObstacleMsg)
return
def updateGraph(self, obstacle):
corners = obstacle.getLocationList()
edges = self.graph.edges()
# for each line in the obstacle
for i in range(len(corners)):
# get one endpoint
x1 = corners[i].getX()
y1 = corners[i].getY()
# get the other endpoint
# may have to wrap around to get endpoint of line
nextPointIndex = (i+1)%len(corners)
x2 = corners[nextPointIndex].getX()
y2 = corners[nextPointIndex].getY()
# for each edge in the graph
for e in edges:
# get the endpoints of the edges
edgeX1 = e[0][0]
edgeY1 = e[0][1]
edgeX2 = e[1][0]
edgeY2 = e[1][1]
# if the edge intersects a line from the new obstacle, remove it
if self.intersects(edgeX1, edgeY1, edgeX2, edgeY2, x1, y1, x2, y2):
self.graph.removeEdge((e[0],e[1]))
