Write before:

   recently, when the robot navigates to select the moving target point, it often selects the obstacle point, resulting in the robot cannot plan the path to reach after the command is issued, so it turns around in situ. The solution is to read out the CostMap, and then select a nearest barrier free point with reference to the CostMap.
   therefore, the main content of this article is to write a Subscriber in python to implement the subscription of CostMap and draw it with opencv.

1. About Topic and CostMap in ROS

   there are many communication mechanisms in ROS. We use Topic mechanism here. We can check all the current topics of the robot through rostatic list; The details of publishers and subscribers of all ongoing topics can be displayed through rostatic list - V. Take some topics of locobot robot as an example:

  CostMap cost map is divided into Local and Global maps, which are used for Local path planning and Global path planning respectively. Let's take the subscription to GlobalCostMap as an example. After finding the topic through rostatic list, you can click rostatic info / locobot / move_ base/Global_ CostMap / CostMap looks up its data type.

    you can see that CostMap is a nav_msgs/OccupancyGrid. After that, we need to import this class like this.

from nav_msgs.msg import OccupancyGrid

   we can also view its specific structure through rosmsg show OccupancyGrid. For more detailed explanation of each field, please refer to this blog: https://blog.csdn.net/tiancailx/article/details/112133226

    int8[] data contains this CostMap (originally a two-dimensional grid map), which is stretched into a one-dimensional array for storage in data. It should be noted that the value of each grid is no longer 0-255 in the well-known RGB image, but - 1 and 0-100- 1 means unknown, 0 means barrier free. This will be used as the judgment condition for color selection when we draw CostMap with Opencv.

2. How to write a Subscriber

   for how to write a subscriber to receive CostMap, please refer to this article. https://blog.csdn.net/Mr_Poohhhh/article/details/103797043
Probably:

class MySubscriber:
    def __init__(self):
    	# Define a receiver
        self.__sub_ = rospy.Subscriber('topic to be subscribed', Sub msg type, self.callback)
        self.data = None
	
	# callback receive msg
    def callback(self, msg):
        assert isinstance(msg, Sub msg Type) 
        # A series of operations after receiving and reading msg
      	data = msg

3. Complete code

#!/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
        
        # The topic name just found through rostatic list
        costmap_topic = '/locobot/move_base/global_costmap/costmap'
        # Write a subscriber parameter as follows:
        # topic name
        # Message type:
        # Bind the callback function renamed by yourself
        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]  # One dimensional to two-dimensional, pay attention to the corresponding relationship
    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):
        	# Judge the obstacle free point, unknown point and obstacle point, and draw different colors
            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. Visualization effect

   open Rviz, and you can see that the GlobalCostMap drawn by Opencv in the upper right corner is the same as that in Rviz.

   finally, I want to say that many visualizations in Rviz are subscribed based on Topic. When we want to obtain information, the name of Topic and the data type contained in the message are displayed in Rviz. After that, you can also try to write Publisher by yourself and subscribe and visualize it in Rviz. I'll stop here today