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]))