如何订阅 AMCL pose 并将其打印为 ground truth pose 率？

Question

我想打印 /amcl_pose 与 ground_truth/state 默认时间步相同。因此，我开发了代码来订阅 /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()

Answer 1

您遇到的问题是在回调中循环。这实际上将永远阻塞并导致您只能看到一个值。相反，您应该在回调中缓存该值，然后在主循环中简单地读取它。请注意，这也可以用来替换 rospy.spin()。同样，这不是以特定速率休眠的正确语法。您所做的只是创建一个速率对象，而不是用它来休眠。

#! /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 pose 并将其打印为 ground truth pose 率？

How to subscribe AMCL pose and print it as the rate of ground truth pose?

python

ros