% bag =
    rosbag('20160527T233932-kitt_01-e9c10a47-0000.bag') % bag.AvailableTopics %
    % bagselect =
        select(bag, 'Topic', {'/driving/Trajectory2D', '/autobox/control'}) %
    bagselect = select(bag, 'Topic', '/driving/Trajectory2D')

            data = load('trajectory_data_matlab');
size_of_point = 12;
number_of_points = (size(data, 2) - 3) / size_of_point

    % % Plot a trajectory : start_ind = 4;
traj_i = 1;
t = data(traj_i, start_ind : 12 : end);
x = data(traj_i, (start_ind + 1) : 12 : end);
y = data(traj_i, (start_ind + 2) : 12 : end);
theta = data(traj_i, (start_ind + 3) : 12 : end) * 180 / pi;
v = data(traj_i, (start_ind + 6) : 12 : end);
a = data(traj_i, (start_ind + 7) : 12 : end);

figure(1), clf;

subplot(1, 3, 1);
p1 = plot(x, y);
p1.XDataSource = 'x';
p1.YDataSource = 'y';
xlabel('x'), ylabel('y'), legend('pos');

subplot(1, 3, 2);
p2 = plot(t, v);
p2.XDataSource = 't';
p2.YDataSource = 'v';
xlabel('time(sec)'), ylabel('Vel'), legend('vel');

subplot(1, 3, 3);
p3 = plot(t, a);
p3.XDataSource = 't';
p3.YDataSource = 'a';
xlabel('time(sec)'), ylabel('acc'), legend('acc');

%% Loop through data and time it:
tic
%linkdata on
for traj_i = 1:size(data,1)
    t = data(traj_i, start_ind:12:end);
x = data(traj_i, (start_ind + 1) : 12 : end);
y = data(traj_i, (start_ind + 2) : 12 : end);
theta = data(traj_i, (start_ind + 3) : 12 : end) * 180 / pi;
v = data(traj_i, (start_ind + 6) : 12 : end);
a = data(traj_i, (start_ind + 7) : 12 : end);
refreshdata;
drawnow();
disp(traj_i);
end toc
