Python 调用 OccupancyGrid 处理栅格地图（一）

栅格地图

• 栅格地图中每一个小格的坐标对应一位数组中的一个数据。实际地图中某点坐标 (x,y) 对应栅格地图中坐标 [x*map.info.width+y] 。

• 栅格地图中每个小格的颜色代表的是该格被占用的概率，颜色越深表明被占用率越高，也就是有更高的可能是障碍物/建筑物。概率的范围是[0,100]，如果是未知区域可以将该位置的概率设置为-1。

创建订阅者（利用Python解析bag文件）

dealOG.py

#!/usr/bin/env python
#coding=utf-8


import rospy
from nav_msgs.msg import OccupancyGrid  # map 数据


class TestMap(object):
    def __init__(self):
        # Give the node a name
        rospy.init_node('test_map', anonymous=False)

        rospy.Subscriber("/map", OccupancyGrid, self.get_map_data, queue_size=10)

    def get_map_data(self, msg):
        height = msg.info.height  # pgm 图片属性的像素值（栅格地图初始化大小——高度上的珊格个数）
        width = msg.info.width  # pgm 图片属性的像素值（栅格地图初始化大小中的宽——宽度上的珊格个数）
        origin_x = msg.info.origin.position.x  # ROS 建图的 origin
        origin_y = msg.info.origin.position.y  # ROS 建图的 origin
        resolution = msg.info.resolution  # ROS 建图的分辨率 resolution（栅格地图分辨率对应栅格地图中一小格的长和宽）
        data_size = len(msg.data)
        print(height, width, origin_x, origin_y, resolution, data_size)
	# (480, 608, -15.4, -12.200000000000001, 0.05000000074505806, 291840)
        
        # 保存数据为 txt 文件
        with open('data.txt', 'w') as f:
            f.write(str(msg.data))
       	print('保存成功')


if __name__ == "__main__":
    try:
        TestMap()
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo('Test Map terminated.')

运行可执行程序

# 打开终端一，启动 ros master
$ roscore

# 打开终端二，回放数据包
$ rosbag play map_test.bag 

# 打开终端二，创建订阅者
$ rosru