Use rospy subscriber and matplotlib FuncAnimation()

edit

Hello,

I am trying to use a bicycle model with ROS. I used matplotlib animation, FuncAnimation(), to make a bicycle model and move a point given linear velocity and steering angle. Now I want to subscribe to a Twist message and move the point while using FuncAnimation().

I used to rospy.wait_for_message() and it is working. But, as soon as I stop publishing, the point is also stopping.

Please refer to the code.

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from matplotlib import pyplot as plt
import numpy as np
from matplotlib import animation
from math import *
rospy.init_node("plot",anonymous=True)
fig = plt.figure()
ax = fig.add_subplot(111)
ax.axis('equal')
t=np.arange(0, 2*np.pi, 2*np.pi/100)
x=50*np.cos(t)
y=50*np.sin(t)
point, = ax.plot([0], [0], 'o')
Q = ax.quiver([0],[0],[0],[0], pivot='mid', color='r', width=0.005)

line, = ax.plot(x,y,color='k')
ax.set_xlim([-150, 150])
ax.set_ylim([-150, 150])

posx = 0
posy = 0
curSpeed = 5
angle =  0
turningspeed = 0
maxspeed = 2
acc = 0.5
dec = 0.1
L = 2.
steeringAngle = 10.
def update_point(n, point,Q):
    flag = 0
    global posx,posy,curSpeed,angle,turningspeed,maxspeed,acc,dec
    pos=rospy.wait_for_message("/cmd_vel",Twist)
    print "thetadot -- ",turningspeed
    print "v -- ",curSpeed
    curSpeed = pos.linear.x
    steeringAngle = pos.angular.z
    steeringAngler = (steeringAngle/180.)*pi
    turningspeed = (curSpeed/L)*tan(steeringAngler)
    angle+=turningspeed
    if(angle>2*pi):
        angle=0
    if(angle<0):
        angle=2*pi
    movex = curSpeed*cos(angle)
    movey = curSpeed*sin(angle)
    posx+=movex
    posy+=movey
    U = np.cos(angle)
    V = np.sin(angle)
    point.set_data(np.array([posx,posy]))
    Q.set_offsets(np.array([posx,posy]))
    Q.set_UVC(U,V)
    print curSpeed
    # point.set_3d_properties(z[n], 'z')
    return point,Q

ani=animation.FuncAnimation(fig, update_point, 100, fargs=(point,Q))

plt.show()

In this scenario how to use callback function that are passed to rospy.Subscriber() to set CurSpeed and angle in above code and run matplotlib animation.

Thanks