diff --git a/bag/.gitignore b/bag/.gitignore
index b31508bea3f98d2266bee18e116937c12bfb5f0c..497d01c10f67b0f921784a1d45f7307ef2172cbb 100644
--- a/bag/.gitignore
+++ b/bag/.gitignore
@@ -1,5 +1,5 @@
*.bag
-CSV/
+CSV*/
recordings/
!analysys/
!.gittignore
diff --git a/bag/analysys/.gitignore b/bag/analysys/.gitignore
index 8368639dff2739e4540c29c0a300bdf06f46fb4e..8728d95f43eaf277999c9999026618b5514f569d 100644
--- a/bag/analysys/.gitignore
+++ b/bag/analysys/.gitignore
@@ -1,2 +1,5 @@
*.asv
*.jpg
+*.fig
+*.m
+*.eps
diff --git a/bag/analysys/compare.m b/bag/analysys/compare.m
index 11bca203905fc13012481fef79169966bd3edb0e..f810641b2176661284198f9a925b58362cf56245 100644
--- a/bag/analysys/compare.m
+++ b/bag/analysys/compare.m
@@ -1,4 +1,8 @@
clc; clear; clearvars; close all;
+%time at which to start counting data
+tstart=0;
+%Number of frames to cut
+N=8;
%Folder where CSV files are saved
CSVFolder = '../CSV/transp/';
@@ -24,13 +28,13 @@ for k = 1:length(myFiles)
fprintf('Now reading file %s\n', baseFileName);
Table = readtable(fullFileName,'PreserveVariableNames',true);
if baseFileName(1) == 'G'
- [Gx, Gy, Gz, Gang, Gt, Gts] = extractGT(Table, 0);
+ [Gx, Gy, Gz, Gang, Gt, Gts] = extractGT(Table, tstart);
Gvec = [Gvec, {baseFileName, Gx, Gy, Gz, Gang, Gt, Gts}'];
- elseif baseFileName(1) == 'E'
- [Ex, Ey, Ez, Eang, Et, Ets] = extractdata(Table, 0);
+ elseif baseFileName(1) == 'E'
+ [Ex, Ey, Ez, Eang, Et, Ets] = extractdata(Table, tstart);
Evec = [Evec, {baseFileName, Ex, Ey, Ez, Eang, Et, Ets}'];
elseif baseFileName(1) == 'T'
- [Tx, Ty, Tz, Tang, Tt, Tts] = extractdata(Table, 0);
+ [Tx, Ty, Tz, Tang, Tt, Tts] = extractdata(Table, tstart);
Tvec = [Tvec, {baseFileName, Tx, Ty, Tz, Tang, Tt, Tts}'];
end
end
@@ -42,8 +46,9 @@ min_len3d = 99999999999999999999999999999999999999999999999999999999999999999999
for i = 1:size(Evec,2)
%Get position and angular error vectors for exepriments comparing them
%to GroundTruth
- [perrors2d, angerrors2d] = geterror(Gvec{7,i}, Evec{7,i});
- [perrors3d, angerrors3d] = geterror(Gvec{7,i}, Tvec{7,i});
+ [perrors2d, angerrors2d, tstart2] = geterror(Gvec{7,i}, Evec{7,i}, N);
+ [perrors3d, angerrors3d, tstart2] = geterror(Gvec{7,i}, Tvec{7,i}, N);
+ tstart = max(tstart, tstart2);
%Calculate RMSE without subsampling vectors
Initial_RMSE2d = [sqrt(mean(perrors2d.^2)), sqrt(mean(angerrors2d.^2))];
@@ -55,33 +60,31 @@ for i = 1:size(Evec,2)
%Print error figures without showing, they will be compiled tiled in
%the following for
figure('visible','off');
- plot(linspace(0,366, size(ERRORS{3,i},2)), ERRORS{3,i})
- title(strcat('2D Position error for timespan', str2double(replace(regexp(ERRORS{1,i},'\d{0,2}_\d', 'once', 'match'), '_', '.')))
+ hold on;
+ plot(linspace(tstart,366, size(ERRORS{3,i},2)), ERRORS{3,i})
+ plot(linspace(tstart,366, size(ERRORS{6,i},2)), ERRORS{6,i})
+ title(strcat('Position error for timespan ', ' ', replace(regexp(ERRORS{1,i},'\d{0,2}_\d', 'once', 'match'), '_', '.')))
+ legend('2D', '3D')
xlabel('seconds')
ylabel('meters')
-
- figure('visible','off');
- plot(linspace(0,366, size(ERRORS{4,i},2)), ERRORS{4,i})
- title(strcat('2D Angular error for timespan', str2double(replace(regexp(ERRORS{1,i},'\d{0,2}_\d', 'once', 'match'), '_', '.')))
- xlabel('seconds')
- ylabel('radians')
+ hold off;
figure('visible','off');
- plot(linspace(0,366, size(ERRORS{6,i},2)), ERRORS{6,i})
- title(strcat('3D Position error for timespan ', str2double(replace(regexp(ERRORS{1,i},'\d{0,2}_\d', 'once', 'match'), '_', '.')))
- xlabel('seconds')
- ylabel('meters')
-
- figure('visible','off');
- plot(linspace(0,366, size(ERRORS{7,i},2)), ERRORS{7,i})
- title(strcat('3D Angular error for timespan ', str2double(replace(regexp(ERRORS{1,i},'\d{0,2}_\d', 'once', 'match'), '_', '.')))
+ hold on;
+ plot(linspace(tstart,366, size(ERRORS{4,i},2)), ERRORS{4,i})
+ plot(linspace(tstart,366, size(ERRORS{7,i},2)), ERRORS{7,i})
+ title(strcat('Angular error for timespan ', ' ', replace(regexp(ERRORS{1,i},'\d{0,2}_\d', 'once', 'match'), '_', '.')))
+ legend('2D', '3D')
xlabel('seconds')
ylabel('radians')
+ hold off;
end
%Plot errors grouped into figures
ploterrorstiled
+ploterrorsoverlapped
+
%Subsample errors uniformly so that they have the same number of sample and
%recalculate the RMSE
for i = 1:size(ERRORS,2)
@@ -119,4 +122,5 @@ for i = 1:size(ERRORS,2)
ERRORS{8,i} = Final_RMSE2d;
ERRORS{9,i} = Final_RMSE3d;
end
-plotrmse(ERRORS)
\ No newline at end of file
+%plotrmse(ERRORS, true)
+plotrmse(ERRORS, false)
\ No newline at end of file
diff --git a/bag/analysys/extractGT.m b/bag/analysys/extractGT.m
index 2a5552dac9bc64fcaefc21483bfa1e0a476e9ab0..4182668a1be91204b1866aeb9c588d332b2eab04 100644
--- a/bag/analysys/extractGT.m
+++ b/bag/analysys/extractGT.m
@@ -1,4 +1,4 @@
-function [Gx, Gy, Gz, Gang, Gt, Gts] = extractGT(GT, t0)
+function [Gx, Gy, Gz, Gang, Gt, Gts] = extractGT(GT, tstart)
%Given a Table object extracts the necessary data. Intended for the
%GroundTruth, with message being a Trajectory
t0 = GT(6,2).Var2;
@@ -19,20 +19,20 @@ function [Gx, Gy, Gz, Gang, Gt, Gts] = extractGT(GT, t0)
for i=1:height(GT)
if (size(regexp(GT(i,1).Var1{1,1}, texpr)) > 0)
GT(i,2).Var2 = (GT(i, 2).Var2 - t0)*1e-9;
- if GT(i,2).Var2 >= t0
+ if GT(i,2).Var2 >= tstart
Gt = [Gt, GT(i,2).Var2];
end
end
- if (GT(i,2).Var2 >= t0) && (size(regexp(GT(i,1).Var1{1,1}, xexpr)) > 0)
+ if ~isempty(Gt) & (Gt(end) >= tstart) & ~isempty(regexp(GT(i,1).Var1{1,1}, xexpr, 'once'))
Gx = [Gx, GT(i,2).Var2];
end
- if (GT(i,2).Var2 >= t0) && (size(regexp(GT(i,1).Var1{1,1}, yexpr)) > 0)
+ if ~isempty(Gt) & (Gt(end) >= tstart) & ~isempty(regexp(GT(i,1).Var1{1,1}, yexpr, 'once'))
Gy = [Gy, GT(i,2).Var2];
end
- if (GT(i,2).Var2 >= t0) && (size(regexp(GT(i,1).Var1{1,1}, zexpr)) > 0)
+ if ~isempty(Gt) & (Gt(end) >= tstart) & ~isempty(regexp(GT(i,1).Var1{1,1}, zexpr, 'once'))
Gz = [Gz, GT(i,2).Var2];
end
- if (GT(i,2).Var2 >= t0) && (size(regexp(GT(i,1).Var1{1,1}, thexpr)) > 0)
+ if ~isempty(Gt) & (Gt(end) >= tstart) & ~isempty(regexp(GT(i,1).Var1{1,1}, thexpr, 'once'))
[r1, r2, r3] = quat2angle([GT(i+3,2).Var2 GT(i,2).Var2 GT(i+1,2).Var2 GT(i+2,2).Var2]);
Gang = [Gang, [r1 r2 r3]'];
end
diff --git a/bag/analysys/extractdata.m b/bag/analysys/extractdata.m
index 0a509aad9354e961cdfc3eccc4d19fe7386329e2..802975bb4667e0e6a864578ad51806a849720dc7 100644
--- a/bag/analysys/extractdata.m
+++ b/bag/analysys/extractdata.m
@@ -1,4 +1,4 @@
-function [gx, gy, gz, gang, gt, gts] = extractdata(GC, t0)
+function [gx, gy, gz, gang, gt, gts] = extractdata(GC, tstart)
%Given a Table object extracts the necessary data. Intended for the 2d and
%3d experiments
@@ -12,8 +12,13 @@ function [gx, gy, gz, gang, gt, gts] = extractdata(GC, t0)
gt = gt-t0;
gt = gt*1e-9;
- n = length(gt>=t0);
-
+ %We cut all elements with a timestamp lower than tstart
+ n = sum(gt<=tstart);
+ gt = gt(n:end);
+ gx = gx(n:end);
+ gy = gy(n:end);
+ gz = gz(n:end);
+ gang = gang(:,n:end);
%remove duplicates. Due to the high sampling rate of the PosePublisher
%some timestamps will have been saved multiple times, this cuts them
diff --git a/bag/analysys/geterror.m b/bag/analysys/geterror.m
index a1836ad4e5cdd06ee5393774af10dc0e7941fbfc..299b2a99f5fc2c0aee99ff3a2515df8adaf017a2 100644
--- a/bag/analysys/geterror.m
+++ b/bag/analysys/geterror.m
@@ -1,12 +1,14 @@
-function [Poserror, Angerror] = geterror(GT, TS)
+function [Poserror, Angerror, tstart] = geterror(GT, TS, N)
%Returns position and angular error vectors given a GT groundtruth
- %timeseries and a TS experiment time series
- npairs = length(GT.Time)-1;
+ %timeseries and a TS experiment time series, skipping N frames at the
+ %beginning
+ npairs = length(GT.Time)-N-1;
Poserror = zeros(1, npairs);
Angerror = zeros(1, npairs);
+ tstart = GT.Time(N+1);
for i = 1:npairs
- [pe, ae] = matchaxes2(GT, TS, i, i+1);
+ [pe, ae] = matchaxes2(GT, TS, i+N, i+N+1);
Poserror(i) = pe;
Angerror(i) = ae;
end
diff --git a/bag/analysys/ploterrorstiled.m b/bag/analysys/ploterrorstiled.m
index ddea792e41e49c297e8039704c5f63c6a2c801a1..143df8d4ac04deeae801c27cfa1b7c3b0c0180f7 100644
--- a/bag/analysys/ploterrorstiled.m
+++ b/bag/analysys/ploterrorstiled.m
@@ -1,13 +1,16 @@
%Takes all previous figures and groups them tiled in groups of 4
a = findobj('Type','axes');
-m = 2;
+m = 1;
n = 2;
for i = 1:size(Evec,2)
- figure
- for k=1:4
+ f = figure;
+ for k=1:2
tmp1 = subplot(m,n,k);
- tmp2 = copyobj(a(((i-1)*4)+k),gcf);
+ tmp2 = copyobj(a(((i-1)*2)+k),gcf);
set(tmp2,'Position',get(tmp1,'Position'))
delete(tmp1)
end
+ saveas(f,strcat('tiled', ' ', int2str(i)),'mfig');
+ saveas(f,strcat('tiled', ' ', int2str(i)),'eps');
+ saveas(f,strcat('tiled', ' ', int2str(i)),'jpg');
end
\ No newline at end of file
diff --git a/bag/analysys/plotrmse.m b/bag/analysys/plotrmse.m
index 7c9d64dc95d488f0e25053bdea1553a467234197..e6032530b43322c1d212f55f26aadbf5e4b93f32 100644
--- a/bag/analysys/plotrmse.m
+++ b/bag/analysys/plotrmse.m
@@ -1,10 +1,18 @@
-function plotrmse(ERRORS)
+function plotrmse(ERRORS, subsampled)
errvec = zeros(5,size(ERRORS,2));
- for i = 1:size(ERRORS,2)
- %Get timespan through regular expression of the string
- timespan = str2double(replace(regexp(ERRORS{1,i},'\d{0,2}_\d', 'once', 'match'), '_', '.'));
- errvec(:,i) = [ERRORS{8,i}(1); ERRORS{8,i}(2); ERRORS{9,i}(1); ERRORS{9,i}(2); timespan];
+ if subsampled
+ for i = 1:size(ERRORS,2)
+ %Get timespan through regular expression of the string
+ timespan = str2double(replace(regexp(ERRORS{1,i},'\d{0,2}_\d', 'once', 'match'), '_', '.'));
+ errvec(:,i) = [ERRORS{8,i}(1); ERRORS{8,i}(2); ERRORS{9,i}(1); ERRORS{9,i}(2); timespan];
+ end
+ else
+ for i = 1:size(ERRORS,2)
+ %Get timespan through regular expression of the string
+ timespan = str2double(replace(regexp(ERRORS{1,i},'\d{0,2}_\d', 'once', 'match'), '_', '.'));
+ errvec(:,i) = [ERRORS{2,i}(1); ERRORS{2,i}(2); ERRORS{5,i}(1); ERRORS{5,i}(2); timespan];
+ end
end
tsp2d = timeseries(errvec(1,:), errvec(5,:), 'Name', '2d Position error');
tsa2d = timeseries(errvec(2,:), errvec(5,:), 'Name', '2d Angular error');
@@ -16,7 +24,11 @@ function plotrmse(ERRORS)
hold on
plot(tsp3d.Time, tsp3d.Data(1,:))
hold off
- title('Position RMSE')
+ if subsampled
+ title('Position RMSE - subsampled')
+ else
+ title('Position RMSE - not subsampled')
+ end
xlabel('Timespan (s)')
ylabel('m')
legend('2D','3D')
@@ -25,27 +37,39 @@ function plotrmse(ERRORS)
hold on
plot(tsa3d.Time, tsa3d.Data(1,:))
hold off
- title('Angular RMSE')
+ if subsampled
+ title('Angular RMSE - subsampled')
+ else
+ title('Angular RMSE - not subsampled')
+ end
xlabel('Timespan (s)')
ylabel('rad')
legend('2D','3D')
- figure
- plot(tsp2d.Time, tsp2d.Data(1,:)./sqrt(tsp2d.Time)')
- hold on
- plot(tsp3d.Time, tsp3d.Data(1,:)./sqrt(tsp3d.Time)')
- hold off
- title('Position RMSE')
- xlabel('Timespan (s)')
- ylabel('m/sqrt(timespan)')
- legend('2D','3D')
- figure
- plot(tsa2d.Time, tsa2d.Data(1,:)./sqrt(tsa2d.Time)')
- hold on
- plot(tsa3d.Time, tsa3d.Data(1,:)./sqrt(tsa3d.Time)')
- hold off
- title('Angular RMSE')
- xlabel('Timespan (s)')
- ylabel('rad/sqrt(timespan)')
- legend('2D','3D')
+% figure
+% plot(tsp2d.Time, tsp2d.Data(1,:)./sqrt(tsp2d.Time)')
+% hold on
+% plot(tsp3d.Time, tsp3d.Data(1,:)./sqrt(tsp3d.Time)')
+% hold off
+% if subsampled
+% title('Position RMSE - subsampled and divided by dt')
+% else
+% title('Position RMSE - not subsampled and divided by dt')
+% end
+% xlabel('Timespan (s)')
+% ylabel('m/sqrt(timespan)')
+% legend('2D','3D')
+% figure
+% plot(tsa2d.Time, tsa2d.Data(1,:)./sqrt(tsa2d.Time)')
+% hold on
+% plot(tsa3d.Time, tsa3d.Data(1,:)./sqrt(tsa3d.Time)')
+% hold off
+% if subsampled
+% title('Angular RMSE - subsampled and divided by dt')
+% else
+% title('Angular RMSE - not subsampled and divided by dt')
+% end
+% xlabel('Timespan (s)')
+% ylabel('rad/sqrt(timespan)')
+% legend('2D','3D')
end
\ No newline at end of file
diff --git a/bag/filter_bag.bash b/bag/filter_bag.bash
index f9458ab76357603251d54cac4b9f7bbc906002eb..dd4909b20d1c2e863ab497f97d1e8b39fd7ddbe3 100644
--- a/bag/filter_bag.bash
+++ b/bag/filter_bag.bash
@@ -1,10 +1,10 @@
#!/bin/bash
if [[ -z "${BAG}" ]]
then
- BAG="ana_lab_2"
+ BAG="outside_6"
fi
rm $BAG\_filtered.bag
rm $BAG\_filtered_notf.bag
-rosbag filter $BAG.bag $BAG\_filtered.bag "topic == '/ana/imu/data' or topic == '/ana/odom' or topic == '/ana/sensors/bno055_imu/imu' or topic == '/ana/sensors/scan' or topic == '/tf'"
+rosbag filter $BAG.bag $BAG\_filtered.bag "topic == '/helena/sensors/imu/data' or topic == '/helena/odom' or topic == '/helena/sensors/bno055_imu/imu' or topic == '/helena/sensors/scan' or topic == '/tf'"
python3 filter_tf.py $BAG\_filtered.bag $BAG\_filtered_notf.bag
diff --git a/bag/generate_trajectory_csv.bash b/bag/generate_trajectory_csv.bash
deleted file mode 100644
index 165722833ba27721f2c105638e4c62705c166ee1..0000000000000000000000000000000000000000
--- a/bag/generate_trajectory_csv.bash
+++ /dev/null
@@ -1,39 +0,0 @@
-#!/bin/bash
-#echo "Press CTRL+C to proceed."
-#trap "pkill -f 'sleep 1h'" INT
-#trap "set +x ; sleep 1h ; set -x" DEBUG
-
-if [[ -z "${BAG}" ]]
-then
- BAG="ana_lab_2"
-fi
-
-#yamlfiles are divided into two types:
-# Starting with G ==> groundtruth, we extract the trajectory
-# Starting with E ==> experiment, we extract the poses
-for yamlfile in $(find ../yaml/trajectory_analysys -maxdepth 1 -type f -printf "%f\n")
-do
- yamlname=${yamlfile%.*}
- rm recordings/trajectory_recording$yamlname.bag
- roslaunch wolf_demo_imu2d imu2d_analysys.launch bag:=$BAG\_filtered_notf test:=$yamlfile speed:=1 suffix:=$yamlname rviz:=false
- if [[ ${yamlname:0:1} == 'G' ]]
- then
- tend=$(rosbag info -y -k end recordings/trajectory_recording$yamlname.bag)
- tend=${tend%.*}
- rosbag filter recordings/trajectory_recording$yamlname.bag recordings/trajectory_recording$yamlname\_tmp.bag "topic == '/wolf_ros_node/trajectory' and t.to_sec() >= $tend"
-
- rm CSV/$yamlname.csv
- rostopic echo -b recordings/trajectory_recording$yamlname\_tmp.bag /wolf_ros_node/trajectory -p > CSV/$yamlname.csv
- rm recordings/trajectory_recording$yamlname\_tmp.bag
-
- rm CSV/transp/$yamlname\_transp.csv
- python3 transpose_csv.py CSV/$yamlname.csv CSV/transp/$yamlname\_transp.csv
-
- elif [[ ${yamlname:0:1} == 'E' ]]
- then
- rm CSV/$yamlname.csv
- rostopic echo -b recordings/trajectory_recording$yamlname.bag /wolf_ros_node/pose_pose_with_cov -p > CSV/$yamlname.csv
- cp CSV/$yamlname.csv CSV/transp/$yamlname\_transp.csv
- fi
-
-done
diff --git a/bag/generate_trajectory_csv2.bash b/bag/generate_trajectory_csv_ana_lab_2.bash
similarity index 100%
rename from bag/generate_trajectory_csv2.bash
rename to bag/generate_trajectory_csv_ana_lab_2.bash
diff --git a/bag/generate_trajectory_csv_outside.bash b/bag/generate_trajectory_csv_outside.bash
new file mode 100644
index 0000000000000000000000000000000000000000..9fdd5272de45144026679793b4144ec2662b2de8
--- /dev/null
+++ b/bag/generate_trajectory_csv_outside.bash
@@ -0,0 +1,74 @@
+#!/bin/bash
+#echo "Press CTRL+C to proceed."
+#trap "pkill -f 'sleep 1h'" INT
+#trap "set +x ; sleep 1h ; set -x" DEBUG
+
+if [[ -z "${BAG}" ]]
+then
+ BAG="outside"
+fi
+
+#yamlfiles are divided into two types:
+# Starting with G ==> groundtruth, we extract the trajectory
+# Starting with E ==> experiment, we extract the poses
+# Starting with T ==> experiment in 3D, we extract the poses
+
+bagarray=("outside_1" "outside_2" "outside_3" "outside_4" "outside_5" "outside_6")
+# this array lists the timespans in which the KeyFrames will be created. For each timespan the three experiments will be executed
+timespanarray=("1.0" "2.0" "3.0" "4.0" "5.0" "6.0" "7.0" "8.0" "9.0" "10.0")
+for BAG in "${bagarray[@]}"
+do
+ mkdir -p CSV_$BAG/transp
+for time_span in "${timespanarray[@]}"
+do
+ for yamlfile in $(find ../yaml/trajectory_analysys -maxdepth 1 -type f -printf "%f\n")
+ do
+ echo "working on $yamlfile"
+ #We manipulate the filename to format it as we want it
+ yamlname=${yamlfile%.*}
+ yamlname=$yamlname\-${time_span/./_}
+ #We use sed to make the necessary replacements in the yaml files algorithmically
+ sed -i "s/.*min_time.*/ min_time: $time_span/" ../yaml/trajectory_analysys/parameters/test_laser_processor.yaml
+ sed -i "s/.*profiling_file.*/ profiling_file: \"~\/profiling\/wolf_demo_profiling_$yamlname\_$BAG.txt\"/" ../yaml/trajectory_analysys/$yamlfile
+
+ #We remove the bag before recording to avoid potential conflicts
+ rm recordings/trajectory_recording$yamlname\_$BAG.bag
+
+ #The roslaunch file takes several useful parameters. We can lower speed if we see that the processor isn't working at real time. The suffix allows us to record different bags every loop. The bag file has to be previously filtered. We disable rviz as we don't need to see anything.
+ roslaunch wolf_demo_imu2d imu2d_analysys.launch bag:=$BAG\_filtered_notf test:=$yamlfile speed:=1 suffix:=$yamlname\_$BAG rviz:=false robot:='helena'
+ #The following ifs distinguish between the 3 types of yaml files
+ if [[ ${yamlname:0:1} == 'G' ]]
+ then
+ #We use rosbag info to extract the final timestamp of the bag. The csv generator below has problems with the trajectory message if we save more than one message, so this is done to save only the very last one.
+ tend=$(rosbag info -y -k end recordings/trajectory_recording$yamlname\_$BAG.bag)
+ tend=${tend%.*}
+ rosbag filter recordings/trajectory_recording$yamlname\_$BAG.bag recordings/trajectory_recording$yamlname\_$BAG\_tmp.bag "topic == '/wolf_ros_node/trajectory' and t.to_sec() >= $tend"
+
+ #The messages are saved to CSV_$BAG and then transposed to work well with matlab
+ rm CSV_$BAG/$yamlname.csv
+ rostopic echo -b recordings/trajectory_recording$yamlname\_$BAG\_tmp.bag /wolf_ros_node/trajectory -p > CSV_$BAG/$yamlname\_$BAG.csv
+ rm recordings/trajectory_recording$yamlname\_$BAG\_tmp.bag
+
+ rm CSV_$BAG/transp/$yamlname\_$BAG\_transp.csv
+ python3 transpose_csv.py CSV_$BAG/$yamlname\_$BAG.csv CSV_$BAG/transp/$yamlname\_$BAG.csv
+
+ elif [[ ${yamlname:0:1} == 'E' ]]
+ then
+ rm CSV_$BAG/$yamlname.csv
+ rostopic echo -b recordings/trajectory_recording$yamlname\_$BAG.bag /wolf_ros_node/pose_pose_with_cov -p > CSV_$BAG/$yamlname\_$BAG.csv
+ cp CSV_$BAG/$yamlname\_$BAG.csv CSV_$BAG/transp/$yamlname\_$BAG.csv
+ elif [[ ${yamlname:0:1} == 'T' ]]
+ then
+ rm CSV_$BAG/$yamlname.csv
+ rostopic echo -b recordings/trajectory_recording$yamlname\_$BAG.bag /wolf_ros_node/pose_pose_with_cov -p > CSV_$BAG/$yamlname\_$BAG.csv
+ cp CSV_$BAG/$yamlname\_$BAG.csv CSV_$BAG/transp/$yamlname.csv
+ elif [[ ${yamlname:0:1} == 'S' ]]
+ then
+ rm CSV_$BAG/$yamlname.csv
+ rostopic echo -b recordings/trajectory_recording$yamlname\_$BAG.bag /wolf_ros_node/pose_pose_with_cov -p > CSV_$BAG/$yamlname\_$BAG.csv
+ cp CSV_$BAG/$yamlname\_$BAG.csv CSV_$BAG/transp/$yamlname.csv
+ fi
+ done
+
+done
+done
diff --git a/launch/imu2d_analysys.launch b/launch/imu2d_analysys.launch
index aa70f2196f9248adb6736269b592d0e449f79b9a..238359a961e6da030956ca87dd1dcf87bbb222d0 100644
--- a/launch/imu2d_analysys.launch
+++ b/launch/imu2d_analysys.launch
@@ -27,20 +27,20 @@
<!-- TF -->
<group>
- <!-- <node pkg="tf"
+ <node pkg="tf"
type="static_transform_publisher"
name="static_tf"
- args="0 0 0 0 0 0 /base_footprint /base_link 100"/>
+ args="0 0 0 0 0 0 /$(arg robot)/base_footprint /$(arg robot)/base_link 100"/>
<node pkg="tf"
type="static_transform_publisher"
name="static_tf2"
- args="0 0 0 0 0 0 /base_link /imu_bno055 100"/>
+ args="0 0 0 0 0 0 /$(arg robot)/base_link /$(arg robot)/imu_bno055 100"/>
<node pkg="tf"
type="static_transform_publisher"
name="static_tf3"
- args="0 0 0 3.14159265 0 0 /base_link /velodyne 100"/> -->
+ args="0 0 0 3.14159265 0 0 /$(arg robot)/base_link /$(arg robot)/velodyne 100"/>
<node pkg="tf"
type="static_transform_publisher"
diff --git a/yaml/trajectory_analysys/E.yaml b/yaml/trajectory_analysys/E.yaml
index daf073c3126a116b1a4ed0dd3b6b901db5259361..a993e15ac1cd8ac0083c2977c735018ce25c19f3 100644
--- a/yaml/trajectory_analysys/E.yaml
+++ b/yaml/trajectory_analysys/E.yaml
@@ -1,7 +1,7 @@
config:
debug:
profiling: true
- profiling_file: "~/profiling/wolf_demo_profiling_E-10_0.txt"
+ profiling_file: "~/profiling/wolf_demo_profiling_E-1_0_outside_1.txt"
print_problem: false
print_period: 2
print_depth: 4
diff --git a/yaml/trajectory_analysys/S.yaml b/yaml/trajectory_analysys/S.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4683ca62805c3f361a80b5d5582647653ecf63df
--- /dev/null
+++ b/yaml/trajectory_analysys/S.yaml
@@ -0,0 +1,149 @@
+config:
+ debug:
+ profiling: true
+ profiling_file: "~/profiling/wolf_demo_profiling_E-10_0.txt"
+ print_problem: false
+ print_period: 2
+ print_depth: 4
+ print_constr_by: false
+ print_metric: true
+ print_state_blocks: true
+ problem:
+ tree_manager:
+ type: "none"
+ frame_structure: "POV"
+ dimension: 2
+ prior:
+ mode: "fix"
+ $state:
+ P: [0,0]
+ O: [0]
+ V: [0,0]
+ time_tolerance: 0.1
+ node_rate: 100
+ map:
+ type: "MapBase"
+ plugin: "core"
+
+ solver:
+ follow: "parameters/solver.yaml"
+
+
+ sensors:
+ -
+ type: "SensorLaser2d"
+ name: "scanner_front_left"
+ plugin: "laser"
+ follow: "parameters/test_laser_params.yaml"
+ -
+ type: "SensorImu2d"
+ name: "bno"
+ plugin: "imu"
+ follow: "parameters/test_imu_params_bno.yaml"
+ -
+ type: "SensorImu2d"
+ name: "microstrain"
+ plugin: "imu"
+ follow: "parameters/test_imu_params_microstrain_sloped.yaml"
+
+ processors:
+ -
+ type: "ProcessorOdomIcp"
+ name: "processorodomicp"
+ sensor_name: "scanner_front_left"
+ plugin: "laser"
+ follow: "parameters/test_laser_processor.yaml"
+ #-
+ # type: "ProcessorLoopClosureIcp"
+ # name: "processorloopclosureicp"
+ # sensor_name: "scanner_front_left"
+ # plugin: "laser"
+ # time_tolerance: 0.1
+ # apply_loss_function: true
+ # keyframe_vote:
+ # voting_active: false
+ # recent_frames_ignored: 10
+ # frames_ignored_after_loop: 0
+ # max_error_threshold: 0.02
+ # min_points_percent: 40
+ # max_loops: 1
+ # max_candidates: 5
+ # max_attempts: 5
+ # candidate_generation: "random" # 'random' or 'tree'
+ # icp:
+ # follow: "parameters/csm.yaml"
+ -
+ type: "ProcessorImu2d"
+ name: "processorimu2dbno"
+ sensor_name: "bno"
+ plugin: "imu"
+ follow: "parameters/test_imu_processor_bno.yaml"
+ -
+ type: "ProcessorImu2d"
+ name: "processorimu2dmicro"
+ sensor_name: "microstrain"
+ plugin: "imu"
+ follow: "parameters/test_imu_processor_microstrain.yaml"
+
+ ROS subscriber:
+ -
+ package: "wolf_ros_laser"
+ type: "SubscriberLaser2d"
+ topic: "/scan"
+ sensor_name: "scanner_front_left"
+ load_params_from_msg: true
+ #-
+ # package: "wolf_ros_imu"
+ # type: "SubscriberImuEnableable"
+ # topic: "/imu_bno"
+ # sensor_name: "bno"
+ # follow: "parameters/test_imu_subscriber_bno.yaml"
+ -
+ package: "wolf_ros_imu"
+ type: "SubscriberImuEnableable"
+ topic: "/imu_micro"
+ sensor_name: "microstrain"
+ follow: "parameters/test_imu_subscriber_microstrain.yaml"
+
+ ROS publisher:
+ -
+ package: "wolf_ros_node"
+ type: "PublisherTf"
+ topic: " "
+ period: 0.1
+ follow: "parameters/test_publisher_ros_node.yaml"
+ -
+ package: "wolf_ros_node"
+ type: "PublisherGraph"
+ topic: "graph"
+ period: 1
+ viz_overlapped_factors: true
+ -
+ package: "wolf_ros_node"
+ type: "PublisherTrajectory"
+ topic: "trajectory"
+ period: 1
+ frame_id: "map"
+ -
+ package: "wolf_ros_node"
+ type: "PublisherPose"
+ topic: "pose"
+ extrinsics: false
+ period: 0.01
+ frame_id: "map"
+ -
+ package: "wolf_ros_laser"
+ type: "PublisherLaserMap"
+ topic: "map"
+ period: 1
+ map_frame_id: "map"
+ update_dist_th: 0.05
+ update_angle_th: 0.05
+ max_n_cells: 1000000
+ grid_size: 0.1
+ p_free: 0.3
+ p_obst: 0.8
+ p_free_th: 0.2
+ p_obst_th: 0.9
+ discard_max_range: true
+
diff --git a/yaml/trajectory_analysys/T.yaml b/yaml/trajectory_analysys/T.yaml
index b208afa0734bdd125c9136c135e7b8046085d4f2..629e3a9a5766a6b10016bdd4266cffa8d7b66bdd 100644
--- a/yaml/trajectory_analysys/T.yaml
+++ b/yaml/trajectory_analysys/T.yaml
@@ -1,7 +1,7 @@
config:
debug:
profiling: true
- profiling_file: "~/profiling/wolf_demo_profiling_T-10_0.txt"
+ profiling_file: "~/profiling/wolf_demo_profiling_T-1_0_outside_1.txt"
print_problem: false
print_period: 2
print_depth: 4
diff --git a/yaml/trajectory_analysys/parameters/test_imu_params_microstrain.yaml b/yaml/trajectory_analysys/parameters/test_imu_params_microstrain.yaml
index 433952de0d12418dbd472d435e5dd416e5a143eb..51ced290ba7db0f5ed336a3c0f8156d4c339893d 100644
--- a/yaml/trajectory_analysys/parameters/test_imu_params_microstrain.yaml
+++ b/yaml/trajectory_analysys/parameters/test_imu_params_microstrain.yaml
@@ -7,4 +7,4 @@ ab_initial_stdev: 0.5 # m/s2 - initial bias
wb_initial_stdev: 0.1 # rad/sec - initial bias
ab_rate_stdev: 0.00001 # m/s2/sqrt(s)
wb_rate_stdev: 0.00001 # rad/s/sqrt(s)
-orthogonal_gravity: true
+orthogonal_gravity: false
diff --git a/yaml/trajectory_analysys/parameters/test_imu_params_microstrain_sloped.yaml b/yaml/trajectory_analysys/parameters/test_imu_params_microstrain_sloped.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..433952de0d12418dbd472d435e5dd416e5a143eb
--- /dev/null
+++ b/yaml/trajectory_analysys/parameters/test_imu_params_microstrain_sloped.yaml
@@ -0,0 +1,10 @@
+#microstrain
+extrinsic:
+ pose: [0,0,0]
+a_noise: 0.05 #0.9
+w_noise: 0.01 #0.01
+ab_initial_stdev: 0.5 # m/s2 - initial bias
+wb_initial_stdev: 0.1 # rad/sec - initial bias
+ab_rate_stdev: 0.00001 # m/s2/sqrt(s)
+wb_rate_stdev: 0.00001 # rad/s/sqrt(s)
+orthogonal_gravity: true
diff --git a/yaml/trajectory_analysys/parameters/test_imu_subscriber_microstrain.yaml b/yaml/trajectory_analysys/parameters/test_imu_subscriber_microstrain.yaml
index c4510187db7463dbaac0874b2709e6c62e3a176e..78c5c3184c072489e143dd00936b6392f542af4a 100644
--- a/yaml/trajectory_analysys/parameters/test_imu_subscriber_microstrain.yaml
+++ b/yaml/trajectory_analysys/parameters/test_imu_subscriber_microstrain.yaml
@@ -8,6 +8,6 @@ in_degrees: false
#all
topic_enable: "/imu_micro/enable"
-static_init_duration: 3
+static_init_duration: 0
lowpass_filter: false
lowpass_cutoff_freq: 5
diff --git a/yaml/trajectory_analysys/parameters/test_laser_processor.yaml b/yaml/trajectory_analysys/parameters/test_laser_processor.yaml
index 74089f1f5abddc70659d00ea0bf3607ad7bb6a77..3aa7b0bcfbe1df1c74eb80792ef8ff16525bf5fb 100644
--- a/yaml/trajectory_analysys/parameters/test_laser_processor.yaml
+++ b/yaml/trajectory_analysys/parameters/test_laser_processor.yaml
@@ -3,7 +3,7 @@ keyframe_vote:
min_features_for_keyframe: 10
min_dist: 999
min_angle: 999
- min_time: 10.0
+ min_time: 1.0
min_error: 999
max_points: 0
max_new_features: 15
diff --git a/yaml/trajectory_analysys/parameters/test_publisher_ros_node.yaml b/yaml/trajectory_analysys/parameters/test_publisher_ros_node.yaml
index 6154f168b55aeaa72593a33059db5ee11367f2a6..492c369e8a7c10b19fa0af9fd4f7c6bd38263ed2 100644
--- a/yaml/trajectory_analysys/parameters/test_publisher_ros_node.yaml
+++ b/yaml/trajectory_analysys/parameters/test_publisher_ros_node.yaml
@@ -1,6 +1,6 @@
map_frame_id: "map"
-odom_frame_id: "ana/odom"
-base_frame_id: "ana/base_footprint"
- #odom_frame_id: "helena/odom"
- #base_frame_id: "helena/base_footprint"
-publish_odom_tf: false
+ #odom_frame_id: "ana/odom"
+ #base_frame_id: "ana/base_footprint"
+odom_frame_id: "helena/odom"
+base_frame_id: "helena/base_footprint"
+publish_odom_tf: true