在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,那么仿真的刷新率会越来越低,运动动画会越来越卡顿。该如何控制住这个函数的时长呢?
下面提供了代码模板,以供日后复用。代码已经过测试。
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()
略
略
略
略