如何订阅AMCL姿势并将其作为地面真实姿势的比率打印?

2022-06-06 00:00:00 python ros

问题描述

我要打印与默认时间步长相同的/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

相关文章