ROS中构建构建栅格地图

1. 要点

• 利用ros自带消息中的nav_msgs/OccupancyGrid作为消息类型

OccupancyGrid解析

• 利用python中PIL库处理png图片（缩放）

PIL库对图片的简单处理

2. 源码

#!/usr/bin/env python3
# coding : utf-8

import rospy
from PIL import Image
import numpy as np
import time
from nav_msgs.msg import OccupancyGrid

global width,height
width=20
height=20

im=Image.open(r"/home/xxy/catkin_ws4/src/map/scripts/map.png")#using Image class to open picture

resized_im=im.resize((width, height), Image.ANTIALIAS)#resize picture size
im_array=np.array(resized_im.convert('L'))#transform picture to array

#binarization
for i in range(width):
    for j in range(height):
        if im_array[i,j]<=150:
            im_array[i,j]=100
        else:
            im_array[i,j]=0

rospy.init_node("map_pub_node")
pub=rospy.Publisher("/map",OccupancyGrid,latch=True)
rate=rospy.Rate(1)
rospy.loginfo('start')
print(im_array)
time.sleep(1)
while not rospy.is_shutdown():
    msg=OccupancyGrid()

    msg.header.frame_id="map"
    msg.header.stamp=rospy.Time.now()
    msg.info.origin.position.x=0
    msg.info.origin.position.y=0
    msg.info.resolution=1.0     #grid size
    msg.info.width=width
    msg.info.height=height

    msg.data=[0]*width*height
    #map evaluate
    for i in range(0,height-1):
        for n in range(0,width-1):
            msg.data[i*height+n]=int(im_array[i,n])
    pub.publish(msg)
    rate.sleep()

3. 代码解析

im=Image.open(r"/home/xxy/catkin_ws4/src/map/scripts/map.png")

在路径前添加r使得得到正确路径。

for i in range(0,height-1):
        for n in range(0,width-1):
            msg.data[i*height+n]=int(im_array[i,n])

msg.data为一维数组