本文介绍了如何订阅AMCL姿势并将其作为地面真实姿势的比率打印？的处理方法，对大家解决问题具有一定的参考价值，需要的朋友们下面随着小编来一起学习吧！
问题描述
我要打印与默认时间步长相同的/amcl_pose。因此，我开发了订阅/amcl_pose并使用while not rospy.is_shutdown()在循环中打印它的代码，如下图所示。我之所以开发这个代码，是因为当我使用rostopic echo /amcl_pose时，它一次只打印一个姿势值，而不是基于默认的时间步长。但是，我的编码有问题。当我将机器人移动到另一个位置时，x，y，z值没有更新，仍然打印机器人姿势的初始值。如何打印与默认时间步长ground_truth/state相同的/amcl_pose值？
#! /usr/bin/env python
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped
def callback(msg):
global d
d = msg.pose.pose.position
while not rospy.is_shutdown():
print (d)
rospy.Rate(10)
rospy.init_node("read_pose")
sub = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, callback)
rospy.spin()
推荐答案
您遇到的问题是在回调内循环。这实际上将永远阻塞，并导致您只看到一个值。相反，您应该将值缓存在回调中，并只需在主循环中读取它。请注意，这也可以用来替换rospy.spin()。同样，这也不是以一定的速度睡眠的正确语法。您所做的只是创建一个Rate对象，而不是将其用于睡眠。
#! /usr/bin/env python
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped
def callback(msg):
global d
d = msg.pose.pose.position
d = None
rospy.init_node("read_pose")
sub = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, callback)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
if d is not None:
print(d)
rate.sleep() #Sleep at 10Hz
这篇关于如何订阅AMCL姿势并将其作为地面真实姿势的比率打印？的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持编程学习网！
本站部分内容来源互联网,如果有图片或者内容侵犯您的权益请联系我们删除！