rospy获取nav_msgs/OccupancyGrid的map话题并可视化

版权声明：转载请标注来源 https://blog.csdn.net/hehedadaq/article/details/82718341

rospy获取nav_msgs/OccupancyGrid的map话题并可视化

前言：

终于要开始撸ros了，之前断断续续的看了一些视频，和整了Kinect的一些东西，但是对于机器人的核心部分slam一点都不了解，现在终于要啃这块硬骨头了。

因为也不知道该从哪里下手，准备获取实时的激光雷达数据图，但是好巧不巧的拿到了OccupancyGrid map的topic，所以先可视化这个吧，明天可视化雷达数据吧。

先上参考资料：

https://stackoverflow.com/questions/36949518/unable-to-publish-a-subscribed-topic-in-rospy
摘选这段代码：

#!/usr/bin/env python

import rospy
import sys
import time
import os
from nav_msgs.msg import OccupancyGrid
from nav_msgs.msg import MapMetaData
from std_msgs.msg import String
from std_msgs.msg import Float64
from std_msgs.msg import Int8MultiArray

def callback(OccupancyGrid):
    mapdata.data = OccupancyGrid.data
    pub = rospy.Publisher('mapprob', Int8MultiArray, queue_size=10)
    pub.publish(mapdata)

def somethingCool():
    global mapdata
    mapdata = Int8MultiArray()
    rospy.init_node('test_name', anonymous=False)
    rospy.Subscriber("map", OccupancyGrid, callback)
    rospy.loginfo(mapdata)
    rospy.spin()


if __name__ == '__main__':
    try:
        somethingCool()
     except rospy.ROSInterruptException:
        pass

参考链接二：
http://www.theconstructsim.com/ros-qa-know-pose-robot-python/
这段得翻墙才能看，我摘取的这段代码：

import rospy
from nav_msgs.msg import Odometry

def callback(msg):
    print(msg.pose.pose)

rospy.init_node('check_odometry')
odom_sub = rospy.Subscriber('/odom', Odometry, callback)
rospy.spin()

我的步骤：

1、启动整体机器人的文件

这个没法说，因为整体的不是我写的，我只会launch，所以你们最好是在启动机器人的主程序之后，rostopic list 有 /map这个 话题。

2、rospy订阅/map话题

我们从上面的两个例程中可以看到，有几个关键的步骤：

• 导入消息类型
• 订阅话题，传到回调函数
• 回调函数进行数据处理
• 数据输出

其中消息类型的选择是有技巧的，回调函数也是比较复杂的，传递的参数和类型。
先上代码吧——

#!/usr/bin/env python

import rospy
import cv2

import numpy as np

#导入消息类型，OccupancyGrid是消息类型
from nav_msgs.msg import OccupancyGrid
import matplotlib.pyplot as plt

class Map(object):
  def __init__(self):
  #rospy订阅map话题，第二个是数据类型，第三个是回调函数
  #将订阅的数据传给回调函数，就是那个mapmsg变量
  #如果有话题来了，就直接调用callback函数
    self.map_sub = rospy.Subscriber("map",OccupancyGrid, self.callback)
    print "get map~"
    #下面输出的是地址，并不是数据
    print self.map_sub

#回调函数的定义，传了mapmsg
  def callback(self,mapmsg):
    try:
      print "into callback"
      #主要是想拿到data，这里存的是地图的信息
      map = mapmsg.data
      #下面是tuple类型
      print type(map)
      #变成可以画图的numpy格式
      map = np.array(map)
      #下面输出的是(368466,)，明显不能画图
      print map.shape
      #需要reshape，将上面的数字在线因数分解，然后算出了两个最大因数
      #于是就大概是这样：
      map = map.reshape((651,566))
      print map
      #可以看到大部分的值是-1，所以需要把值规整一下
      row,col = map.shape
      print row,col
      tem = np.zeros((row,col))
      for i in range(row):
        for j in range(col):
          if(map[i,j]==-1):
             tem[i,j]=255
          else:
             tem[i,j]=map[i,j]
      print map.shape
      cv2.imshow("map",tem)
      cv2.waitKey(0)
#      plt.imshow(map)
#      plt.show()
    except Exception,e:
      print e
      rospy.loginfo('convert rgb image error')

  def getImage():
    return self.rgb_image

def main(_):
  rospy.init_node('map',anonymous=True)
  v=Map()
  rospy.spin()

if __name__=='__main__':
  main('_')

代码解释：

1、查看OccupancyGrid的消息类型

rosmsg show OccupancyGrid

总结：

基本上就先是这样，感觉能把这些乱七八糟的数据，可视化还是很开心的，希望明天能拿到雷达可视化的数据。

扫描二维码关注公众号，回复： 3649799 查看本文章