diff --git a/bag/generate_csvs.bash b/bag/generate_csvs.bash
index e29dcf01ba6c483479d42267983ca76661b0e611..26484d19968a64004755fe7370efba1672c259b9 100755
--- a/bag/generate_csvs.bash
+++ b/bag/generate_csvs.bash
@@ -1,6 +1,7 @@
#!/bin/bash
-SPEED=0.5
+RVIZ="false"
+SPEED=0.2
EXPERIMENTS=('inside' \
'outside1' \
'outside2' \
@@ -15,7 +16,7 @@ BAGS=('ana_lab_2' \
'data_2021-07-27-11-39-04' \
'data_2021-07-29-13-17-57' \
'data_2021-07-29-13-20-21')
-INDEXS=(1)
+INDEXS=(0 1 2)
ROBOTS=('ana'\
'helena' \
'helena' \
@@ -41,7 +42,7 @@ for i in "${INDEXS[@]}"; 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
+ roslaunch wolf_demo_imu2d imu2d_analysys.launch bag:=${BAGS[i]} test:=$method speed:=$SPEED robot:=${ROBOTS[i]} record_name:=${EXPERIMENTS[i]} rviz:=$RVIZ
#Generate CSVs
rostopic echo -b recordings/${EXPERIMENTS[i]}_${method}.bag /wolf_ros_node/odom_icp -p > csv/${EXPERIMENTS[i]}_odom_icp_${method}.csv
diff --git a/launch/imu2d_analysys.launch b/launch/imu2d_analysys.launch
index 560091e1b6825b2b61eb7adf3916e470f3d1c920..0317c5974863ad6dcdf5cabd015fcf0420c5f943 100644
--- a/launch/imu2d_analysys.launch
+++ b/launch/imu2d_analysys.launch
@@ -45,7 +45,7 @@
<node pkg="tf"
type="static_transform_publisher"
name="static_tf3"
- args="0 0 0 3.14159265 0 0 /$(arg robot)/base_link /$(arg robot)/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"
@@ -56,6 +56,22 @@
type="static_transform_publisher"
name="static_tf5"
args="-0.11 0.01 0 0 0 0 /$(arg robot)/base_link /$(arg robot)/imu_microstrain 100"/>
+
+ <node pkg="tf"
+ type="static_transform_publisher"
+ name="static_tf6"
+ args="0 0 0 3.14159265 0 0 /$(arg robot)/base_link /$(arg robot)/robosense 100"/>
+ </group>
+
+ <group if="$(eval arg('robot') =='helena')">
+ <node pkg="tf"
+ type="static_transform_publisher"
+ name="static_tf7"
+ args="0 0 0 0 0 0 /ana/odom /helena/odom 100"/>
+ <node pkg="tf"
+ type="static_transform_publisher"
+ name="static_tf8"
+ args="0 0 0 0 0 0 /helena/base_link /ana/base_footprint 100"/>
</group>
<!-- using "clock" option to use the simulated time. Rosbag launch as the first node -->
diff --git a/rviz/imu2d_demo_helena.rviz b/rviz/imu2d_demo_helena.rviz
index ef9c97ea73dd577308282ae46b46dad83c8c05cd..c2368e6920cb2e50745a7884623e942dca9b8b71 100644
--- a/rviz/imu2d_demo_helena.rviz
+++ b/rviz/imu2d_demo_helena.rviz
@@ -18,7 +18,7 @@ Panels:
- /PointCloud21
- /Imu1
Splitter Ratio: 0.708737850189209
- Tree Height: 839
+ Tree Height: 689
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@@ -64,84 +64,10 @@ Visualization Manager:
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz/TF
- Enabled: true
+ Enabled: false
Frame Timeout: 15
Frames:
All Enabled: false
- helena/base_footprint:
- Value: false
- helena/base_link:
- Value: true
- helena/camera_bottom_screw_frame:
- Value: false
- helena/camera_color_frame:
- Value: false
- helena/camera_color_optical_frame:
- Value: false
- helena/camera_depth_frame:
- Value: false
- helena/camera_depth_optical_frame:
- Value: false
- helena/camera_infra1_frame:
- Value: false
- helena/camera_infra1_optical_frame:
- Value: false
- helena/camera_infra2_frame:
- Value: false
- helena/camera_infra2_optical_frame:
- Value: false
- helena/camera_link:
- Value: false
- helena/front_left_axle:
- Value: false
- helena/front_left_hub:
- Value: false
- helena/front_left_wheel:
- Value: false
- helena/front_right_axle:
- Value: false
- helena/front_right_hub:
- Value: false
- helena/front_right_wheel:
- Value: false
- helena/front_sonar:
- Value: false
- helena/helena_box:
- Value: false
- helena/helena_realsense_support:
- Value: false
- helena/imu_bno055:
- Value: false
- helena/imu_bno055_base:
- Value: false
- helena/imu_microstrain:
- Value: true
- helena/odom:
- Value: true
- helena/rear_left_axle:
- Value: false
- helena/rear_left_hub:
- Value: false
- helena/rear_left_wheel:
- Value: false
- helena/rear_right_axle:
- Value: false
- helena/rear_right_hub:
- Value: false
- helena/rear_right_wheel:
- Value: false
- helena/rear_sonar:
- Value: false
- helena/robosense:
- Value: false
- helena/robosense_base:
- Value: false
- helena/top_plate:
- Value: false
- imu:
- Value: true
- map:
- Value: true
Marker Alpha: 1
Marker Scale: 5
Name: TF
@@ -149,60 +75,9 @@ Visualization Manager:
Show Axes: true
Show Names: true
Tree:
- map:
- helena/odom:
- helena/base_footprint:
- helena/base_link:
- helena/front_left_axle:
- helena/front_left_hub:
- helena/front_left_wheel:
- {}
- helena/front_right_axle:
- helena/front_right_hub:
- helena/front_right_wheel:
- {}
- helena/front_sonar:
- {}
- helena/imu_microstrain:
- {}
- helena/rear_left_axle:
- helena/rear_left_hub:
- helena/rear_left_wheel:
- {}
- helena/rear_right_axle:
- helena/rear_right_hub:
- helena/rear_right_wheel:
- {}
- helena/rear_sonar:
- {}
- helena/top_plate:
- helena/helena_box:
- {}
- helena/helena_realsense_support:
- helena/camera_bottom_screw_frame:
- helena/camera_link:
- helena/camera_color_frame:
- helena/camera_color_optical_frame:
- {}
- helena/camera_depth_frame:
- helena/camera_depth_optical_frame:
- {}
- helena/camera_infra1_frame:
- helena/camera_infra1_optical_frame:
- {}
- helena/camera_infra2_frame:
- helena/camera_infra2_optical_frame:
- {}
- helena/robosense_base:
- helena/robosense:
- {}
- imu:
- {}
- helena/imu_bno055_base:
- helena/imu_bno055:
- {}
+ {}
Update Interval: 0
- Value: true
+ Value: false
- Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
@@ -279,9 +154,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
@@ -380,7 +255,7 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
- Distance: 32.773921966552734
+ Distance: 41.11160659790039
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
@@ -396,9 +271,9 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
- Pitch: 0.7547971606254578
+ Pitch: 0.7347972393035889
Target Frame: map
- Yaw: 0.05999409034848213
+ Yaw: 0.9699934720993042
Saved:
- Angle: 0
Class: rviz/TopDownOrtho
@@ -445,10 +320,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:
@@ -457,6 +332,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 3e5d4ace58863d9c3a5d8e594f9eb0cd6c331de0..7663a7dcbd7c30ad366a324ef37370298721046c 100644
--- a/yaml/trajectory_analysys/E.yaml
+++ b/yaml/trajectory_analysys/E.yaml
@@ -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"
+ # follow: "parameters/test_imu_params_bno.yaml"
+ # orthogonal_gravity: true
-
type: "SensorImu2d"
name: "microstrain"
plugin: "imu"
follow: "parameters/test_imu_params_microstrain.yaml"
+ orthogonal_gravity: true
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"
diff --git a/yaml/trajectory_analysys/S.yaml b/yaml/trajectory_analysys/S.yaml
index a86afe09c94af4689fcb9a0a1099557b7ed43ab5..c543460014ff5daae51e72b190403eb55ea75acf 100644
--- a/yaml/trajectory_analysys/S.yaml
+++ b/yaml/trajectory_analysys/S.yaml
@@ -45,8 +45,8 @@ config:
type: "SensorImu2d"
name: "microstrain"
plugin: "imu"
- orthogonal_gravity: false
follow: "parameters/test_imu_params_microstrain.yaml"
+ orthogonal_gravity: false
processors:
-
diff --git a/yaml/trajectory_analysys/T.yaml b/yaml/trajectory_analysys/T.yaml
index 98e5f805325b58fbd4e86d7218a0f1458c8f2bc5..169a94793b57d13d698fd05e41ac514d0dc474ae 100644
--- a/yaml/trajectory_analysys/T.yaml
+++ b/yaml/trajectory_analysys/T.yaml
@@ -48,7 +48,7 @@ config:
plugin: "imu"
extrinsic:
pose: [0,0,0,0,0,0,1]
- follow: "parameters/test_imu_params_microstrain.yaml"
+ follow: "parameters/test_imu_params_microstrain.yaml"
processors:
-
diff --git a/yaml/trajectory_analysys/parameters/solver.yaml b/yaml/trajectory_analysys/parameters/solver.yaml
index cb52fa3d63983c71bcc06e5bc4c4f6b1daa6d28a..2c60c7682239a575d41de0974caa5fd2ba991de2 100644
--- a/yaml/trajectory_analysys/parameters/solver.yaml
+++ b/yaml/trajectory_analysys/parameters/solver.yaml
@@ -8,6 +8,6 @@ min_num_iterations: 5 #if update immediately
compute_cov: false
cov_enum: 3 # if compute_cov
cov_period: 1 # if compute_cov
-function_tolerance: 1e-9
-gradient_tolerance: 1e-10
+function_tolerance: 1e-10
+gradient_tolerance: 1e-11
use_nonmonotonic_steps: false
diff --git a/yaml/trajectory_analysys/parameters/test_imu_params_bno.yaml b/yaml/trajectory_analysys/parameters/test_imu_params_bno.yaml
index a958a695eaa28b8cc4973f233b09f19e7b3bef33..7f4fe88afa09d0ce8ecbc494c1b0d350121245a7 100644
--- a/yaml/trajectory_analysys/parameters/test_imu_params_bno.yaml
+++ b/yaml/trajectory_analysys/parameters/test_imu_params_bno.yaml
@@ -7,4 +7,3 @@ ab_initial_stdev: 5 # m/s2 - initial bias
wb_initial_stdev: 1 # rad/sec - initial bias
ab_rate_stdev: 0.0001 # m/s2/sqrt(s)
wb_rate_stdev: 0.0001 # rad/s/sqrt(s)
-orthogonal_gravity: true
diff --git a/yaml/trajectory_analysys/parameters/test_imu_params_microstrain.yaml b/yaml/trajectory_analysys/parameters/test_imu_params_microstrain.yaml
index 433952de0d12418dbd472d435e5dd416e5a143eb..40bf77b37d3846d28c713bfa8c96d4705b82c5c3 100644
--- a/yaml/trajectory_analysys/parameters/test_imu_params_microstrain.yaml
+++ b/yaml/trajectory_analysys/parameters/test_imu_params_microstrain.yaml
@@ -5,6 +5,5 @@ 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
+ab_rate_stdev: 0.000001 # m/s2/sqrt(s)
+wb_rate_stdev: 0.000001 # rad/s/sqrt(s)