diff --git a/bag/.gitignore b/bag/.gitignore
index 497d01c10f67b0f921784a1d45f7307ef2172cbb..484bf525588edcaaee0779698230b37be6049990 100644
--- a/bag/.gitignore
+++ b/bag/.gitignore
@@ -1,5 +1,6 @@
*.bag
CSV*/
+csv/
recordings/
!analysys/
!.gittignore
diff --git a/bag/generate_csvs.bash b/bag/generate_csvs.bash
new file mode 100755
index 0000000000000000000000000000000000000000..e29dcf01ba6c483479d42267983ca76661b0e611
--- /dev/null
+++ b/bag/generate_csvs.bash
@@ -0,0 +1,55 @@
+#!/bin/bash
+
+SPEED=0.5
+EXPERIMENTS=('inside' \
+ 'outside1' \
+ 'outside2' \
+ 'outside3' \
+ 'outside4' \
+ 'outside5' \
+ 'outside6')
+BAGS=('ana_lab_2' \
+ 'data_2021-07-27-11-31-13' \
+ 'data_2021-07-27-11-34-18' \
+ 'data_2021-07-27-11-36-31' \
+ 'data_2021-07-27-11-39-04' \
+ 'data_2021-07-29-13-17-57' \
+ 'data_2021-07-29-13-20-21')
+INDEXS=(1)
+ROBOTS=('ana'\
+ 'helena' \
+ 'helena' \
+ 'helena' \
+ 'helena' \
+ 'helena' \
+ 'helena')
+
+#There are three methods:
+# Starting with E ==> experiment in 2D: odom_icp + imu2d
+# Starting with T ==> experiment in 3D: odom_icp(3D) + imu3d
+# Starting with S ==> experiment in 2D: odom_icp + imu2d sloped
+METHODS=("E" "T" "S")
+
+#make dirs
+mkdir -pv csv
+mkdir -pv recordings
+
+# all experiments will be executed
+for i in "${INDEXS[@]}"; do
+ echo "Experiment ${i}: ${EXPERIMENTS[i]}..."
+ for method in "${METHODS[@]}"; do
+ echo "Method $method..."
+
+ #The roslaunch file takes several useful parameters. We can lower speed if we see that the processor isn't working at real time. We disable rviz as we don't need to see anything.
+ roslaunch wolf_demo_imu2d imu2d_analysys.launch bag:=${BAGS[i]} test:=$method speed:=$SPEED robot:=${ROBOTS[i]} record_name:=${EXPERIMENTS[i]} rviz:=false
+
+ #Generate CSVs
+ rostopic echo -b recordings/${EXPERIMENTS[i]}_${method}.bag /wolf_ros_node/odom_icp -p > csv/${EXPERIMENTS[i]}_odom_icp_${method}.csv
+ rostopic echo -b recordings/${EXPERIMENTS[i]}_${method}.bag /imu_micro_pose_from_origin -p > csv/${EXPERIMENTS[i]}_odom_imu_${method}.csv
+ rostopic echo -b recordings/${EXPERIMENTS[i]}_${method}.bag /imu_micro_bias_accel -p > csv/${EXPERIMENTS[i]}_bias_accel_${method}.csv
+ rostopic echo -b recordings/${EXPERIMENTS[i]}_${method}.bag /imu_micro_bias_gyro -p > csv/${EXPERIMENTS[i]}_bias_gyro_${method}.csv
+
+ #Move profiling file
+ mv ~/profiling/wolf_demo_profiling_${method}.txt recordings/profiling_${EXPERIMENTS[i]}_${method}.txt
+ done
+done
\ No newline at end of file
diff --git a/launch/imu2d_analysys.launch b/launch/imu2d_analysys.launch
index f1f9b703a9356887c1de69688718301858ce13f8..560091e1b6825b2b61eb7adf3916e470f3d1c920 100644
--- a/launch/imu2d_analysys.launch
+++ b/launch/imu2d_analysys.launch
@@ -6,9 +6,10 @@
<arg name="sec" default="0" />
<arg name="profiling" default="false" />
<arg name="gdb" default="false" />
- <arg name="test" default="1" />
+ <arg name="test" default="E" />
<arg name="bag" default="test_no_map" />
- <arg name="suffix" default="" />
+ <arg name="record_suffix" default="" />
+ <arg name="record_name" default="experiment" />
<arg name="launch_pref" default="" unless="$(eval profiling or gdb)"/>
<arg name="launch_pref" value="valgrind --tool=callgrind --callgrind-out-file='callgrind.wolf.%p'" if="$(arg profiling)" />
<arg name="launch_pref" value="gdb -ex run --args" if="$(arg gdb)" />
@@ -22,7 +23,7 @@
<node pkg="rosbag"
type="record"
name="recorder"
- args='record -O $(find wolf_demo_imu2d)/bag/recordings/trajectory_recording$(arg suffix).bag
+ args='record -O $(find wolf_demo_imu2d)/bag/recordings/$(arg record_name)_$(arg test)$(arg record_suffix).bag
"/wolf_ros_node/odom_icp"
"/imu_micro_bias_accel"
"/imu_micro_bias_gyro"
@@ -87,6 +88,6 @@
required="true"
output="screen"
launch-prefix="$(arg launch_pref)">
- <param name="~yaml_file_path" value="$(find wolf_demo_imu2d)/yaml/trajectory_analysys/$(arg test)" />
+ <param name="~yaml_file_path" value="$(find wolf_demo_imu2d)/yaml/trajectory_analysys/$(arg test).yaml" />
</node>
</launch>
diff --git a/rviz/imu2d_demo_ana.rviz b/rviz/imu2d_demo_ana.rviz
index e61b151fbbb3d5674fb709fd833897c8c591355a..9527475e9e795a14bfc538fa4a1491b27de7c0d7 100644
--- a/rviz/imu2d_demo_ana.rviz
+++ b/rviz/imu2d_demo_ana.rviz
@@ -11,7 +11,7 @@ Panels:
- /Factors1/Namespaces1
- /Trajectory1/Namespaces1
Splitter Ratio: 0.708737850189209
- Tree Height: 839
+ Tree Height: 689
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@@ -147,9 +147,9 @@ Visualization Manager:
Marker Topic: /wolf_ros_node/graph_factors
Name: Factors
Namespaces:
- factors_processorimumicro: true
+ factors_processorimu2dmicro: true
factors_processorodomicp: true
- factors_text_processorimumicro: false
+ factors_text_processorimu2dmicro: true
factors_text_processorodomicp: false
factors_text_unnamed_processor: false
factors_unnamed_processor: true
@@ -264,9 +264,9 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
- Pitch: 0.2547972500324249
+ Pitch: 1.354796290397644
Target Frame: map
- Yaw: 4.588164329528809
+ Yaw: 4.453161239624023
Saved:
- Angle: 0
Class: rviz/TopDownOrtho
@@ -313,10 +313,10 @@ Visualization Manager:
Window Geometry:
Displays:
collapsed: false
- Height: 1136
+ Height: 986
Hide Left Dock: false
Hide Right Dock: true
- QMainWindow State: 000000ff00000000fd000000040000000000000205000003d2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003d2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007420000003efc0100000002fb0000000800540069006d0065010000000000000742000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000537000003d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd0000000400000000000002050000033cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000033c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006560000003efc0100000002fb0000000800540069006d0065010000000000000656000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000044b0000033c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -325,6 +325,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: true
- Width: 1858
- X: 62
+ Width: 1622
+ X: 58
Y: 27
diff --git a/yaml/trajectory_analysys/E.yaml b/yaml/trajectory_analysys/E.yaml
index 362381fed2587afd29c3902083917662a70b14fd..3e5d4ace58863d9c3a5d8e594f9eb0cd6c331de0 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-5_0.txt"
+ profiling_file: "~/profiling/wolf_demo_profiling_E.txt"
print_problem: false
print_period: 2
print_depth: 4
@@ -14,11 +14,15 @@ config:
frame_structure: "POV"
dimension: 2
prior:
- mode: "fix"
+ mode: "factor"
$state:
P: [0,0]
O: [0]
V: [0,0]
+ $sigma:
+ P: [0.01, 0.01]
+ O: [1]
+ V: [0.01, 0.01]
time_tolerance: 0.1
node_rate: 100
@@ -120,13 +124,13 @@ config:
# topic: "trajectory"
# period: 1
# frame_id: "map"
- -
- package: "wolf_ros_node"
- type: "PublisherPose"
- topic: "pose"
- extrinsics: false
- period: 0.1
- frame_id: "map"
+ # -
+ # package: "wolf_ros_node"
+ # type: "PublisherPose"
+ # topic: "pose"
+ # extrinsics: false
+ # period: 0.1
+ # frame_id: "map"
# -
# package: "wolf_ros_laser"
# type: "PublisherLaserMap"
diff --git a/yaml/trajectory_analysys/G.yaml b/yaml/trajectory_analysys/G.yaml
index dcc3d60fd015a15dd66d004f1ce9c33e28640959..d973ecca3b758ff91417ef5f3d1ee090838d9cc0 100644
--- a/yaml/trajectory_analysys/G.yaml
+++ b/yaml/trajectory_analysys/G.yaml
@@ -1,7 +1,7 @@
config:
debug:
profiling: true
- profiling_file: "~/profiling/wolf_demo_profiling_G-5_0.txt"
+ profiling_file: "~/profiling/wolf_demo_profiling_G.txt"
print_problem: false
print_period: 2
print_depth: 4
diff --git a/yaml/trajectory_analysys/S.yaml b/yaml/trajectory_analysys/S.yaml
index e4d07ccf613c5af198f45d13e5be96079ee15dd5..a86afe09c94af4689fcb9a0a1099557b7ed43ab5 100644
--- a/yaml/trajectory_analysys/S.yaml
+++ b/yaml/trajectory_analysys/S.yaml
@@ -1,7 +1,7 @@
config:
debug:
profiling: true
- profiling_file: "~/profiling/wolf_demo_profiling_S-5_0.txt"
+ profiling_file: "~/profiling/wolf_demo_profiling_S.txt"
print_problem: false
print_period: 2
print_depth: 4
@@ -14,20 +14,20 @@ config:
frame_structure: "POV"
dimension: 2
prior:
- mode: "fix"
+ mode: "factor"
$state:
P: [0,0]
O: [0]
V: [0,0]
+ $sigma:
+ P: [0.01, 0.01]
+ O: [1]
+ V: [0.01, 0.01]
time_tolerance: 0.1
node_rate: 100
- map:
- type: "MapBase"
- plugin: "core"
solver:
follow: "parameters/solver.yaml"
-
sensors:
-
@@ -35,16 +35,18 @@ config:
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: "bno"
+ # plugin: "imu"
+ # orthogonal_gravity: true
+ # follow: "parameters/test_imu_params_bno.yaml"
-
type: "SensorImu2d"
name: "microstrain"
plugin: "imu"
- follow: "parameters/test_imu_params_microstrain_sloped.yaml"
+ orthogonal_gravity: false
+ follow: "parameters/test_imu_params_microstrain.yaml"
processors:
-
@@ -53,31 +55,12 @@ config:
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: "processorimu2dbno"
+ # sensor_name: "bno"
+ # plugin: "imu"
+ # follow: "parameters/test_imu_processor_bno.yaml"
-
type: "ProcessorImu2d"
name: "processorimu2dmicro"
@@ -118,19 +101,19 @@ config:
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_node"
+ # type: "PublisherTrajectory"
+ # topic: "trajectory"
+ # period: 1
+ # frame_id: "map"
+ # -
+ # package: "wolf_ros_node"
+ # type: "PublisherPose"
+ # topic: "pose"
+ # extrinsics: false
+ # period: 0.1
+ # frame_id: "map"
-
package: "wolf_ros_laser"
type: "PublisherLaserMap"
@@ -146,4 +129,10 @@ config:
p_free_th: 0.2
p_obst_th: 0.9
discard_max_range: true
+ -
+ package: "wolf_ros_laser"
+ type: "PublisherOdomIcp"
+ topic: "odom_icp"
+ period: 0.1
+ processor_name: "processorodomicp"
diff --git a/yaml/trajectory_analysys/T.yaml b/yaml/trajectory_analysys/T.yaml
index 9632b31b704f5cfc5e78fa4c0a7d4dfa7ebc33f9..98e5f805325b58fbd4e86d7218a0f1458c8f2bc5 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-5_0.txt"
+ profiling_file: "~/profiling/wolf_demo_profiling_T.txt"
print_problem: false
print_period: 2
print_depth: 4
@@ -47,7 +47,7 @@ config:
name: "microstrain"
plugin: "imu"
extrinsic:
- pose: [0,0,0,0,0,0,1]
+ pose: [0,0,0,0,0,0,1]
follow: "parameters/test_imu_params_microstrain.yaml"
processors:
@@ -109,13 +109,13 @@ config:
# topic: "trajectory"
# period: 1
# frame_id: "map"
- -
- package: "wolf_ros_node"
- type: "PublisherPose"
- topic: "pose"
- extrinsics: false
- period: 0.1
- frame_id: "map"
+ # -
+ # package: "wolf_ros_node"
+ # type: "PublisherPose"
+ # topic: "pose"
+ # extrinsics: false
+ # period: 0.1
+ # frame_id: "map"
# -
# package: "wolf_ros_laser"
# type: "PublisherLaserMap"
diff --git a/yaml/trajectory_analysys/parameters/csm.yaml b/yaml/trajectory_analysys/parameters/csm.yaml
index 2dd8f8680123cbf977c7bd7defcb0c65dd1d33f5..3732d3785d9cd098a633cd28d25a04e1cc4fadd3 100644
--- a/yaml/trajectory_analysys/parameters/csm.yaml
+++ b/yaml/trajectory_analysys/parameters/csm.yaml
@@ -42,4 +42,4 @@ sigma: 0.2 # Noise of the scan
# COVARIANCE
do_compute_covariance: true
-cov_factor: 5 # Factor multiplying the cov output of csm
+cov_factor: 1 # Factor multiplying the cov output of csm
diff --git a/yaml/trajectory_analysys/parameters/test_imu_params_microstrain_sloped.yaml b/yaml/trajectory_analysys/parameters/test_imu_params_microstrain_sloped.yaml
deleted file mode 100644
index 51ced290ba7db0f5ed336a3c0f8156d4c339893d..0000000000000000000000000000000000000000
--- a/yaml/trajectory_analysys/parameters/test_imu_params_microstrain_sloped.yaml
+++ /dev/null
@@ -1,10 +0,0 @@
-#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: false
diff --git a/yaml/trajectory_analysys/parameters/test_imu_subscriber_microstrain.yaml b/yaml/trajectory_analysys/parameters/test_imu_subscriber_microstrain.yaml
index 78c5c3184c072489e143dd00936b6392f542af4a..081af371289a0635166a7a928aed767c652cdd38 100644
--- a/yaml/trajectory_analysys/parameters/test_imu_subscriber_microstrain.yaml
+++ b/yaml/trajectory_analysys/parameters/test_imu_subscriber_microstrain.yaml
@@ -9,5 +9,5 @@ in_degrees: false
#all
topic_enable: "/imu_micro/enable"
static_init_duration: 0
-lowpass_filter: false
+lowpass_filter: true
lowpass_cutoff_freq: 5