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()

代码解释

结果

延伸阅读

参考文献

你可能感兴趣的:(复用代码块,机器人,python,ROS,Rviz,可视化)