python在ROS中订阅地图消息,在opencv中可视化

在本例中,博主将介绍如何把OccupancyGrid的地图数据在opencv中可视化。

首先,我们阅读OccupancyGrid的文档,可以发现这个结构体主要分为两部分,其MapMetaData存储的是地图的元数据,比如宽度,高度,分辨率等信息。而int8[] data 是一个行主序的数组,数组的值为当前区域被占用的可能性,值域为[0,100],特别地,未知区域的值为-1。

下面是关键部分的示例代码,我们首先订阅一次/map 的消息,并且获取地图元信息,然后我们创建一个numpy数组,并且将可能性的值转换为0-255的颜色。不过在下面的代码中,我将忽略这一特性。所以在probability > 0 这直接写了color=0 ,实际上你可以使用color = (1 – probability) * 255 来代替这一行。最后,因为map_server生成的默认地图未知区域的颜色为205,所以空的numpy数组在一开始用了205来进行填充。

from nav_msgs.msg import OccupancyGrid
map_message = rospy.wait_for_message("/map", OccupancyGrid, 3)  # type: OccupancyGrid
map_width = map_message.info.width
map_height = map_message.info.height
resolution = map_message.info.resolution
origin = map_message.info.origin.position
map_data = np.reshape(map_message.data, (map_width, map_height))
map_rgb = np.zeros((map_width, map_height, 3), np.uint8)
map_rgb.fill(205)
for row in range(map_height):
    for col in range(map_width):
        probability = map_data[row, col]
        if probability == -1:
            continue
        if probability > 0:
            color = 0
        else:
            color = 255
        map_rgb[row, col] = (color, color, color)
map_rgb = cv2.flip(map_rgb, 0)