diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index b604e9a22b84096a3691f7b756504c0d2bc80cac..59acc15d8e48b0cc2304570ad1edc000fc610190 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -140,8 +140,8 @@ stages:
- catkin_make
- roscd wolf_demo_gnss_imu/bag
- wget https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_ros/demos/demo_rosbags/-/raw/master/gnss_imu/M600b_1.bag
- - roslaunch wolf_demo_gnss_imu demo_gnss_raw.launch rviz:=false bag:=M600b_1
- - roslaunch wolf_demo_gnss_imu demo_imu_gnss_raw.launch rviz:=false bag:=M600b_1
+ - roslaunch wolf_demo_gnss_imu demo_gnss_imu.launch rviz:=false bag:=M600b_1
+ - roslaunch wolf_demo_gnss_imu demo_gnss_imu.launch rviz:=false bag:=M600b_1 only_gnss:=true
############ UBUNTU 16.04 TEST ############
demo:xenial:
diff --git a/launch/demo_gnss.launch b/launch/demo_gnss.launch
deleted file mode 100644
index b15a38f91c712597468b6e298f998edf8ece5ab2..0000000000000000000000000000000000000000
--- a/launch/demo_gnss.launch
+++ /dev/null
@@ -1,49 +0,0 @@
-<!-- -->
-<launch>
- <arg name="rviz" default="false" />
- <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 -->
- <param name="use_sim_time" value="true" />
-
- <!--TF-->
- <node pkg="tf"
- type="static_transform_publisher"
- name="odom_base"
- args="0 0 0 0 0 0 odom base 1000" />
-
- <!--WOLF-->
- <node type="wolf_ros_node"
- name="wolf_ros_node"
- pkg="wolf_ros_node"
- output="screen"
- required="false"
- launch-prefix="$(arg launch_pref)">
- <param name="~yaml_file_path" value="$(find wolf_demo_gnss_imu)/yaml/params_gnss_raw.yaml"/>
- </node>
-
- <!--VISUALIZATION-->
- <group if="$(arg rviz)">
- <node name="rviz"
- pkg="rviz"
- type="rviz"
- args="-d $(find wolf_demo_gnss_imu)/rviz/gnss.rviz" />
- </group>
-
- <!--ROSBAG PLAY-->
- <node pkg="rosbag"
- type="play"
- name="player"
- required="true"
- args="-r $(arg speed)
- -s $(arg sec)
- --clock
- $(find wolf_demo_gnss_imu)/bag/mini8_2020-09-28-11-28-08.bag"/>
-
-</launch>
diff --git a/launch/demo_gnss_imu.launch b/launch/demo_gnss_imu.launch
index 569a62bec736afc6cb4e46c0b9bd350e278080ac..508208eb8efae5724ac77a52b0beec2282937532 100644
--- a/launch/demo_gnss_imu.launch
+++ b/launch/demo_gnss_imu.launch
@@ -1,12 +1,14 @@
<!-- -->
<launch>
+ <arg name="only_gnss" default="false" />
+
<arg name="rviz" default="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" default="" unless="$(arg gdb)"/>
<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 -->
@@ -23,9 +25,10 @@
name="wolf_ros_node"
pkg="wolf_ros_node"
output="screen"
- required="false"
+ required="true"
launch-prefix="$(arg launch_pref)">
- <param name="~yaml_file_path" value="$(find wolf_demo_gnss_imu)/yaml/params_gnss_imu.yaml"/>
+ <param name="~yaml_file_path" value="$(find wolf_demo_gnss_imu)/yaml/params_gnss_imu.yaml" unless="$(arg only_gnss)"/>
+ <param name="~yaml_file_path" value="$(find wolf_demo_gnss_imu)/yaml/params_gnss.yaml" if="$(arg only_gnss)"/>
</node>
<!--VISUALIZATION-->
@@ -36,12 +39,7 @@
args="-d $(find wolf_demo_gnss_imu)/rviz/gnss.rviz" />
</group>
- <!--<arg name="bag" default="rpa_sensors_tucan_2021-05-28-11-00-22"/>-->
- <!--<arg name="bag" default="mini8_2020-09-28-11-28-08"/>-->
- <!--<arg name="bag" default="rpa_sensors_M600b_2021-09-16-12-25-14"/>-->
- <!--<arg name="bag" default="rpa_sensors_M600b_2021-09-16-14-41-46"/>-->
- <arg name="bag" default="rpa_sensors_M600b_2021-09-17-11-00-51"/>
- <!--<arg name="bag" default="rpa_sensors_M600b_2021-09-17-11-31-55"/>-->
+ <arg name="bag" default="M600b_1"/> <!-- M600b_1, M600b_2, tucan_1 -->
<!--ROSBAG PLAY-->
<node pkg="rosbag"
diff --git a/yaml/params_gnss_raw.yaml b/yaml/params_gnss.yaml
similarity index 78%
rename from yaml/params_gnss_raw.yaml
rename to yaml/params_gnss.yaml
index 2b9dd47b7062fa05ab896dd7f69ef51704d16936..13389dbcd29994b5786f3b657105d592da6f429e 100644
--- a/yaml/params_gnss_raw.yaml
+++ b/yaml/params_gnss.yaml
@@ -1,18 +1,22 @@
config:
- profiling:
- enabled: false
- file_name: "~/wolf_gnss_raw_profiling.txt"
+ profiling:
+ debug:
+ profiling: true
+ profiling_file: "~/wolf_gnss_profiling.txt"
+ print_problem: false
+ print_depth: 4
+ print_constr_by: false
+ print_metric: false
+ print_state_blocks: false
+ print_period: 1
problem:
frame_structure: "PO"
dimension: 3
+ node_rate: 100
tree_manager:
- type: "TreeManagerSlidingWindowDualRate"
- n_frames: 10
- n_frames_recent: 5
- rate_old_frames: 10
- fix_first_frame: false
- viral_remove_empty_parent: true
+ follow: "sliding_window.yaml"
+ #follow: "sliding_window_dual.yaml"
prior:
mode: "factor"
time_tolerance: 0.1
@@ -24,15 +28,7 @@ config:
O: [0.31, 0.31, 0.31]
solver:
- max_num_iterations: 20
- verbose: 0
- period: 0.0
- n_threads: 2
- update_immediately: true
- min_num_iterations: 5 #if update immediately
- compute_cov: false
- cov_enum: 3
- cov_period: 1
+ follow: "solver.yaml"
sensors:
-
@@ -51,7 +47,6 @@ config:
pitch_fixed: true
yaw_fixed: true
translation_fixed: true
- #ENU_latlonalt: [0.7092812341, 0.3998234412, 71.8593953932] # [40.6388212031 deg, 22.9081957302 deg, 71.8593953932 m]
processors:
-
@@ -69,10 +64,12 @@ config:
init_frames: false
pseudo_ranges: true
enu_map_fix_dist: 5
+ fix: false
keyframe_vote:
voting_active: true
- voting_aux_active: false
max_time_span: 0.01
+ max_time_span: 1
+ min_features_for_keyframe: 4
gnss:
sateph: 6 # satellite ephemeris/clock: EPHOPT_BRDC(0):broadcast ephemeris, EPHOPT_PREC(1): precise ephemeris, EPHOPT_SBAS(2): broadcast + SBAS, EPHOPT_SSRAPC(3): broadcast + SSR_APC, EPHOPT_SSRCOM(4): broadcast + SSR_COM, EPHOPT_LEX(5): QZSS LEX ephemeris, EPHOPT_SBAS2(6):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), EPHOPT_SBAS3(7):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_SBAS2), EPHOPT_SBAS4(8):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_BRDC)
ionoopt: 9 # ionosphere option: IONOOPT_OFF(0):correction off, IONOOPT_BRDC(1):broadcast mode, IONOOPT_SBAS(2):SBAS model, IONOOPT_IFLC(3):L1/L2 or L1/L5, IONOOPT_EST(4):estimation, IONOOPT_TEC(5):IONEX TEC model, IONOOPT_QZS(6):QZSS broadcast, IONOOPT_LEX(7):QZSS LEX ionosphere, IONOOPT_STEC(8):SLANT TEC mode, IONOOPT_SBAS2(9):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), IONOOPT_SBAS3(10):broadcast + SBAS(IONOOPT_SBAS if possible, otherwise IONOOPT_SBAS2), IONOOPT_SBAS4(8):broadcast + SBAS(IONOOPT_SBAS if possible, otherwise IONOOPT_BRDC)
@@ -85,7 +82,7 @@ config:
constellations:
GPS: true
SBS: false
- GLO: true
+ GLO: false
GAL: true
QZS: false
CMP: false
@@ -93,25 +90,26 @@ config:
LEO: false
tdcp:
enabled: true
- corr_iono: false
- corr_tropo: false
+ corr_iono: true
+ corr_tropo: true
+ corr_clock: true
loss_function: false
- time_window: 10 # s
- sigma_atm: 0.3 # m/sqrt(s)
- sigma_carrier: 0.1 # m
+ time_window: 100 # s
+ sigma_atm: 0.0016 # m/sqrt(s)
+ sigma_carrier: 0.0011 # m
multi_freq: false
remove_outliers: false
batch: false
+ structure: first-all
ROS subscriber:
-
- #type: "SubscriberGnssNovatel"
- #topic: "/novatel/oem7/oem7raw"
type: "SubscriberGnssUblox"
topic: "/ublox_gps/raw_data_stream"
sensor_name: "gnss"
package: "wolf_ros_gnss"
process_not_available: false
+ reset_receiver: false
publish_fix:
enabled: true
topic: "RTKLIB_fix"
@@ -126,26 +124,12 @@ config:
constellations:
GPS: true
SBS: false
- GLO: true
+ GLO: false
GAL: true
QZS: false
CMP: false
IRN: false
LEO: false
- -
- type: "SubscriberGnssFixPublisherEcef"
- topic: "/ublox_gps/fix"
- sensor_name: "gnss"
- package: "wolf_ros_gnss"
- marker_color: [0,0,1,1]
- period: 0.1
- -
- type: "SubscriberGnssFixNovatelPublisherEcef"
- topic: "/novatel/oem7/bestpos"
- sensor_name: "gnss"
- package: "wolf_ros_gnss"
- marker_color: [0,1,0,1]
- period: 0.1
ROS publisher:
-
@@ -154,18 +138,26 @@ config:
package: "wolf_ros_node"
marker_color: [1,0,0,1]
period: 0.01
- extrinsics: true
- sensor: "gnss"
+ extrinsics: false
frame_id: "ENU"
+ -
+ type: "PublisherTf"
+ topic: " "
+ package: "wolf_ros_node"
+ period: 0.1
+ map_frame_id: "map"
+ odom_frame_id: "odom"
+ base_frame_id: "base"
+ publish_odom_tf: false
-
type: "PublisherGnssTf"
- topic: "whatever"
+ topic: " "
package: "wolf_ros_gnss"
- period: 0.01
+ period: 0.1
sensor_gnss_name: "gnss"
-
type: "PublisherGraph"
topic: "graph"
package: "wolf_ros_node"
- period: 0.1
+ period: 0.2
diff --git a/yaml/params_gnss_imu.yaml b/yaml/params_gnss_imu.yaml
index e8b301c6079e76bb84842a97b06af811dc46d1b5..2a7958ac7185adcceb56ae4515000b336fadb659 100644
--- a/yaml/params_gnss_imu.yaml
+++ b/yaml/params_gnss_imu.yaml
@@ -1,7 +1,7 @@
config:
debug:
- profiling: false
- profiling_file: "~/wolf_gauss_profiling.txt"
+ profiling: true
+ profiling_file: "~/wolf_gnss_imu_profiling.txt"
print_problem: false
print_depth: 4
print_constr_by: false
@@ -14,81 +14,40 @@ config:
dimension: 3
node_rate: 100
tree_manager:
- type: "TreeManagerSlidingWindow"#"TreeManagerSlidingWindowDualRate"
- n_frames: 10
- n_frames_recent: 10
- rate_old_frames: 10
- fix_first_frame: false
- viral_remove_empty_parent: true
- n_fix_first_frames: 0
+ follow: "sliding_window.yaml"
+ #follow: "sliding_window_dual.yaml"
prior:
- mode: "factor"
+ mode: "fix"
time_tolerance: 0.1
$state:
P: [0,0,0]
O: [0,0,0,1]
V: [0,0,0]
- $sigma:
- P: [0.31, 0.31, 0.31]
- O: [0.31, 0.31, 0.31]
- V: [0.31, 0.31, 0.31]
solver:
- minimizer: LEVENBERG_MARQUARDT
- max_num_iterations: 20
- verbose: 0
- period: 0.0
- n_threads: 2
- update_immediately: false
- min_num_iterations: 5 #if update immediately
- compute_cov: false
- cov_enum: 3
- cov_period: 1
- function_tolerance: 1e-8
- gradient_tolerance: 1e-9
- use_nonmonotonic_steps: false
+ follow: "solver.yaml"
sensors:
- ## MICROSTRAIN
- -
+ -
type: "SensorImu"
name: "IMU_odometer"
plugin: "imu"
extrinsic:
pose: [0,0,0,0,0,0,1]
- a_noise: 0.053 #0.098 # standard deviation of Acceleration noise (same for all the axis) in m/s2 (1/1000 max value)
- w_noise: 0.0011 #0.0012 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec (1/1000 max value)
+ a_noise: 0.53 # standard deviation of Acceleration noise (same for all the axis) in m/s2 (1/1000 max value)
+ w_noise: 0.011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec (1/1000 max value)
prior_bias: true
ab_initial_stdev: 0.05 # m/s2 - initial bias
wb_initial_stdev: 0.01 # rad/sec - initial bias
bias_dynamic: false
- ab_rate_stdev: 0.00001 # m/s2/sqrt(s)
- wb_rate_stdev: 0.00001 # rad/s/sqrt(s)
-
- ## BNO055
-# -
-# type: "SensorImu"
-# name: "IMU_odometer"
-# plugin: "imu"
-# extrinsic:
-# pose: [0,0,0,0,0,0,1]
-# a_noise: 0.53 # standard deviation of Acceleration noise (same for all the axis) in m/s2 (1/1000 max value)
-# w_noise: 0.011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec (1/1000 max value)
-# prior_bias: true
-# ab_initial_stdev: 0.05 # m/s2 - initial bias
-# wb_initial_stdev: 0.01 # rad/sec - initial bias
-# bias_dynamic: false
-# #ab_rate_stdev: 0.00001 # m/s2/sqrt(s)
-# #wb_rate_stdev: 0.00001 # rad/s/sqrt(s)
+ #ab_rate_stdev: 0.00001 # m/s2/sqrt(s)
+ #wb_rate_stdev: 0.00001 # rad/s/sqrt(s)
-
type: "SensorGnss"
name: "gnss"
plugin: "gnss"
extrinsic:
pose: [0, 0, 0]
- #pose: [-0.135, -0.145, 0.230] # microstrain M600
- #pose: [0.1, 1.36, 0.260] # MICROSTRAIN
- #pose: [1.36, -0.04, 0.280] # BNO055
extrinsics_fixed: true
clock_bias_GPS_GLO_dynamic: false
clock_bias_GPS_GAL_dynamic: false
@@ -137,7 +96,7 @@ config:
fix: false
keyframe_vote:
voting_active: true
- max_time_span: 0.5
+ max_time_span: 1
min_features_for_keyframe: 4
gnss:
sateph: 6 # satellite ephemeris/clock: EPHOPT_BRDC(0):broadcast ephemeris, EPHOPT_PREC(1): precise ephemeris, EPHOPT_SBAS(2): broadcast + SBAS, EPHOPT_SSRAPC(3): broadcast + SSR_APC, EPHOPT_SSRCOM(4): broadcast + SSR_COM, EPHOPT_LEX(5): QZSS LEX ephemeris, EPHOPT_SBAS2(6):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), EPHOPT_SBAS3(7):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_SBAS2), EPHOPT_SBAS4(8):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_BRDC)
@@ -161,9 +120,9 @@ config:
enabled: true
corr_iono: true
corr_tropo: true
- corr_clock: false
+ corr_clock: true
loss_function: false
- time_window: 10 # s
+ time_window: 100 # s
sigma_atm: 0.0016 # m/sqrt(s)
sigma_carrier: 0.0011 # m
multi_freq: false
@@ -173,8 +132,6 @@ config:
ROS subscriber:
-
- #type: "SubscriberGnssNovatel"
- #topic: "/novatel/oem7/oem7raw"
type: "SubscriberGnssUblox"
topic: "/ublox_gps/raw_data_stream"
sensor_name: "gnss"
@@ -195,7 +152,7 @@ config:
constellations:
GPS: true
SBS: false
- GLO: true
+ GLO: false
GAL: true
QZS: false
CMP: false
@@ -203,30 +160,19 @@ config:
LEO: false
-
type: "SubscriberImuEnableable"
- topic: "/bno055_imu/imu" #/imu/data
+ topic: "/bno055_imu/imu"
sensor_name: "IMU_odometer"
package: "wolf_ros_imu"
topic_enable: "gnss_available"
- #imu_x_axis: 1
- #imu_y_axis: -2
- #imu_z_axis: -3
imu_x_axis: 1
imu_y_axis: 2
imu_z_axis: 3
- cov_source: "msg"
+ cov_source: "sensor"
in_degrees: true
- static_init_duration: 30.0
+ static_init_duration: 130.0
lowpass_filter: true
lowpass_cutoff_freq: 10
dt: 0.01
-
- #-
- #type: "SubscriberGnssFixNovatelPublisherEcef"
- #topic: "/novatel/oem7/bestpos"
- #sensor_name: "gnss"
- #package: "wolf_ros_gnss"
- #marker_color: [0,1,0,1]
- #period: 0.1
ROS publisher:
-
@@ -235,8 +181,7 @@ config:
package: "wolf_ros_node"
marker_color: [1,0,0,1]
period: 0.1
- extrinsics: true
- sensor: "gnss"
+ extrinsics: false
frame_id: "ENU"
-
type: "PublisherTf"
diff --git a/yaml/sliding_window.yaml b/yaml/sliding_window.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..e5cc21021a8068e82f63027b53ce378f3c3a2f2a
--- /dev/null
+++ b/yaml/sliding_window.yaml
@@ -0,0 +1,4 @@
+type: "TreeManagerSlidingWindow"
+n_frames: 10
+viral_remove_empty_parent: true
+n_fix_first_frames: 0
\ No newline at end of file
diff --git a/yaml/sliding_window_dual.yaml b/yaml/sliding_window_dual.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..cda538bfc46da3f0343fc79833935f408606e72d
--- /dev/null
+++ b/yaml/sliding_window_dual.yaml
@@ -0,0 +1,7 @@
+type: "TreeManagerSlidingWindowDualRate"
+n_frames: 10
+n_frames_recent: 5
+rate_old_frames: 10
+fix_first_frame: false
+viral_remove_empty_parent: true
+n_fix_first_frames: 0
\ No newline at end of file
diff --git a/yaml/solver.yaml b/yaml/solver.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..1f1e1a6905a6c5fca5a554ab613308c121a8d99c
--- /dev/null
+++ b/yaml/solver.yaml
@@ -0,0 +1,13 @@
+minimizer: LEVENBERG_MARQUARDT
+max_num_iterations: 20
+verbose: 0
+period: 0.0
+n_threads: 2
+update_immediately: false
+min_num_iterations: 5 #if update immediately
+compute_cov: false
+cov_enum: 3
+cov_period: 1
+function_tolerance: 1e-8
+gradient_tolerance: 1e-9
+use_nonmonotonic_steps: false
\ No newline at end of file