代码收藏家 ROS2 Python在rviz2中实时显示轨迹和点
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) > 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()这个程序实现了一个从原点出发的匀速圆周运动
这个圆周运动的速度是
圆周运动的半径是
下面详细解释一下运动轨迹为什么是一个圆：
我们知道向量的二维旋转（v向量逆时针旋转θ角以后得到v'向量）有下面三种等价写法，
每隔dt时间，运动质点的速度向量就逆时针旋转Δθ角，并沿着旋转后的速度矢量方向按照的速度前进，如上图所示
θ角（th）是从0开始由Δθ角不断累加得到的，任意时刻运动质点的速度向量就相当于将初始时刻的运动矢量逆时针旋转了θ角（th）。
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
ROS Rviz 显示轨迹 Python_ros2 rviz2显示posestamp-CSDN博客
path3d_node.py参考自 ，原文使用ROS2 C++
ROS2在rviz2中实时显示轨迹和点_tf2::createquaternionmsgfromyaw-CSDN博客
作者：xehuosh
