如何订阅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