Python GridCells.header方法代码示例

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

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import header [as 别名]
def publishPoint(point, pub, mapInfo):
    gridCells = GridCells()

    gridCells = GridCells()
    gridCells.header = Header()
    gridCells.header.frame_id = "map"
    gridCells.cell_width = mapInfo.resolution
    gridCells.cell_height = mapInfo.resolution
    gridCells.cells = [point]

    pub.publish(gridCells)

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import header [as 别名]
def Frontier_init():
    Frontier = rospy.Publisher('/Frontier', GridCells)
    #find maximum point in empty space in the x and y drection
    Array_EmptyCells = PtArr_Empty
    OCCUPIED = False
    MAX_BOOL = False
    MIN_BOOL = False
    while Array_EmptyCells:
        MAX_Point = max(Array_EmptyCells)
        MIN_Point = min(Array_EmptyCells)
        Frontier_Array = []
        while True:
            #check If the point after the max_point.x is occupied
            for elem in PtArr_Occupied:
                if (numpy.close(MAX_Point.x + RESOLUTION, elem.x) and numpy.close(MAX_Point.y, elem.y)):
                    OCCUPIED = True
                    MAX_BOOL = True
                    break
            for elem in PtArr_Occupied:
                if (numpy.close(MIN_Point.x - RESOLUTION, elem.x) and numpy.close(MIN_Point.y, elem.y)):
                    MIN_BOOL = True
                    OCCUPIED = True
                    break
            if not OCCUPIED:
                if MAX_BOOL:
                    Frontier_Array.append(MAX_Point)    #add it to frontier array
                if MIN_BOOL:
                    Frontier_Array.append(MIN_Point)    #add it to frontier array
        Frontier_MaxPoint = Max_Point
        Frontier_MinPoint = Min_Point
        while True:
            Frontier_MaxPoint.y = Frontier_MaxPoint.y + RESOLUTION    #Next frontier point
            for elem in PtArr_Occupied:
                if numpy.close(Frontier_MaxPoint.y, elem.y ) and numpy.close(MAX_Point.x, elem.x) :
                    Frontier_Array.append(Frontier_MaxPoint)
                    break
            for elem in PtArr_Occupied:
                if numpy.close(Frontier_MinPoint.y, elem.y ) and numpy.close(Frontier_MinPoint.x, elem.x) :
                    Frontier_Array.append(Frontier_MinPoint)
                    break
            break
        #Remove row from frontier array
        for elem in Array_EmptyCells:
            if numpy.close(MAX_Point.y, elem.y):
                Array_EmptyCells.remove(elem)
        
    gridcells_frontier = GridCells()
    gridcells_frontier.header = HEADER
    gridcells_frontier.cell_width = RESOLUTION
    gridcells_frontier.cell_height = RESOLUTION
    gridcells_frontier.cells = Frontier_Array
    Frontier.publish(gridcells_frontier)
    return

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import header [as 别名]
def publishGridList(list_of_cells, mapInfo, pub):
    cells = []
    for x in list_of_cells:
        cells.append(gridToGlobal(x, mapInfo))
    
    gridCells = GridCells()
    gridCells.header = Header()
    gridCells.header.frame_id = "map"
    gridCells.cell_width = mapInfo.resolution
    gridCells.cell_height = mapInfo.resolution
    gridCells.cells = cells

    pub.publish(gridCells)

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import header [as 别名]
def Occupancy_Grid_init():  #initialize visual aid for Occupancy Grid
    global PtArr_Empty, PtArr_Unknown, PtArr_Occupied
    GridCells_Occupied = rospy.Publisher('/GridCellsOccupied', GridCells)
    GridCells_Unknown =  rospy.Publisher('/GridCellsUnknown', GridCells)
    GridCells_Empty = rospy.Publisher('/GridCellsEmpty', GridCells)
    PtArr_Occupied = []
    PtArr_Unknown = []
    PtArr_Empty = []
    for i in range(len(MAP_DATA)):
        row = round(((i / WIDTH)*RESOLUTION) + (RESOLUTION/2), 4) + ORIGIN_Y
        column = round(((i % WIDTH)*RESOLUTION) + (RESOLUTION/2), 4) + ORIGIN_X
        if MAP_DATA[i] > 50: #Occupied
           PtArr_Occupied.append(Point(column, row, 0))
        elif MAP_DATA[i] == -1: #Unknown
           PtArr_Unknown.append(Point(column, row, 0))
        else:  #Empty
           PtArr_Empty.append(Point(column, row, 0))      
    gridcells_Occupied = GridCells()
    gridcells_Occupied.header = HEADER
    gridcells_Occupied.cell_width = RESOLUTION
    gridcells_Occupied.cell_height = RESOLUTION
    gridcells_Occupied.cells = PtArr_Occupied
    
    gridcells_Unknown = GridCells()
    gridcells_Unknown.header = HEADER
    gridcells_Unknown.cell_width = RESOLUTION
    gridcells_Unknown.cell_height = RESOLUTION
    gridcells_Unknown.cells = PtArr_Unknown
    
    gridcells_Empty  = GridCells()
    gridcells_Empty.header = HEADER
    gridcells_Empty.cell_width = RESOLUTION
    gridcells_Empty.cell_height = RESOLUTION
    gridcells_Empty.cells = PtArr_Empty
    
    GridCells_Occupied.publish(gridcells_Occupied)
    GridCells_Unknown.publish(gridcells_Unknown)
    GridCells_Empty.publish(gridcells_Empty)

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import header [as 别名]
def pubMap(publisher, mapInfo, mapData):
    cells = []
    for i in range(mapInfo.width * mapInfo.height):
        if mapData[i] == 100:
            gridPoint = indexToGrid(i, mapInfo)
            gp = gridToGlobal(gridPoint, mapInfo)
            cells.append(gp)

    gridCells = GridCells()
    gridCells.header = Header()
    gridCells.header.frame_id = "map"
    gridCells.cell_width = mapInfo.resolution
    gridCells.cell_height = mapInfo.resolution
    gridCells.cells = cells
    publisher.publish(gridCells)

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import header [as 别名]
def Expanded_GridCell(cell):    #Expand Cell visually on rviz when debug is activated
    EXPANDED_PUBLISHER = rospy.Publisher('/Expanded', GridCells) #Expanded Grid Cells Publisher   
    cell_row = cell.x 
    cell_column = cell.y
    Expanded = []
    Expanded.append(Point(cell_row, cell_column, 0))
    Expanded_GridCell  = GridCells()
    Expanded_GridCell.header = HEADER
    Expanded_GridCell.cell_width = RESOLUTION
    Expanded_GridCell.cell_height = RESOLUTION
    Expanded_GridCell.cells = Expanded
    count = 0
    while count < 1500:
        EXPANDED_PUBLISHER.publish(Expanded_GridCell)
        count = count+1

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import header [as 别名]
def find_centroid():    #returns centroid of frontier
    Frontier_Centroid = rospy.Publisher('/FrontierCentroid', GridCells)
    x_c = y_c = count = 0
    for elem in PtArr_Empty:
        x_c = elem.x + x_c
        y_c = elem.y + y_c
        count = count + 1
    x_c = round_to_nearest_cell(x_c/count, RESOLUTION) + (RESOLUTION/2)
    y_c = round_to_nearest_cell(y_c/count, RESOLUTION) + (RESOLUTION/2)
    Centroid = Point(x_c, y_c, 0)
    gridcells_centroid  = GridCells()
    gridcells_centroid.header = HEADER
    gridcells_centroid.cell_width = RESOLUTION
    gridcells_centroid.cell_height = RESOLUTION
    gridcells_centroid.cells = [Centroid]
    Frontier_Centroid.publish(gridcells_centroid)
    Centroid_Pose = Pose()
    Centroid_Pose.position = Centroid
    Centroid_Pose.orientation.w = 0
    return Centroid_Pose

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import header [as 别名]
    def make_grid_cell(self):
        resolution = self.og.info.resolution
        width = self.og.info.width
        height = self.og.info.height
        offsetX = self.og.info.origin.position.x
        offsetY = self.og.info.origin.position.y

        # resolution and offset of the map
        k = 0
        cells = GridCells()
        cells.header = self.og.header
        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
                if self.onedmap[k] < self.threshold:
                    cells.cells.append(getPoint((j, i), resolution, offsetX, offsetY))
                k += 1

        return cells

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import header [as 别名]
def setGoal(msg):
    global goal, pub_goal, goalPoint
    point = msg.pose.position

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

    #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 = [goalPoint]
    gridCells.cells = cells

    pub_goal.publish(gridCells)
    print "goal set at"
    print goal

# 需要导入模块: from nav_msgs.msg import GridCells [as 别名]
# 或者: from nav_msgs.msg.GridCells import header [as 别名]
def readOdom(msg):
    global start, pub_start, mapInfo, mapData, pose, yaw, odom_list
    pose = msg.pose.pose
    point = msg.pose.pose.position

    quaternion = (
        pose.orientation.x,
        pose.orientation.y,
        pose.orientation.z,
        pose.orientation.w)
    euler = tf.transformations.euler_from_quaternion(quaternion)
    yaw = euler[2]

    start = globalToGrid(point, mapInfo)
    #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 = [gridToGlobal(start, mapInfo)]
    gridCells.cells = cells