Python GridCells.cells方法代码示例

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import cells [as 别名]
def generatePath(cmap, goal, parents, start, width):
	global path_pub
	global way_pub
	global curmap
	#Create GridCells() message to display path and waypoints in rviz
	path = GridCells()
	path.cell_width = curmap.info.resolution
	path.cell_height = curmap.info.resolution
	path.header.frame_id = 'map'
	way = GridCells()
	way.cell_width = curmap.info.resolution
	way.cell_height = curmap.info.resolution
	way.header.frame_id = 'map'
	waycells = [mapToWorldPos(cmap, goal)]
	#Create a list of cells starting with the start position
	cells = [mapToWorldPos(cmap, start)]
	#trace path from goal back to start
	current = parents[normalize(goal, width)]
	lastpt = goal
	last_ang = math.atan2((current.y-lastpt.y),(current.x-lastpt.x))
	while current != start:
	    cells.append(mapToWorldPos(cmap, current))
	    #if we change travel direction, add a waypoint
	    ang = math.atan2((current.y-lastpt.y),(current.x-lastpt.x))
	    if (abs(ang-last_ang) > 0.1):
		waycells.append(mapToWorldPos(cmap, lastpt))
	    last_ang = ang
	    lastpt = current
	    current = parents[normalize(current, width)]
	path.cells = cells
	way.cells = list(reversed(waycells))
	path_pub.publish(path)
	way_pub.publish(way)

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import cells [as 别名]
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)

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import cells [as 别名]
 def pub_cells(self, cells, cell_pub, cells_as_points=False):
     msg = GridCells()
     msg.cell_height = star.map_resolution
     msg.cell_width = star.map_resolution
     msg.header.frame_id = '/map'
     if cells_as_points:
         msg.cells = cells
     else:
         msg.cells = star.cell_coords_to_points( cells )
     
     cell_pub.publish(msg)

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import cells [as 别名]
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)

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import cells [as 别名]
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

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import cells [as 别名]
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

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import cells [as 别名]
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

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import cells [as 别名]
def MakeGridCellsFromList (cellList):
	gridCells = GridCells()
	gridCells.cell_width = .2
	gridCells.cell_height = .2
	gridCells.cells = cellList
	gridCells.header.frame_id = 'map'
	return gridCells

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import cells [as 别名]
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)

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import cells [as 别名]
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)

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import cells [as 别名]
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"

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import cells [as 别名]
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)

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import cells [as 别名]
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)

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import cells [as 别名]
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)

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import cells [as 别名]
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)