写python脚本订阅/amcl_pose坐标

源码链接:公众号"爱玩嵌入式"内回复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。查看脚本监控窗口
​

​