学习更多嵌入式Linux、Qt以及嵌入式单片机关注公众号“爱玩嵌入式”:
源码链接:公众号"爱玩嵌入式"内回复qita0002获取
一个简单的python小脚本用来订阅/amcl_pose的坐标,代码如下:
#! /usr/bin/env python
#coding=utf-8
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped,PoseStamped
def PoseCallBack(msg):
data=""
#订阅到的坐标信息
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
#订阅到的四元数的信息,用来表示朝向
orien_z = msg.pose.pose.orientation.z
orien_w = msg.pose.pose.orientation.w
data = str(x) + "," + str(y)+ "," + str(orien_z)+ "," + str(orien_w)
rospy.loginfo(data)
def PoseSub():
rospy.init_node('pose_sub',anonymous=False)
#监控话题,并在回调函数中处理
rospy.Subscriber('/amcl_pose',PoseWithCovarianceStamped,PoseCallBack)
rospy.spin()
if __name__=='__main__':
try:
PoseSub()
except:
rospy.loginfo("出错,退出中...")
测试:启动roscore->启动amcl并调用一张地图->启动rviz->启动刚才写的脚本用来实时监控话题->点击rviz中的2D Pose Estimate。查看脚本监控窗口