From 1d47b25d2a9a5f1a86c18552e6f8dff986990f03 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Tue, 22 Feb 2022 18:36:40 +0100
Subject: [PATCH] wip
---
bag/generate_csvs.bash | 7 +-
launch/imu2d_analysys.launch | 18 ++-
rviz/imu2d_demo_helena.rviz | 151 ++----------------
yaml/trajectory_analysys/E.yaml | 43 ++---
yaml/trajectory_analysys/S.yaml | 2 +-
yaml/trajectory_analysys/T.yaml | 2 +-
.../parameters/solver.yaml | 4 +-
.../parameters/test_imu_params_bno.yaml | 1 -
.../test_imu_params_microstrain.yaml | 5 +-
9 files changed, 53 insertions(+), 180 deletions(-)
diff --git a/bag/generate_csvs.bash b/bag/generate_csvs.bash
index e29dcf0..26484d1 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 560091e..0317c59 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 ef9c97e..c2368e6 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 3e5d4ac..7663a7d 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 a86afe0..c543460 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 98e5f80..169a947 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 cb52fa3..2c60c76 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 a958a69..7f4fe88 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 433952d..40bf77b 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)
--
GitLab