Make the program quit and print an Error message when the provided 'imuTopic' does match any in rosbag

bagconvert/src/main.cpp

@@ -83,6 +83,15 @@ int main(int argc, char **argv) {
             dataIMU.push_back(s1->angular_velocity.y);
             dataIMU.push_back(s1->angular_velocity.z);
         }
+	else if(imuTopic != m.getTopic())
+	{
+	    ROS_INFO("Imu topic %s, Does not match any in rosbag: %s\n"
+		     "Please provide the correct topic name," 
+		     "qualified by appropriate namespaces\n"
+		     "eg: /<my-namespace>/<my-sub-namespace>/<my-imu-topic>\n",
+		      imuTopic.c_str(),pathBag.c_str());
+	    exit(EXIT_FAILURE);
+	}
 
     }
 

matlab/SCRIPT_allan_matparallel.m

@@ -8,7 +8,7 @@
 % Our bag information
 %mat_path = '../data/imu_mtig700.mat';
 %mat_path = '../data/imu_tango.mat';
-mat_path = '../data/imu_visensor.mat';
+mat_path = '/home/talha/rosbag_data/imu.mat';
 
 % IMU information (todo: move this to the yaml file)
 %update_rate = 400;

matlab/SCRIPT_process_results.m

@@ -13,8 +13,8 @@
 %titlestr = 'Tango Yellowstone #1';
 %mat_path = '../data/bags/results_20171031T115123.mat';
 
-titlestr = 'ADIS16448 VI-Sensor';
-mat_path = '../data/bags/results_20180206T140217.mat';
+titlestr = 'ADIS16364BMLZ';
+mat_path = '../data/results_20200621T112259.mat';
 
 % Load the mat file (should load "data_imu" matrix)
 fprintf('=> opening the mat file.\n')
@@ -24,7 +24,7 @@
 %% Get the calculated sigmas
 
 fprintf('=> plotting accelerometer.\n')
-[fh1,sigma_a,sigma_ba] = gen_chart(results_ax.tau1,results_ax.sig2,...
+[fh1,sigma_a,sigma_ba] = gen_chart1(results_ax.tau1,results_ax.sig2,...
                                     results_ay.sig2,results_az.sig2,...
                                     titlestr,'acceleration','m/s^2',...
                                     'm/s^2sqrt(Hz)','m/s^3sqrt(Hz)');

matlab/functions/gen_chart1.m

@@ -0,0 +1,91 @@
+function [fh1, sigmaw, sigmab] = gen_chart1(tau,sigx,sigy,sigz,titlestr,name,unit0,unit1,unit2)
+
+% Calculate our average sigma values
+sigavg = mean([sigx;sigy;sigz]);
+
+% =======================================================================
+% Plot the results on a figure
+mfh1 = figure;
+loglog(tau, sigx); hold on;
+loglog(tau, sigy); hold on;
+loglog(tau, sigz); hold on;
+loglog(tau, sigavg); hold on;
+grid on;
+title([titlestr,' ',name]);
+xlabel('\tau [sec]');
+ylabel(['Normal Allan Deviation [',unit0,']']);
+legend(['x-',name],['y-',name],['z-',name],'average','Location','southeast');
+
+% =======================================================================
+% Find location of tau=1 by finding where difference is near zero
+tauref = 1;
+taudiff = abs(tau-tauref);
+tauid1 = find(taudiff == min(taudiff),1);
+fprintf('tau = %.2f | tauid1 = %d\n',tau(tauid1),tauid1);
+
+% We will fit our "white-noise" line where tau is 1
+windowsize = 50;
+%window = tauid1-windowsize:tauid1+windowsize;
+minid = find(sigavg == min(sigavg)); % find where the min is
+window = 1:minid-windowsize; % go from start to min
+
+% Get our x and y values
+x = tau(window);
+y = sigavg(window);
+nanx = isfinite(y);
+
+% Fit to log-log scale (slope of -1/2)
+%coeffs = polyfit(log(x(nanx)), log(y(nanx)), 1);
+coeffs(1)= -0.5;
+intcs = log(y(nanx)./x(nanx).^coeffs(1));
+coeffs(2) = mean(intcs);
+fprintf('h_fit1 slope = %.4f | y-intercept = %.4f\n',coeffs(1),coeffs(2));
+
+% Convert from logarithmic scale to linear scale and plot
+h_fit1 = tau.^coeffs(1).*exp(coeffs(2));
+pltlin = plot(tau, h_fit1,'Color','r','LineWidth',2); hold on;
+pltlin.Color(4) = 0.5; % make it semi-transparent
+
+
+% =======================================================================
+% Next we should fit a 1/2 line to the bias side of the allan plot
+minid = 1200; % find where the min is
+window = minid+windowsize:1870; % go from min to the end
+
+% Get our x and y values
+x = tau(window);
+y = sigavg(window);
+nanx = isfinite(y);
+
+% Calculate the intercept given the slope of +1/2
+coeffs(1)= 0.5;
+intcs = log(y(nanx)./x(nanx).^coeffs(1));
+coeffs(2) = mean(intcs);
+fprintf('h_fit2 slope = %.4f | y-intercept = %.4f\n',0.5,mean(intcs));
+
+% Convert from logarithmic scale to linear scale and plot
+h_fit2 = tau.^coeffs(1).*exp(coeffs(2));
+pltlin = plot(tau, h_fit2,'Color','b','LineWidth',2); hold on;
+pltlin.Color(4) = 0.5; % make it semi-transparent
+
+
+% Get what our bias should be (should be at a tau of 3)
+tauref = 3;
+taudiff = abs(tau-tauref);
+tauid2 = find(taudiff == min(taudiff),1);
+fprintf('tau = %.2f | tauid2 = %d\n',tau(tauid2),tauid2);
+
+
+% =======================================================================
+% Record the values for the output
+sigmaw = h_fit1(tauid1);
+sigmab = h_fit2(tauid2);
+
+% Plot the values
+str1 = sprintf('\\sigma = %.6f %s',h_fit1(tauid1),unit1);
+str2 = sprintf('\\sigma_{b} = %.6f %s',h_fit2(tauid2),unit2);
+text(.1,.15,{str1,str2},'Units','normalized'); %'FontWeight','bold'
+
+
+end
+