rospy通过matplotlib画出/odom轨迹图

注意事项

1. Python没有回调函数这个东西&＃xff0c;所以ROS中回调函数的实现其实是通过多线程来解决的
2. 又因为是多线程&＃xff0c;所以其实rospy.spin()并不是必须的&＃xff0c;只要确保主线程(main)存活那么rospy中的回调函数就可以一直接收数据

代码

实现了同时监听两个 /odom 并画图进行对比

import rospy
from matplotlib import pyplot as plt
from nav_msgs.msg import Odometry
import numpy as np
import timeclass DrawError(object):odom_time, cb_time &＃61; 0, 0x_odom &＃61; []y_odom &＃61; []x_rf2o &＃61; []y_rf2o &＃61; []def __init__(self):passdef isCBAlive(self):return False if self.cb_time - self.odom_time > 2 else Truedef odomCallBack(self, data):self.x_odom.append(data.pose.pose.position.x)self.y_odom.append(data.pose.pose.position.y)self.odom_time &＃61; time.time()def rf2oCallBack(self, data):self.x_rf2o.append(data.pose.pose.position.x)self.y_rf2o.append(data.pose.pose.position.y)# check if data is not received. If so, exit and plot the figuredef cbMonitor(self, plt):self.cb_time &＃61; time.time()if not self.isCBAlive():rospy.logwarn("No data received!Exit!")return Trueelse:return Falseif __name__ &＃61;&＃61; "__main__":rospy.init_node("drawingError")de &＃61; DrawError()# gazebo&＃39;s odom, without nosierospy.Subscriber("odom", Odometry, de.odomCallBack)# rf2o_odometry&＃39;s odomrospy.Subscriber("odom_rf2o", Odometry, de.rf2oCallBack)rospy.Timer(rospy.Duration(2), de.cbMonitor)plt.title("odom display")while not rospy.is_shutdown():if de.cbMonitor(plt) &＃61;&＃61; True:breakplt.plot(de.x_odom, de.y_odom, color&＃61;"b", label&＃61;"/odom")plt.plot(de.x_rf2o, de.y_rf2o, color&＃61;"r", label&＃61;"/odom_rf2o")plt.legend()plt.show()


最终效果