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
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, map_size # 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