clear all
close all

bagfilename = 'MKZ_sample_datafile.bag';
bag = rosbag(bagfilename);
%%
%show all topics
topics=bag.AvailableTopics
%select topic
imu_topic = select(bag,'Topic','/vehicle/imu/data_raw');
%read topic message
msgStructs = readMessages(imu_topic,'DataFormat','struct');

for i=1:length(msgStructs)
    %Parse Accel
    imu.accel_x(i) = msgStructs{i}.LinearAcceleration.X;
    imu.accel_y(i) = msgStructs{i}.LinearAcceleration.Y;
    imu.accel_z(i) = msgStructs{i}.LinearAcceleration.Z;
    %Parse Gryo
    imu.gyro_x(i) = msgStructs{i}.AngularVelocity.X;
    imu.gyro_y(i) = msgStructs{i}.AngularVelocity.Y;
    imu.gyro_z(i) = msgStructs{i}.AngularVelocity.Z;
    %Parse Time (Time is saved as an integegers so must be converted to
    %double precision to covert the nanoseconds and add to seconds)
    imu.time(i)=double(msgStructs{i}.Header.Stamp.Sec)+double(msgStructs{i}.Header.Stamp.Nsec)*1e-9;
    
end
    
imu.time=imu.time-imu.time(1);

plot(imu.time,imu.gyro_x);

% othertopics of interest:
%   /vehicle/throttle_report    % throttle
%   /vehicle/gps/time           % GPS Time                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               }
%    /vehicle/gps/vel           % GPS Velocity
%   /vehicle/gps/fix           % GPS Position
%   /nav_box/odom              % Truth Position
%   /vehicle/steering_report   % steer angle and torque
%  /vehicle/wheel_speed_report % wheelspeeds

TOPIC='/vehicle/throttle_report';
topic = select(bag,'Topic',TOPIC);
%read topic message
structs = readMessages(topic,'DataFormat','struct');

TOPIC='/vehicle/gps/vel';
topic = select(bag,'Topic',TOPIC);
%read topic message
structs = readMessages(topic,'DataFormat','struct');

TOPIC='/nav_box/attitude';
topic = select(bag,'Topic',TOPIC);
%read topic message
structs = readMessages(topic,'DataFormat','struct');

TOPIC='/vehicle/gps/fix';
topic = select(bag,'Topic',TOPIC);
%read topic message
structs = readMessages(topic,'DataFormat','struct');

TOPIC='/vehicle/gps/time';
topic = select(bag,'Topic',TOPIC);
%read topic message
structs2 = readMessages(topic,'DataFormat','struct');

for i=1:length(structs)
    %Parse Accel
    GPS.lat(i) = structs{i}.Latitude;
    GPS.long(i) = structs{i}.Longitude;
    GPS.alt(i) = structs{i}.Altitude;
    
    %Parse Time (Time is saved as an integegers so must be converted to
    %double precision to covert the nanoseconds and add to seconds)
    %Note this is NOT GPS Time (this is the CPU time of the measurement
    %recording).  GPS Time is in topic '/vehicle/gps/time' and is
    %structs{1}.TimeRef.Sec
    GPS.time.CPU(i)=double(structs{i}.Header.Stamp.Sec)+double(structs{i}.Header.Stamp.Nsec)*1e-9;
    GPS.time.RCVR(i)=double(structs2{i}.TimeRef.Sec);  % Not sure if this is true GPS time
end
  GPS.time.CPU=GPS.time.CPU-GPS.time.CPU(1);
