注意事项
- Python没有回调函数这个东西,所以ROS中回调函数的实现其实是通过多线程来解决的
- 又因为是多线程,所以其实
rospy.spin()
并不是必须的,只要确保主线程(main)存活那么rospy
中的回调函数就可以一直接收数据
代码
实现了同时监听两个 /odom 并画图进行对比
import rospy
from matplotlib import pyplot as plt
from nav_msgs.msg import Odometry
import numpy as np
import time
class DrawError(object):
odom_time, cb_time = 0, 0
x_odom = []
y_odom = []
x_rf2o = []
y_rf2o = []
def __init__(self):
pass
def isCBAlive(self):
return False if self.cb_time - self.odom_time > 2 else True
def odomCallBack(self, data):
self.x_odom.append(data.pose.pose.position.x)
self.y_odom.append(data.pose.pose.position.y)
self.odom_time = 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 figure
def cbMonitor(self, plt):
self.cb_time = time.time()
if not self.isCBAlive():
rospy.logwarn("No data received!Exit!")
return True
else:
return False
if __name__ == "__main__":
rospy.init_node("drawingError")
de = DrawError()
# gazebo's odom, without nosie
rospy.Subscriber("odom", Odometry, de.odomCallBack)
# rf2o_odometry's odom
rospy.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) == True:
break
plt.plot(de.x_odom, de.y_odom, color="b", label="/odom")
plt.plot(de.x_rf2o, de.y_rf2o, color="r", label="/odom_rf2o")
plt.legend()
plt.show()