rfid example

matlab/plot_rfid.m

+close('all')
+
+NS = 'p2';
+
+% Create a bag file object with the file name
+bag = rosbag('/home/bsb/WorkingCopies/mrc/smsundey/mrc_project/data/two_bots.bag')
+ 
+% Display a list of the topics and message types in the bag file
+bag.AvailableTopics
+
+
+%% Odometry estimate from P3AT
+
+bagselect = select(bag,'Topic',['/' NS '/my_p3at/pose']);
+% Create a time series object based on the fields of the message type
+ts_odom = timeseries(bagselect,'Pose.Pose.Position.X','Pose.Pose.Position.Y',...
+    'Pose.Pose.Orientation.X','Pose.Pose.Orientation.Y',...
+    'Pose.Pose.Orientation.Z','Pose.Pose.Orientation.W',...
+    'Twist.Twist.Linear.X');
+% Extract the data as simple vectors.
+x_odom = ts_odom.Data(:,1);
+y_odom = ts_odom.Data(:,2);
+angles = quat2eul([ts_odom.Data(:,6), ts_odom.Data(:,3),ts_odom.Data(:,4),ts_odom.Data(:,5)]);
+yaw_odom = angles(:,1);
+vel_odom=ts_odom.Data(:,7);
+
+figure();
+clf();
+plot(x_odom,y_odom);
+hold on
+plot(x_odom(1),y_odom(1),'go','markerfacecolor','g')
+plot(x_odom(end),y_odom(end),'ro','markerfacecolor','r')
+xlabel('Odom X [m]')
+ylabel('Odom Y [m]')
+title('Raw Odometry Estimate')
+title('Odometry Estimate')
+legend('Path','Start','End')
+axis('equal')
+grid on;
+
+%% Geonav from GPS/IMU
+bagselect = select(bag,'Topic',['/' NS '/geonav_odom']);
+
+% Create a time series object based on the fields of the message type
+ts_nav = timeseries(bagselect,'Pose.Pose.Position.X','Pose.Pose.Position.Y',...
+    'Pose.Pose.Orientation.X','Pose.Pose.Orientation.Y',...
+    'Pose.Pose.Orientation.Z','Pose.Pose.Orientation.W',...
+    'Twist.Twist.Linear.X');
+% Extract the data as simple vectors.
+x = ts_nav.Data(:,1);
+y = ts_nav.Data(:,2);
+angles = quat2eul([ts_nav.Data(:,6), ts_nav.Data(:,3),ts_nav.Data(:,4),ts_nav.Data(:,5)]);
+yaw = angles(:,1);
+vel=ts_nav.Data(:,7);
+
+% Plot the path with endpoints
+figure();
+clf();
+plot(x,y);
+hold on
+plot(x(1),y(1),'go','markerfacecolor','g')
+plot(x(end),y(end),'ro','markerfacecolor','r')
+xlabel('Geonav X [m]')
+ylabel('Geonav Y [m]')
+legend('Path','Start','End')
+axis('equal')
+grid on;
+
+% Quiver plot to check the heading alignment.
+figure();
+clf()
+u = vel.*cos(yaw);
+v = vel.*sin(yaw);
+ii = 1:10:length(x);  % Decimate the data so that it plot only every Nth point.
+quiver(x(ii),y(ii),u(ii)*10,v(ii)*10);
+hold on
+plot(x(1),y(1),'go','markerfacecolor','g')
+plot(x(end),y(end),'ro','markerfacecolor','r')
+xlabel('Geonav X [m]')
+ylabel('Geonav Y [m]')
+axis('equal')
+grid on;
+
+% Extract the covariance estimates from the navigation solution
+% as a measure of navigation quality
+varX = [];
+varY = [];
+msgs = readMessages(bagselect);
+for ii = 1:length(msgs)
+    varX(ii) = msgs{ii}.Pose.Covariance(1);
+    varY(ii) = msgs{ii}.Pose.Covariance(8);
+end
+
+figure(4);
+clf();
+plot(ts_nav.Time,varX,ts_nav.Time,varY);
+legend('\sigma_x^2','\sigma_y^2');
+xlabel('Time[s]')
+ylabel('Variance [m^2]')
+
+%% Compare odometry and geonav estimates
+% Normalize the odometry with the initial geonav estimate
+dyaw = yaw_odom(1)-yaw(1);  % rotation angle for odom frame 
+dx = x_odom-x_odom(1);
+dy = y_odom-y_odom(1);
+x_o = (dx.*cos(dyaw)-dy.*sin(dyaw))+x(1);
+y_o = (dx.*sin(dyaw)+dy.*cos(dyaw))+y(1);
+
+figure();
+clf();
+plot(x_o,y_o,x,y);
+legend('Odometry','Geonav');
+hold on
+plot(x(1),y(1),'go','markerfacecolor','g')
+plot(x(end),y(end),'ro','markerfacecolor','r')
+plot(x_o(1),y_o(1),'go','markerfacecolor','g')
+plot(x_o(end),y_o(end),'ro','markerfacecolor','r')
+grid on;
+axis('equal')
+xlabel('X [m]')
+ylabel('Y [m]')
+
+%% Navigation Sensor Status
+% Plot the status of the navigation solution
+bagstatus = select(bag,'Topic',['/' NS '/nav/status']);
+msgs = readMessages(bagstatus);
+status = []
+for ii = 1:length(msgs)
+    status(ii,:) = msgs{ii}.Data;
+end
+ts_status = timeseries(bagstatus);
+
+figure();
+clf();
+plot(ts_status.Time,status)
+legend('FilterState','DynamicsMode','StatusFlags');
+title('Navigation Kalman Filter Status')
+xlabel('Time [s]')
+ylabel('Value')
+
+
+%%  RFID tag times
+bagrfid = select(bag,'Topic',['/' NS '/rfid']);
+ts_rfid = timeseries(bagrfid);
+
+% Plot the times of the rfid tag collections
+figure()
+plot(ts_rfid.Time-ts_rfid.Time(1),1:length(ts_rfid.Time),'o')
+xlabel('Time [s]')
+ylabel('RFID detection index [N]')
+grid on
+
+% Estract the tag IDs as a cell array
+msgs = readMessages(bagrfid);
+rfids = {};
+for ii = 1:length(msgs)
+    rfids{ii} = msgs{ii}.Data;
+end
+
+% Re-plot the geonav positions
+% Plot the path with endpoints
+figure(10);
+clf();
+plot(ts_nav.Data(:,1),ts_nav.Data(:,2));
+hold on
+plot(ts_nav.Data(1,1),ts_nav.Data(1,2),'go','markerfacecolor','g')
+plot(ts_nav.Data(end,1),ts_nav.Data(end,2),'ro','markerfacecolor','r')
+xlabel('Geonav X [m]')
+ylabel('Geonav Y [m]')
+legend('Path','Start','End')
+axis('equal')
+grid on;
+
+% Find the unique ID numbers
+rfid_u = unique(rfids);
+
+% For each unique ID, determine the x/y locations via interpolation
+clist = hsv(length(rfid_u));  % list of colors
+for ii = 1:length(rfid_u)
+    II = find(ismember(rfids,rfid_u{ii}));
+    if ~isempty(II)
+        t_rfid = ts_rfid.Time(II);
+        % Interpolation
+        x_rfid = interp1(ts_nav.Time,ts_nav.Data(:,1),t_rfid,'nearest');
+        y_rfid = interp1(ts_nav.Time,ts_nav.Data(:,2),t_rfid,'nearest');
+        % Plot each detection
+        plot(x_rfid,y_rfid,'o','col',clist(ii,:))
+        % Then plot the average location (centroid)
+        plot(mean(x_rfid),mean(y_rfid),'x','col',clist(ii,:),'markersize',20)
+    end
+    
+end
+
+    

