Python msg.GridCells类代码示例

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)           

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)

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)

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)

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"

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

def MakeGridCellsFromList (cellList):
	gridCells = GridCells()
	gridCells.cell_width = .2
	gridCells.cell_height = .2
	gridCells.cells = cellList
	gridCells.header.frame_id = 'map'
	return gridCells

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'

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

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

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)

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)

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)

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)