Exemplo n.º 1
0
def printTarget():
    print('printing')
    pub = rospy.Publisher('colorcells2', GridCells, queue_size=1)
    global mapRes
    global startX
    global startY
    global gridWidth
    global gridHeight
    global stuff
    global origPosX
    global origPosY
    global closest
    print(closest)
    grid = GridCells()
    grid.header.frame_id = 'map'
    grid.cell_width = mapRes
    grid.cell_height = mapRes
    global xFinal
    global yFinal
    xFinal = (closest[0]) * mapRes + origPosX
    yFinal = (closest[1]) * mapRes + origPosY
    t = Point()
    t.y = xFinal
    t.x = yFinal
    t.z = 0

    grid.cells.append(t)
    while 1:
        pub.publish(grid)
Exemplo n.º 2
0
def publishCells(grid, num):
    global ckd
    global bud
    global front
    global xOffset
    global yOffset
    # print "publishing"
    k = 0
    cells = GridCells()
    cells.header.frame_id = "map"
    cells.cell_width = 0.2  # edit for grid size .3 for simple map
    cells.cell_height = 0.2  # edit for grid size

    for square in grid:  # height should be set to hieght of grid
        # print k # used for debugging
        point = Point()
        point.x = square.x * cells.cell_width + cells.cell_width + xOffset  # edit for grid size
        point.y = square.y * cells.cell_height + cells.cell_width + yOffset  # edit for grid size
        point.z = 0
        cells.cells.append(point)

    # print cells # used for debugging
    if num == 100:
        pub.publish(cells)
    if num == 1:
        ckd.publish(cells)
    if num == 2:
        front.publish(cells)
    if num == 4:
        bud.publish(cells)
Exemplo n.º 3
0
def publishCells(grid):
    global pub
    print "publishing"

    # resolution and offset of the map
    k = 0
    cells = GridCells()
    cells.header.frame_id = 'map'
    cells.cell_width = resolution
    cells.cell_height = resolution

    for i in range(1, height):  #height should be set to hieght of grid
        k = k + 1
        for j in range(1, width):  #width should be set to width of grid
            k = k + 1
            #print k # used for debugging
            if (grid[k] == 100):
                point = Point()
                point.x = (j * resolution) + offsetX + (
                    1.5 * resolution)  # added secondary offset
                point.y = (i * resolution) + offsetY - (
                    .5 * resolution)  # added secondary offset ... Magic ?
                point.z = 0
                cells.cells.append(point)
    pub.publish(cells)
Exemplo n.º 4
0
 def point_to_grid_cells(self,list):
     msg=GridCells()
     msg.header.frame_id='map'
     msg.cell_width=self.metadata.resolution
     msg.cell_height=self.metadata.resolution
     msg.cells=[self.grid_to_world(loc) for loc in list]
     return msg
Exemplo n.º 5
0
def map_callback(ret):
    rospy.loginfo("Map Callback")
    #print ret
    map_data = OccupancyMap(ret)
    centroids = map_data.findFrontiers()

    # Generate all grid cells info
    gridCells = GridCells()
    gridCells.cell_width = ret.info.resolution
    gridCells.cell_height = ret.info.resolution
    gridCells.header.frame_id = 'map'
    gridCells.header.stamp = rospy.Time.now()

    #Generate all of the point data
    for centroid in centroids:
        point = Point()
        cell = transform_grid_cells_to_map_meters((centroid[0], centroid[1]), ret.info)
        point.x = cell[0]
        point.y = cell[1]
        point.z = 0
        gridCells.cells.append(point)

    # Publish this data
    global frontierPub
    rospy.loginfo("Publishing frontier")
    frontierPub.publish(gridCells)
Exemplo n.º 6
0
    def calc_grid(self):
        if None in [
                self.pos, self.map_pos, self.map, self.costmap,
                self.measurements
        ] or len(self.map.data) <= 0:
            return

        if self.costmap[self.map_pos.x][self.map_pos.y] >= 100:
            print("position in wall")
            return

        if not self._calc_lock.acquire(False):
            return

        if self.dists is None or self.dists[self.map_pos.x][
                self.map_pos.y] <= -1:
            self.calc_dists()

        grid = GridCells()
        grid.header.stamp = rospy.Time.now()
        grid.header.frame_id = '/map'
        grid.cell_width = grid.cell_height = self.map.info.resolution
        grid.cells = list(
            Point(
                x * self.map.info.resolution +
                self.map.info.origin.position.x, y * self.map.info.resolution +
                self.map.info.origin.position.y, 0)
            for x, y in itertools.product(xrange(len(self.costmap)),
                                          xrange(len(self.costmap[0])))
            if self.dists[x][y] > -1 and self.measurements[x][y] <= -1)
        self.grid = grid

        self._calc_lock.release()
        self.send()
Exemplo n.º 7
0
    def convertMapToPoints(self):
        points = GridCells()        
        pts = 0
        points.header.frame_id = "/my_frame"
        points.header.stamp = rospy.Time.now()
        points.cell_height = self.cellHeight
        points.cell_width = self.cellWidth

        map = self.map
        # map[0,0] = 1
        # map[0, self.height-1]=1
        # map[self.width-1,0]=1
        # map[self.width-1, self.height-1]=1

        for i in range(len(map)):
            for j in range(len(map[0])):
                if map[i, j]>0:
                    pts += 1
                    m = Point()
                    m.x = i * self.cellWidth
                    m.y = j * self.cellHeight

                    points.cells.append(m)

        return points