matlab/post_odom.m

@@ -3,6 +3,7 @@ close('all')
 % Create a bag file object with the file name
 %bag = rosbag('../data/perimeter_all3.bag')
 bag = rosbag('2017-05-30-20-02-36.bag');
+
  
 % Display a list of the topics and message types in the bag file
 bag.AvailableTopics

matlab/post_rfid.m

+close('all')
+
+% Create a bag file object with the file name
+bag = rosbag('/home/bsb/WorkingCopies/mrc/smsundey/mrc_project/data/two_bots.bag')
+ 
+% Display a list of the topics and message types in the bag file
+bag.AvailableTopics
+ 
+
+%% Odometry estimate from P3AT
+
+bagselect = select(bag,'Topic','/p5/my_p3at/pose');
+% Create a time series object based on the fields of the message type
+ts_odom = timeseries(bagselect,'Pose.Pose.Position.X','Pose.Pose.Position.Y',...
+    'Pose.Pose.Orientation.X','Pose.Pose.Orientation.Y',...
+    'Pose.Pose.Orientation.Z','Pose.Pose.Orientation.W',...
+    'Twist.Twist.Linear.X');
+% Extract the data as simple vectors.
+x_odom = ts_odom.Data(:,1);
+y_odom = ts_odom.Data(:,2);
+angles = quat2eul([ts_odom.Data(:,6), ts_odom.Data(:,3),ts_odom.Data(:,4),ts_odom.Data(:,5)]);
+yaw_odom = angles(:,1);
+vel_odom=ts_odom.Data(:,7);
+
+figure();
+clf();
+plot(x_odom,y_odom);
+hold on
+plot(x_odom(1),y_odom(1),'go','markerfacecolor','g')
+plot(x_odom(end),y_odom(end),'ro','markerfacecolor','r')
+xlabel('Odom X [m]')
+ylabel('Odom Y [m]')
+title('Raw Odometry Estimate')
+title('Odometry Estimate')
+legend('Path','Start','End')
+axis('equal')
+grid on;
+
+%% Geonav from GPS/IMU
+bagselect = select(bag,'Topic','/p5/geonav_odom');
+
+% Create a time series object based on the fields of the message type
+ts_nav = timeseries(bagselect,'Pose.Pose.Position.X','Pose.Pose.Position.Y',...
+    'Pose.Pose.Orientation.X','Pose.Pose.Orientation.Y',...
+    'Pose.Pose.Orientation.Z','Pose.Pose.Orientation.W',...
+    'Twist.Twist.Linear.X');
+% Extract the data as simple vectors.
+x = ts_nav.Data(:,1);
+y = ts_nav.Data(:,2);
+angles = quat2eul([ts_nav.Data(:,6), ts_nav.Data(:,3),ts_nav.Data(:,4),ts_nav.Data(:,5)]);
+yaw = angles(:,1);
+vel=ts_nav.Data(:,7);
+
+% Plot the path with endpoints
+figure();
+clf();
+plot(x,y);
+hold on
+plot(x(1),y(1),'go','markerfacecolor','g')
+plot(x(end),y(end),'ro','markerfacecolor','r')
+xlabel('Geonav X [m]')
+ylabel('Geonav Y [m]')
+legend('Path','Start','End')
+axis('equal')
+grid on;
+
+% Quiver plot to check the heading alignment.
+figure();
+clf()
+u = vel.*cos(yaw);
+v = vel.*sin(yaw);
+ii = 1:10:length(x);  % Decimate the data so that it plot only every Nth point.
+quiver(x(ii),y(ii),u(ii)*10,v(ii)*10);
+hold on
+plot(x(1),y(1),'go','markerfacecolor','g')
+plot(x(end),y(end),'ro','markerfacecolor','r')
+xlabel('Geonav X [m]')
+ylabel('Geonav Y [m]')
+axis('equal')
+grid on;
+
+% Extract the covariance estimates from the navigation solution
+% as a measure of navigation quality
+varX = [];
+varY = [];
+msgs = readMessages(bagselect);
+for ii = 1:length(msgs)
+    varX(ii) = msgs{ii}.Pose.Covariance(1);
+    varY(ii) = msgs{ii}.Pose.Covariance(8);
+end
+
+figure(4);
+clf();
+plot(ts_nav.Time,varX,ts_nav.Time,varY);
+legend('\sigma_x^2','\sigma_y^2');
+xlabel('Time[s]')
+ylabel('Variance [m^2]')
+
+%% Compare odometry and geonav estimates
+% Normalize the odometry with the initial geonav estimate
+dyaw = yaw_odom(1)-yaw(1);  % rotation angle for odom frame 
+dx = x_odom-x_odom(1);
+dy = y_odom-y_odom(1);
+x_o = (dx.*cos(dyaw)-dy.*sin(dyaw))+x(1);
+y_o = (dx.*sin(dyaw)+dy.*cos(dyaw))+y(1);
+
+figure();
+clf();
+plot(x_o,y_o,x,y);
+legend('Odometry','Geonav');
+hold on
+plot(x(1),y(1),'go','markerfacecolor','g')
+plot(x(end),y(end),'ro','markerfacecolor','r')
+plot(x_o(1),y_o(1),'go','markerfacecolor','g')
+plot(x_o(end),y_o(end),'ro','markerfacecolor','r')
+grid on;
+axis('equal')
+xlabel('X [m]')
+ylabel('Y [m]')
+
+%% Navigation Sensor Status
+% Plot the status of the navigation solution
+bagstatus = select(bag,'Topic','/p5/nav/status');
+msgs = readMessages(bagstatus);
+status = []
+for ii = 1:length(msgs)
+    status(ii,:) = msgs{ii}.Data;
+end
+ts_status = timeseries(bagstatus);
+
+figure();
+clf();
+plot(ts_status.Time,status)
+legend('FilterState','DynamicsMode','StatusFlags');
+title('Navigation Kalman Filter Status')
+xlabel('Time [s]')
+ylabel('Value')
+