ROS 发布读取Odometry里程计话题并可视化

ROS 读取Odometry里程计话题并保存绘图

Odometry消息

在做导航定位式，通常最后的定位信息通过nav_msgs/Odometry消息类型发布，再将消息可视化在Rviz上。这种消息类型提供了机器人的位置和速度的综合信息，包括线速度和角速度，通常与时间戳和参考坐标系一起发布。
首先看一下消息定义：

Header header：标准的消息头，包含时间戳和坐标系信息。
string child_frame_id：机器人的坐标系名称。
geometry_msgs/PoseWithCovariance pose：包含位置和方向的信息，以及这些信息的协方差。
geometry_msgs/TwistWithCovariance twist：包含线速度和角速度的信息，以及这些信息的协方差。

如何发布和订阅Odometry消息

发布Odometry消息的程序

import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3

def publish_odometry():
    rospy.init_node('odometry_publisher')
    pub = rospy.Publisher('/odom', Odometry, queue_size=50)
    rate = rospy.Rate(10)  # 10 Hz

    while not rospy.is_shutdown():
        odom = Odometry()
        odom.header.stamp = rospy.Time.now()
        odom.header.frame_id = "odom"
        odom.child_frame_id = "base_link"

        # 填充位置信息
        odom.pose.pose.position = Point(0.0, 0.0, 0.0)
        odom.pose.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0)

        # 填充速度信息
        odom.twist.twist.linear = Vector3(