예제 #1
0
def displayProgress(e_array, f_array):
    global explored_pub
    global frontier_pub
    global map
    
    cells = GridCells()
    cells.cell_height = map.info.resolution
    cells.cell_width = map.info.resolution
    cells.header.frame_id = "map"
    
    #publish explored cells
    array = []
    for i in e_array:
        x = (i.x + map.info.width+0.5)*map.info.resolution + map.info.origin.position.x
        y = -((i.y+0.5)*map.info.resolution - map.info.origin.position.y)
        array.append(Point(x,y,0))
    cells.cells = array
    explored_pub.publish(cells)
    
    #publish frontier cells
    array = []
    for i in f_array:
        x = (i.x + map.info.width+0.5)*map.info.resolution + map.info.origin.position.x
        y = -((i.y+0.5)*map.info.resolution - map.info.origin.position.y)
        array.append(Point(x,y,0))
    cells.cells = array
    frontier_pub.publish(cells)
예제 #2
0
def displayPath(p_array):
    global path_pub
    global map
    global arrayTravelled
    
    cells = GridCells()
    cells.cell_height = map.info.resolution
    cells.cell_width = map.info.resolution
    cells.header.frame_id = "map"
    
    #publish path cells
    array = []
    for i in p_array:
        x = (i.x + map.info.width+0.5)*map.info.resolution + map.info.origin.position.x
        y = -((i.y+0.5)*map.info.resolution - map.info.origin.position.y)
        array.append(Point(x,y,0))
    cells.cells = array
    path_pub.publish(cells)
예제 #3
0
def read_map(msg):
    global goal_pub
    
    mapResolution = msg.info.resolution
    mapWidth = msg.info.width
    inputArray = msg.data
    result = GridCells();
    
    n = 0
    #print "expanding obstacles"
    print "searching for new frontier cells"
    numExpansions = 1
    freeSpaceValue = 50
    
    for n in range(len(inputArray)):
        
        #look at unknown cells
        if inputArray[n] == -1:
            #determine L bound
            if n%mapWidth < numExpansions:
                L = n%mapWidth
            else:
                L = numExpansions
            #determine Upper bound
            if n/mapWidth < numExpansions:
                D = trunc(n/mapWidth)
            else:
                D = numExpansions        
            #determine R bound
            if mapWidth - n%mapWidth - 1 < numExpansions:
                R = mapWidth - n%mapWidth - 1
            else:
                R = numExpansions
            #determine depth
            if len(inputArray) - n <= numExpansions * mapWidth:
                U = trunc((len(inputArray) - n - 1)/mapWidth)
            else:
                U = numExpansions
        
            n2 = 0
            n3 = 0
        
            stop = 0
            for n2 in range(int(U+D+1)):
                for n3 in range(int(L+R+1)):
                    tempLoc = int(n + mapWidth * (U - n2) + (n3 - L))
                    #check if this unknown cell is next to free space
                    if inputArray[tempLoc] != -1 and inputArray[tempLoc] < freeSpaceValue:
                        result.cells.append(Point(n%mapWidth, int(n/mapWidth), 0))
                        stop = 1
                        break
                if stop == 1:
                    break
    
    #convert to position
    for i in result.cells:
        i.x = (i.x+0.5) * mapResolution + msg.info.origin.position.x
        i.y = (i.y+0.5) * mapResolution + msg.info.origin.position.y
    
    #now publish the list of frontier cells
    result.header = msg.header
    result.cell_height = mapResolution
    result.cell_width = mapResolution
    goal_pub.publish(result)