1. 缘由

3月一直在调试设备，还要持续一段时间，没空余时间
工作上也遇到很多非技术问题
同事的帮忙，最近状态才调整过来，看淡了

最近也有人来私信来问 ROS Rviz 怎么显示轨迹
在网上的资料基本都是C++版本的
“你行，我也行”，这里就补一下Python版本

2. Python实现

#!/usr/bin/env python3


import rospy
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped, Quaternion
import tf
import math

# 起始运动状态
x, y, th = 0, 0, 0


def DataUpdating(path_pub, path_record):
    """
    数据更新函数
    """
    global x, y, th

    # 时间戳
    current_time = rospy.Time.now()

    # 发布tf
    br = tf.TransformBroadcaster()
    br.sendTransform((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0),
                     rospy.Time.now(), "odom", "map")

    # 配置运动
    dt = 1 / 50
    vx = 0.25
    vy = 0.25
    vth = 0.2
    delta_x = (vx * math.cos(th) - vy * math.sin(th)) * dt
    delta_y = (vx * math.sin(th) + vy * math.cos(th)) * dt
    delta_th = vth * dt
    x += delta_x
    y += delta_y
    th += delta_th

    # 四元素转换
    quat = tf.transformations.quaternion_from_euler(0, 0, th)

    # 配置姿态
    pose = PoseStamped()
    pose.header.stamp = current_time
    pose.header.frame_id = 'odom'
    pose.pose.position.x = x
    pose.pose.position.y = y
    pose.pose.orientation.x = quat[0]
    pose.pose.orientation.y = quat[1]
    pose.pose.orientation.z = quat[2]
    pose.pose.orientation.w = quat[3]

    # 配置路径
    path_record.header.stamp = current_time
    path_record.header.frame_id = 'odom'
    path_record.poses.append(pose)

    # 路径数量限制
    if len(path_record.poses) > 1000:
        path_record.poses.pop(0)

    # 发布路径
    path_pub.publish(path_record)


def node():
    """
    节点启动函数
    """
    try:

        # 初始化节点path
        rospy.init_node('PathRecord')

        # 定义发布器 path_pub 发布 trajectory
        path_pub = rospy.Publisher('trajectory', Path, queue_size=50)

        # 初始化循环频率
        rate = rospy.Rate(50)

        # 定义路径记录
        path_record = Path()

        # 在程序没退出的情况下
        while not rospy.is_shutdown():
            # 数据更新函数
            DataUpdating(path_pub, path_record)

            # 休眠
            rate.sleep()

    except rospy.ROSInterruptException:
        pass


if __name__ == '__main__':
    node()

3. 效果

怎么启动上述文件就不再描述了
直接上图看效果