ROS2 Python在rviz2中实时显示轨迹和点（Markdown重制）

xehuosh

已于 2024-08-15 11:13:11 修改

阅读量967

点赞数 22

文章标签： python 机器人无人机嵌入式硬件 linux

于 2024-08-15 09:37:15 首次发布

本文链接：https://blog.csdn.net/m0_73885374/article/details/141206236

版权

ROS2提供了强大的rviz2工具，之前绘制轨迹使用过python中的matplotlib，还有Qt C++中的qwtplot3d，但是由于水平有限，效果都不尽人意。这次使用rviz2显示动态轨迹，觉得效果不错。

下面来看操作步骤：

首先打开终端，创建功能包path_pkg，添加依赖项rclpy、nav_msgs、geometry_msgs和tf2_geometry_msgs

cd ~/colcon_ws/src
ros2 pkg create --build-type ament_python path_pkg --dependencies rclpy nav_msgs geometry_msgs tf2_geometry_msgs

关闭终端

在path_pkg功能包的path_pkg文件夹下创建两个python文件：

path2d_node.py和path3d_node.py

path2d_node.py中输入

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped, Quaternion
import math

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

def rpy2quaternion(roll, pitch, yaw):
    x=math.sin(pitch/2)*math.sin(yaw/2)*math.cos(roll/2)+math.cos(pitch/2)*math.cos(yaw/2)*math.sin(roll/2)
    y=math.sin(pitch/2)*math.cos(yaw/2)*math.cos(roll/2)+math.cos(pitch/2)*math.sin(yaw/2)*math.sin(roll/2)
    z=math.cos(pitch/2)*math.sin(yaw/2)*math.cos(roll/2)-math.sin(pitch/2)*math.cos(yaw/2)*math.sin(roll/2)
    w=math.cos(pitch/2)*math.cos(yaw/2)*math.cos(roll/2)-math.sin(pitch/2)*math.sin(yaw/2)*math.sin(roll/2)
    return x, y, z, w

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

    # 时间戳
    current_time = node.get_clock().now()

    # 配置运动
    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

    # 四元素转换
    orientation = Quaternion()
    orientation.x, orientation.y, orientation.z, orientation.w = rpy2quaternion(0, 0, th)

    # 配置姿态
    pose = PoseStamped()
    pose.header.stamp = current_time.to_msg()
    pose.header.frame_id = 'odom'
    pose.pose.position.x = x
    pose.pose.position.y = y
    pose.pose.orientation = orientation

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

    # 路径数量限制
    if len(path_record.poses) &gt; 1000:
        path_record.poses.pop(0)
        
    node.get_logger().info(f'current_x: {x:.3f}')
    node.get_logger().info(f'current_y: {y:.3f}')
    node.get_logger().info(f'pos: ({x:.3f}, {y:.3f})')
    # 发布路径
    path_pub.publish(path_record)

