【ROS】python rospy定义一个节点实现CostMap的订阅并用Opencv作图

写在前面：

  最近机器人导航做移动目标点选择的时候，经常会选到障碍点上，导致指令发出之后机器人规划不出到达的路径从而在原地转圈。解决思路是把CostMap读出来，之后可以参考CostMap选择一个最近的无障碍点。所以，本篇的主要内容是python写一个Subscriber实现CostMap的订阅以及用opencv去把它画出来。

1. 关于ROS中的Topic和CostMap

  ROS中有多种通信的机制，我们这里用到的是Topic机制。我们可以通过rostopic list去查阅机器人当前正在进行的所有话题；通过rostopic list -v可以显示所有正在进行的话题的发布者和订阅者的详细信息。以locobot机器人的部分Topic为例：

  CostMap代价地图分Local和Global两张，分别用于局部路径规划和全局路径规划。我们这里以订阅GlobalCostMap为例，在通过rostopic list找到该话题后，可以通过rostopic info /locobot/move_base/global_costmap/costmap查阅到其数据类型。

  可以看到CostMap是一个nav_msgs/OccupancyGrid的类型，之后我们需要这样去import这个类。

from nav_msgs.msg import OccupancyGrid

  我们同样可以通过rosmsg show OccupancyGrid去查看其具体结构。每个字段更为详细的解释可以参考这篇博客：https://blog.csdn.net/tiancailx/article/details/112133226

  int8[] data中包含的就是这个CostMap(原本应该是一个二维的栅格地图)，在data中被拉伸成了一个一维数组存储。需要注意的是，每一个网格的值不再是我们熟知的RGB图像中的0-255，而是-1和0-100。-1代表未知，0代表无障碍，这个在我们之后用Opencv画CostMap的时候会作为颜色选择的判断条件。

2. 如何写一个Subscriber

  关于如何写一个订阅器去接收到CostMap, 可以参考这一篇文章。https://blog.csdn.net/Mr_Poohhhh/article/details/103797043
大概就是：

class MySubscriber:
    def __init__(self):
    	# 定义一个接收器
        self.__sub_ = rospy.Subscriber('topic to be subscribed', Sub msg type, self.callback)
        self.data = None
	
	# callback接收msg
    def callback(self, msg):
        assert isinstance(msg, Sub msg Type) 
        # 接收读取msg等之后一系列操作
      	data = msg

3. 完整代码

#!/usr/bin/env python
 
import rospy
import numpy as np
import copy
import cv2
from nav_msgs.msg import OccupancyGrid
import message_filters

class CostMap(object):
    """
    This is a class to get the Global Cost Map.
    """

    def __init__(self):
        """
        Constructor for Global Cost map class.
        :param configs: configurations for costmap
        :type configs: OccupancyGrid
        """
        self.occupancygrid_msg = None
        self.map_data = None
        self.width = None
        self.height = None
        
        # 刚刚通过rostopic list查阅出来的话题名
        costmap_topic = '/locobot/move_base/global_costmap/costmap'
        # 编写一个subscriber 参数分别为：
        # topic名
        # 消息类型
        # 绑定自己重命名的callback function
        self.costmap = rospy.Subscriber(costmap_topic, OccupancyGrid, self.get_cost_map)

    def get_cost_map(self, msg):
        """
        costmap subscriber's callback function.
        """
        self.occupancygrid_msg = msg
        self.map_data = msg.data
        self.width = msg.info.width
        self.height = msg.info.height
        
    def get_map(self):
        """
        This function returns the size and 2D costmap.
        :rtype: [int, int], np.ndarray
        """
        while(not self.occupancygrid_msg):
            continue
        
        width = copy.deepcopy(self.width)
        height = copy.deepcopy(self.height)
        size = [width, height]
        costmap = copy.deepcopy(self.map_data)
        
        return size, costmap

def draw_map(map_size, map_dat):
    """
        This function returns the RGB image perceived by the camera.
        :rtype: np.ndarray or None
    """
    row, col = map_size[1], map_size[0]  # 一维转二维，注意对应关系
    costmap = np.array(map_data)
    costmap = costmap.reshape((row, col))
    img = np.zeros((row, col, 3))
    for i in range(row):
        for j in range(col):
        	# 判断无障碍点、未知点和障碍点 画不同颜色
            if costmap[i][j] == 0:
                img[i][j] = [255, 255, 255]
            elif costmap[i][j] == -1:
                img[i][j] = [255, 0, 0]
            else:
                img[i][j] = [0, 0, 0]
    cv2.imshow('map',img)
    cv2.waitKey(0)

def main():
    print('Start')
    costmap = CostMap()
    
    print('Load...')
    map_size, map_data = costmap.get_map()
	
    draw_map(map_size, map_data)
    print('Finished!')
    
if __name__ == "__main__":
    main()

4. 可视化效果

  打开Rviz，可以看到小窗口我们用Opencv画出来的GlobalCostMap（配色有点丑，要用的话可以自己改改）和Rviz中的是一样的。最后还想说，其实Rviz中很多可视化也都是基于Topic去做订阅，我们想要获取信息的时候，其实Rviz里都显示了Topic的名字和消息中都包含什么数据类型。包括之后也可以尝试自己去写Publisher然后在Rviz中去订阅和可视化展现。今天就先到这里…