做机器人导航调试的时候除了在 rviz 里盯着机器人实时跑我经常需要把/odom发出来的轨迹单独拉出来画成一张图——比如比赛复盘、实验报告、论文插图或者单纯想对比两版参数谁的路径更平滑。这个需求看着不起眼真动手写的时候很多新手会卡在同一个地方rosbag 或者 rospy 回调里数据拿到了但 matplotlib 的图就是刷不出来。这篇文章我会从/odom消息结构讲起把离线画轨迹和实时画轨迹两条路都走一遍最后附上我踩过的坑和排查清单。适合刚接触 ROS 导航、想用 Python 做轨迹可视化的朋友也适合需要批量处理 bag 轨迹数据的同学。1. 拿到/odom先搞清楚这段数据值多少钱1.1/odom到底在说什么/odom是机器人导航里最常见的里程计话题。它的数据通常来自轮式编码器、IMU或者两者的融合结果描述的是机器人相对于起点odom 坐标系原点的位置和姿态变化。这里有个容易混淆的点/odom跟/map不是一回事。/odom是局部里程计以机器人启动时的位置为原点而且会随着打滑、累计误差慢慢漂移/map是地图坐标系一般由 AMCL 或类似的定位节点发布。我们画轨迹调试导航算法大多数时候画的是/odom因为它频率高、更新快能看出机器人实际走了什么路径。想看全局定位精度那才需要/map或者/amcl_pose。这个轨迹图能解决什么问题快速判断机器人走出的路径是不是符合预期比如有没有绕远路、原地转圈。对比不同参数下路径的平滑度和长度。验证里程计本身有没有问题比如轮子打滑导致的轨迹畸形。在实验报告、答辩 PPT 里放一张标准的路径对比图。1.2 为什么选 matplotlib 而不是盯着 rvizrviz 当然能显示轨迹但它的定位是实时调试工具不是出图工具。我选择用 matplotlib 的原因很实际可交付。matplotlib 直接输出 PNG/SVG/PDF放在论文或报告里格式可控。可叠加分析。可以把多组 bag 的轨迹画在同一张图上对比rviz 里做这种对比要频繁切换显示项效率低。可量化。拿到 x、y 数据之后路径长度、平均速度、最大偏差这些指标都可以顺手算出来这是 rviz 给不了的。依赖简单。只要 ROS 自带的 Python 环境能 import rospy 和 matplotlib这个方案就能跑不需要额外装图形工具。不过 matplotlib 也有明显的短板绘制大量点时性能一般、交互性不如 rviz。所以我的建议是实时调试看 rviz出数据、做分析用 matplotlib。两者互补不冲突。2. 动手前先拆消息结构字段、坐标系、四元数2.1 nav_msgs/Odometry 逐层看写代码之前我习惯先用rostopic echo /odom -n 1看一条实时消息。这里的字段路径直接决定后面取数的写法建议先自己敲一遍。rostopic echo /odom -n 1然后对着消息结构拆开看字段层级字段名类型含义headerstamp / frame_idstd_msgs/Header时间戳、坐标系名称pose.pose.positionx / y / zgeometry_msgs/Point位置单位米pose.pose.orientationx / y / z / wgeometry_msgs/Quaternion姿态四元数pose.covariance36 个数值float64[36]位置和姿态的协方差矩阵twist.twist.linearx / y / zgeometry_msgs/Vector3线速度单位 m/stwist.twist.angularx / y / zgeometry_msgs/Vector3角速度单位 rad/stwist.covariance36 个数值float64[36]速度协方差矩阵画二维轨迹只需要msg.pose.pose.position.x和msg.pose.pose.position.y两个字段。如果你还想在轨迹上标出机器人的朝向就需要处理orientation的四元数把 x、y、z、w 转成欧拉角里的 yaw偏航角。2.2 坐标系约定与航向角计算ROS 默认遵循 REP-103 的坐标约定x 向前、y 向左、z 向上也就是常说的 ENU东-北-天在二维平面上的投影。移动机器人底盘大多数遵循这个约定。但有些特殊平台可能做了内部变换所以先说一个排查技巧让机器人原地左转一圈观察 y 值的变化方向。如果 y 先变成正数再回到 0说明 y 向左符合预期如果相反说明这个平台的坐标系约定不一样画图时可能需要自己做镜像处理。四元数转 yaw 的公式看起来复杂其实很好记。用math手写也就三行q msg.pose.pose.orientation yaw math.atan2(2.0 * (q.w * q.z q.x * q.y), 1.0 - 2.0 * (q.y * q.y q.z * q.z))如果你装了tf依赖也可以用现成的from tf.transformations import euler_from_quaternion roll, pitch, yaw euler_from_quaternion([q.x, q.y, q.z, q.w])两类方式我都用过最终习惯用手写那版少一个依赖而且性能在回调里几乎无差别。3. 离线路线从 rosbag 画全局轨迹推荐先做3.1 准备 bag现场录制还是用已有数据离线方案有一劳永逸的好处录一次 bag以后想怎么画怎么画不用让机器人重新跑一遍。录制很简单终端里执行rosbag record /odom -O my_odom.bag如果只有仿真环境也可以在 Gazebo 里让机器人走一段同时录/odom。录制完先看下 bag 的基本信息rosbag info my_odom.bag确认里面有/odom话题以及消息数量再继续。这里有个小坑录 bag 的时候建议把/tf和/odom一起录。虽然画轨迹用不上/tf但后续如果要分析坐标变换关系、或者对比/odom和真实位姿没有/tf就得重录一次很浪费。3.2 读取 bag 生成轨迹图的具体实现下面这段代码是我平时最常用的离线脚本直接跑就能出图#!/usr/bin/env python3 # -*- coding: utf-8 -*- import rosbag import matplotlib.pyplot as plt bag rosbag.Bag(my_odom.bag) xs, ys [], [] for topic, msg, t in bag.read_messages(topics[/odom]): x msg.pose.pose.position.x y msg.pose.pose.position.y xs.append(x) ys.append(y) bag.close() fig, ax plt.subplots(figsize(8, 6)) ax.plot(xs, ys, b-, linewidth1.5, label轨迹) if xs: ax.scatter(xs[0], ys[0], cgreen, s60, label起点) ax.scatter(xs[-1], ys[-1], cred, s60, label终点) ax.set_xlabel(x / m) ax.set_ylabel(y / m) ax.set_title(ODOM 轨迹rosbag 离线) ax.legend() ax.grid(True) ax.set_aspect(equal, adjustabledatalim) plt.savefig(odom_offline.png, dpi150) plt.show()有几个地方必须说明set_aspect(equal)一定要加。不加的话matplotlib 会自动拉伸坐标轴圆形的路径会被压成椭圆形影响判断。read_messages(topics[/odom])只读/odom哪怕 bag 里还有其他话题也不影响。如果 bag 很大、odom 频率很高比如 100Hz可以先降采样避免画图卡顿count 0 for topic, msg, t in bag.read_messages(topics[/odom]): if count % 5 ! 0: count 1 continue count 1 xs.append(msg.pose.pose.position.x) ys.append(msg.pose.pose.position.y)3.3 为什么离线方案适合新手离线方案的优点是逻辑单一、容易调试。你不需要考虑线程问题不需要处理实时绘制性能bag 读一遍数据就全在手上了。我建议第一次做这个需求的朋友先跑通离线版再去碰实时版。很多人一上来就想边跑边画结果被各种卡顿和刷新问题劝退其实先出静态图整个流程就成功了一半。4. 实时路线rospy 订阅加 matplotlib 动画4.1 卡死问题的根源两个主循环抢线程如果你直接写一个脚本rospy.init_node之后rospy.spin()再在回调里plt.plot()、plt.show()会发现图刷不出来甚至窗口无响应。原因很简单rospy.spin()是一个阻塞式循环plt.show()也要求占用主线程做事件循环。两个都要主线程必然抢死。解决办法有两条路把 rospy 的 spin 放到子线程主线程留给 matplotlib。这是最干净的做法推荐。不用spin()改成rospy.spinOnce()加plt.pause()组合轮询。代码看起来简单但plt.pause()的实现效率不高频繁调用 CPU 占用会很难看。我最终使用的是第一条路启动一个守护线程专门跑rospy.spin()主线程用FuncAnimation驱动 matplotlib 定时刷新。4.2 FuncAnimation 实时画轨迹完整代码直接贴一个可运行的版本注意几个关键细节我都写在注释里了#!/usr/bin/env python3 # -*- coding: utf-8 -*- import rospy import math import threading import matplotlib.pyplot as plt from collections import deque from matplotlib.animation import FuncAnimation from nav_msgs.msg import Odometry MAX_POINTS 3000 # 最多保留3000个点防止内存无限增长 class OdomTrajectory: def __init__(self): self.xs deque(maxlenMAX_POINTS) self.ys deque(maxlenMAX_POINTS) self.yaws deque(maxlenMAX_POINTS) self.lock threading.Lock() rospy.Subscriber(/odom, Odometry, self.odom_cb) def odom_cb(self, msg): with self.lock: self.xs.append(msg.pose.pose.position.x) self.ys.append(msg.pose.pose.position.y) q msg.pose.pose.orientation yaw math.atan2(2.0 * (q.w * q.z q.x * q.y), 1.0 - 2.0 * (q.y * q.y q.z * q.z)) self.yaws.append(yaw) def get_data(self): with self.lock: return list(self.xs), list(self.ys), list(self.yaws) def ros_spin(): rospy.spin() def main(): rospy.init_node(odom_traj_plot, anonymousTrue) traj OdomTrajectory() fig, ax plt.subplots(figsize(8, 6)) def update(frame): xs, ys, yaws traj.get_data() if not xs: return ax.clear() ax.plot(xs, ys, b-, linewidth1.5) ax.scatter(xs[-1], ys[-1], cred, s40) # 用箭头标出当前朝向 dx 0.4 * math.cos(yaws[-1]) dy 0.4 * math.sin(yaws[-1]) ax.arrow(xs[-1], ys[-1], dx, dy, head_width0.15, head_length0.2, fcred, ecred) ax.set_xlabel(x / m) ax.set_ylabel(y / m) ax.set_title(实时 /odom 轨迹) ax.grid(True) ax.set_aspect(equal, adjustabledatalim) threading.Thread(targetros_spin, daemonTrue).start() ani FuncAnimation(fig, update, interval100, cache_frame_dataFalse) try: plt.show() except KeyboardInterrupt: pass if __name__ __main__: main()4.3 实时版本我加了哪些细节上面这段代码看着简单但我踩过几个坑逐个说清楚第一deque 的 maxlen 参数。实时跑一晚上回调几十万次如果普通 list 一直 append内存迟早爆掉。deque(maxlen3000)会自动把最前面的点丢掉轨迹在屏幕上表现为滚动窗口内存占用恒定。你不需要手动写删除逻辑。第二锁的使用。rospy.Subscriber的回调运行在 spin 线程里而 matplotlib 的 update 运行在主线程里两个线程同时读写同一个 list 理论上存在竞争。CPython 的 GIL 让简单 append 在大多数时候不会崩但加了self.lock之后心理上和实际上都更稳。注意get_data()里用list()做了拷贝避免把内部 deque 直接丢给绘图线程。第三cache_frame_dataFalse。FuncAnimation默认会缓存每一帧的数据用于 blit 等优化。但对于我们这种动态追加数据的场景缓存旧帧完全没意义还会让内存一直涨。关掉这个参数之后长时间跑也没问题。第四interval100代表每 100ms 刷新一帧也就是 10FPS。这个刷新率对轨迹显示完全够用。调成interval20会更跟手但 CPU 占用会明显升高而且 odom 一般也就 10~50Hz太快没意义。实时画的替代方案也有比如在主循环里这样做rate rospy.Rate(30) while not rospy.is_shutdown(): rospy.spinOnce() plt.pause(0.02) rate.sleep()能用但我实际对比下来 CPU 占用偏高。如果只是临时看一眼可以用长期跑还是推荐线程加 FuncAnimation 的方案。5. 常见问题排查实录从没图到乱图的几次翻车5.1 现象一画出来是白板或只有一个点这个是最常见的问题。先确认三件事第一话题名对不对。有的机器人会把 odom 发在/robot/odom这种带命名空间的话题上你订阅/odom当然什么也收不到。用rostopic list看一眼实际话题名再决定订阅哪个。第二消息有没有在发。rostopic hz /odom如果输出 0说明话题没有数据。此时检查发布节点是否启动或者录 bag 的时候是不是没录到/odom。第三机器人动了没有。轨迹只有一个点大概率是机器人压根没动或者底盘在打滑而里程计认为自己没动。这个和代码无关但经常浪费新手一晚上时间。建议先手动推着机器人走一小段再画。5.2 现象二图形窗口卡死、不刷新窗口白屏、鼠标能移动但内容不更新基本都是线程模型问题。检查你的代码是不是把rospy.spin()和plt.show()放在了同一个线程。如果答案是是改成第四节那种方案spin 进子线程主线程动画刷新。还有一个容易被忽略的情况在回调里直接调用plt.plot()。这等于在 spin 线程里操作 GUI轻则刷新异常重则直接崩溃。回调函数里只做数据收集绘图统一放到动画刷新函数里这个纪律要守住。5.3 现象三轨迹乱飘、方向不对、越走越糊轨迹方向不对先怀疑坐标系。按 2.2 节的方法原地转圈看曲线旋转方向。如果发现 y 轴方向和预期相反可以在追加数据时取负号ys.append(-msg.pose.pose.position.y)轨迹越走越糊通常不是代码问题而是里程计漂移。轮式里程计长时间运行必然累计误差可以用以下方式缓解只在短时间段内画轨迹比如只画最近 30 秒。对坐标做滑动平均滤波把抖动抹掉。和/tf里的位姿交叉验证确认不是话题数据本身错了。6. 进阶玩法与个人习惯6.1 用速度信息给轨迹上色轨迹线只能看到位置变化看不到机器人是快是慢。把twist.twist.linear转成速率然后用颜色映射画散点能一眼看出机器人在哪里减速、哪里加速。核心代码如下v math.hypot(msg.twist.twist.linear.x, msg.twist.twist.linear.y)离线读取时把速度存成一个列表画图时用cv_list传给scattersc ax.scatter(xs, ys, cvs, cmapviridis, s5) plt.colorbar(sc, label速度 m/s)这个技巧在分析机器人路径是否平滑时特别有用。减速点往往意味着导航算法在做原地调整颜色图上会看得非常清楚。6.2 多组轨迹叠加对比调参不再靠猜调导航参数时我习惯把同一场景下不同参数组录成多个 bag然后离线脚本一次读多个文件画在一张图上files [run_default.bag, run_tuned.bag] labels [默认参数, 优化参数] for f, lab in zip(files, labels): xs, ys [], [] bag rosbag.Bag(f) for topic, msg, t in bag.read_messages(topics[/odom]): xs.append(msg.pose.pose.position.x) ys.append(msg.pose.pose.position.y) bag.close() ax.plot(xs, ys, linewidth1.2, labellab) ax.legend() plt.show()对比图出来之后路径长度、绕路次数、轨迹平滑度这些都可以肉眼判断。比在 rviz 里来回切换显示项效率高一个量级。6.3 我的复盘习惯与建议说实话我平时 90% 的情况用的是离线方案实机调试先录 bag之后回放反复看。实时图用来排查定位明显异常的时候比较多比如机器人中途转圈、路径跳变这类突发问题。如果你现在目标是给机器人画出一条走到终点的轨迹图我建议先跑通第三节的离线脚本把图导出来再去碰第四节的实时版本。两个都走一遍你对 rospy 订阅、消息结构、matplotlib 动画这三块就都打通了。后面无论换到/amcl_pose还是其他导航话题套路都是一样的回调取数、存列表、画图。希望这篇能让你少走几步弯路。
做机器人导航调试的时候除了在 rviz 里盯着机器人实时跑我经常需要把/odom发出来的轨迹单独拉出来画成一张图——比如比赛复盘、实验报告、论文插图或者单纯想对比两版参数谁的路径更平滑。这个需求看着不起眼真动手写的时候很多新手会卡在同一个地方rosbag 或者 rospy 回调里数据拿到了但 matplotlib 的图就是刷不出来。这篇文章我会从/odom消息结构讲起把离线画轨迹和实时画轨迹两条路都走一遍最后附上我踩过的坑和排查清单。适合刚接触 ROS 导航、想用 Python 做轨迹可视化的朋友也适合需要批量处理 bag 轨迹数据的同学。1. 拿到/odom先搞清楚这段数据值多少钱1.1/odom到底在说什么/odom是机器人导航里最常见的里程计话题。它的数据通常来自轮式编码器、IMU或者两者的融合结果描述的是机器人相对于起点odom 坐标系原点的位置和姿态变化。这里有个容易混淆的点/odom跟/map不是一回事。/odom是局部里程计以机器人启动时的位置为原点而且会随着打滑、累计误差慢慢漂移/map是地图坐标系一般由 AMCL 或类似的定位节点发布。我们画轨迹调试导航算法大多数时候画的是/odom因为它频率高、更新快能看出机器人实际走了什么路径。想看全局定位精度那才需要/map或者/amcl_pose。这个轨迹图能解决什么问题快速判断机器人走出的路径是不是符合预期比如有没有绕远路、原地转圈。对比不同参数下路径的平滑度和长度。验证里程计本身有没有问题比如轮子打滑导致的轨迹畸形。在实验报告、答辩 PPT 里放一张标准的路径对比图。1.2 为什么选 matplotlib 而不是盯着 rvizrviz 当然能显示轨迹但它的定位是实时调试工具不是出图工具。我选择用 matplotlib 的原因很实际可交付。matplotlib 直接输出 PNG/SVG/PDF放在论文或报告里格式可控。可叠加分析。可以把多组 bag 的轨迹画在同一张图上对比rviz 里做这种对比要频繁切换显示项效率低。可量化。拿到 x、y 数据之后路径长度、平均速度、最大偏差这些指标都可以顺手算出来这是 rviz 给不了的。依赖简单。只要 ROS 自带的 Python 环境能 import rospy 和 matplotlib这个方案就能跑不需要额外装图形工具。不过 matplotlib 也有明显的短板绘制大量点时性能一般、交互性不如 rviz。所以我的建议是实时调试看 rviz出数据、做分析用 matplotlib。两者互补不冲突。2. 动手前先拆消息结构字段、坐标系、四元数2.1 nav_msgs/Odometry 逐层看写代码之前我习惯先用rostopic echo /odom -n 1看一条实时消息。这里的字段路径直接决定后面取数的写法建议先自己敲一遍。rostopic echo /odom -n 1然后对着消息结构拆开看字段层级字段名类型含义headerstamp / frame_idstd_msgs/Header时间戳、坐标系名称pose.pose.positionx / y / zgeometry_msgs/Point位置单位米pose.pose.orientationx / y / z / wgeometry_msgs/Quaternion姿态四元数pose.covariance36 个数值float64[36]位置和姿态的协方差矩阵twist.twist.linearx / y / zgeometry_msgs/Vector3线速度单位 m/stwist.twist.angularx / y / zgeometry_msgs/Vector3角速度单位 rad/stwist.covariance36 个数值float64[36]速度协方差矩阵画二维轨迹只需要msg.pose.pose.position.x和msg.pose.pose.position.y两个字段。如果你还想在轨迹上标出机器人的朝向就需要处理orientation的四元数把 x、y、z、w 转成欧拉角里的 yaw偏航角。2.2 坐标系约定与航向角计算ROS 默认遵循 REP-103 的坐标约定x 向前、y 向左、z 向上也就是常说的 ENU东-北-天在二维平面上的投影。移动机器人底盘大多数遵循这个约定。但有些特殊平台可能做了内部变换所以先说一个排查技巧让机器人原地左转一圈观察 y 值的变化方向。如果 y 先变成正数再回到 0说明 y 向左符合预期如果相反说明这个平台的坐标系约定不一样画图时可能需要自己做镜像处理。四元数转 yaw 的公式看起来复杂其实很好记。用math手写也就三行q msg.pose.pose.orientation yaw math.atan2(2.0 * (q.w * q.z q.x * q.y), 1.0 - 2.0 * (q.y * q.y q.z * q.z))如果你装了tf依赖也可以用现成的from tf.transformations import euler_from_quaternion roll, pitch, yaw euler_from_quaternion([q.x, q.y, q.z, q.w])两类方式我都用过最终习惯用手写那版少一个依赖而且性能在回调里几乎无差别。3. 离线路线从 rosbag 画全局轨迹推荐先做3.1 准备 bag现场录制还是用已有数据离线方案有一劳永逸的好处录一次 bag以后想怎么画怎么画不用让机器人重新跑一遍。录制很简单终端里执行rosbag record /odom -O my_odom.bag如果只有仿真环境也可以在 Gazebo 里让机器人走一段同时录/odom。录制完先看下 bag 的基本信息rosbag info my_odom.bag确认里面有/odom话题以及消息数量再继续。这里有个小坑录 bag 的时候建议把/tf和/odom一起录。虽然画轨迹用不上/tf但后续如果要分析坐标变换关系、或者对比/odom和真实位姿没有/tf就得重录一次很浪费。3.2 读取 bag 生成轨迹图的具体实现下面这段代码是我平时最常用的离线脚本直接跑就能出图#!/usr/bin/env python3 # -*- coding: utf-8 -*- import rosbag import matplotlib.pyplot as plt bag rosbag.Bag(my_odom.bag) xs, ys [], [] for topic, msg, t in bag.read_messages(topics[/odom]): x msg.pose.pose.position.x y msg.pose.pose.position.y xs.append(x) ys.append(y) bag.close() fig, ax plt.subplots(figsize(8, 6)) ax.plot(xs, ys, b-, linewidth1.5, label轨迹) if xs: ax.scatter(xs[0], ys[0], cgreen, s60, label起点) ax.scatter(xs[-1], ys[-1], cred, s60, label终点) ax.set_xlabel(x / m) ax.set_ylabel(y / m) ax.set_title(ODOM 轨迹rosbag 离线) ax.legend() ax.grid(True) ax.set_aspect(equal, adjustabledatalim) plt.savefig(odom_offline.png, dpi150) plt.show()有几个地方必须说明set_aspect(equal)一定要加。不加的话matplotlib 会自动拉伸坐标轴圆形的路径会被压成椭圆形影响判断。read_messages(topics[/odom])只读/odom哪怕 bag 里还有其他话题也不影响。如果 bag 很大、odom 频率很高比如 100Hz可以先降采样避免画图卡顿count 0 for topic, msg, t in bag.read_messages(topics[/odom]): if count % 5 ! 0: count 1 continue count 1 xs.append(msg.pose.pose.position.x) ys.append(msg.pose.pose.position.y)3.3 为什么离线方案适合新手离线方案的优点是逻辑单一、容易调试。你不需要考虑线程问题不需要处理实时绘制性能bag 读一遍数据就全在手上了。我建议第一次做这个需求的朋友先跑通离线版再去碰实时版。很多人一上来就想边跑边画结果被各种卡顿和刷新问题劝退其实先出静态图整个流程就成功了一半。4. 实时路线rospy 订阅加 matplotlib 动画4.1 卡死问题的根源两个主循环抢线程如果你直接写一个脚本rospy.init_node之后rospy.spin()再在回调里plt.plot()、plt.show()会发现图刷不出来甚至窗口无响应。原因很简单rospy.spin()是一个阻塞式循环plt.show()也要求占用主线程做事件循环。两个都要主线程必然抢死。解决办法有两条路把 rospy 的 spin 放到子线程主线程留给 matplotlib。这是最干净的做法推荐。不用spin()改成rospy.spinOnce()加plt.pause()组合轮询。代码看起来简单但plt.pause()的实现效率不高频繁调用 CPU 占用会很难看。我最终使用的是第一条路启动一个守护线程专门跑rospy.spin()主线程用FuncAnimation驱动 matplotlib 定时刷新。4.2 FuncAnimation 实时画轨迹完整代码直接贴一个可运行的版本注意几个关键细节我都写在注释里了#!/usr/bin/env python3 # -*- coding: utf-8 -*- import rospy import math import threading import matplotlib.pyplot as plt from collections import deque from matplotlib.animation import FuncAnimation from nav_msgs.msg import Odometry MAX_POINTS 3000 # 最多保留3000个点防止内存无限增长 class OdomTrajectory: def __init__(self): self.xs deque(maxlenMAX_POINTS) self.ys deque(maxlenMAX_POINTS) self.yaws deque(maxlenMAX_POINTS) self.lock threading.Lock() rospy.Subscriber(/odom, Odometry, self.odom_cb) def odom_cb(self, msg): with self.lock: self.xs.append(msg.pose.pose.position.x) self.ys.append(msg.pose.pose.position.y) q msg.pose.pose.orientation yaw math.atan2(2.0 * (q.w * q.z q.x * q.y), 1.0 - 2.0 * (q.y * q.y q.z * q.z)) self.yaws.append(yaw) def get_data(self): with self.lock: return list(self.xs), list(self.ys), list(self.yaws) def ros_spin(): rospy.spin() def main(): rospy.init_node(odom_traj_plot, anonymousTrue) traj OdomTrajectory() fig, ax plt.subplots(figsize(8, 6)) def update(frame): xs, ys, yaws traj.get_data() if not xs: return ax.clear() ax.plot(xs, ys, b-, linewidth1.5) ax.scatter(xs[-1], ys[-1], cred, s40) # 用箭头标出当前朝向 dx 0.4 * math.cos(yaws[-1]) dy 0.4 * math.sin(yaws[-1]) ax.arrow(xs[-1], ys[-1], dx, dy, head_width0.15, head_length0.2, fcred, ecred) ax.set_xlabel(x / m) ax.set_ylabel(y / m) ax.set_title(实时 /odom 轨迹) ax.grid(True) ax.set_aspect(equal, adjustabledatalim) threading.Thread(targetros_spin, daemonTrue).start() ani FuncAnimation(fig, update, interval100, cache_frame_dataFalse) try: plt.show() except KeyboardInterrupt: pass if __name__ __main__: main()4.3 实时版本我加了哪些细节上面这段代码看着简单但我踩过几个坑逐个说清楚第一deque 的 maxlen 参数。实时跑一晚上回调几十万次如果普通 list 一直 append内存迟早爆掉。deque(maxlen3000)会自动把最前面的点丢掉轨迹在屏幕上表现为滚动窗口内存占用恒定。你不需要手动写删除逻辑。第二锁的使用。rospy.Subscriber的回调运行在 spin 线程里而 matplotlib 的 update 运行在主线程里两个线程同时读写同一个 list 理论上存在竞争。CPython 的 GIL 让简单 append 在大多数时候不会崩但加了self.lock之后心理上和实际上都更稳。注意get_data()里用list()做了拷贝避免把内部 deque 直接丢给绘图线程。第三cache_frame_dataFalse。FuncAnimation默认会缓存每一帧的数据用于 blit 等优化。但对于我们这种动态追加数据的场景缓存旧帧完全没意义还会让内存一直涨。关掉这个参数之后长时间跑也没问题。第四interval100代表每 100ms 刷新一帧也就是 10FPS。这个刷新率对轨迹显示完全够用。调成interval20会更跟手但 CPU 占用会明显升高而且 odom 一般也就 10~50Hz太快没意义。实时画的替代方案也有比如在主循环里这样做rate rospy.Rate(30) while not rospy.is_shutdown(): rospy.spinOnce() plt.pause(0.02) rate.sleep()能用但我实际对比下来 CPU 占用偏高。如果只是临时看一眼可以用长期跑还是推荐线程加 FuncAnimation 的方案。5. 常见问题排查实录从没图到乱图的几次翻车5.1 现象一画出来是白板或只有一个点这个是最常见的问题。先确认三件事第一话题名对不对。有的机器人会把 odom 发在/robot/odom这种带命名空间的话题上你订阅/odom当然什么也收不到。用rostopic list看一眼实际话题名再决定订阅哪个。第二消息有没有在发。rostopic hz /odom如果输出 0说明话题没有数据。此时检查发布节点是否启动或者录 bag 的时候是不是没录到/odom。第三机器人动了没有。轨迹只有一个点大概率是机器人压根没动或者底盘在打滑而里程计认为自己没动。这个和代码无关但经常浪费新手一晚上时间。建议先手动推着机器人走一小段再画。5.2 现象二图形窗口卡死、不刷新窗口白屏、鼠标能移动但内容不更新基本都是线程模型问题。检查你的代码是不是把rospy.spin()和plt.show()放在了同一个线程。如果答案是是改成第四节那种方案spin 进子线程主线程动画刷新。还有一个容易被忽略的情况在回调里直接调用plt.plot()。这等于在 spin 线程里操作 GUI轻则刷新异常重则直接崩溃。回调函数里只做数据收集绘图统一放到动画刷新函数里这个纪律要守住。5.3 现象三轨迹乱飘、方向不对、越走越糊轨迹方向不对先怀疑坐标系。按 2.2 节的方法原地转圈看曲线旋转方向。如果发现 y 轴方向和预期相反可以在追加数据时取负号ys.append(-msg.pose.pose.position.y)轨迹越走越糊通常不是代码问题而是里程计漂移。轮式里程计长时间运行必然累计误差可以用以下方式缓解只在短时间段内画轨迹比如只画最近 30 秒。对坐标做滑动平均滤波把抖动抹掉。和/tf里的位姿交叉验证确认不是话题数据本身错了。6. 进阶玩法与个人习惯6.1 用速度信息给轨迹上色轨迹线只能看到位置变化看不到机器人是快是慢。把twist.twist.linear转成速率然后用颜色映射画散点能一眼看出机器人在哪里减速、哪里加速。核心代码如下v math.hypot(msg.twist.twist.linear.x, msg.twist.twist.linear.y)离线读取时把速度存成一个列表画图时用cv_list传给scattersc ax.scatter(xs, ys, cvs, cmapviridis, s5) plt.colorbar(sc, label速度 m/s)这个技巧在分析机器人路径是否平滑时特别有用。减速点往往意味着导航算法在做原地调整颜色图上会看得非常清楚。6.2 多组轨迹叠加对比调参不再靠猜调导航参数时我习惯把同一场景下不同参数组录成多个 bag然后离线脚本一次读多个文件画在一张图上files [run_default.bag, run_tuned.bag] labels [默认参数, 优化参数] for f, lab in zip(files, labels): xs, ys [], [] bag rosbag.Bag(f) for topic, msg, t in bag.read_messages(topics[/odom]): xs.append(msg.pose.pose.position.x) ys.append(msg.pose.pose.position.y) bag.close() ax.plot(xs, ys, linewidth1.2, labellab) ax.legend() plt.show()对比图出来之后路径长度、绕路次数、轨迹平滑度这些都可以肉眼判断。比在 rviz 里来回切换显示项效率高一个量级。6.3 我的复盘习惯与建议说实话我平时 90% 的情况用的是离线方案实机调试先录 bag之后回放反复看。实时图用来排查定位明显异常的时候比较多比如机器人中途转圈、路径跳变这类突发问题。如果你现在目标是给机器人画出一条走到终点的轨迹图我建议先跑通第三节的离线脚本把图导出来再去碰第四节的实时版本。两个都走一遍你对 rospy 订阅、消息结构、matplotlib 动画这三块就都打通了。后面无论换到/amcl_pose还是其他导航话题套路都是一样的回调取数、存列表、画图。希望这篇能让你少走几步弯路。
