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. 效果

怎么啟動上述檔案就不再描述了
直接上圖看效果
Rviz 顯示軌跡

轉載請註明出處，本文鏈接：https://www.uj5u.com/houduan/272243.html

標籤：python

上一篇：LeetCode第一題，兩數之和

下一篇：Python 計算思維訓練——輸入和錯誤處理練習（一）