From 9816c3f018a73b7e366eea82a2c3eac82231b65a Mon Sep 17 00:00:00 2001
From: Idril Geer <igeer@iri.upc.edu>
Date: Mon, 14 Feb 2022 19:58:44 +0100
Subject: [PATCH] work on the analysys scripts
---
bag/.gitignore | 2 +-
bag/analysys/.gitignore | 3 +
bag/analysys/compare.m | 50 +++---
bag/analysys/extractGT.m | 12 +-
bag/analysys/extractdata.m | 11 +-
bag/analysys/geterror.m | 10 +-
bag/analysys/ploterrorstiled.m | 11 +-
bag/analysys/plotrmse.m | 74 ++++++---
bag/filter_bag.bash | 4 +-
bag/generate_trajectory_csv.bash | 39 -----
...=> generate_trajectory_csv_ana_lab_2.bash} | 0
bag/generate_trajectory_csv_outside.bash | 74 +++++++++
launch/imu2d_analysys.launch | 8 +-
yaml/trajectory_analysys/E.yaml | 2 +-
yaml/trajectory_analysys/S.yaml | 149 ++++++++++++++++++
yaml/trajectory_analysys/T.yaml | 2 +-
.../test_imu_params_microstrain.yaml | 2 +-
.../test_imu_params_microstrain_sloped.yaml | 10 ++
.../test_imu_subscriber_microstrain.yaml | 2 +-
.../parameters/test_laser_processor.yaml | 2 +-
.../parameters/test_publisher_ros_node.yaml | 10 +-
21 files changed, 356 insertions(+), 121 deletions(-)
delete mode 100644 bag/generate_trajectory_csv.bash
rename bag/{generate_trajectory_csv2.bash => generate_trajectory_csv_ana_lab_2.bash} (100%)
create mode 100644 bag/generate_trajectory_csv_outside.bash
create mode 100644 yaml/trajectory_analysys/S.yaml
create mode 100644 yaml/trajectory_analysys/parameters/test_imu_params_microstrain_sloped.yaml
diff --git a/bag/.gitignore b/bag/.gitignore
index b31508b..497d01c 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 8368639..8728d95 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 11bca20..f810641 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 2a5552d..4182668 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 0a509aa..802975b 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 a1836ad..299b2a9 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 ddea792..143df8d 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 7c9d64d..e603253 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 f9458ab..dd4909b 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 1657228..0000000
--- 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 0000000..9fdd527
--- /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 aa70f21..238359a 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 daf073c..a993e15 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 0000000..4683ca6
--- /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 b208afa..629e3a9 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 433952d..51ced29 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 0000000..433952d
--- /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 c451018..78c5c31 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 74089f1..3aa7b0b 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 6154f16..492c369 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
--
GitLab