Exemplo n.º 8
0
def publishCells(nodes, publisher):

    """
    Publish Cells
    Publishes a list of nodes as GridCells to RViz
    Uses the given publisher
    """

    global offsetY
    global offsetX

    print "publishing frontiers"

    # resolution and offset of the map
    k=0
    cells = GridCells()
    cells.header.frame_id = 'map'
    cells.cell_width = resolution
    cells.cell_height = resolution

    for node in nodes:
        point=Point()
        point.x = (node.x * resolution) + offsetX + (0.72 / resolution)
        point.y = (node.y * resolution) + offsetY + (0.643 / resolution)
        point.z = 0
        cells.cells.append(point)

    publisher.publish(cells)
Exemplo n.º 9
0
def publishChecked(grid):
    global ckd
    print "publishing"
    k=0
    cells = GridCells()
    cells.header.frame_id = 'map'
    cells.cell_width = 0.3 # edit for grid size .3 for simple map
    cells.cell_height = 0.3 # edit for grid size

    for i in range(1,10): #height should be set to hieght of grid
        for j in range(1,9): #height should be set to hieght of grid
            #print k # used for debugging
            if (grid[k] == 0):
                point=Point()
                point.x=j*.3+.32 # edit for grid size
                point.y=i*.3-.15 # edit for grid size
                point.z=0
                cells.cells.append(point)
            k=k+1
        k=k+1
        if (grid[k] == 0):
            point=Point()
            point.x=j*.3+.62 # edit for grid size
            point.y=i*.3-.15 # edit for grid size
            point.z=0
            cells.cells.append(point)

    #print cells # used for debugging
    ckd.publish(cells)           
Exemplo n.º 10
0
def publishCells(grid):
    global wallpub
    print "publishing"
    k = 0
    cells = GridCells()
    cells.header.frame_id = "map"
    cells.cell_width = 0.3  # edit for grid size .3 for simple map
    cells.cell_height = 0.3  # edit for grid size

    for i in range(1, height):  # height should be set to hieght of grid
        for j in range(1, width):  # height should be set to hieght of grid
            # print k # used for debugging
            if grid[k] == 100:
                point = Point()
                point.x = j * 0.3 + 0.32  # edit for grid size
                point.y = i * 0.3 - 0.15  # edit for grid size
                point.z = 0
                cells.cells.append(point)
            k = k + 1
        k = k + 1
        if grid[k] == 100:
            point = Point()
            point.x = j * 0.3 + 0.62  # edit for grid size
            point.y = i * 0.3 - 0.15  # edit for grid size
            point.z = 0
            cells.cells.append(point)

    # print cells # used for debugging
    wallpub.publish(cells)
Exemplo n.º 11
0
def publishFrontier(grid):
	global pubfrontier
	# print "publishing frontier"

	# resolution and offset of the map
	k=0
	cells = GridCells()
	cells.header.frame_id = 'map'
	res = resolution * 2
	cells.cell_width = res 
	cells.cell_height = res

	for i in range(1,height/2): #height should be set to hieght of grid
		k=k+1
		for j in range(1,width/2): #width should be set to width of grid
			k=k+1
			#print k # used for debugging
			if (grid[k] == 100):
				point=Point()
				point.x=(j*res)+offsetX + (1.5 * res) # added secondary offset 
				# point.x=(j*res) + (.5 * res)
				point.y=(i*res)+offsetY - (.5 * res) # added secondary offset ... Magic ?
				# point.y=(i*res) + (.5 * res)
				point.z=0
				cells.cells.append(point)
	pubfrontier.publish(cells)
def publishGridCells(listOfPoints, topicNum):
        
    # create new grid cells object
    gridCells = GridCells()

    # set the list of points, frame, and cell size
    gridCells.cells =  listOfPoints
    gridCells.header.frame_id = "/map"

    gridCells.cell_width = mapRes
    gridCells.cell_height = mapRes

    # publish the grid to the correct topic
    if topicNum == 1:
        gridCellPub.publish(gridCells)
    elif topicNum == 2:
        exploredCellsPub.publish(gridCells)
    elif topicNum == 3:
        frontierCellsPub.publish(gridCells)
    elif topicNum == 4:
        wayPointsPub.publish(gridCells)
    elif topicNum == 5:
        pathPub.publish(gridCells)
    elif topicNum == 6:
        goalCellPub.publish(gridCells)
Exemplo n.º 13
0
def waveFront(start, graph, depthLimit):
    openSet = Queue()
    found = []
    frontier = []
    openSet.put(start)

    resolution = globalMap.info.resolution

    cells = GridCells()
    cells.header.frame_id = 'map'
    cells.cell_width = resolution 
    cells.cell_height = resolution
    
    while not openSet.empty() and not rospy.is_shutdown():
        current = openSet.queue[0]
      
        # found an unknown cell
        if graph.cellValue(current) == -1:
           
            found.append(current)
            publishCells(frontier, opB_pub, globalMap)
        else:
            for n in graph.getNeighbors(current):
                if graph.cellValue(n) == -1 and current not in frontier and graph.cellValue(current) < 50:
                    frontier.append(current)
                # n.g - how far away fro start the cell is
                if n not in found and n not in openSet.queue and n.g < depthLimit:
                    openSet.put(n)
        found.append(openSet.get())
    # sub_status = rospy.Subscriber('/move_base/status', GoalStatusArray, findWantedNode, queue_size = 10)
    return frontier
Exemplo n.º 14
0
def map_to_cells(my_map):
    resolution = my_map.info.resolution

    gc = GridCells()
    gc.cell_width = resolution
    gc.cell_height = resolution
    gc.header.frame_id = "map"

    width = my_map.info.width
    height = my_map.info.height
    xOrigin = my_map.info.origin.position.x
    yOrigin = my_map.info.origin.position.y

    for i in range(0, width, 1):
        for j in range(0, height, 1):
            index = point_to_index((i, j), my_map)
            if my_map.data[index] == 0:
                gcPoint = Point()

                point = convert_location((i, j), my_map)

                gcPoint.x = point[0]
                gcPoint.y = point[1]
                gcPoint.z = 0

                gc.cells.append(gcPoint)

    return gc
Exemplo n.º 15
0
def publishCells(grid):
    global pub
    global cellMsg
    print "publishing"

    # resolution and offset of the map
    k = 0
    cells = GridCells()
    cells.header.frame_id = 'map'
    # Resolution is set to 0.05m/cell, multiplying by 10 increases this to .5m/cell
    # This is approximately equal to the widest point on the turtlebot plus a little
    cells.cell_width = resolution
    cells.cell_height = resolution

    for i in range(0, height):  #height should be set to hieght of grid
        for j in range(0, width):  #width should be set to width of grid
            #print k # used for debugging
            if (grid[k] == 100):
                point = Point()
                point.x = (j * resolution) + offsetX + (
                    0.5 * resolution)  # added secondary offset
                point.y = (i * resolution) + offsetY + (
                    0.5 * resolution)  # added secondary offset ... Magic ?
                point.z = 0
                cells.cells.append(point)
            k = k + 1
        #print "row: ", i+1
    #cells.cells = numpy.reshape(grid.data, (grid.info.width,grid.info.height))
    cellMsg = cells
    pub.publish(cells)
Exemplo n.º 16
0
def publishPointListAsGridCells(points, publisher):
    gridCells = GridCells()  # make an empty grid cells
    gridCells.cell_height = G_GridResolution
    gridCells.cell_width = G_GridResolution
    gridCells.header.frame_id = "map"
    gridCells.cells = points
    publisher.publish(gridCells)
Exemplo n.º 17
0
    def drawNodes(self, nodes, topic):
        publisher = None
        if topic == "frontier":
            publisher = self._grid_frontier_pub
        elif topic == "explored":
            publisher = self._grid_explored_pub
        elif topic == "path":
            publisher = self._grid_path_pub
        elif topic == "wall":
            publisher = self._grid_wall_pub
        else:
            return

        cells = GridCells()
        cells.header.frame_id = self._map.header.frame_id
        cells.cell_width = self._map.info.resolution
        cells.cell_height = self._map.info.resolution

        cell_points = []
        for node in nodes:
            (xc, yc) = self.mapCoordsToWorld(node.x, node.y)

            cell_x = xc
            cell_y = yc
            cell_points.append(Point(cell_x, cell_y, 0))

        cells.cells = cell_points
        publisher.publish(cells)

        rospy.sleep(0.01)
Exemplo n.º 18
0
 def to_grid_cells(self):
     msg=GridCells()
     msg.header.frame_id='map'
     msg.cell_width=self.metadata.resolution
     msg.cell_height=self.metadata.resolution
     msg.cells=[self.grid_to_world(loc) for loc,val in np.ndenumerate(self.data) if val > 0]
     return msg
Exemplo n.º 19
0
def getGridUpdate(grid):
    global gridIndices

    # Initialize variables
    cells = GridCells()
    cells.header.frame_id = 'map'
    cells.cell_width = resolution
    cells.cell_height = resolution
    k = 0

    for i in range(0, height):
        for j in range(0, width):
            if (grid[k] < 50):
                indexChecklist = getNearbyIndices(k)
                for index in indexChecklist:
                    if index not in wallIndices:
                        wallIndices.append(index)
                k = k + 1
    gridIndices = 1
    wallPoints = getPointsFromIndicies(wallIndices)
    #print("Length of wallPoints: ",len(wallIndices))
    for point in wallPoints:
        outPoint = Point()
        outPoint.x = point[0]
        outPoint.y = point[1]
        outPoint.z = 0

        cells.cells.append(outPoint)
        #print(point)
    mapPub.publish(cells)
Exemplo n.º 20
0
def setStart(msg):
    global start, pub_start, mapInfo, mapData

    #define a point object that will represent our start point on the grid
    point = msg.pose.pose.position #this needs to be adjuseted depending on how the gridcells object is using the point object.

    #set the starting point for the search to the gridpoint defined by the user mouse click.
    start = globalToGrid(point, mapInfo)

    #convert the point to a grid position
    point.x = round(point.x/mapInfo.resolution) * mapInfo.resolution
    point.y = round(point.y/mapInfo.resolution) * mapInfo.resolution

    #define a new gridcells object 
    gridCells = GridCells()
    
    #start construction our message
    gridCells.header = msg.header
    gridCells.cell_width = mapInfo.resolution
    gridCells.cell_height = mapInfo.resolution
    cells = [point]
    gridCells.cells = cells

    pub_start.publish(gridCells)
    print "startpoint set"
Exemplo n.º 21
0
def og_pub_gridcells(points, z, pub, og):
    """
	Brief: Publishes list of points (i, j) as gridcells
	Inputs:
		points [list(tuple(i, j))]: List of grid coordinates
		z [float]: Height to print at
		pub [rospy.Publisher]: Publisher
		og [OccupancyGrid]: Reference grid
	Return: None
	"""

    # Creaty empty grid cells object
    grid_cells = GridCells()
    grid_cells.header.frame_id = og.header.frame_id
    grid_cells.cell_width = og.info.resolution
    grid_cells.cell_height = og.info.resolution

    # Convert points to grid cell array
    cells = []
    for point in points:
        i = point[0]
        j = point[1]
        x, y = og_ij_to_xy(i, j, og)
        cells.append(Point(x, y, z))
    grid_cells.cells = cells

    # Publish on given publisher
    pub.publish(grid_cells)
Exemplo n.º 22
0
def publishCells(grid, num):

    global frontierPub
    global clearPub
    global wallPub
    global xOffset
    global yOffset
    #print "publishing"
    k=0
    cells = GridCells()
    cells.header.frame_id = 'map'
    cells.cell_width = 0.2 # edit for grid size .3 for simple map
    cells.cell_height = 0.2 # edit for grid size

    for square in grid: #height should be set to hieght of grid
            #print k # used for debugging
            point=Point()
            point.x=square.x*cells.cell_width+cells.cell_width+xOffset # edit for grid size
            point.y=square.y*cells.cell_height+cells.cell_width+yOffset # edit for grid size
            point.z=0
            cells.cells.append(point)

    #print cells # used for debugging
    if(num == 0):    
        frontierPub.publish(cells)
    elif(num == 1):    
        clearPub.publish(cells)
    elif(num == 2):    
        wallPub.publish(cells)
def publishGridCells(listOfPoints, topicNum):

    # create new grid cells object
    gridCells = GridCells()

    # set the list of points, frame, and cell size
    gridCells.cells = listOfPoints
    gridCells.header.frame_id = "/map"

    gridCells.cell_width = mapRes
    gridCells.cell_height = mapRes

    # publish the grid to the correct topic
    if topicNum == 7:
        frontierPub.publish(gridCells)
    else:
        time.sleep(0.5)
        if topicNum == 8:
            smallFrontPub0.publish(gridCells)
        elif topicNum == 9:
            smallFrontPub1.publish(gridCells)
        elif topicNum == 10:
            smallFrontPub2.publish(gridCells)
        elif topicNum == 11:
            smallFrontPub3.publish(gridCells)
        elif topicNum == 12:
            smallFrontPub4.publish(gridCells)
def publishGridCells(listOfPoints, topicNum):
        
    # create new grid cells object
    gridCells = GridCells()

    # set the list of points, frame, and cell size
    gridCells.cells =  listOfPoints
    gridCells.header.frame_id = "/map"

    gridCells.cell_width = mapRes
    gridCells.cell_height = mapRes

    # publish the grid to the correct topic
    if topicNum == 7:
        frontierPub.publish(gridCells)
    else:
        time.sleep(0.5)
        if topicNum == 8:
            smallFrontPub0.publish(gridCells)
        elif topicNum == 9:
            smallFrontPub1.publish(gridCells)
        elif topicNum == 10:
            smallFrontPub2.publish(gridCells)
        elif topicNum == 11:
            smallFrontPub3.publish(gridCells)
        elif topicNum == 12:
            smallFrontPub4.publish(gridCells)
Exemplo n.º 25
0
def publishClosedCellsReduce(map2D):
    global resolution
    global scale

    global reducedHeight
    global reducedWidth
    global x0
    global x1
    global y0
    global y1

    gridCells = GridCells()
    gridCells.header.frame_id = "/map"
    gridCells.header.stamp = rospy.Time.now()
    gridCells.cell_width = resolution*scale
    gridCells.cell_height = resolution*scale
    xyscale = 1.0/(resolution*scale)
    pointList = []


    for x in range(reducedWidth/scale):
        for y in range(reducedHeight/scale):
            if(map2D[y][x] > obsThresh):
                p = Point()
                p.x = float((x+x0/scale)/xyscale)+1/(2*xyscale) + originx
                p.y = float((y+y0/scale)/xyscale)+1/(2*xyscale) + originy
                p.z = 0
                pointList.append(p)
    gridCells.cells = pointList
    closedPub.publish(gridCells)
Exemplo n.º 26
0
def publishGridCellList(lst,typ):
    global resolution
    global scale
    gridCells = GridCells()
    gridCells.header.frame_id = "/map"
    gridCells.header.stamp = rospy.Time.now()
    gridCells.cell_width = resolution*scale
    gridCells.cell_height = resolution*scale
    xyscale = 1.0/(resolution*scale)
    
    pntList=[]
    for pnt in lst:
        p = Point()
        # p.x= float(pnt.x/xyscale)+1/(2*xyscale)
        # p.y= float(pnt.y/xyscale)+1/(2*xyscale)
        p.x = float((pnt.x+x0/scale)/xyscale)+1/(2*xyscale) + originx
        p.y = float((pnt.y+y0/scale)/xyscale)+1/(2*xyscale) + originy
        p.z=0
        pntList.append(p)

    gridCells.cells=pntList
    if(typ==0):
        openPub.publish(gridCells)
    if(typ==1):
        closedPub.publish(gridCells)
    if(typ==2):
        pathVizPub.publish(gridCells)
    if(typ==3):
        astarVizPub.publish(gridCells)
Exemplo n.º 27
0
def newGoalCallback(msg):
	global endpos
	global regen_map
	global goal_pub	
	global has_goal	
	global curmap
	global has_map
	if (has_map):
		#extract point from message
		point = msg.pose.position
		print 'Got new gloal position, regenerating map'
		#round point values to nearest integer
		endpos = point; 
		print "x: ", endpos.x
		print "y: ", endpos.y	
		#send rounded goal point as a GridCells message to /lab3/astar/goal
		end = GridCells()
		end.cell_width = curmap.info.resolution
		end.cell_height = curmap.info.resolution
		end.cells = [mapToWorldPos(curmap, worldToMapCell(curmap, endpos))]
		end.header.frame_id = 'map'
		goal_pub.publish(end)
		#trigger A* path regeneration
		regen_map = 1
		#indicate that we have received a goal position
		has_goal = 1
Exemplo n.º 28
0
def check_occupancy(current, debug):    #check whether the cell is occupied
    FRONTIER_PUBLISH = rospy.Publisher('/Frontier', GridCells)   #Frontier Grid Cells Publisher   
    
    neighbors_all = find_all_neighbors(current)
    #print neighbors_all
    neighbors_empty = []
    Frontier = []
    
    for neighbor_index, neighbor in enumerate(neighbors_all):
        #cell_index = ( neighbor.y - (RESOLUTION/2) ) * WIDTH
        #if MAP_DATA[int(cell_index)] < 50:   #EMPTY CELL
        for elem in PtArr_Empty:
            if numpy.allclose([elem.x], [neighbor.position.x]) and numpy.allclose([elem.y], [neighbor.position.y]):
            #if elem.x == neighbor.position.x and elem.y == neighbor.position.y:
            #print "index:", cell_index
            #print int(cell_index)
                Frontier.append(Point(neighbor.position.x, neighbor.position.y, 0))
                neighbors_empty.append(neighbor.position)
               # print "adding neighbor: ", neighbor_index
            #if elem.x == neighbor.position.x:
               # print "element_x: ", elem.x
                #print "element_y: ", elem.y
    #print PtArr_Empty
    Frontier_Empty  = GridCells()
    Frontier_Empty.header = HEADER
    Frontier_Empty.cell_width = RESOLUTION
    Frontier_Empty.cell_height = RESOLUTION
    Frontier_Empty.cells = Frontier
    if debug:
        count = 0
        while count < 1500:
            FRONTIER_PUBLISH.publish(Frontier_Empty)
            count = count+1
    return neighbors_empty
Exemplo n.º 29
0
def path_to_gridcell(tuple_list, my_map):
    """
        converts a list of tuples (path) to a gridcells object
        :param tuple_list: a list of tuples representing the points in a path
        :return: GridCells object
    """
    resolution = my_map.info.resolution

    gc = GridCells()
    gc.cell_width = resolution
    gc.cell_height = resolution
    gc.header.frame_id = "map"

    xOrigin = my_map.info.origin.position.x
    yOrigin = my_map.info.origin.position.y

    for i in tuple_list:
        gcPoint = Point()

        point = convert_location((i[0], i[1]), my_map)

        gcPoint.x = point[0]
        gcPoint.y = point[1]
        gcPoint.z = .1

        gc.cells.append(gcPoint)

    return gc
Exemplo n.º 30
0
def mapCallback(OccupancyGrid):
    print 'Got Occupancy Map'

    global expandGrid
    global ocGridMeta
    global ocGrid
    expandGrid = GridCells()
    ocGridMeta = OccupancyGrid.info
    ocGrid = [[0 for _ in range(OccupancyGrid.info.height)] for _ in range(OccupancyGrid.info.width)] #Generate the ocgrid as a 2-d array
    for row in range(OccupancyGrid.info.height):
        for col in range(OccupancyGrid.info.width):
            i = ( row * OccupancyGrid.info.width) + col
            if( OccupancyGrid.data[i] >= 30):  #if there is a 60% propabability of obsticle
                ocGrid[col][row] = 1 #flag
                expandObsticals(col, row) #expand the discovered obstical
            elif OccupancyGrid.data[i] == -1:  #if the cell is unknown
                ocGrid[col][row] = 0 #flag
                
    expandGrid.cell_height = ocGridMeta.resolution 
    expandGrid.cell_width = ocGridMeta.resolution
    expandGrid.header.frame_id = 'map'
    ExpandPub.publish(expandGrid)
    global position
    global startNode
    x = position.x
    y = position.y
    x = int(round((x - ocGridMeta.origin.position.x)/ocGridMeta.resolution))
    y = int(round((y - ocGridMeta.origin.position.y)/ocGridMeta.resolution))
    print "Got a start Position"
    startNode =  node(x, y,None,0,0)
    if goalNode != None:
        aStar(startNode, goalNode)
    print 'import done'
Exemplo n.º 31
0
def MakeGridCellsFromList(cellList):
    gridCells = GridCells()
    gridCells.cell_width = 1
    gridCells.cell_height = 1
    gridCells.cells = cellList
    gridCells.header.frame_id = 'map'
    return gridCells
Exemplo n.º 32
0
    def calc_cspace(self, mapdata):
        """
        Calculates the C-Space, i.e., makes the obstacles in the map thicker.
        :return        [OccupancyGrid] The C-Space.
        """

        rospy.loginfo("Calculating C-Space")
        # Go through each cell in the occupancy grid
        # Inflate the obstacles where necessary
        padding = 2
        newmap = deepcopy(mapdata)
        # for each cell in mapdata
        #   if (cell is walkable) and (cell has an obstacle within padding)
        #     add padding in newmap at current cell

        # input (x,y,pad)
        # sx = min(max(0, x-pad), width)
        # sy = min(max(0, y-pad), height)
        # ex = max(min(width, x+pad), 0)
        # ey = max(min(height, y+pad), 0)
        # for each xx in range(sx,ex)
        #   for each yy in range(sy,ey)
        #     if cell(xx,yx) has obstacle
        #        return True
        #   return False

        new_data = []
        for item in mapdata.data:
            new_data.append(item)

        for h in range(mapdata.info.height - 1):
            for w in range(mapdata.info.width - 1):
                neighbors = ExpandMap.neighbors_of_8(mapdata, w, h)
                if len(neighbors) < 8:
                    new_data[ExpandMap.grid_to_index(mapdata, w, h)] = 100

        #Create a GridCells message and publish it
        expanded_cells = GridCells()
        expanded_cells.header.frame_id = "map"
        expanded_cells.cell_width = mapdata.info.resolution
        expanded_cells.cell_height = mapdata.info.resolution
        expanded_cells.cells = []

        for h in range(mapdata.info.height):
            for w in range(mapdata.info.width):
                index = ExpandMap.grid_to_index(mapdata, w, h)
                if new_data[index] == 100:
                    msg_Point = Point()
                    world_point = PathPlanner.grid_to_world(mapdata, w, h)
                    msg_Point.x = world_point.x
                    msg_Point.y = world_point.y
                    msg_Point.z = 0
                    expanded_cells.cells.append(msg_Point)

        self.expanded_cells.publish(expanded_cells)
        ## Return the C-space
        #print("END")
        #print(mapdata)
        newmap.data = new_data
        return newmap
Exemplo n.º 33
0
def MakeGridCellsFromList (cellList):
	gridCells = GridCells()
	gridCells.cell_width = .2
	gridCells.cell_height = .2
	gridCells.cells = cellList
	gridCells.header.frame_id = 'map'
	return gridCells
Exemplo n.º 34
0
	def merge(self, other):
		overlap_counter = 0
		self.f_ids.sort()
		other.f_ids.sort()

		# merge the cells
		for new_cell in other.cells:
			if new_cell not in self.cells:
				self.add_cell(new_cell.x, new_cell.y)
			else:
				overlap_counter += 1

		# add the sizes
		self.size += other.size - overlap_counter

		# when in debug mode, merge the gridcells and publish
		if DEBUG:
			for gridcell in other.gridcells.cells:
				if gridcell not in self.gridcells.cells:
					self.gridcells.cells.append(gridcell)
			# copy over the publisher
			if self.f_ids[0] > other.f_ids[0]:
				dummy_cells = GridCells()
				dummy_cells.header.frame_id = 'map'
				dummy_cells.cell_width = resolution
				dummy_cells.cell_height = resolution
				self.publisher.publish(dummy_cells)
				self.publisher = other.publisher
				self.publish()

		# merge the frontier ids
		for f_id in other.f_ids:
			if f_id not in self.f_ids:
				self.f_ids.append(f_id)
				self.f_ids.sort()
Exemplo n.º 35
0
def rvizObstacles(w_map):
    global obstacles_pub

    obstacles_GC = GridCells()
    obstacles_GC.cell_width = w_map.info.resolution
    obstacles_GC.cell_height = w_map.info.resolution
    obstacles_GC.cells = []
    obstacle_pts = []
    expanded_pts = set([])
    e_o_i = set([])

    for index, node in enumerate(w_map.data):
        if node > 50:
            obstacle_pts.append(index)

    for index in obstacle_pts:
        for i in range(-4, 5):
            for j in range(-4, 5):
                point = i_to_p(index, w_map.info.width)
                point.x += i
                point.y += j
                e_o_i.add(p_to_i(point, w_map.info.width))
                expanded_pts.add(lab_4.gridToWorld(point, w_map))
    
    obstacles_GC.cells = list(expanded_pts)

    obstacles_GC.header.frame_id = 'map'
    obstacles_pub.publish(obstacles_GC)
    return list(e_o_i)
def publishCells(grid):
    global wallIndices

    # Initialize variables
    k = 0
    cells = GridCells()
    cells.header.frame_id = 'map'
    cells.cell_width = resolution
    cells.cell_height = resolution

    for i in range(0, height):  #height should be set to hieght of grid
        for j in range(0, width):  #width should be set to width of grid

            # grid values are probability percentages of occupancy. The following condition gives all the cells where
            # occupancy probability is more than 50%
            if (grid[k] > 50):

                point = Point()
                # 0.5*resolution moves the point to the center of the grid cell
                point.x = (j * resolution) + offsetX + (0.5 * resolution)
                point.y = (i * resolution) + offsetY + (0.5 * resolution)
                point.z = 0
                cells.cells.append(point)
                index = i * width + j

                # Store a list of Wall Indices - for checking neighbors
                if index not in wallIndices:
                    wallIndices.append(index)
            k = k + 1
    # Display walls in rviz
    mapPub.publish(cells)
Exemplo n.º 37
0
def publishWaypoints(points):
    global pubway
    print "Publishing Waypoints"
    k = 0
    cells = GridCells()
    cells.header.frame_id = 'map'
    cells.cell_width = resolution
    cells.cell_height = resolution

    waypoints = Path()
    waypoints.header.frame_id = 'map'

    for i in range(0, len(points)):
        point = Point()
        point.x = (points[i].x * resolution) + offsetX + (.5 * resolution)
        point.y = (points[i].y * resolution) + offsetY + (.5 * resolution)
        point.z = 0
        cells.cells.append(point)
        temp = PoseStamped()
        temp.pose.position.x = point.x
        temp.pose.position.y = point.y
        #temp.pose.orientation.z = 0
        waypoints.poses.append(temp)

    pubway.publish(waypoints)
Exemplo n.º 38
0
def make_gridcells(data):
    obs = GridCells()
    obs.cell_width = 1
    obs.cell_height = 1
    obs.header.frame_id = 'map'
    obs.cells = data
    return obs
Exemplo n.º 39
0
def newStartCallback(msg):
	global startpos
	global regen_map
	global start_pub
	global has_start
	global curmap
	global has_map
	if (has_map):
		#extract point from message
		point = msg.pose.pose.position
		print 'Got new starting position, regenerating path'
		#round point values to nearest integer
		startpos = point;
		print "x: ", startpos.x
		print "y: ", startpos.y
		#send rounded start point as a GridCells message to /lab3/astar/start
		start = GridCells()
		start.cell_width = curmap.info.resolution
		start.cell_height = curmap.info.resolution
		start.cells = [mapToWorldPos(curmap, worldToMapCell(curmap, startpos))]
		start.header.frame_id = 'map'
		start_pub.publish(start)
		#trigger A* path regeneration
		regen_map = 1
		#indicate that we have receive a start position
		has_start = 2
Exemplo n.º 40
0
def GCfromList(List):
    # from referenced Source and previous labs
    gridCells = GridCells()
    gridCells.cell_width = 1
    gridCells.cell_height = 1
    gridCells.cells = List
    gridCells.header.frame_id = 'map'
    return gridCells
Exemplo n.º 41
0
def publishGrid (nodeList, rez, where):
    pub = rospy.Publisher(where, GridCells, queue_size=1)
    gridCell = GridCells()
    gridCell.cell_width = rez
    gridCell.cell_height = rez
    gridCell.header.frame_id = "map"
    gridCell.cells = nodeList
    pub.publish(gridCell)
Exemplo n.º 42
0
def publishTwist(position):
    pub = rospy.Publisher('~gridCellData', GridCells)
    cells = GridCells()
    cells.cell_width = 1
    cells.cell_height = 1
    for i in range(0, len(position) - 1):
	cells.cells.append([position[i][0], position[i][1], 0])
    pub.publish(cells)
Exemplo n.º 43
0
def pubPath(path):
    trail = GridCells()
    trail.header.frame_id = "map"
    trail.cell_width = mapper.resolution
    trail.cell_height = mapper.resolution
    trail.cells = path
    
    trailPublisher.publish(trail)
Exemplo n.º 44
0
	def toGridCell(self, resolution, origin):
		grid = GridCells()
		grid.header.frame_id = "map"
		grid.cell_width = resolution
		grid.cell_height = resolution
		grid.cells.append(self.toRosPoint(resolution,origin))
		
		return grid
Exemplo n.º 45
0
def publishGrid(nodeList):
    pub = rospy.Publisher('/path', GridCells, queue_size=1)
    gridCell = GridCells()
    gridCell.cell_width = mapRez
    gridCell.cell_height = mapRez
    gridCell.header.frame_id = "map"
    gridCell.cells = nodeList
    pub.publish(gridCell)
Exemplo n.º 46
0
def publishGrid(nodeList, where):
    pub = rospy.Publisher(where, GridCells, queue_size=1)
    gridCell = GridCells()
    gridCell.cell_width = rmap.rez
    gridCell.cell_height = rmap.rez
    gridCell.header.frame_id = "map"
    gridCell.cells = nodeList
    pub.publish(gridCell)
Exemplo n.º 47
0
def publishGrid (nodeList):
	pub = rospy.Publisher('/path', GridCells, queue_size=1)
	gridCell = GridCells()
        gridCell.cell_width = mapRez
        gridCell.cell_height = mapRez
        gridCell.header.frame_id = "map"
        gridCell.cells = nodeList
	pub.publish(gridCell)
Exemplo n.º 48
0
def publishCells(grid):

    #begin test:
    testGrid = grid
    #   grid = expandObsticals(testGrid)

    #end test:

    global pub
    row = ""
    global navigable_gridpos

    navigable_gridpos = []
    unnavigable_gridpos = []

    # resolution and offset of the map
    k = 0
    cells = GridCells()
    cells.header.frame_id = 'map'
    cells.cell_width = resolution
    cells.cell_height = resolution

    for i in range(0, height):  # height should be set to hieght of grid
        for j in range(0, width):  # width should be set to width of grid
            #print grid[] # used for debugging

            if (grid[k] < 50):
                row = row + "    "
                navigable_gridpos.append((j, i))
            if (grid[k] == 100):
                unnavigable_gridpos.append((j, i))
                if (i > 1):
                    point = getPoint((j, i - 1))
                    cells.cells.append(point)
                    unnavigable_gridpos.append(point)

                if (j > 1):
                    point = getPoint((j - 1, i))
                    cells.cells.append(point)
                    unnavigable_gridpos.append(point)

                if (width > j):
                    point = getPoint((j + 1, i))
                    cells.cells.append(point)
                    unnavigable_gridpos.append(point)
                if (height > i):
                    point = getPoint((j, i + 1))
                    cells.cells.append(point)
                    unnavigable_gridpos.append(point)


#                row = row + " " + str (grid[k]);
#                point = getPoint((j, i))
#                cells.cells.append(point)
            k = k + 1
        print "\n" + row
        row = ""
    pub.publish(cells)
Exemplo n.º 49
0
def Visualize_Path(path):   #Function to publish path
    print "visualizing path"
    Path = rospy.Publisher('/Path', GridCells)
    Path_GridCells = GridCells()
    Path_GridCells.header = HEADER
    Path_GridCells.cell_width = RESOLUTION
    Path_GridCells.cell_height = RESOLUTION
    Path_GridCells.cells = path
    Path.publish(Path_GridCells)
Exemplo n.º 50
0
def aSTAR(start,goal):
    global currentPoint
    global mapData
    global cells_met
    cells_met = GridCells();
    cells_met.header.frame_id = 'map'
    cells_met.cell_width = 0.3 #change based off grid size
    cells_met.cell_height = 0.3 #change based off grid size
    startPos = start.pose.position
    goalPos = goal.position

    #publishObjectCells(mapData)
    while(not doneFlag and not rospy.is_shutdown()):
        #lowest
        #direction
        costFront=9001
        costLeft=9001
        costRight=9001
        costBack=9001 #derp I capitalized C by accident##############################################################################################################
        hugh = heuristic(startPos,goalPos)
        readCurrentPos()
        mrRogers(currentPoint)
        if(not cellOccupied(front)):#im adding not cause I think I did logic wrong
            costFront=distanceFormula(currentPoint,front)#seems to fix it but still i think there is a problem with if it is occupied nvm(its capitalization)
            costFront+=heuristic(front,goalPos)
        if(not cellOccupied(back)):
            costBack=distanceFormula(currentPoint,back)
            costBack+=heuristic(back,goalPos)
        if(not cellOccupied(left)):
            costLeft=distanceFormula(currentPoint,left)
            costLeft+=heuristic(left,goalPos)
        if(not cellOccupied(right)):
            costRight=distanceFormula(currentPoint,right)
            costRight+=heuristic(right,goalPos)
        if(costFront<costLeft):
            lowest=costFront
            direction= "front";
        else:
            lowest=costLeft
            direction="left"
        if(lowest>costBack):
            lowest=costBack
            direction="back"
        if(lowest>costRight):
            lowest=costRight
            direction="right"

        if(direction=="front"):
            goFront()
        elif(direction=="right"):
            goRight()
        elif(direction=="back"):
            goBack()
        elif(direction=="left"):
            goLeft()
    print "DONE"
Exemplo n.º 51
0
    def publish_path(self):
        # publish path topic
        cs = GridCells()

        cs.header.frame_id = 'map'
        cs.cell_width = self.map_res
        cs.cell_height = self.map_res
        cs.cells = self.path_cells

        self.path_pub.publish(cs)
Exemplo n.º 52
0
def publishFrontierCells(frontier, pub):
    
    cells = GridCells()
    cells.header.frame_id = 'map'
    cells.cell_width = 0.05
    cells.cell_height = 0.05
    for i in range(0, len(frontier)):
	point = frontier[i]
	cells.cells.append(point)
    pub.publish(cells)
Exemplo n.º 53
0
def rvizPath(cell_list, worldMap):
    global path_pub
    path_GC = GridCells()
    path_GC.cell_width = worldMap.info.resolution
    path_GC.cell_height = worldMap.info.resolution
    path_GC.cells = []
    for cell in cell_list:
        path_GC.cells.append(gridToWorld(cell, worldMap))
    path_GC.header.frame_id = 'map'
    path_pub.publish(path_GC)
Exemplo n.º 54
0
    def publish_expanded(self):
        # publish expanded topic
        cs = GridCells()

        cs.header.frame_id = 'map'
        cs.cell_width = self.map_res
        cs.cell_height = self.map_res
        cs.cells = self.exp_cells

        self.exp_pub.publish(cs)
Exemplo n.º 55
0
    def publish_frontier(self):
        # pubish frontier topic
        cs = GridCells()

        cs.header.frame_id = 'map'
        cs.cell_width = self.map_res
        cs.cell_height = self.map_res
        cs.cells = self.front_cells

        self.front_pub.publish(cs)
Exemplo n.º 56
0
def rvizFrontier(cell_list, w_map):
    global frontier_pub

    frontier_GC = GridCells()
    frontier_GC.cell_width = w_map.info.resolution
    frontier_GC.cell_height = w_map.info.resolution
    frontier_GC.cells = []
    for cell in cell_list:
        frontier_GC.cells.append(lab_4.gridToWorld(i_to_p(cell, w_map.info.width), w_map))
    frontier_GC.header.frame_id = 'map'
    frontier_pub.publish(frontier_GC)
Exemplo n.º 57
0
def rvizUnexplored(cell_list, w_map):
    global unexplored_pub

    unexplored_GC = GridCells()
    unexplored_GC.cell_width = w_map.info.resolution
    unexplored_GC.cell_height = w_map.info.resolution
    unexplored_GC.cells = []
    for cell in cell_list:
        unexplored_GC.cells.append(lab_4.gridToWorld(i_to_p(cell, w_map.info.width), w_map))
    unexplored_GC.header.frame_id = 'map'
    unexplored_pub.publish(unexplored_GC)
def createGridCellsMessage():
    gridCells = GridCells()
    gridCells.header.seq = 0
    gridCells.header.stamp = rospy.Time.now()
    gridCells.header.frame_id = "map"
    gridCells.cell_width = originalGridMessage.info.resolution
    gridCells.cell_height = originalGridMessage.info.resolution

    gridCells.cells = []

    return gridCells