diff --git a/bag/test_no_map.bag b/bag/test_no_map.bag
new file mode 100644
index 0000000000000000000000000000000000000000..129d9940c5a20c18c203aa582dc976af35bd05d4
Binary files /dev/null and b/bag/test_no_map.bag differ
diff --git a/launch/imu2d_demo.launch b/launch/imu2d_demo.launch
index 3f2da8e16b4302452dffaa7a2c817d768ba3a700..97f98df7fa7b600a0802996448327e31bf5da509 100644
--- a/launch/imu2d_demo.launch
+++ b/launch/imu2d_demo.launch
@@ -10,25 +10,32 @@
<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_tf2"
+ 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_tf"
+ 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 0 0 0 /ana/base_link /ana/velodyne 100"/>
+ 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/test3_filtered2_nomap.bag"/>
+ args="-s $(arg sec) -r $(arg speed) -k --clock $(find wolf_demo_imu2d)/bag/test_no_map.bag"/>
<node type="rviz"
name="rviz"
diff --git a/rviz/imu2d_demo.rviz b/rviz/imu2d_demo.rviz
index 10c1926278078b375aced5fa774e90a62a861149..1fb31303cb74517c54c7bb6ee596a72ba74c0621 100644
--- a/rviz/imu2d_demo.rviz
+++ b/rviz/imu2d_demo.rviz
@@ -6,7 +6,6 @@ Panels:
Expanded:
- /Global Options1
- /TF1
- - /TF1/Status1
- /TF1/Frames1
- /TF1/Tree1
- /Odometry1
@@ -40,7 +39,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
- SyncSource: ""
+ SyncSource: LaserScan
Toolbars:
toolButtonStyle: 2
Visualization Manager:
@@ -79,23 +78,46 @@ Visualization Manager:
Value: false
ana/front_right_axle:
Value: false
+ ana/imu_bno055:
+ Value: true
+ ana/imu_microstrain:
+ Value: true
ana/odom:
Value: true
ana/rear_left_axle:
Value: false
ana/rear_right_axle:
Value: false
+ ana/velodyne:
+ Value: true
+ map:
+ Value: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
- ana/odom:
- ana/base_footprint:
- {}
- ana/base_footprint_robot:
- {}
+ map:
+ ana/odom:
+ ana/base_footprint:
+ ana/base_link:
+ ana/front_left_axle:
+ {}
+ ana/front_right_axle:
+ {}
+ ana/imu_bno055:
+ {}
+ ana/imu_microstrain:
+ {}
+ ana/rear_left_axle:
+ {}
+ ana/rear_right_axle:
+ {}
+ ana/velodyne:
+ {}
+ ana/base_footprint_robot:
+ {}
Update Interval: 0
Value: true
- Angle Tolerance: 0.100000001
@@ -175,17 +197,22 @@ Visualization Manager:
Marker Topic: /wolf_ros_node/graph_factors
Name: Factors
Namespaces:
- {}
+ factors_processorimu2d: true
+ factors_processorodom2d: true
+ factors_processorodomicp: false
+ factors_text_processorimu2d: false
+ factors_text_processorodom2d: false
+ factors_text_processorodomicp: false
Queue Size: 100
Value: true
- Class: rviz/MarkerArray
- Enabled: true
+ Enabled: false
Marker Topic: /wolf_ros_node/graph_trajectory
Name: Trajectory
Namespaces:
{}
Queue Size: 100
- Value: true
+ Value: false
- Class: rviz/Axes
Enabled: true
Length: 1
@@ -236,17 +263,17 @@ Visualization Manager:
- Alpha: 1
Class: rviz_plugin_tutorials/Imu
Color: 204; 51; 204
- Enabled: true
+ Enabled: false
History Length: 1
Name: Imu
Topic: /ana/sensors/bno055_imu/imu_remapped
Unreliable: false
- Value: true
+ Value: false
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
- Fixed Frame: ana/odom
+ Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
@@ -276,11 +303,11 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
- Scale: 27.2575378
+ Scale: 77.8121338
Target Frame: ana/base_link
Value: TopDownOrtho (rviz)
- X: -6.06173658
- Y: -5.61819553
+ X: -0.064257361
+ Y: -0.334138036
Saved: ~
Window Geometry:
Displays:
@@ -288,7 +315,7 @@ Window Geometry:
Height: 1056
Hide Left Dock: false
Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd00000004000000000000027f00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003a50000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd00000004000000000000027f00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
diff --git a/yaml/imu2d.yaml b/yaml/imu2d.yaml
index 6704e15fd04dac83e13c34704cad6a962238a360..2dfaaf112c5686cc2f02a602870abae3754ed15b 100644
--- a/yaml/imu2d.yaml
+++ b/yaml/imu2d.yaml
@@ -1,7 +1,9 @@
config:
- profiling:
- enabled: true
- file_name: "~/wolf_demo_profiling_imu2d.txt"
+ debug:
+ profiling: true
+ profiling_file: "~/wolf_demo_profiling_imu2d.txt"
+ print_problem: true
+ print_period: 5
problem:
tree_manager:
type: "none"
@@ -35,7 +37,7 @@ config:
extrinsic:
pose: [0,0,0]
k_disp_to_disp: 0.1
- k_rot_to_rot: 0.2
+ k_rot_to_rot: 1.5
-
type: "SensorLaser2d"
name: "scanner_front_left"
@@ -43,7 +45,7 @@ config:
extrinsic:
# pose: [0.15,0.15,0]
# pose: [0.2,0.1, 1.57]
- pose: [0,0,0] #Last value was pi
+ pose: [0,0,3.1415926535] #Last value was pi
LaserScanParams:
angle_min: -3.14159274101
angle_max: 3.14159274101
@@ -55,17 +57,29 @@ config:
angle_std_dev: 0.01
-
type: "SensorImu2d"
- name: "sensorimu2d"
+ name: "microstrain"
plugin: "imu"
extrinsic:
- pose: [0,0,0]
- a_noise: 0.53
- w_noise: 0.11
+ 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.00001 # rad/s/sqrt(s)
+ wb_rate_stdev: 0.0000001 # rad/s/sqrt(s)
+ -
+ type: "SensorImu2d"
+ name: "bno"
+ 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.00001 # rad/s/sqrt(s)
processors:
-
@@ -74,7 +88,7 @@ config:
sensor_name: "odom2d"
plugin: "core"
keyframe_vote:
- voting_active: false
+ voting_active: true
max_time_span: 1
max_buff_length: 999
dist_traveled: 999
@@ -82,6 +96,8 @@ config:
cov_det: 999
unmeasured_perturbation_std: 0.01
time_tolerance: 0.05 #Half the sample time
+ state_getter: false
+ state_priority: 1
apply_loss_function: false
-
type: "ProcessorOdomIcp"
@@ -89,37 +105,55 @@ config:
sensor_name: "scanner_front_left"
plugin: "laser"
keyframe_vote:
- voting_active: true
+ voting_active: false
min_features_for_keyframe: 10
max_new_features: 15
follow: "csm.yaml"
vfk_min_dist: 999
vfk_min_angle: 999
- vfk_min_time: 1
+ vfk_min_time: 2
vfk_min_error: 0.05
vfk_max_points: 140
time_tolerance: 0.05 #Half the sample time
- apply_loss_function: true
+ state_getter: false
+ state_priority: 1
+ apply_loss_function: false
+ #-
+ # 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
-
- 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:
+ type: "ProcessorImu2d"
+ name: "processorimu2d"
+ sensor_name: "bno"
+ plugin: "imu"
+ time_tolerance: 0.008 #Half the sample time, bno
+ unmeasured_perturbation_std: 0.1
+ keyframe_vote:
voting_active: false
- follow: "csm.yaml"
- apply_loss_function: true
+ 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
-
type: "ProcessorImu2d"
name: "processorimu2d"
- sensor_name: "sensorimu2d"
+ sensor_name: "microstrain"
plugin: "imu"
- time_tolerance: 0.008 #Half the sample time
+ time_tolerance: 0.005 #Half the sample time, microstrain
unmeasured_perturbation_std: 0.1
keyframe_vote:
voting_active: false
@@ -127,8 +161,9 @@ config:
max_buff_length: 1000000
dist_traveled: 10
angle_turned: 3.14
+ state_getter: true
+ state_priority: 2
apply_loss_function: false
-
ROS subscriber:
-
@@ -139,19 +174,41 @@ config:
-
package: "wolf_ros_laser"
type: "SubscriberLaser2d"
- topic: "/ana/sensors/scan"
+ topic: "/ana/sensors/scan_mal"
sensor_name: "scanner_front_left"
- load_params_from_msg: false
+ load_params_from_msg: true
-
package: "wolf_ros_imu"
- type: "SubscriberImu"
- topic: "/ana/sensors/bno055_imu/imu_mal"
- sensor_name: "sensorimu2d"
+ type: "SubscriberImuEnableable"
+ topic: "/ana/sensors/bno055_imu/imu"
+ 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: "bno"
+ #bno:
imu_x_axis: 3
- imu_y_axis: 2
- imu_z_axis: -1
+ imu_y_axis: -2
+ 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
ROS publisher:
-