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中去订阅和可视化展现。今天就先到这里…

在这里插入图片描述

版权声明:本文内容由互联网用户自发贡献,该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 dio@foxmail.com 举报,一经查实,本站将立刻删除。

相关推荐


学习编程是顺着互联网的发展潮流,是一件好事。新手如何学习编程?其实不难,不过在学习编程之前你得先了解你的目的是什么?这个很重要,因为目的决定你的发展方向、决定你的发展速度。
IT行业是什么工作做什么?IT行业的工作有:产品策划类、页面设计类、前端与移动、开发与测试、营销推广类、数据运营类、运营维护类、游戏相关类等,根据不同的分类下面有细分了不同的岗位。
女生学Java好就业吗?女生适合学Java编程吗?目前有不少女生学习Java开发,但要结合自身的情况,先了解自己适不适合去学习Java,不要盲目的选择不适合自己的Java培训班进行学习。只要肯下功夫钻研,多看、多想、多练
Can’t connect to local MySQL server through socket \'/var/lib/mysql/mysql.sock问题 1.进入mysql路径
oracle基本命令 一、登录操作 1.管理员登录 # 管理员登录 sqlplus / as sysdba 2.普通用户登录
一、背景 因为项目中需要通北京网络,所以需要连vpn,但是服务器有时候会断掉,所以写个shell脚本每五分钟去判断是否连接,于是就有下面的shell脚本。
BETWEEN 操作符选取介于两个值之间的数据范围内的值。这些值可以是数值、文本或者日期。
假如你已经使用过苹果开发者中心上架app,你肯定知道在苹果开发者中心的web界面,无法直接提交ipa文件,而是需要使用第三方工具,将ipa文件上传到构建版本,开...
下面的 SQL 语句指定了两个别名,一个是 name 列的别名,一个是 country 列的别名。**提示:**如果列名称包含空格,要求使用双引号或方括号:
在使用H5混合开发的app打包后,需要将ipa文件上传到appstore进行发布,就需要去苹果开发者中心进行发布。​
+----+--------------+---------------------------+-------+---------+
数组的声明并不是声明一个个单独的变量,比如 number0、number1、...、number99,而是声明一个数组变量,比如 numbers,然后使用 nu...
第一步:到appuploader官网下载辅助工具和iCloud驱动,使用前面创建的AppID登录。
如需删除表中的列,请使用下面的语法(请注意,某些数据库系统不允许这种在数据库表中删除列的方式):
前不久在制作win11pe,制作了一版,1.26GB,太大了,不满意,想再裁剪下,发现这次dism mount正常,commit或discard巨慢,以前都很快...
赛门铁克各个版本概览:https://knowledge.broadcom.com/external/article?legacyId=tech163829
实测Python 3.6.6用pip 21.3.1,再高就报错了,Python 3.10.7用pip 22.3.1是可以的
Broadcom Corporation (博通公司,股票代号AVGO)是全球领先的有线和无线通信半导体公司。其产品实现向家庭、 办公室和移动环境以及在这些环境...
发现个问题,server2016上安装了c4d这些版本,低版本的正常显示窗格,但红色圈出的高版本c4d打开后不显示窗格,
TAT:https://cloud.tencent.com/document/product/1340