rospy获取nav_msgs OccupancyGrid的map话题并可视化 - maohaihua/ros_study GitHub Wiki

https://blog.csdn.net/hehedadaq/article/details/82718341?utm_source=blogxgwz7

rospy获取nav_msgs/OccupancyGrid的map话题并可视化
    前言：
    先上参考资料：
    我的步骤：
        1、启动整体机器人的文件
        2、rospy订阅/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

总结：

rospy获取nav_msgs OccupancyGrid的map话题并可视化 - maohaihua/ros_study GitHub Wiki

plt.imshow(map)

plt.show()

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