ROS : Map - 利用 opencv 删除无意义的未知区域

本文档主要是使用 opencv 对 ROS 地图数据进行操作的一次尝试。

整体思路

这里的思路很简单，利用 opencv 找到地图的外接矩形 ，取出该外接矩形的地图数据并发布出来。

代码实现 Python

需要注意的是 ROS 中 Map 是从左下角开始保存的，即左下角的索引为 0 ，而 opencv 图片左上角的索引为 0 。

#!/usr/bin/env python
# -*- coding: utf-8 -*-

"""
Created on 20-05-13

Updated on 20-05-13

@author: eln

@requirements: Ubuntu 16.04.6 LTS, Python 2.7.12 (default, Nov 12 2018, 14:36:49), ROS Version: kinetic 1.12.13

@decription: 删除无意义的未知区域
"""
import numpy as np
import copy
import cv2
import rospy
from nav_msgs.msg import OccupancyGrid


class Reducing(object):
    def __init__(self):
        rospy.init_node('map_reducing', anonymous=False)

        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(len(original), map_adj.info.width * map_adj.info.height, map_adj.info.width, map_adj.info.height)

        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)
        print(type(img_gray), img_gray.shape, img_gray.dtype)
        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(x, y, w, h)

        h_min = y
        h_max = y + h
        w_min = x
        w_max = x + w
        print(h_min, h_max, w_min, w_max)

        arr = np.array(original).reshape(map_adj.info.height, map_adj.info.width)
        print(type(arr), arr.shape, arr.dtype)
        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(len(map_adj.data), map_adj.info.width * map_adj.info.height, map_adj.info.width, map_adj.info.height)

        self.map_adj_pub.publish(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.')

仿真效果

修改后的地图：

原始地图：