rospy获取nav_msgs/OccupancyGrid的map话题并可视化
前言:先上参考资料:我的步骤:1、启动整体机器人的文件2、rospy订阅/map话题代码解释:总结:前言:终于要开始撸ros了,之前断断续续的看了一些视频,和整了Kinect的一些东西,但是对于机器人的核心部分slam一点都不了解,现在终于要啃这块硬骨头了。因为也不知道该从哪里下手,准备获取实时的激光雷达数据图,但是好巧不巧的拿到了OccupancyGrid map的topic,所以先可视化这个吧
·
前言:
终于要开始撸ros了,之前断断续续的看了一些视频,和整了Kinect的一些东西,但是对于机器人的核心部分slam一点都不了解,现在终于要啃这块硬骨头了。
因为也不知道该从哪里下手,准备获取实时的激光雷达数据图,但是好巧不巧的拿到了OccupancyGrid map的topic,所以先可视化这个吧,明天可视化雷达数据吧。
先上参考资料:
python - Unable to publish a subscribed topic in rospy - Stack Overflow
摘选这段代码:
#!/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
参考链接二:
[ROS Q&A] How to know the Pose of a robot (Python)? | The Construct
这段得翻墙才能看,我摘取的这段代码:
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
总结:
基本上就先是这样。
更多推荐
所有评论(0)