如何订阅AMCL位姿并按照ground truth位姿的频率打印输出

问题原因 #

• 你将死循环写在了/amcl_pose的订阅回调函数里，第一次接收到位姿消息后，程序就会一直卡在回调的while循环中，后续新的位姿消息再也无法触发回调更新全局变量，因此只能一直打印初始位姿
• 另外你定义的rospy.Rate(10)没有调用sleep()方法，实际不会生效，循环会以最高频率运行占用大量CPU

正确实现方案 #

实现逻辑为：用全局变量缓存最新的amcl_pose位姿，在主循环里按照ground_truth/state的默认频率定时打印缓存的最新位姿即可，参考代码如下：

#! /usr/bin/env python
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped   

# 全局变量缓存最新位姿
latest_pose = None

def pose_callback(msg):
    global latest_pose
    # 每次收到新的amcl_pose消息就更新缓存
    latest_pose = msg.pose.pose.position

if __name__ == "__main__":
    rospy.init_node("amcl_pose_printer")
    # 订阅amcl_pose话题
    sub = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, pose_callback)
    
    # 可先通过rostopic hz /ground_truth/state查询实际频率，替换下方参数，ground_truth默认一般为10Hz
    print_rate = rospy.Rate(10) 
    
    while not rospy.is_shutdown():
        # 确保已经收到过位姿数据再打印
        if latest_pose is not None:
            print(latest_pose)
        print_rate.sleep()

补充说明 #

如果需要严格对齐ground_truth/state的时间步长而非仅仅频率对齐，可以同时订阅ground_truth/state，每次收到ground_truth/state的消息时就打印当前缓存的最新amcl_pose位姿即可。

内容的提问来源于stack exchange，提问作者AmirulJ