Skip to content
Snippets Groups Projects
Commit 11196f78 authored by kylewong975's avatar kylewong975
Browse files

Finish animated paths

parent 1b003a18
No related merge requests found
Hide whitespace changes
Inline Side-by-side
functions/data_analysis/realtime_plot.py
+ 64
− 21
View file @ 11196f78
from matplotlib import pyplot as plt
from matplotlib import animation
from matplotlib.font_manager import FontProperties
import numpy as np
import time
......@@ -25,6 +26,21 @@ def update_point_gt(i, fig, scat, robot_loc_time_unit, dataset_labels, robotNum)
return scat,
# estimate
def update_path_est(i, fig, l, robot_loc_time_unit, dataset_labels, robotNum):
label = robotNum
x = robot_loc_time_unit[label]['est_x']
y = robot_loc_time_unit[label]['est_y']
l.set_data(x[:i], y[:i])
return l,
# groundtruth
def update_path_gt(i, fig, l, robot_loc_time_unit, dataset_labels, robotNum):
label = robotNum
x = robot_loc_time_unit[label]['gt_x']
y = robot_loc_time_unit[label]['gt_y']
l.set_data(x[:i], y[:i])
return l,
def animate_plot(dataset_labels, data_recorder, analyzer, lm = None):
......@@ -34,6 +50,7 @@ def animate_plot(dataset_labels, data_recorder, analyzer, lm = None):
num_robots = int(len(dataset_labels))
fig = plt.figure()
ax = fig.add_subplot(111)
fig.set_size_inches((18,18))
ax = plt.axes(xlim=(-6, 6), ylim=(-6, 8))
......@@ -45,36 +62,46 @@ def animate_plot(dataset_labels, data_recorder, analyzer, lm = None):
y1e = robot_loc_time_unit[1]['est_y']
x1g = robot_loc_time_unit[1]['gt_x']
y1g = robot_loc_time_unit[1]['gt_y']
scat1e = plt.scatter(x1e[0], y1e[0])
scat1g = plt.scatter(x1g[0], y1g[0])
scat1e = plt.scatter(x1e[0], y1e[0], c='b')
line1e, = ax.plot(x1e, y1e, 'b:', label='Robot 1 estimate')
scat1g = plt.scatter(x1g[0], y1g[0], c='b')
line1g, = ax.plot(x1g, y1g, 'b-', label='Robot 1 groundtruth')
x2e = robot_loc_time_unit[2]['est_x']
y2e = robot_loc_time_unit[2]['est_y']
x2g = robot_loc_time_unit[2]['gt_x']
y2g = robot_loc_time_unit[2]['gt_y']
scat2e = plt.scatter(x2e[0], y2e[0])
scat2g = plt.scatter(x2g[0], y2g[0])
scat2e = plt.scatter(x2e[0], y2e[0], c='g')
line2e, = ax.plot(x2e, y2e, 'g:', label='Robot 2 estimate')
scat2g = plt.scatter(x2g[0], y2g[0], c='g')
line2g, = ax.plot(x2g, y2g, 'g-', label='Robot 2 groundtruth')
x3e = robot_loc_time_unit[3]['est_x']
y3e = robot_loc_time_unit[3]['est_y']
x3g = robot_loc_time_unit[3]['gt_x']
y3g = robot_loc_time_unit[3]['gt_y']
scat3e = plt.scatter(x3e[0], y3e[0])
scat3g = plt.scatter(x3g[0], y3g[0])
scat3e = plt.scatter(x3e[0], y3e[0], c='r')
line3e, = ax.plot(x3e, y3e, 'r:', label='Robot 3 estimate')
scat3g = plt.scatter(x3g[0], y3g[0], c='r')
line3g, = ax.plot(x3g, y3g, 'r-', label='Robot 3 groundtruth')
x4e = robot_loc_time_unit[4]['est_x']
y4e = robot_loc_time_unit[4]['est_y']
x4g = robot_loc_time_unit[4]['gt_x']
y4g = robot_loc_time_unit[4]['gt_y']
scat4e = plt.scatter(x4e[0], y4e[0])
scat4g = plt.scatter(x4g[0], y4g[0])
scat4e = plt.scatter(x4e[0], y4e[0], c='c')
line4e, = ax.plot(x4e, y4e, 'c:', label='Robot 4 estimate')
scat4g = plt.scatter(x4g[0], y4g[0], c='c')
line4g, = ax.plot(x4g, y4g, 'c-', label='Robot 4 groundtruth')
x5e = robot_loc_time_unit[5]['est_x']
y5e = robot_loc_time_unit[5]['est_y']
x5g = robot_loc_time_unit[5]['gt_x']
y5g = robot_loc_time_unit[5]['gt_y']
scat5e = plt.scatter(x4e[0], y4e[0])
scat5g = plt.scatter(x4g[0], y4g[0])
scat5e = plt.scatter(x5e[0], y5e[0], c='k')
line5e, = ax.plot(x5e, y5e, 'k:', label='Robot 5 estimate')
scat5g = plt.scatter(x5g[0], y5g[0], c='k')
line5g, = ax.plot(x5g, y5g, 'k-', label='Robot 5 groundtruth')
# time stamps for robotd 1 to 5
......@@ -87,22 +114,38 @@ def animate_plot(dataset_labels, data_recorder, analyzer, lm = None):
print(time_array)
# set the interval accordingly, higher the interval, the slower it is
# robot 1
ani1e=animation.FuncAnimation(fig, update_point_est, fargs = (fig, scat1e, robot_loc_time_unit, dataset_labels, 1) ,frames=len(time_array), interval=50, blit = False)
ani1g=animation.FuncAnimation(fig, update_point_gt, fargs = (fig, scat1g, robot_loc_time_unit, dataset_labels, 1) ,frames=len(time_array), interval=50, blit = False)
#ani1e=animation.FuncAnimation(fig, update_point_est, fargs = (fig, scat1e, robot_loc_time_unit, dataset_labels, 1) ,frames=len(time_array), interval=50, blit = False)
ani1e_path=animation.FuncAnimation(fig, update_path_est, fargs = (fig, line1e, robot_loc_time_unit, dataset_labels, 1), frames=len(time_array), interval=50, blit=False)
ani1g_path=animation.FuncAnimation(fig, update_path_gt, fargs = (fig, line1g, robot_loc_time_unit, dataset_labels, 1) ,frames=len(time_array), interval=50, blit = False)
ani1e_dot=animation.FuncAnimation(fig, update_point_est, fargs = (fig, scat1e, robot_loc_time_unit, dataset_labels, 1), frames=len(time_array), interval=50, blit=False)
ani1g_dot=animation.FuncAnimation(fig, update_point_gt, fargs = (fig, scat1g, robot_loc_time_unit, dataset_labels, 1) ,frames=len(time_array), interval=50, blit = False)
# robot 2
ani2e=animation.FuncAnimation(fig, update_point_est, fargs = (fig, scat2e, robot_loc_time_unit, dataset_labels, 2) ,frames=len(time_array2), interval=50, blit = False)
ani2g=animation.FuncAnimation(fig, update_point_gt, fargs = (fig, scat2g, robot_loc_time_unit, dataset_labels, 2) ,frames=len(time_array2), interval=50, blit = False)
ani2e_path=animation.FuncAnimation(fig, update_path_est, fargs = (fig, line2e, robot_loc_time_unit, dataset_labels, 2) ,frames=len(time_array2), interval=50, blit = False)
ani2g_path=animation.FuncAnimation(fig, update_path_gt, fargs = (fig, line2g, robot_loc_time_unit, dataset_labels, 2) ,frames=len(time_array2), interval=50, blit = False)
ani2e_dot=animation.FuncAnimation(fig, update_point_est, fargs = (fig, scat2e, robot_loc_time_unit, dataset_labels, 2) ,frames=len(time_array2), interval=50, blit = False)
ani2g_dot=animation.FuncAnimation(fig, update_point_gt, fargs = (fig, scat2g, robot_loc_time_unit, dataset_labels, 2) ,frames=len(time_array2), interval=50, blit = False)
# robot 3
ani3e=animation.FuncAnimation(fig, update_point_est, fargs = (fig, scat3e, robot_loc_time_unit, dataset_labels, 3) ,frames=len(time_array3), interval=50, blit = False)
ani3g=animation.FuncAnimation(fig, update_point_gt, fargs = (fig, scat3g, robot_loc_time_unit, dataset_labels, 3) ,frames=len(time_array3), interval=50, blit = False)
ani3e_path=animation.FuncAnimation(fig, update_path_est, fargs = (fig, line3e, robot_loc_time_unit, dataset_labels, 3) ,frames=len(time_array3), interval=50, blit = False)
ani3g_path=animation.FuncAnimation(fig, update_path_gt, fargs = (fig, line3g, robot_loc_time_unit, dataset_labels, 3) ,frames=len(time_array3), interval=50, blit = False)
ani3e_dot=animation.FuncAnimation(fig, update_point_est, fargs = (fig, scat3e, robot_loc_time_unit, dataset_labels, 3) ,frames=len(time_array3), interval=50, blit = False)
ani3g_dot=animation.FuncAnimation(fig, update_point_gt, fargs = (fig, scat3g, robot_loc_time_unit, dataset_labels, 3) ,frames=len(time_array3), interval=50, blit = False)
# robot 4
ani4e=animation.FuncAnimation(fig, update_point_est, fargs = (fig, scat4e, robot_loc_time_unit, dataset_labels, 4) ,frames=len(time_array4), interval=50, blit = False)
ani4g=animation.FuncAnimation(fig, update_point_gt, fargs = (fig, scat4g, robot_loc_time_unit, dataset_labels, 4) ,frames=len(time_array4), interval=50, blit = False)
ani4e_path=animation.FuncAnimation(fig, update_path_est, fargs = (fig, line4e, robot_loc_time_unit, dataset_labels, 4) ,frames=len(time_array4), interval=50, blit = False)
ani4g_path=animation.FuncAnimation(fig, update_path_gt, fargs = (fig, line4g, robot_loc_time_unit, dataset_labels, 4) ,frames=len(time_array4), interval=50, blit = False)
ani4e_dot=animation.FuncAnimation(fig, update_point_est, fargs = (fig, scat4e, robot_loc_time_unit, dataset_labels, 4) ,frames=len(time_array4), interval=50, blit = False)
ani4g_dot=animation.FuncAnimation(fig, update_point_gt, fargs = (fig, scat4g, robot_loc_time_unit, dataset_labels, 4) ,frames=len(time_array4), interval=50, blit = False)
# robot 5
ani5e=animation.FuncAnimation(fig, update_point_est, fargs = (fig, scat5e, robot_loc_time_unit, dataset_labels, 5) ,frames=len(time_array5), interval=50, blit = False)
ani5g=animation.FuncAnimation(fig, update_point_gt, fargs = (fig, scat5g, robot_loc_time_unit, dataset_labels, 5) ,frames=len(time_array5), interval=50, blit = False)
ani5e_path=animation.FuncAnimation(fig, update_path_est, fargs = (fig, line5e, robot_loc_time_unit, dataset_labels, 5) ,frames=len(time_array5), interval=50, blit = False)
ani5g_path=animation.FuncAnimation(fig, update_path_gt, fargs = (fig, line5g, robot_loc_time_unit, dataset_labels, 5) ,frames=len(time_array5), interval=50, blit = False)
ani5e_dot=animation.FuncAnimation(fig, update_point_est, fargs = (fig, scat5e, robot_loc_time_unit, dataset_labels, 5) ,frames=len(time_array5), interval=50, blit = False)
ani5g_dot=animation.FuncAnimation(fig, update_point_gt, fargs = (fig, scat5g, robot_loc_time_unit, dataset_labels, 5) ,frames=len(time_array5), interval=50, blit = False)
# Show legend
fontP = FontProperties()
fontP.set_size('small')
plt.legend(prop=fontP, bbox_to_anchor=(1.0, 1.0), loc=9, ncol=1)#, (line1e, line1g, line2e, line2g, line3e, line3g, line4e, line4g, line5e, line5g), ('Robot 1 estimate', 'Robot 1 groundtruth', 'Robot 2 estimate', 'Robot 2 groundtruth', 'Robot 3 estimate', 'Robot 3 groundtruth', 'Robot 4 estimate', 'Robot 4 groundtruth', 'Robot 5 estimate', 'Robot 5 groundtruth'))
plt.legend((scat1e, scat1g, scat2e, scat2g, scat3e, scat3g, scat4e, scat4g, scat5e, scat5g), ('Robot 1 estimate', 'Robot 1 groundtruth', 'Robot 2 estimate', 'Robot 2 groundtruth', 'Robot 3 estimate', 'Robot 3 groundtruth', 'Robot 4 estimate', 'Robot 4 groundtruth', 'Robot 5 estimate', 'Robot 5 groundtruth'), loc='upper right', shadow=True, fontsize=8)
# Show graph
plt.show()
'''
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment