diff --git a/launch/imu2d_demo.launch b/launch/imu2d_demo.launch
index 97f98df7fa7b600a0802996448327e31bf5da509..5f0396bef90ed8ffe93b057e7ef0ce5f6270b1a3 100644
--- a/launch/imu2d_demo.launch
+++ b/launch/imu2d_demo.launch
@@ -5,6 +5,8 @@
<arg name="sec" default="0" />
<arg name="profiling" default="false" />
<arg name="gdb" default="false" />
+ <arg name="test" default="1" />
+ <arg name="bag" default="test_filtered_no_map" />
<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)" />
@@ -35,7 +37,7 @@
<node pkg="rosbag"
type="play"
name="player"
- args="-s $(arg sec) -r $(arg speed) -k --clock $(find wolf_demo_imu2d)/bag/test_no_map.bag"/>
+ args="-s $(arg sec) -r $(arg speed) -k --clock $(find wolf_demo_imu2d)/bag/$(arg bag).bag"/>
<node type="rviz"
name="rviz"
@@ -47,6 +49,6 @@
required="true"
output="screen"
launch-prefix="$(arg launch_pref)">
- <param name="~yaml_file_path" value="$(find wolf_demo_imu2d)/yaml/imu2d.yaml" />
+ <param name="~yaml_file_path" value="$(find wolf_demo_imu2d)/yaml/imu2d_test$(arg test).yaml" />
</node>
</launch>
diff --git a/launch/imu2d_gravity_demo.launch b/launch/imu2d_gravity_demo.launch
new file mode 100644
index 0000000000000000000000000000000000000000..848352beb6a453e1d56ff36cb7fbb0c753f1cc27
--- /dev/null
+++ b/launch/imu2d_gravity_demo.launch
@@ -0,0 +1,54 @@
+<!-- -->
+<launch>
+ <param name="use_sim_time" value="true" />
+ <arg name="speed" default="1" />
+ <arg name="sec" default="0" />
+ <arg name="profiling" default="false" />
+ <arg name="gdb" default="false" />
+ <arg name="test" default="1" />
+ <arg name="bag" default="test_filtered_no_map" />
+ <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)" />
+ <!-- using "clock" option to use the simulated time. Rosbag launch as the first node -->
+
+ <node pkg="rostopic" type="rostopic" name="" args='pub /helena/sensors/bno055_imu/imu/topic_enable std_msgs/Bool "data: True"'/>
+
+ <node pkg="tf"
+ type="static_transform_publisher"
+ name="static_tf"
+ args="0 0 0 0 0 0 /helena/base_footprint /helena/base_link 100"/>
+
+ <node pkg="tf"
+ type="static_transform_publisher"
+ name="static_tf2"
+ args="0 0 0 0 0 0 /helena/base_link /helena/imu_bno055 100"/>
+
+ <node pkg="tf"
+ type="static_transform_publisher"
+ name="static_tf3"
+ args="0 0 0 3.14159265 0 0 /helena/base_link /helena/velodyne 100"/>
+
+ <node pkg="tf"
+ type="static_transform_publisher"
+ name="static_tf4"
+ args="-0.11 0.01 0 0 0 0 /helena/base_link /helena/imu_microstrain 100"/>
+
+ <node pkg="rosbag"
+ type="play"
+ name="player"
+ args="-s $(arg sec) -r $(arg speed) -k --clock $(find wolf_demo_imu2d)/bag/Sloped/$(arg bag).bag"/>
+
+ <node type="rviz"
+ name="rviz"
+ pkg="rviz"
+ args="-d $(find wolf_demo_imu2d)/rviz/imu2d_demo.rviz"/>
+ <node type="wolf_ros_node"
+ name="wolf_ros_node"
+ pkg="wolf_ros_node"
+ required="true"
+ output="screen"
+ launch-prefix="$(arg launch_pref)">
+ <param name="~yaml_file_path" value="$(find wolf_demo_imu2d)/yaml/Gravity/imu2d_gravity_test$(arg test).yaml" />
+ </node>
+</launch>
diff --git a/launch/odom-icp.launch b/launch/odom-icp.launch
new file mode 100644
index 0000000000000000000000000000000000000000..a011f059bccec6c50a41d59e1a56b976660e8ee7
--- /dev/null
+++ b/launch/odom-icp.launch
@@ -0,0 +1,52 @@
+<!-- -->
+<launch>
+ <param name="use_sim_time" value="true" />
+ <arg name="speed" default="1" />
+ <arg name="sec" default="0" />
+ <arg name="profiling" default="false" />
+ <arg name="gdb" default="false" />
+ <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)" />
+ <!-- using "clock" option to use the simulated time. Rosbag launch as the first node -->
+
+ <node pkg="rostopic" type="rostopic" name="" args='pub /ana/sensors/bno055_imu/imu/topic_enable std_msgs/Bool "data: True"'/>
+
+ <node pkg="tf"
+ type="static_transform_publisher"
+ name="static_tf"
+ args="0 0 0 0 0 0 /ana/base_footprint /ana/base_link 100"/>
+
+ <node pkg="tf"
+ type="static_transform_publisher"
+ name="static_tf2"
+ args="0 0 0 0 0 0 /ana/base_link /ana/imu_bno055 100"/>
+
+ <node pkg="tf"
+ type="static_transform_publisher"
+ name="static_tf3"
+ args="0 0 0 3.14159265 0 0 /ana/base_link /ana/velodyne 100"/>
+
+ <node pkg="tf"
+ type="static_transform_publisher"
+ name="static_tf4"
+ args="-0.11 0.01 0 0 0 0 /ana/base_link /ana/imu_microstrain 100"/>
+
+ <node pkg="rosbag"
+ type="play"
+ name="player"
+ args="-s $(arg sec) -r $(arg speed) -k --clock $(find wolf_demo_imu2d)/bag/test-fme-slalom.bag"/>
+
+ <node type="rviz"
+ name="rviz"
+ pkg="rviz"
+ args="-d $(find wolf_demo_imu2d)/rviz/imu2d_demo.rviz"/>
+ <node type="wolf_ros_node"
+ name="wolf_ros_node"
+ pkg="wolf_ros_node"
+ required="true"
+ output="screen"
+ launch-prefix="$(arg launch_pref)">
+ <param name="~yaml_file_path" value="$(find wolf_demo_imu2d)/yaml/odom-icp.yaml" />
+ </node>
+</launch>
diff --git a/rviz/imu2d_demo.rviz b/rviz/imu2d_demo.rviz
index 1fb31303cb74517c54c7bb6ee596a72ba74c0621..d157f0be3f0c03fcdebceb50c120f0de64cde1b6 100644
--- a/rviz/imu2d_demo.rviz
+++ b/rviz/imu2d_demo.rviz
@@ -5,10 +5,8 @@ Panels:
Property Tree Widget:
Expanded:
- /Global Options1
- - /TF1
- /TF1/Frames1
- /TF1/Tree1
- - /Odometry1
- /Odometry1/Shape1
- /LaserScan1
- /Landmarks1
@@ -39,14 +37,14 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
- SyncSource: LaserScan
+ SyncSource: PointCloud2
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
- Cell Size: 1
+ Cell Size: 5
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
@@ -70,14 +68,12 @@ Visualization Manager:
All Enabled: false
ana/base_footprint:
Value: true
- ana/base_footprint_robot:
- Value: true
ana/base_link:
- Value: false
+ Value: true
ana/front_left_axle:
- Value: false
+ Value: true
ana/front_right_axle:
- Value: false
+ Value: true
ana/imu_bno055:
Value: true
ana/imu_microstrain:
@@ -85,9 +81,9 @@ Visualization Manager:
ana/odom:
Value: true
ana/rear_left_axle:
- Value: false
+ Value: true
ana/rear_right_axle:
- Value: false
+ Value: true
ana/velodyne:
Value: true
map:
@@ -116,8 +112,6 @@ Visualization Manager:
{}
ana/velodyne:
{}
- ana/base_footprint_robot:
- {}
Update Interval: 0
Value: true
- Angle Tolerance: 0.100000001
@@ -165,8 +159,8 @@ Visualization Manager:
Class: rviz/LaserScan
Color: 250; 84; 255
Color Transformer: FlatColor
- Decay Time: 0
- Enabled: true
+ Decay Time: 100
+ Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 47
@@ -183,26 +177,28 @@ Visualization Manager:
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
- Value: true
+ Value: false
- Class: rviz/MarkerArray
- Enabled: true
+ Enabled: false
Marker Topic: /wolf_ros_node/graph_landmarks
Name: Landmarks
Namespaces:
{}
Queue Size: 100
- Value: true
+ Value: false
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /wolf_ros_node/graph_factors
Name: Factors
Namespaces:
- factors_processorimu2d: true
+ factors_processorimu2dbno: true
factors_processorodom2d: true
- factors_processorodomicp: false
- factors_text_processorimu2d: false
+ factors_processorodomicp: true
+ factors_text_processorimu2dbno: false
factors_text_processorodom2d: false
factors_text_processorodomicp: false
+ factors_text_unnamed_processor: false
+ factors_unnamed_processor: true
Queue Size: 100
Value: true
- Class: rviz/MarkerArray
@@ -266,7 +262,7 @@ Visualization Manager:
Enabled: false
History Length: 1
Name: Imu
- Topic: /ana/sensors/bno055_imu/imu_remapped
+ Topic: /ana/imu/data_remapped
Unreliable: false
Value: false
Enabled: true
@@ -293,28 +289,78 @@ Visualization Manager:
Value: true
Views:
Current:
- Angle: 0
- Class: rviz/TopDownOrtho
+ Class: rviz/Orbit
+ Distance: 35.0371132
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
+ Focal Point:
+ X: 2.42303967
+ Y: -1.90124702
+ Z: 1.00247037
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
- Scale: 77.8121338
- Target Frame: ana/base_link
- Value: TopDownOrtho (rviz)
- X: -0.064257361
- Y: -0.334138036
- Saved: ~
+ Pitch: 0.594797194
+ Target Frame: map
+ Value: Orbit (rviz)
+ Yaw: 4.00817299
+ Saved:
+ - Angle: 0
+ Class: rviz/TopDownOrtho
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.0599999987
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Invert Z Axis: false
+ Name: TopDownOrtho
+ Near Clip Distance: 0.00999999978
+ Scale: 64.6596146
+ Target Frame: map
+ Value: TopDownOrtho (rviz)
+ X: 8.82501411
+ Y: -0.175666362
+ - Angle: 0
+ Class: rviz/TopDownOrtho
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.0599999987
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Invert Z Axis: false
+ Name: TopDownOrtho
+ Near Clip Distance: 0.00999999978
+ Scale: 50.072403
+ Target Frame: map
+ Value: TopDownOrtho (rviz)
+ X: 1.94783008
+ Y: 3.96870375
+ - Angle: -1.57000005
+ Class: rviz/TopDownOrtho
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.0599999987
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Invert Z Axis: false
+ Name: TopDownOrtho
+ Near Clip Distance: 0.00999999978
+ Scale: 42.1874428
+ Target Frame: map
+ Value: TopDownOrtho (rviz)
+ X: 1.97467852
+ Y: 4.4872756
Window Geometry:
Displays:
collapsed: false
Height: 1056
Hide Left Dock: false
- Hide Right Dock: false
+ Hide Right Dock: true
QMainWindow State: 000000ff00000000fd00000004000000000000027f00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
@@ -323,7 +369,7 @@ Window Geometry:
Tool Properties:
collapsed: false
Views:
- collapsed: false
+ collapsed: true
Width: 1855
X: 65
Y: 24
diff --git a/yaml/Gravity/csm.yaml b/yaml/Gravity/csm.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..59f62f628e15cb484f40d3b0ce50b404dfdd317d
--- /dev/null
+++ b/yaml/Gravity/csm.yaml
@@ -0,0 +1,35 @@
+max_iterations: 50
+max_correspondence_dist: 10
+use_corr_tricks: 1
+outliers_maxPerc: 0.9
+use_point_to_line_distance: 1
+outliers_adaptive_order: 0.7
+outliers_adaptive_mult: 1.5
+do_compute_covariance: 1
+
+max_angular_correction_deg: 10
+max_linear_correction: 1
+epsilon_xy: 0
+epsilon_theta: 0
+sigma: 0.2
+restart: 0
+restart_threshold_mean_error: 0
+restart_dt: 0
+restart_dtheta: 0
+clustering_threshold: 0
+orientation_neighbourhood: 0
+do_alpha_test: 0
+do_alpha_test_thresholdDeg: 0
+do_visibility_test: 0
+outliers_remove_doubles: 0
+debug_verify_tricks: 0
+gpm_theta_bin_size_deg: 0
+gpm_extend_range_deg: 0
+gpm_interval: 0
+min_reading: 0.023
+max_reading: 60
+use_ml_weights: 0
+use_sigma_weights: 0
+
+
+cov_factor: 1
diff --git a/yaml/Gravity/csm_inactive.yaml b/yaml/Gravity/csm_inactive.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b99171d815291adbd3d320cc2e67bd53a128fd85
--- /dev/null
+++ b/yaml/Gravity/csm_inactive.yaml
@@ -0,0 +1,35 @@
+max_iterations: 50
+max_correspondence_dist: 10
+use_corr_tricks: 1
+outliers_maxPerc: 0.9
+use_point_to_line_distance: 1
+outliers_adaptive_order: 0.7
+outliers_adaptive_mult: 1.5
+do_compute_covariance: 1
+
+max_angular_correction_deg: 10
+max_linear_correction: 1
+epsilon_xy: 0
+epsilon_theta: 0
+sigma: 0.2
+restart: 0
+restart_threshold_mean_error: 0
+restart_dt: 0
+restart_dtheta: 0
+clustering_threshold: 0
+orientation_neighbourhood: 0
+do_alpha_test: 0
+do_alpha_test_thresholdDeg: 0
+do_visibility_test: 0
+outliers_remove_doubles: 0
+debug_verify_tricks: 0
+gpm_theta_bin_size_deg: 0
+gpm_extend_range_deg: 0
+gpm_interval: 0
+min_reading: 0.023
+max_reading: 60
+use_ml_weights: 0
+use_sigma_weights: 0
+
+
+cov_factor: 9999999999999999999999
diff --git a/yaml/Gravity/imu2d_gravity_test1.yaml b/yaml/Gravity/imu2d_gravity_test1.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b6509469d423dc8ecf4e97f8fc9216c81250927d
--- /dev/null
+++ b/yaml/Gravity/imu2d_gravity_test1.yaml
@@ -0,0 +1,153 @@
+config:
+ debug:
+ profiling: true
+ profiling_file: "~/wolf_demo_profiling_imu2d_test1.txt"
+ print_problem: false
+ print_period: 5
+ 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
+ map:
+ type: "MapGrid2dGravity"
+ plugin: "imu"
+ resolution: 4
+
+ solver:
+ max_num_iterations: 100
+ verbose: 0
+ period: 0
+ update_immediately: false
+ n_threads: 2
+ compute_cov: false
+
+ sensors:
+ -
+ type: "SensorOdom2d"
+ name: "odom2d"
+ plugin: "core"
+ follow: "test_odom2d_params.yaml"
+ -
+ type: "SensorLaser2d"
+ name: "scanner_front_left"
+ plugin: "laser"
+ follow: "test_laser_params.yaml"
+ -
+ type: "SensorImu2d"
+ name: "bno"
+ plugin: "imu"
+ follow: "test_imu_params.yaml"
+ -
+ type: "SensorImu2d"
+ name: "microstrain"
+ plugin: "imu"
+ follow: "test_imu_params.yaml"
+ -
+ type: "SensorGnss"
+ name: "ublox"
+ plugin: "gnss"
+ follow: "test_gnss_params.yaml"
+
+ processors:
+ -
+ type: "ProcessorOdom2d"
+ name: "processorodom2d"
+ sensor_name: "odom2d"
+ plugin: "core"
+ follow: "test_odom2d_processor.yaml"
+ -
+ type: "ProcessorOdomIcp"
+ name: "processorodomicp"
+ sensor_name: "scanner_front_left"
+ plugin: "laser"
+ follow: "test_laser_processor.yaml"
+ #-
+ # type: "ProcessorImu2d"
+ # name: "processorimu2dbno"
+ # sensor_name: "bno"
+ # plugin: "imu"
+ # follow: "test_imu_processor.yaml"
+ -
+ type: "ProcessorImu2d"
+ name: "processorimu2dmicro"
+ sensor_name: "microstrain"
+ plugin: "imu"
+ follow: "test_imu_processor.yaml"
+ -
+ type: "ProcessorGnssFix"
+ name: "processorgnss"
+ sensor_name: "ublox"
+ plugin: "gnss"
+ follow: "test_gnss_processor.yaml"
+
+ ROS subscriber:
+ -
+ package: "wolf_ros_node"
+ type: "SubscriberOdom2d"
+ topic: "/helena/odom"
+ sensor_name: "odom2d"
+ -
+ package: "wolf_ros_laser"
+ type: "SubscriberLaser2d"
+ topic: "/helena/sensors/scan"
+ sensor_name: "scanner_front_left"
+ load_params_from_msg: true
+ -
+ package: "wolf_ros_imu"
+ type: "SubscriberImuEnableable"
+ #topic: "/helena/sensors/bno055_imu/imu"
+ #sensor_name: "bno"
+ topic: "/helena/sensors/imu/data"
+ sensor_name: "microstrain"
+ follow: "test_imu_subscriber.yaml"
+ -
+ package: "wolf_ros_gnss"
+ type: "SubscriberGnssFix"
+ topic: "/helena/sensors/ublox_gps/fix"
+ sensor_name: "ublox"
+ cov_mode: "factor"
+ cov_factor: 10
+
+ ROS publisher:
+ -
+ package: "wolf_ros_node"
+ type: "PublisherTf"
+ topic: " "
+ period: 0.1
+ follow: "test_publisher_ros_node.yaml"
+ -
+ package: "wolf_ros_node"
+ type: "PublisherGraph"
+ topic: "graph"
+ period: 1
+ viz_overlapped_factors: true
+ -
+ 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
+ -
+ package: "wolf_ros_node"
+ type: "PublisherStateBlock"
+ topic: "gravity"
+ period: 0.1
+ sensor: "microstrain"
+ key: "G"
diff --git a/yaml/Gravity/test_gnss_params.yaml b/yaml/Gravity/test_gnss_params.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7290a7da2e05aa526762ba849bf147625d9c9e91
--- /dev/null
+++ b/yaml/Gravity/test_gnss_params.yaml
@@ -0,0 +1,12 @@
+extrinsic:
+ pose: [0.270,0.160,0]
+extrinsics_fixed: true
+clock_bias_GPS_GLO_dynamic: false
+clock_bias_GPS_GAL_dynamic: false
+clock_bias_GPS_CMP_dynamic: false
+ENU:
+ mode: "auto"
+ roll_fixed: true
+ pitch_fixed: true
+ yaw_fixed: true
+ translation_fixed: true
diff --git a/yaml/Gravity/test_gnss_processor.yaml b/yaml/Gravity/test_gnss_processor.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..af7fa5dc0062eba4f4b5042992da66768bfd7ae5
--- /dev/null
+++ b/yaml/Gravity/test_gnss_processor.yaml
@@ -0,0 +1,13 @@
+keyframe_vote:
+ voting_active: true
+ max_time_span: 1
+ dist_traveled: 999
+time_tolerance: 0.1
+init_enu_map: false
+#enu_map_init_dist_min:
+#enu_map_init_dist_max:
+enu_map_fix_dist: 0
+fix_from_raw: false
+remove_outliers: false
+outlier_residual_th: 0
+apply_loss_function: false
diff --git a/yaml/Gravity/test_imu_params.yaml b/yaml/Gravity/test_imu_params.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..fdc8d87838a2681956302b317c4773c37e8688e0
--- /dev/null
+++ b/yaml/Gravity/test_imu_params.yaml
@@ -0,0 +1,20 @@
+##bno
+#extrinsic:
+# pose: [0,0,0]
+#a_noise: 0.96 #best: 0.96
+#w_noise: 0.2
+#ab_initial_stdev: 0.5 # m/s2 - initial bias
+#wb_initial_stdev: 0.1 # rad/sec - initial bias
+#ab_rate_stdev: 0.0000001 # m/s2/sqrt(s)
+#wb_rate_stdev: 0.0000001 # rad/s/sqrt(s)
+#orthogonal_gravity: false
+#microstrain
+extrinsic:
+ pose: [0,0,0]
+a_noise: 0.900
+w_noise: 0.1
+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/Gravity/test_imu_processor.yaml b/yaml/Gravity/test_imu_processor.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3ea39004a54e1051b0d791ec931992eaa5a499af
--- /dev/null
+++ b/yaml/Gravity/test_imu_processor.yaml
@@ -0,0 +1,12 @@
+time_tolerance: 0.008 #Half the sample time, bno
+# time_tolerance: 0.005 #Half the sample time, microstrain
+unmeasured_perturbation_std: 0.1
+keyframe_vote:
+ voting_active: false
+ max_time_span: 2
+ max_buff_length: 1000000
+ dist_traveled: 10
+ angle_turned: 3.14
+state_getter: true
+state_priority: 1
+apply_loss_function: false
diff --git a/yaml/Gravity/test_imu_subscriber.yaml b/yaml/Gravity/test_imu_subscriber.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0a77d7c3e92e6afd7babda98e5cb615db9b4bfd1
--- /dev/null
+++ b/yaml/Gravity/test_imu_subscriber.yaml
@@ -0,0 +1,20 @@
+##bno:
+#dt: 0.016
+#imu_x_axis: 3
+#imu_y_axis: -2
+#imu_z_axis: 1
+#cov_source: "sensor"
+#in_degrees: true
+#microstrain:
+dt: 0.01
+imu_x_axis: 1
+imu_y_axis: -2
+imu_z_axis: -3
+cov_source: "sensor"
+in_degrees: false
+
+#all
+topic_enable: "/helena/sensors/bno055_imu/imu/topic_enable"
+static_init_duration: 5
+lowpass_filter: true
+lowpass_cutoff_freq: 5
diff --git a/yaml/Gravity/test_laser_params.yaml b/yaml/Gravity/test_laser_params.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f8f9091baa45244adde53ddd856b20980bf71a39
--- /dev/null
+++ b/yaml/Gravity/test_laser_params.yaml
@@ -0,0 +1,11 @@
+extrinsic:
+ pose: [0.198,-0.002,3.141592653589793]
+LaserScanParams:
+ angle_min: -3.14159274101
+ angle_max: 3.14159274101
+ angle_step: 0.007 #0.00700000021607
+ scan_time: 0.0
+ range_min: 0.0
+ range_max: 200
+ range_std_dev: 0.2
+ angle_std_dev: 0.01
diff --git a/yaml/Gravity/test_laser_processor.yaml b/yaml/Gravity/test_laser_processor.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..06847e9355446b14a201ee058fd0c5617f027897
--- /dev/null
+++ b/yaml/Gravity/test_laser_processor.yaml
@@ -0,0 +1,15 @@
+keyframe_vote:
+ voting_active: true
+ min_features_for_keyframe: 10
+ min_dist: 999
+ min_angle: 999
+ min_time: 1
+ min_error: 999
+ max_points: 0
+max_new_features: 15
+follow: "csm.yaml"
+time_tolerance: 0.05 #Half the sample time
+state_getter: false
+state_priority: 10
+apply_loss_function: false
+initial_guess: "state"
diff --git a/yaml/Gravity/test_laser_processor_inactive.yaml b/yaml/Gravity/test_laser_processor_inactive.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..e1de1ee0aaba9d581fa9f5c0c6e1037db61b2600
--- /dev/null
+++ b/yaml/Gravity/test_laser_processor_inactive.yaml
@@ -0,0 +1,15 @@
+keyframe_vote:
+ voting_active: false
+ min_features_for_keyframe: 10
+ min_dist: 999
+ min_angle: 999
+ min_time: 2
+ min_error: 999
+ max_points: 0
+max_new_features: 15
+follow: "csm_inactive.yaml"
+time_tolerance: 0.05 #Half the sample time
+state_getter: true
+state_priority: 10
+apply_loss_function: false
+initial_guess: "zero"
diff --git a/yaml/Gravity/test_odom2d_params.yaml b/yaml/Gravity/test_odom2d_params.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b203455bf11cecd7ab4aa62c5827484670b228ab
--- /dev/null
+++ b/yaml/Gravity/test_odom2d_params.yaml
@@ -0,0 +1,4 @@
+extrinsic:
+ pose: [0.198,-0.002,0]
+k_disp_to_disp: 0.08
+k_rot_to_rot: 0.6
diff --git a/yaml/Gravity/test_odom2d_processor.yaml b/yaml/Gravity/test_odom2d_processor.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2fc31b13f3a668053b7a252abca74011684a30cf
--- /dev/null
+++ b/yaml/Gravity/test_odom2d_processor.yaml
@@ -0,0 +1,12 @@
+keyframe_vote:
+ voting_active: false
+ max_time_span: 1
+ max_buff_length: 999
+ dist_traveled: 999
+ angle_turned: 3.14
+ cov_det: 999
+unmeasured_perturbation_std: 0.01
+time_tolerance: 0.05 #Half the sample time
+state_getter: true
+state_priority: 2
+apply_loss_function: false
diff --git a/yaml/Gravity/test_publisher_ros_node.yaml b/yaml/Gravity/test_publisher_ros_node.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b4abe64574d3775edbfa5182497f268a82947ac4
--- /dev/null
+++ b/yaml/Gravity/test_publisher_ros_node.yaml
@@ -0,0 +1,4 @@
+map_frame_id: "map"
+odom_frame_id: "helena/odom"
+base_frame_id: "helena/base_footprint"
+publish_odom_tf: false
diff --git a/yaml/csm.yaml b/yaml/csm.yaml
index 86a844a093df26002959cceae25d6b1c0d00b8a4..59f62f628e15cb484f40d3b0ce50b404dfdd317d 100644
--- a/yaml/csm.yaml
+++ b/yaml/csm.yaml
@@ -1,5 +1,5 @@
max_iterations: 50
-max_correspondence_dist: 1
+max_correspondence_dist: 10
use_corr_tricks: 1
outliers_maxPerc: 0.9
use_point_to_line_distance: 1
@@ -7,8 +7,8 @@ outliers_adaptive_order: 0.7
outliers_adaptive_mult: 1.5
do_compute_covariance: 1
-max_angular_correction_deg: 0
-max_linear_correction: 0
+max_angular_correction_deg: 10
+max_linear_correction: 1
epsilon_xy: 0
epsilon_theta: 0
sigma: 0.2
@@ -26,18 +26,10 @@ debug_verify_tricks: 0
gpm_theta_bin_size_deg: 0
gpm_extend_range_deg: 0
gpm_interval: 0
-laser_x: 0
-laser_y: 0
-laser_theta: 0
min_reading: 0.023
max_reading: 60
use_ml_weights: 0
use_sigma_weights: 0
-hsm_linear_cell_size: 0.03
-hsm_angular_cell_size_deg: 1
-hsm_num_angular_hypotheses: 8
-hsm_xc_directions_min_distance_deg: 10
-hsm_xc_ndirections: 3
-hsm_angular_hyp_min_distance_deg: 10
-hsm_linear_xc_max_npeaks: 5
-hsm_linear_xc_peaks_min_distance: 5
\ No newline at end of file
+
+
+cov_factor: 1
diff --git a/yaml/csm_inactive.yaml b/yaml/csm_inactive.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b99171d815291adbd3d320cc2e67bd53a128fd85
--- /dev/null
+++ b/yaml/csm_inactive.yaml
@@ -0,0 +1,35 @@
+max_iterations: 50
+max_correspondence_dist: 10
+use_corr_tricks: 1
+outliers_maxPerc: 0.9
+use_point_to_line_distance: 1
+outliers_adaptive_order: 0.7
+outliers_adaptive_mult: 1.5
+do_compute_covariance: 1
+
+max_angular_correction_deg: 10
+max_linear_correction: 1
+epsilon_xy: 0
+epsilon_theta: 0
+sigma: 0.2
+restart: 0
+restart_threshold_mean_error: 0
+restart_dt: 0
+restart_dtheta: 0
+clustering_threshold: 0
+orientation_neighbourhood: 0
+do_alpha_test: 0
+do_alpha_test_thresholdDeg: 0
+do_visibility_test: 0
+outliers_remove_doubles: 0
+debug_verify_tricks: 0
+gpm_theta_bin_size_deg: 0
+gpm_extend_range_deg: 0
+gpm_interval: 0
+min_reading: 0.023
+max_reading: 60
+use_ml_weights: 0
+use_sigma_weights: 0
+
+
+cov_factor: 9999999999999999999999
diff --git a/yaml/imu2d.yaml b/yaml/imu2d.yaml
index 2dfaaf112c5686cc2f02a602870abae3754ed15b..011f4e63a65e3416eaf265e5f792e9ba51308e5f 100644
--- a/yaml/imu2d.yaml
+++ b/yaml/imu2d.yaml
@@ -2,7 +2,7 @@ config:
debug:
profiling: true
profiling_file: "~/wolf_demo_profiling_imu2d.txt"
- print_problem: true
+ print_problem: false
print_period: 5
problem:
tree_manager:
@@ -55,18 +55,18 @@ config:
range_max: 200
range_std_dev: 0.2
angle_std_dev: 0.01
- -
- type: "SensorImu2d"
- name: "microstrain"
- plugin: "imu"
- extrinsic:
- pose: [0,0,0]
- a_noise: 0.053
- w_noise: 0.011
- 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.0000001 # rad/s/sqrt(s)
+ #-
+ # type: "SensorImu2d"
+ # name: "microstrain"
+ # plugin: "imu"
+ # extrinsic:
+ # pose: [0,0,0]
+ # a_noise: 0.053
+ # w_noise: 0.011
+ # 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.0000001 # rad/s/sqrt(s)
-
type: "SensorImu2d"
@@ -88,15 +88,15 @@ config:
sensor_name: "odom2d"
plugin: "core"
keyframe_vote:
- voting_active: true
- max_time_span: 1
+ voting_active: false
+ max_time_span: 5
max_buff_length: 999
dist_traveled: 999
angle_turned: 3.14
cov_det: 999
unmeasured_perturbation_std: 0.01
time_tolerance: 0.05 #Half the sample time
- state_getter: false
+ state_getter: true
state_priority: 1
apply_loss_function: false
-
@@ -105,19 +105,20 @@ config:
sensor_name: "scanner_front_left"
plugin: "laser"
keyframe_vote:
- voting_active: false
+ voting_active: true
min_features_for_keyframe: 10
max_new_features: 15
follow: "csm.yaml"
vfk_min_dist: 999
vfk_min_angle: 999
- vfk_min_time: 2
- vfk_min_error: 0.05
- vfk_max_points: 140
+ vfk_min_time: 3
+ vfk_min_error: 0.1
+ vfk_max_points: 200
time_tolerance: 0.05 #Half the sample time
- state_getter: false
- state_priority: 1
+ state_getter: true
+ state_priority: 10
apply_loss_function: false
+ initial_guess_from_state: true
#-
# type: "ProcessorLoopClosureIcp"
# name: "processorloopclosureicp"
@@ -134,7 +135,7 @@ config:
# apply_loss_function: true
-
type: "ProcessorImu2d"
- name: "processorimu2d"
+ name: "processorimu2dbno"
sensor_name: "bno"
plugin: "imu"
time_tolerance: 0.008 #Half the sample time, bno
@@ -146,24 +147,24 @@ config:
dist_traveled: 10
angle_turned: 3.14
state_getter: true
- state_priority: 1
- apply_loss_function: false
- -
- type: "ProcessorImu2d"
- name: "processorimu2d"
- sensor_name: "microstrain"
- plugin: "imu"
- time_tolerance: 0.005 #Half the sample time, microstrain
- unmeasured_perturbation_std: 0.1
- keyframe_vote:
- voting_active: false
- max_time_span: 2
- max_buff_length: 1000000
- dist_traveled: 10
- angle_turned: 3.14
- state_getter: true
state_priority: 2
apply_loss_function: false
+ #-
+ # type: "ProcessorImu2d"
+ # name: "processorimu2dmicro"
+ # sensor_name: "microstrain"
+ # plugin: "imu"
+ # time_tolerance: 0.005 #Half the sample time, microstrain
+ # unmeasured_perturbation_std: 0.1
+ # keyframe_vote:
+ # voting_active: false
+ # max_time_span: 2
+ # max_buff_length: 1000000
+ # dist_traveled: 10
+ # angle_turned: 3.14
+ # state_getter: true
+ # state_priority: 3
+ # apply_loss_function: false
ROS subscriber:
-
@@ -174,7 +175,7 @@ config:
-
package: "wolf_ros_laser"
type: "SubscriberLaser2d"
- topic: "/ana/sensors/scan_mal"
+ topic: "/ana/sensors/scan"
sensor_name: "scanner_front_left"
load_params_from_msg: true
-
@@ -182,7 +183,7 @@ config:
type: "SubscriberImuEnableable"
topic: "/ana/sensors/bno055_imu/imu"
topic_enable: "/ana/sensors/bno055_imu/imu/topic_enable"
- static_init_duration: 0
+ static_init_duration: 14
lowpass_filter: false
lowpass_cutoff_freq: 5
dt: 0.016
@@ -193,22 +194,22 @@ config:
imu_z_axis: 1
cov_source: "sensor"
in_degrees: true
- -
- package: "wolf_ros_imu"
- type: "SubscriberImuEnableable"
- topic: "/ana/imu/data_mal"
- topic_enable: "/ana/sensors/bno055_imu/imu/topic_enable"
- static_init_duration: 0
- lowpass_filter: false
- lowpass_cutoff_freq: 5
- dt: 0.016
- sensor_name: "microstrain"
- #microstrain:
- imu_x_axis: 1
- imu_y_axis: -2
- imu_z_axis: -3
- cov_source: "sensor"
- in_degrees: false
+ #-
+ # package: "wolf_ros_imu"
+ # type: "SubscriberImuEnableable"
+ # topic: "/ana/imu/data_mal"
+ # topic_enable: "/ana/sensors/bno055_imu/imu/topic_enable"
+ # static_init_duration: 10
+ # lowpass_filter: false
+ # lowpass_cutoff_freq: 5
+ # dt: 0.016
+ # sensor_name: "microstrain"
+ # #microstrain:
+ # imu_x_axis: 1
+ # imu_y_axis: -2
+ # imu_z_axis: -3
+ # cov_source: "sensor"
+ # in_degrees: false
ROS publisher:
-
@@ -218,8 +219,8 @@ config:
period: 0.1
map_frame_id: "map"
odom_frame_id: "ana/odom"
- base_frame_id: "ana/base_link"
- publish_odom_tf: false
+ base_frame_id: "ana/base_footprint"
+ publish_odom_tf: true
-
package: "wolf_ros_node"
type: "PublisherGraph"
diff --git a/yaml/imu2d_test1.yaml b/yaml/imu2d_test1.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..32bc9486a0adaeed477d032490bd88c3586c5205
--- /dev/null
+++ b/yaml/imu2d_test1.yaml
@@ -0,0 +1,132 @@
+config:
+ debug:
+ profiling: true
+ profiling_file: "~/wolf_demo_profiling_imu2d_test1.txt"
+ print_problem: true
+ print_period: 5
+ print_depth: 4
+ print_constr_by: false
+ print_metric: true
+ print_state_blocks: false
+ 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
+ map:
+ type: "MapBase"
+ plugin: "core"
+
+ solver:
+ max_num_iterations: 100
+ verbose: 0
+ period: 0
+ update_immediately: false
+ n_threads: 2
+ compute_cov: false
+
+ sensors:
+ -
+ type: "SensorOdom2d"
+ name: "odom2d"
+ plugin: "core"
+ follow: "test_odom2d_params.yaml"
+ -
+ type: "SensorLaser2d"
+ name: "scanner_front_left"
+ plugin: "laser"
+ follow: "test_laser_params.yaml"
+ -
+ type: "SensorImu2d"
+ name: "bno"
+ plugin: "imu"
+ follow: "test_imu_params.yaml"
+ -
+ type: "SensorImu2d"
+ name: "microstrain"
+ plugin: "imu"
+ follow: "test_imu_params.yaml"
+
+ processors:
+ -
+ type: "ProcessorOdom2d"
+ name: "processorodom2d"
+ sensor_name: "odom2d"
+ plugin: "core"
+ follow: "test_odom2d_processor.yaml"
+ -
+ type: "ProcessorOdomIcp"
+ name: "processorodomicp"
+ sensor_name: "scanner_front_left"
+ plugin: "laser"
+ follow: "test_laser_processor.yaml"
+ -
+ type: "ProcessorImu2d"
+ name: "processorimu2dbno"
+ sensor_name: "bno"
+ plugin: "imu"
+ follow: "test_imu_processor.yaml"
+ #-
+ # type: "ProcessorImu2d"
+ # name: "processorimu2dmicro"
+ # sensor_name: "microstrain"
+ # plugin: "imu"
+ # follow: "test_imu_processor.yaml"
+
+ ROS subscriber:
+ -
+ package: "wolf_ros_node"
+ type: "SubscriberOdom2d"
+ topic: "/ana/odom"
+ sensor_name: "odom2d"
+ -
+ package: "wolf_ros_laser"
+ type: "SubscriberLaser2d"
+ topic: "/ana/sensors/scan"
+ sensor_name: "scanner_front_left"
+ load_params_from_msg: true
+ -
+ package: "wolf_ros_imu"
+ type: "SubscriberImuEnableable"
+ topic: "/ana/sensors/bno055_imu/imu"
+ sensor_name: "bno"
+ #topic: "/ana/imu/data"
+ #sensor_name: "microstrain"
+ follow: "test_imu_subscriber.yaml"
+
+ ROS publisher:
+ -
+ package: "wolf_ros_node"
+ type: "PublisherTf"
+ topic: " "
+ period: 0.1
+ follow: "test_publisher_ros_node.yaml"
+ -
+ package: "wolf_ros_node"
+ type: "PublisherGraph"
+ topic: "graph"
+ period: 1
+ viz_overlapped_factors: true
+ -
+ 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/imu2d_test2.yaml b/yaml/imu2d_test2.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..9581a6eb62b4466ec32c53857ecbef8850564acb
--- /dev/null
+++ b/yaml/imu2d_test2.yaml
@@ -0,0 +1,93 @@
+config:
+ debug:
+ profiling: true
+ profiling_file: "~/wolf_demo_profiling_imu2d_test2.txt"
+ print_problem: false
+ print_period: 5
+ problem:
+ tree_manager:
+ type: "none"
+ frame_structure: "PO"
+ dimension: 2
+ prior:
+ mode: "fix"
+ $state:
+ P: [0,0]
+ O: [0]
+ time_tolerance: 0.1
+
+ solver:
+ max_num_iterations: 100
+ verbose: 0
+ period: 0
+ update_immediately: false
+ n_threads: 2
+ compute_cov: false
+
+ sensors:
+ -
+ type: "SensorOdom2d"
+ name: "odom2d"
+ plugin: "core"
+ follow: "test_odom2d_params.yaml"
+ -
+ type: "SensorLaser2d"
+ name: "scanner_front_left"
+ plugin: "laser"
+ follow: "test_laser_params.yaml"
+
+ processors:
+ -
+ type: "ProcessorOdom2d"
+ name: "processorodom2d"
+ sensor_name: "odom2d"
+ plugin: "core"
+ follow: "test_odom2d_processor.yaml"
+ -
+ type: "ProcessorOdomIcp"
+ name: "processorodomicp"
+ sensor_name: "scanner_front_left"
+ plugin: "laser"
+ follow: "test_laser_processor_inactive.yaml"
+
+ ROS subscriber:
+ -
+ package: "wolf_ros_node"
+ type: "SubscriberOdom2d"
+ topic: "/ana/odom"
+ sensor_name: "odom2d"
+ -
+ package: "wolf_ros_laser"
+ type: "SubscriberLaser2d"
+ topic: "/ana/sensors/scan"
+ sensor_name: "scanner_front_left"
+ load_params_from_msg: true
+
+ ROS publisher:
+ -
+ package: "wolf_ros_node"
+ type: "PublisherTf"
+ topic: " "
+ period: 0.1
+ follow: "test_publisher_ros_node.yaml"
+ -
+ package: "wolf_ros_node"
+ type: "PublisherGraph"
+ topic: "graph"
+ period: 1
+ viz_overlapped_factors: true
+ -
+ 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/imu2d_test3.yaml b/yaml/imu2d_test3.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..668270350f786fc3b909e68e10792e6f74933212
--- /dev/null
+++ b/yaml/imu2d_test3.yaml
@@ -0,0 +1,124 @@
+config:
+ debug:
+ profiling: true
+ profiling_file: "~/wolf_demo_profiling_imu2d_test3.txt"
+ print_problem: false
+ print_period: 5
+ 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
+
+ solver:
+ max_num_iterations: 100
+ verbose: 0
+ period: 0
+ update_immediately: false
+ n_threads: 2
+ compute_cov: false
+
+ sensors:
+ -
+ type: "SensorOdom2d"
+ name: "odom2d"
+ plugin: "core"
+ follow: "test_odom2d_params.yaml"
+ -
+ type: "SensorLaser2d"
+ name: "scanner_front_left"
+ plugin: "laser"
+ follow: "test_laser_params.yaml"
+ -
+ type: "SensorImu2d"
+ name: "bno"
+ plugin: "imu"
+ follow: "test_imu_params.yaml"
+ -
+ type: "SensorImu2d"
+ name: "microstrain"
+ plugin: "imu"
+ follow: "test_imu_params.yaml"
+
+ processors:
+ -
+ type: "ProcessorOdom2d"
+ name: "processorodom2d"
+ sensor_name: "odom2d"
+ plugin: "core"
+ follow: "test_odom2d_processor.yaml"
+ -
+ type: "ProcessorOdomIcp"
+ name: "processorodomicp"
+ sensor_name: "scanner_front_left"
+ plugin: "laser"
+ follow: "test_laser_processor_inactive.yaml"
+ -
+ type: "ProcessorImu2d"
+ name: "processorimu2dbno"
+ sensor_name: "bno"
+ plugin: "imu"
+ follow: "test_imu_processor.yaml"
+ #-
+ # type: "ProcessorImu2d"
+ # name: "processorimu2dmicro"
+ # sensor_name: "microstrain"
+ # plugin: "imu"
+ # follow: "test_imu_processor.yaml"
+
+ ROS subscriber:
+ -
+ package: "wolf_ros_node"
+ type: "SubscriberOdom2d"
+ topic: "/ana/odom"
+ sensor_name: "odom2d"
+ -
+ package: "wolf_ros_laser"
+ type: "SubscriberLaser2d"
+ topic: "/ana/sensors/scan"
+ sensor_name: "scanner_front_left"
+ load_params_from_msg: true
+ -
+ package: "wolf_ros_imu"
+ type: "SubscriberImuEnableable"
+ topic: "/ana/sensors/bno055_imu/imu"
+ sensor_name: "bno"
+ #topic: "/ana/imu/data"
+ #sensor_name: "microstrain"
+ follow: "test_imu_subscriber.yaml"
+
+ ROS publisher:
+ -
+ package: "wolf_ros_node"
+ type: "PublisherTf"
+ topic: " "
+ period: 0.1
+ follow: "test_publisher_ros_node.yaml"
+ -
+ package: "wolf_ros_node"
+ type: "PublisherGraph"
+ topic: "graph"
+ period: 1
+ viz_overlapped_factors: true
+ -
+ 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/imu2d_test4.yaml b/yaml/imu2d_test4.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..e9dd597b83e9c1cc4bfe09d68018bc5ce9f2e9cb
--- /dev/null
+++ b/yaml/imu2d_test4.yaml
@@ -0,0 +1,110 @@
+config:
+ debug:
+ profiling: true
+ profiling_file: "~/wolf_demo_profiling_imu2d_test4.txt"
+ print_problem: false
+ print_period: 5
+ 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
+
+ solver:
+ max_num_iterations: 100
+ verbose: 0
+ period: 0
+ update_immediately: false
+ n_threads: 2
+ compute_cov: false
+
+ sensors:
+ -
+ type: "SensorLaser2d"
+ name: "scanner_front_left"
+ plugin: "laser"
+ follow: "test_laser_params.yaml"
+ -
+ type: "SensorImu2d"
+ name: "bno"
+ plugin: "imu"
+ follow: "test_imu_params.yaml"
+ -
+ type: "SensorImu2d"
+ name: "microstrain"
+ plugin: "imu"
+ follow: "test_imu_params.yaml"
+
+ processors:
+ -
+ type: "ProcessorOdomIcp"
+ name: "processorodomicp"
+ sensor_name: "scanner_front_left"
+ plugin: "laser"
+ follow: "test_laser_processor_inactive.yaml"
+ #-
+ # type: "ProcessorImu2d"
+ # name: "processorimu2dbno"
+ # sensor_name: "bno"
+ # plugin: "imu"
+ # follow: "test_imu_processor.yaml"
+ -
+ type: "ProcessorImu2d"
+ name: "processorimu2dmicro"
+ sensor_name: "microstrain"
+ plugin: "imu"
+ follow: "test_imu_processor.yaml"
+
+ ROS subscriber:
+ -
+ package: "wolf_ros_laser"
+ type: "SubscriberLaser2d"
+ topic: "/ana/sensors/scan"
+ sensor_name: "scanner_front_left"
+ load_params_from_msg: true
+ -
+ package: "wolf_ros_imu"
+ type: "SubscriberImuEnableable"
+ ##bno
+ #topic: "/ana/sensors/bno055_imu/imu"
+ #sensor_name: "bno"
+ #microstrain
+ topic: "/ana/imu/data"
+ sensor_name: "microstrain"
+ follow: "test_imu_subscriber.yaml"
+
+ ROS publisher:
+ -
+ package: "wolf_ros_node"
+ type: "PublisherTf"
+ topic: " "
+ period: 0.1
+ follow: "test_publisher_ros_node.yaml"
+ -
+ package: "wolf_ros_node"
+ type: "PublisherGraph"
+ topic: "graph"
+ period: 1
+ viz_overlapped_factors: true
+ -
+ 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/imu2d_test5.yaml b/yaml/imu2d_test5.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2546db51174ac3caf7269eeeb256bdfc0e96d40b
--- /dev/null
+++ b/yaml/imu2d_test5.yaml
@@ -0,0 +1,109 @@
+config:
+ debug:
+ profiling: true
+ profiling_file: "~/wolf_demo_profiling_imu2d_test1.txt"
+ print_problem: true
+ print_period: 5
+ 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
+
+ solver:
+ max_num_iterations: 100
+ verbose: 0
+ period: 0
+ update_immediately: false
+ n_threads: 2
+ compute_cov: false
+
+ sensors:
+ -
+ type: "SensorLaser2d"
+ name: "scanner_front_left"
+ plugin: "laser"
+ follow: "test_laser_params.yaml"
+ -
+ type: "SensorImu2d"
+ name: "bno"
+ plugin: "imu"
+ follow: "test_imu_params.yaml"
+ -
+ type: "SensorImu2d"
+ name: "microstrain"
+ plugin: "imu"
+ follow: "test_imu_params.yaml"
+
+ processors:
+ -
+ type: "ProcessorOdomIcp"
+ name: "processorodomicp"
+ sensor_name: "scanner_front_left"
+ plugin: "laser"
+ follow: "test_laser_processor.yaml"
+ -
+ type: "ProcessorImu2d"
+ name: "processorimu2dbno"
+ sensor_name: "bno"
+ plugin: "imu"
+ follow: "test_imu_processor.yaml"
+ #-
+ # type: "ProcessorImu2d"
+ # name: "processorimu2dmicro"
+ # sensor_name: "microstrain"
+ # plugin: "imu"
+ # follow: "test_imu_processor.yaml"
+
+ ROS subscriber:
+ -
+ package: "wolf_ros_laser"
+ type: "SubscriberLaser2d"
+ topic: "/ana/sensors/scan"
+ sensor_name: "scanner_front_left"
+ load_params_from_msg: true
+ -
+ package: "wolf_ros_imu"
+ type: "SubscriberImuEnableable"
+ topic: "/ana/sensors/bno055_imu/imu"
+ sensor_name: "bno"
+ #topic: "/ana/imu/data"
+ #sensor_name: "microstrain"
+ follow: "test_imu_subscriber.yaml"
+
+ ROS publisher:
+ -
+ package: "wolf_ros_node"
+ type: "PublisherTf"
+ topic: " "
+ period: 0.1
+ follow: "test_publisher_ros_node.yaml"
+ -
+ package: "wolf_ros_node"
+ type: "PublisherGraph"
+ topic: "graph"
+ period: 1
+ viz_overlapped_factors: true
+ -
+ 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/imu2d_test6.yaml b/yaml/imu2d_test6.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..01ccb1b039f2aba6ff4e5aa6deffb1626d04d42b
--- /dev/null
+++ b/yaml/imu2d_test6.yaml
@@ -0,0 +1,95 @@
+config:
+ debug:
+ profiling: true
+ profiling_file: "~/wolf_demo_profiling_imu2d_test1.txt"
+ print_problem: false
+ print_period: 5
+ 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
+
+ solver:
+ max_num_iterations: 100
+ verbose: 0
+ period: 0
+ update_immediately: false
+ n_threads: 2
+ compute_cov: false
+
+ sensors:
+ -
+ type: "SensorOdom2d"
+ name: "odom2d"
+ plugin: "core"
+ follow: "test_odom2d_params.yaml"
+ -
+ type: "SensorLaser2d"
+ name: "scanner_front_left"
+ plugin: "laser"
+ follow: "test_laser_params.yaml"
+
+ processors:
+ -
+ type: "ProcessorOdom2d"
+ name: "processorodom2d"
+ sensor_name: "odom2d"
+ plugin: "core"
+ follow: "test_odom2d_processor.yaml"
+ -
+ type: "ProcessorOdomIcp"
+ name: "processorodomicp"
+ sensor_name: "scanner_front_left"
+ plugin: "laser"
+ follow: "test_laser_processor.yaml"
+
+ ROS subscriber:
+ -
+ package: "wolf_ros_node"
+ type: "SubscriberOdom2d"
+ topic: "/ana/odom"
+ sensor_name: "odom2d"
+ -
+ package: "wolf_ros_laser"
+ type: "SubscriberLaser2d"
+ topic: "/ana/sensors/scan"
+ sensor_name: "scanner_front_left"
+ load_params_from_msg: true
+
+ ROS publisher:
+ -
+ package: "wolf_ros_node"
+ type: "PublisherTf"
+ topic: " "
+ period: 0.1
+ follow: "test_publisher_ros_node.yaml"
+ -
+ package: "wolf_ros_node"
+ type: "PublisherGraph"
+ topic: "graph"
+ period: 1
+ viz_overlapped_factors: true
+ -
+ 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/odom-icp.yaml b/yaml/odom-icp.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..44a122fff0a2da32523ecb46b8f4fb8a7b68141b
--- /dev/null
+++ b/yaml/odom-icp.yaml
@@ -0,0 +1,155 @@
+config:
+ debug:
+ profiling: true
+ profiling_file: "~/wolf_demo_profiling_odom-icp.txt"
+ print_problem: false
+ print_period: 5
+ problem:
+ tree_manager:
+ type: "none"
+ frame_structure: "PO"
+ dimension: 2
+ prior:
+ mode: "fix"
+ $state:
+ P: [0,0]
+ O: [0]
+ time_tolerance: 0.1
+ #mode: "factor"
+ #state: [0,0,0]
+ #cov: [[3,3],.1,0,0,0,.1,0,0,0,.1]
+ #time_tolerance: 0.1
+
+ solver:
+ max_num_iterations: 100
+ verbose: 0
+ period: 0
+ update_immediately: false
+ n_threads: 2
+ compute_cov: false
+
+ sensors:
+ -
+ type: "SensorOdom2d"
+ name: "odom2d"
+ plugin: "core"
+ extrinsic:
+ pose: [0,0,0]
+ k_disp_to_disp: 0.1
+ k_rot_to_rot: 1.5
+ -
+ type: "SensorLaser2d"
+ name: "scanner_front_left"
+ plugin: "laser"
+ extrinsic:
+ # pose: [0.15,0.15,0]
+ # pose: [0.2,0.1, 1.57]
+ pose: [0,0,3.1415926535] #Last value was pi
+ LaserScanParams:
+ angle_min: -3.14159274101
+ angle_max: 3.14159274101
+ angle_step: 0.007 #0.00700000021607
+ scan_time: 0.0
+ range_min: 0.0
+ range_max: 200
+ range_std_dev: 0.2
+ angle_std_dev: 0.01
+
+ processors:
+ -
+ type: "ProcessorOdom2d"
+ name: "processorodom2d"
+ sensor_name: "odom2d"
+ plugin: "core"
+ keyframe_vote:
+ voting_active: false
+ max_time_span: 5
+ max_buff_length: 999
+ dist_traveled: 999
+ angle_turned: 3.14
+ cov_det: 999
+ unmeasured_perturbation_std: 0.01
+ time_tolerance: 0.05 #Half the sample time
+ state_getter: true
+ state_priority: 1
+ apply_loss_function: false
+ -
+ type: "ProcessorOdomIcp"
+ name: "processorodomicp"
+ sensor_name: "scanner_front_left"
+ plugin: "laser"
+ keyframe_vote:
+ voting_active: true
+ min_features_for_keyframe: 10
+ max_new_features: 15
+ follow: "csm.yaml"
+ vfk_min_dist: 999
+ vfk_min_angle: 999
+ vfk_min_time: 3
+ vfk_min_error: 0.1
+ vfk_max_points: 200
+ time_tolerance: 0.05 #Half the sample time
+ state_getter: true
+ state_priority: 10
+ apply_loss_function: false
+ initial_guess_from_state: true
+ #-
+ # type: "ProcessorLoopClosureIcp"
+ # name: "processorloopclosureicp"
+ # sensor_name: "scanner_front_left"
+ # plugin: "laser"
+ # recent_key_frames_ignored: 2
+ # key_frames_to_wait: 2
+ # max_error_threshold: 0.01
+ # min_points_percent: 50
+ # time_tolerance: 0.05 #Half the sample time
+ # keyframe_vote:
+ # voting_active: false
+ # follow: "csm.yaml"
+ # apply_loss_function: true
+
+ ROS subscriber:
+ -
+ package: "wolf_ros_node"
+ type: "SubscriberOdom2d"
+ topic: "/ana/odom"
+ sensor_name: "odom2d"
+ -
+ package: "wolf_ros_laser"
+ type: "SubscriberLaser2d"
+ topic: "/ana/sensors/scan"
+ sensor_name: "scanner_front_left"
+ load_params_from_msg: true
+
+ ROS publisher:
+ -
+ package: "wolf_ros_node"
+ type: "PublisherTf"
+ topic: " "
+ period: 0.1
+ map_frame_id: "map"
+ odom_frame_id: "ana/odom"
+ base_frame_id: "ana/base_footprint"
+ publish_odom_tf: true
+ -
+ package: "wolf_ros_node"
+ type: "PublisherGraph"
+ topic: "graph"
+ period: 1
+ viz_overlapped_factors: true
+ -
+ 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/test_imu_params.yaml b/yaml/test_imu_params.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..90a159bf44c203d3820b54c9a709b438daaea360
--- /dev/null
+++ b/yaml/test_imu_params.yaml
@@ -0,0 +1,20 @@
+#bno
+extrinsic:
+ pose: [0,0,0]
+a_noise: 0.96 #best: 0.96
+w_noise: 0.2
+ab_initial_stdev: 0.5 # m/s2 - initial bias
+wb_initial_stdev: 0.1 # rad/sec - initial bias
+ab_rate_stdev: 0.0000001 # m/s2/sqrt(s)
+wb_rate_stdev: 0.0000001 # rad/s/sqrt(s)
+orthogonal_gravity: true
+#microstrain
+#extrinsic:
+# pose: [0,0,0]
+#a_noise: 0.900
+#w_noise: 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/test_imu_processor.yaml b/yaml/test_imu_processor.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6e0ef2737d4ff6289e6d2d189d87508ee7b0ac60
--- /dev/null
+++ b/yaml/test_imu_processor.yaml
@@ -0,0 +1,13 @@
+time_tolerance: 0.008 #Half the sample time, bno
+# time_tolerance: 0.005 #Half the sample time, microstrain
+unmeasured_perturbation_std: 0.1
+keyframe_vote:
+ voting_active: false
+ max_time_span: 2
+ max_buff_length: 1000000
+ dist_traveled: 10
+ angle_turned: 3.14
+state_getter: true
+state_priority: 1
+apply_loss_function: false
+use_gravity_grid: false
diff --git a/yaml/test_imu_subscriber.yaml b/yaml/test_imu_subscriber.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..07f52959aac6dd33b54ed4b0652fd6c954ff8527
--- /dev/null
+++ b/yaml/test_imu_subscriber.yaml
@@ -0,0 +1,20 @@
+#bno:
+dt: 0.016
+imu_x_axis: 3
+imu_y_axis: -2
+imu_z_axis: 1
+cov_source: "sensor"
+in_degrees: true
+##microstrain:
+#dt: 0.01
+#imu_x_axis: 1
+#imu_y_axis: -2
+#imu_z_axis: -3
+#cov_source: "sensor"
+#in_degrees: false
+
+#all
+topic_enable: "/ana/sensors/bno055_imu/imu/topic_enable"
+static_init_duration: 14
+lowpass_filter: true
+lowpass_cutoff_freq: 5
diff --git a/yaml/test_laser_params.yaml b/yaml/test_laser_params.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0fb41c67ad15463c536ae63b6744979b1ea5ef90
--- /dev/null
+++ b/yaml/test_laser_params.yaml
@@ -0,0 +1,11 @@
+extrinsic:
+ pose: [0,0,3.1415926535] #Last value was pi
+LaserScanParams:
+ angle_min: -3.14159274101
+ angle_max: 3.14159274101
+ angle_step: 0.007 #0.00700000021607
+ scan_time: 0.0
+ range_min: 0.0
+ range_max: 200
+ range_std_dev: 0.2
+ angle_std_dev: 0.01
diff --git a/yaml/test_laser_processor.yaml b/yaml/test_laser_processor.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..06847e9355446b14a201ee058fd0c5617f027897
--- /dev/null
+++ b/yaml/test_laser_processor.yaml
@@ -0,0 +1,15 @@
+keyframe_vote:
+ voting_active: true
+ min_features_for_keyframe: 10
+ min_dist: 999
+ min_angle: 999
+ min_time: 1
+ min_error: 999
+ max_points: 0
+max_new_features: 15
+follow: "csm.yaml"
+time_tolerance: 0.05 #Half the sample time
+state_getter: false
+state_priority: 10
+apply_loss_function: false
+initial_guess: "state"
diff --git a/yaml/test_laser_processor_inactive.yaml b/yaml/test_laser_processor_inactive.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a584d2cde4e2f8852b231a822d6160c2a08add4b
--- /dev/null
+++ b/yaml/test_laser_processor_inactive.yaml
@@ -0,0 +1,15 @@
+keyframe_vote:
+ voting_active: true
+ min_features_for_keyframe: 10
+ min_dist: 999
+ min_angle: 999
+ min_time: 2
+ min_error: 999
+ max_points: 0
+max_new_features: 15
+follow: "csm_inactive.yaml"
+time_tolerance: 0.05 #Half the sample time
+state_getter: true
+state_priority: 10
+apply_loss_function: false
+initial_guess: "zero"
diff --git a/yaml/test_odom2d_params.yaml b/yaml/test_odom2d_params.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0e0922a64325ef75f6981deb6db802c65c91fbe0
--- /dev/null
+++ b/yaml/test_odom2d_params.yaml
@@ -0,0 +1,4 @@
+extrinsic:
+ pose: [0,0,0]
+k_disp_to_disp: 0.08
+k_rot_to_rot: 0.6
diff --git a/yaml/test_odom2d_processor.yaml b/yaml/test_odom2d_processor.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2fc31b13f3a668053b7a252abca74011684a30cf
--- /dev/null
+++ b/yaml/test_odom2d_processor.yaml
@@ -0,0 +1,12 @@
+keyframe_vote:
+ voting_active: false
+ max_time_span: 1
+ max_buff_length: 999
+ dist_traveled: 999
+ angle_turned: 3.14
+ cov_det: 999
+unmeasured_perturbation_std: 0.01
+time_tolerance: 0.05 #Half the sample time
+state_getter: true
+state_priority: 2
+apply_loss_function: false
diff --git a/yaml/test_publisher_ros_node.yaml b/yaml/test_publisher_ros_node.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2f38284a05c0481b3f542d8860cca3534d8e4233
--- /dev/null
+++ b/yaml/test_publisher_ros_node.yaml
@@ -0,0 +1,4 @@
+map_frame_id: "map"
+odom_frame_id: "ana/odom"
+base_frame_id: "ana/base_footprint"
+publish_odom_tf: true
diff --git a/yaml/tests.txt b/yaml/tests.txt
new file mode 100644
index 0000000000000000000000000000000000000000..17350ce9403c82c7c85d40489182f86273a94ef6
--- /dev/null
+++ b/yaml/tests.txt
@@ -0,0 +1,12 @@
+test1:
+ Tot: laser, odom2d, imu
+test2:
+ Només odom2d
+test3:
+ odom2d + imu
+test4:
+ Només imu
+test5
+ laser + imu
+test6:
+ laser + odom