引子
我们在做机器人实验的时候，一般需要实时地观察机械臂的各个状态信息，所以动态绘图是不可避免的，下面我们将介绍如何对机械臂的关节速度进行动态绘图。
Codes
实现代码比较简单，我们以baxter机器人单个关节的关节角（“right_s0”）为例介绍如何对其速度信息进行实时绘图：
#! /usr/bin/python
import rospy
from sensor_msgs.msg import JointState
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
# global variable for vel.
vel_info = np.zeros(100)
vel_info_temp = vel_info
vel_temp = 0
# global variables for plot.
fig, ax = plt.subplots()
line, = ax.plot(vel_info)
ax.set_ylim(-6e-2, 6e-2)
ax.set_ylabel("joint vel: rad/s")
ax.set_xlabel("time")
def stateMsgReceived(state_msg):
global vel_temp
vel_temp = state_msg.velocity[15]
print vel_temp
def update(frame):
global vel_info
global vel_info_temp
global line
vel_info[0:-1] = vel_info_temp[1:]
vel_info[-1] = vel_temp
vel_info_temp = vel_info
line.set_ydata(vel_info)
return line
def main():
global vel_info
global fig
rospy.init_node("joint_vel_drawer")
joint_sub = rospy.Subscriber("/robot/joint_states", JointState, stateMsgReceived)
ani = animation.FuncAnimation(fig, update, frames=None, interval=50)
plt.show()
if __name__ == "__main__":
main()代码理解
首先是导入了ROS相关的包：
import rospy
from sensor_msgs.msg import JointState然后是导入数据处理包numpy以及绘图包matplotlib：
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation设置用于绘制速度曲线的全局变量：
# global variable for vel.
vel_curve = np.zeros(100)
vel_curve_temp = vel_curve
vel_temp = 0其中vel_curve表示用于绘图的点的向量，比如这里用100个点来绘图；vel_curve_temp用于暂时保存vel_curve的数据，并在后面用于对vel_curve中的数据进行更新；vel_temp保存最近时刻的速度数据。
接着我们对所需绘制的图像进行设置，比如y值范围是(-6e-2, 6e-2)，x、y轴的label分别为time和joint vel：
# global variables for plot.
fig, ax = plt.subplots()
line, = ax.plot(vel_info)
ax.set_ylim(-6e-2, 6e-2)
ax.set_ylabel("joint vel: rad/s")
ax.set_xlabel("time")然后我们定义了/robot/joint_states话题的回调函数：
def stateMsgReceived(state_msg):
global vel_temp
vel_temp = state_msg.velocity[15]
print vel_temp主要也就是取出关节“right_s0”的数据放置到vel_temp中。
接下来定义最最最重要的动态绘图更新函数：
def update(frame):
global vel_info
global vel_info_temp
global line
vel_info[0:-1] = vel_info_temp[1:]
vel_info[-1] = vel_temp
vel_info_temp = vel_info
line.set_ydata(vel_info)
return line在这里，我们首先将vel_info中的数据往前移一个点，也即将最早的一个速度点扔掉，然后用vel_temp这一最新数据补充到vel_info中，最后将我们的fig要绘制的曲线line中的数据设置为vel_info即可。
理解了上面的内容之后，主函数的内容也就不难理解了：
def main():
global vel_info
global fig
rospy.init_node("joint_vel_drawer")
joint_sub = rospy.Subscriber("/robot/joint_states", JointState, stateMsgReceived)
ani = animation.FuncAnimation(fig, update, frames=None, interval=50)
plt.show()首先是订阅/joint_states话题，因为是用的rospy，所以会自己开辟一个线程/进程去执行回调函数，如果是用C++写的话，使用ros::AsyncSpinner()即可。然后用animation.FuncAnimation来绘制曲线，其中fig表示我们要绘制的图像，update表示更新图像数据所调用的函数，interval=50表示的是每隔50ms调用一次update函数对数据进行更新，并在fig上显示出来。最后，我们使用plt.show()打开显示窗口即可~
绘制得到的关节速度动态曲线如下（这里就不放动态图了……）：
本文就写到这里咯，如果有对动态绘图感兴趣的，可以参考博文《Matplotlib画动态图：animation模块的使用》。