ROS中Rviz实时路径可视化的高效性能优化技巧

摘要

在ROS环境下使用Rviz时，随着时间的增加，通过Python可视化函数visualize_actual_path不断向Path消息中追加新的位置数据会导致处理和传输数据的时间显著增长，进而影响到rviz的刷新率和仿真流畅性。为了解决这一问题，本博客提出了两种应对方法，并提供了代码模板，实现高效地绘制和更新机器人的实际路径。

关键词

ROS、Rviz、实时路径可视化、Python、性能优化

前言

由于要在rviz中绘制实际路径，在每一次循环的时候，都要调用下面函数：

def visualize_actual_path(self, n, e):
        msg_pose = PoseStamped()
        msg_pose.pose.position.x, msg_pose.pose.position.y = n, e
        self.msg_path.poses.append(msg_pose)
        self.msg_path.header.frame_id = "NED"
        self.pub_path.publish(self.msg_path)

其中，

self.pub_path = rospy.Publisher("/topic_msgs_path", Path, queue_size=10)
self.msg_path = Path() 

这个函数的作用是可视化机器人的实际运动路径。然而随着时间的推移， self.msg_path.poses 数组会变得越来越大。由于在每次迭代中都会向该数组中追加新的位置，而没有清除旧的数据，因此发布到 ROS 主题 /topic_msgs_path 的 Path 消息将会越来越大，进而导致处理和传输这些消息的时间变长，三分钟以后，光是执行这个函数的时间就达到了一开始的将近10倍。如果开了Rviz，那么仿真的刷新率会越来越低，运动动画会越来越卡顿。该如何控制住这个函数的时长呢？

基本思路

1. 限制 Path 消息中的 Pose 数量，即只保留 self.msg_path.poses 数组中的最新 N 个位置。
2. 由于实际运动路径不需要每次循环（一般主程序的循环频率都是100HZ这个量级的，非常快）都作更新，完全可以在一个列表中积累一定数量的路径点，然后在一个较慢的频率下发布它们。

代码模板

下面提供了代码模板，以供日后复用。代码已经过测试。

def visualize_actual_path(self, n, e, MAX_PATH_LENGTH = 10000):
    msg_pose = PoseStamped()
    msg_pose.pose.position.x, msg_pose.pose.position.y = n, e
    if len(self.msg_path.poses) >= MAX_PATH_LENGTH:
        self.msg_path.poses.pop(0)  # 移除最老的位置信息
    self.msg_path.poses.append(msg_pose)
    self.msg_path.header.frame_id = "NED"
    # self.pub_path.publish(self.msg_path)
    if rospy.Time.now() - self.last_publish_time > rospy.Duration(1):  # 比如每1秒发布一次
        self.pub_path.publish(self.msg_path)
        self.last_publish_time = rospy.Time.now()

代码解释

略

结果

略

延伸阅读

略

参考文献

略