def main():
    """
    主函数
    """
    rclpy.init(args=None)
    # 创建节点
    node = Node('path2d_node')
    # 定义发布器 path_pub 发布 trajectory
    path_pub = node.create_publisher(Path, 'trajectory', 10)
    # 定义路径记录
    path_record = Path()
    # 创建循环频率对象
    loop_rate = node.create_rate(50)
    # 在程序没退出的情况下
    while rclpy.ok():
        # 数据更新函数
        data_updating(node, path_pub, path_record)
        rclpy.spin_once(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

这个程序实现了一个从原点出发的匀速圆周运动

这个圆周运动的速度是 $\sqrt{v_{x}^{2} + v_{y}^{2}}$
圆周运动的半径是 $\frac{v}{\omega} = \frac{\sqrt{v_{x}^{2} + v_{y}^{2}}}{v_{th}}$

下面详细解释一下运动轨迹为什么是一个圆：

我们知道向量的二维旋转（v向量逆时针旋转θ角以后得到v’向量）有下面三种等价写法，
$\left\{ \begin{array}{l} \boldsymbol{v'}=\left( \begin{matrix} \cos \theta& -\sin \theta\\ \sin \theta& \cos \theta\\ \end{matrix} \right) \boldsymbol{v}\\ \boldsymbol{v'}=\left( \cos \theta +\text{i}\sin \theta \right) \boldsymbol{v}\\ \boldsymbol{v'}=\text{e}^{\text{i}\theta}\boldsymbol{v}\\ \end{array} \right.$

每隔 $\text{d}t$ 时间，运动质点的速度向量就逆时针旋转 $\Delta \theta$ 角，并沿着旋转后的速度矢量方向按照 $\sqrt{v_{x}^{2} + v_{y}^{2}}$ 的速度前进，如上图所示。

$\theta$ 角（th）是从 $0$ 开始由 $\Delta \theta$ 角不断累加得到的，任意时刻运动质点的速度向量就相当于将初始时刻的运动矢量逆时针旋转了 $\theta$ 角。
这样就得到了下面的代码：

delta_x = (vx * math.cos(th) - vy * math.sin(th)) * dt
delta_y = (vx * math.sin(th) + vy * math.cos(th)) * dt

同时在程序中路径一旦超过1000个点，就抛弃前面的点，所以看起来效果是一段旋转的圆环

path3d_node.py中输入

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Path
from geometry_msgs.msg import PointStamped, PoseStamped,Quaternion
import rclpy.time
import random
import math

def rpy2quaternion(roll, pitch, yaw):
    x=math.sin(pitch/2)*math.sin(yaw/2)*math.cos(roll/2)+math.cos(pitch/2)*math.cos(yaw/2)*math.sin(roll/2)
    y=math.sin(pitch/2)*math.cos(yaw/2)*math.cos(roll/2)+math.cos(pitch/2)*math.sin(yaw/2)*math.sin(roll/2)
    z=math.cos(pitch/2)*math.sin(yaw/2)*math.cos(roll/2)-math.sin(pitch/2)*math.cos(yaw/2)*math.sin(roll/2)
    w=math.cos(pitch/2)*math.cos(yaw/2)*math.cos(roll/2)-math.sin(pitch/2)*math.sin(yaw/2)*math.sin(roll/2)
    return x, y, z, w

def main():
    rclpy.init(args=None)
    node = Node('path3d_node')

    path_pub = node.create_publisher(Path, 'trajectory', 1)
    point_pub = node.create_publisher(PointStamped, 'point', 1)

    path = Path()
    path.header.stamp = node.get_clock().now().to_msg()
    path.header.frame_id = 'odom'

    x = 0.0
    y = 0.0
    z = 0.0
    th = 0.0
    vx = 0.1 + 0.2 * random.random()
    vy = -0.1 + 0.2 * random.random()
    vth = 0.1

    loop_rate = node.create_rate(10)  # 10 Hz

    while rclpy.ok():
        current_time = node.get_clock().now()

        dt = 1.0
        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
        z += 0.005
        th += delta_th

        this_pose_stamped = PoseStamped()
        this_point_stamped = PointStamped()

        this_pose_stamped.header.stamp = current_time.to_msg()
        this_pose_stamped.header.frame_id = 'odom'
        this_pose_stamped.pose.position.x = x
        this_pose_stamped.pose.position.y = y
        this_pose_stamped.pose.position.z = z

        this_point_stamped.header.stamp = current_time.to_msg()
        this_point_stamped.header.frame_id = 'odom'
        this_point_stamped.point.x = x
        this_point_stamped.point.y = y
        this_point_stamped.point.z = z

        node.get_logger().info(f'current_x: {x:.3f}')
        node.get_logger().info(f'current_y: {y:.3f}')
        node.get_logger().info(f'pos: ({x:.3f}, {y:.3f})')

        # 转换四元数
        orientation = Quaternion()
        orientation.x, orientation.y, orientation.z, orientation.w = rpy2quaternion(0, 0, th)
        this_pose_stamped.pose.orientation = orientation

        path.poses.append(this_pose_stamped)

        path_pub.publish(path)
        point_pub.publish(this_point_stamped)
        rclpy.spin_once(node)

        last_time = current_time
        loop_rate.sleep()

    rclpy.shutdown()

if __name__ == '__main__':
    main()

这段三维动态显示的代码本质上与前一个代码相同，就是在匀速圆周运动的同时，在 $z$ 轴上有一个速度，整体就是一个螺旋上升的效果，运动轨迹酷似一个弹簧。

在setup.py中添加：

entry_points={
        'console_scripts': [
            'path2d_node = path_pkg.path2d_node:main',
            'path3d_node = path_pkg.path3d_node:main',
        ],
    },

Ctrl+Alt+T打开终端，进行编译并运行节点

cd ~/colcon_ws
colcon build
source install/setup.bash
ros2 run path_pkg path2d_node

新打开一个终端运行

rviz2

进入rviz2界面以后将Fixed Frame从“map”改为“odom”

再点击左下角的Add按钮

点击“By topic”后选择“/trajectory”下的“Path”

点击“OK”后就能看到轨迹曲线了。

实时显示轨迹效果如下

另一个节点的运行方式相同，在rviz2中的配置与第一个节点完全一致

ros2 run path_pkg path3d_node

实时显示轨迹效果如下

path2d_node.py参考自，原文使用ROS1 Python
path_record

path3d_node.py参考自，原文使用ROS2 C++
showpath

xehuosh

关注

22
点赞
踩
12

收藏

觉得还不错? 一键收藏
0
评论
复制链接

分享到 QQ

分享到新浪微博

扫一扫