Plotting PID controller results vs desired trajectories

plot_stuff.py

import rosbag
from geometry_msgs.msg import PoseStamped, TwistStamped
import matplotlib.pyplot as plt
import numpy as np
import sys

print(sys.argv[1])

bag = rosbag.Bag(sys.argv[1], 'r')

X = []
Y = []
for topic, msg, t in bag.read_messages(topics=["/vrpn_client_node/rc_car/pose"]):
  if topic == '/vrpn_client_node/rc_car/pose':
    X.append(msg.pose.position.x)
    Y.append(msg.pose.position.y)
    
plt.plot(X, Y)
if sys.argv[1] == "2018-04-23-02-54-31.bag":
  plt.plot(1.25/2*np.sin(np.linspace(0,2*np.pi,100))+0.75, 1.25/2*np.cos(np.linspace(0,2*np.pi,100))+0.75, 'r', linewidth=4.0)
if sys.argv[1] == "2018-04-23-03-26-40.bag":
  plt.plot(1.24/2*np.sin(np.linspace(0,4*np.pi,100))+1, 1.75*1.25/2*np.cos(np.linspace(0,2*np.pi,100)), 'r', linewidth=4.0)
    
plt.axis('equal')
plt.show()
    

bag.close()