Python 调用 OccupancyGrid 处理栅格地图
创建订阅者并处理数据(利用Python解析bag文件)
"""
Created on 2020-06-06
Updated on 2020-06-06
@author: 小小磊
@requirements: Ubuntu 16.04.6 LTS, Python 3.5, ROS Version: kinetic 1.12.13
@decription: 删除地图中无意义的未知区域
@ref: https://www.cnblogs.com/silence-cho/p/10926248.html
"""
import numpy as np
import copy
import rospy
from nav_msgs.msg import OccupancyGrid
import cv2
from collections import defaultdict
class Reducing(object):
def __init__(self):
rospy.init_node('map_reducing', anonymous=False)
self.last_map = OccupancyGrid()
self.map_adj_pub = rospy.Publisher('/map_reducing', OccupancyGrid, queue_size=10,
latch=True)
rospy.Subscriber("/map", OccupancyGrid, self.map_callback, queue_size=10)
def map_callback(self, msg):
map_adj = OccupancyGrid()
map_adj.header.frame_id = '/map'
map_adj.header.stamp = rospy.Time.now()
map_adj.info = msg.info
original = msg.data
print("原始地图信息:")
print(len(original), map_adj.info.width * map_adj.info.height, map_adj.info.width, map_adj.info.height,
msg.info.origin.position.x, msg.info.origin.position.y)
img_tmp = self.map2grayscale(original)
img_gray = np.array(img_tmp).reshape(map_adj.info.height, map_adj.info.width)
img_gray = img_gray.astype(np.uint8)
ret, thresh = cv2.threshold(img_gray, 180, 255, cv2.THRESH_BINARY)
binary, contours, hierarchy = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
cnt = contours[0]
x, y, w, h = cv2.boundingRect(cnt)
print("原始地图转为灰度图后获取的最大外接矩形的信息:")
print(x, y, w, h)
h_min = y
h_max = y + h
w_min = x
w_max = x + w
print("最大外接矩形对应于地图数据的范围:")
print(h_min, h_max, w_min, w_max)
arr = np.array(original).reshape(map_adj.info.height, map_adj.info.width)
arr_a = arr[h_min:h_max]
new = arr_a[:, w_min:w_max]
new = new.flatten()
origin_index = h_min * map_adj.info.width + w_min
origin_point = point_of_index(map_adj.info, origin_index)
map_adj.info.width = w_max - w_min
map_adj.info.height = h_max - h_min
map_adj.info.origin.position.x = origin_point[0]
map_adj.info.origin.position.y = origin_point[1]
map_adj.data = new
print("剔除冗余数据后的地图信息:")
print(len(map_adj.data), map_adj.info.width * map_adj.info.height, map_adj.info.width, map_adj.info.height,
map_adj.info.origin.position.x, map_adj.info.origin.position.y)
"""
外接矩形:原点坐标不变
"""
if map_adj.info.origin.position.x == self.last_map.info.origin.position.x and \
map_adj.info.origin.position.y == self.last_map.info.origin.position.y and \
map_adj.info.width == self.last_map.info.width and \
map_adj.info.height == self.last_map.info.height and \
self.last_map is not None:
dd = defaultdict(list)
for i, v in enumerate(map_adj.data):
if map_adj.data[i] != self.last_map.data[i]:
dd[i] = v
print("外接矩形:原点坐标不变,宽不变,高不变,输出差异数据的位置和差异值:")
print(len(dd))
if map_adj.info.origin.position.x == self.last_map.info.origin.position.x and \
map_adj.info.origin.position.y == self.last_map.info.origin.position.y and \
map_adj.info.width == self.last_map.info.width and \
map_adj.info.height != self.last_map.info.height and \
self.last_map is not None:
dd = defaultdict(list)
for i, v in enumerate(map_adj.data):
if len(map_adj.data) > len(self.last_map.data):
if i < len(self.last_map.data):
if map_adj.data[i] != self.last_map.data[i]:
dd[i] = v
else:
dd[i] = v
else:
if i < len(map_adj.data):
if map_adj.data[i] != self.last_map.data[i]:
dd[i] = v
print("外接矩形:原点坐标不变,宽不变,高变,输出差异数据的位置和差异值:")
print(len(dd))
if map_adj.info.origin.position.x == self.last_map.info.origin.position.x and \
map_adj.info.origin.position.y == self.last_map.info.origin.position.y and \
map_adj.info.width != self.last_map.info.width and \
map_adj.info.height == self.last_map.info.height and \
self.last_map is not None:
dd = defaultdict(list)
for i, v in enumerate(map_adj.data):
if map_adj.info.width > self.last_map.info.width:
if i < self.last_map.info.width:
if map_adj.data[i] != self.last_map.data[i]:
dd[i] = v
elif self.last_map.info.width <= i < map_adj.info.width:
dd[i] = v
else:
if i % map_adj.info.width >= map_adj.info.width-self.last_map.info.width:
j = (int(i/map_adj.info.width))*self.last_map.info.width + \
((i % map_adj.info.width)-(
map_adj.info.width-self.last_map.info.width))
if map_adj.data[i] != self.last_map.data[j]:
dd[i] = v
else:
dd[i] = v
else:
if i < map_adj.info.width:
if map_adj.data[i] != self.last_map.data[i]:
dd[i] = v
else:
j = (int(i/map_adj.info.width))*self.last_map.info.width + \
((i % map_adj.info.width)+(
self.last_map.info.width-map_adj.info.width))
if map_adj.data[i] != self.last_map.data[j]:
dd[i] = v
print("外接矩形:原点坐标不变,宽变,高不变,输出差异数据的位置和差异值:")
print(len(dd))
if map_adj.info.origin.position.x == self.last_map.info.origin.position.x and \
map_adj.info.origin.position.y == self.last_map.info.origin.position.y and \
map_adj.info.width != self.last_map.info.width and \
map_adj.info.height != self.last_map.info.height and \
self.last_map is not None:
dd = defaultdict(list)
for i, v in enumerate(map_adj.data):
if map_adj.info.width > self.last_map.info.width:
if map_adj.info.height > self.last_map.info.height:
if i < self.last_map.info.height*map_adj.info.width:
if i < self.last_map.info.width:
if map_adj.data[i] != self.last_map.data[i]:
dd[i] = v
elif self.last_map.info.width <= i < map_adj.info.width:
dd[i] = v
else:
if i % map_adj.info.width >= map_adj.info.width-self.last_map.info.width:
j = (int(i/map_adj.info.width))*self.last_map.info.width + \
((i % map_adj.info.width)-(
map_adj.info.width-self.last_map.info.width))
if map_adj.data[i] != self.last_map.data[j]:
dd[i] = v
else:
dd[i] = v
else:
dd[i] = v
else:
if i < self.last_map.info.width:
if map_adj.data[i] != self.last_map.data[i]:
dd[i] = v
elif self.last_map.info.width <= i < map_adj.info.width:
dd[i] = v
else:
if i % map_adj.info.width >= map_adj.info.width-self.last_map.info.width:
j = (int(i/map_adj.info.width))*self.last_map.info.width + \
((i % map_adj.info.width)-(
map_adj.info.width-self.last_map.info.width))
if map_adj.data[i] != self.last_map.data[j]:
dd[i] = v
else:
dd[i] = v
else:
if map_adj.info.height > self.last_map.info.height:
if i < map_adj.info.height * map_adj.info.width:
if i < map_adj.info.width:
if map_adj.data[i] != self.last_map.data[i]:
dd[i] = v
else:
j = (int(i/map_adj.info.width))*self.last_map.info.width + \
((i % map_adj.info.width)+(
self.last_map.info.width-map_adj.info.width))
if map_adj.data[i] != self.last_map.data[j]:
dd[i] = v
else:
dd[i] = v
else:
if i < map_adj.info.width:
if map_adj.data[i] != self.last_map.data[i]:
dd[i] = v
else:
j = (int(i/map_adj.info.width))*self.last_map.info.width + \
((i % map_adj.info.width)+(
self.last_map.info.width-map_adj.info.width))
if map_adj.data[i] != self.last_map.data[j]:
dd[i] = v
print("外接矩形:原点坐标不变,宽变,高变,输出差异数据的位置和差异值:")
print(len(dd))
"""
外接矩形:原点坐标变(外接矩形数据不匹配,输出变化后的外接矩形内的全部数据)
"""
if map_adj.info.origin.position.x != self.last_map.info.origin.position.x or \
map_adj.info.origin.position.y != self.last_map.info.origin.position.y:
print("外接矩形:原点坐标变,输出变化后的外接矩形内的全部数据:")
print(len(map_adj.data))
self.last_map = map_adj
print("========= end =========")
@staticmethod
def map2grayscale(data):
data_new = copy.deepcopy(list(data))
for idx, value in enumerate(data):
if value == -1:
data_new[idx] = 0
else:
data_new[idx] = 255
return data_new
def point_of_index(map_info, i):
y = map_info.origin.position.y + (i / map_info.width) * map_info.resolution
x = map_info.origin.position.x + (i - (i / map_info.width) * map_info.width) * map_info.resolution
return [x, y]
if __name__ == '__main__':
try:
Reducing()
rospy.spin()
except rospy.ROSInterruptException:
rospy.loginfo('Reducing /map redundant data terminated.')
运行可执行程序
$ roscore
$ rosbag play map_test.bag
$ chmod 777 *
$ rosrun beginner_tutorials Map_Reducing.py
链接:
Python 调用 OccupancyGrid 处理栅格地图(一)