From 448b5c6749f7852eba80485ba73c7d7febaf47f8 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Wed, 31 Mar 2021 18:39:12 +0200
Subject: [PATCH] working with rosbag
---
launch/rpa_positioning_rosbag.launch | 8 +-
yaml/IRI/params.yaml | 145 ++++++++++++++++++++
yaml/IRI/params_only_gnss.yaml | 104 ++++++++++++++
yaml/IRI/processor_gnss.yaml | 42 ++++++
yaml/IRI/publisher_rpa_state_info.yaml | 7 +
yaml/IRI/subscriber_imu.yaml | 16 +++
yaml/tree_manager/sliding_window_10_0.yaml | 4 +
yaml/tree_manager/sliding_window_10_1.yaml | 4 +
yaml/tree_manager/sliding_window_20_0.yaml | 4 +
yaml/tree_manager/sliding_window_20_1.yaml | 4 +
yaml/tree_manager/sliding_window_20_10.yaml | 4 +
11 files changed, 338 insertions(+), 4 deletions(-)
create mode 100644 yaml/IRI/params.yaml
create mode 100644 yaml/IRI/params_only_gnss.yaml
create mode 100644 yaml/IRI/processor_gnss.yaml
create mode 100644 yaml/IRI/publisher_rpa_state_info.yaml
create mode 100644 yaml/IRI/subscriber_imu.yaml
create mode 100644 yaml/tree_manager/sliding_window_10_0.yaml
create mode 100644 yaml/tree_manager/sliding_window_10_1.yaml
create mode 100644 yaml/tree_manager/sliding_window_20_0.yaml
create mode 100644 yaml/tree_manager/sliding_window_20_1.yaml
create mode 100644 yaml/tree_manager/sliding_window_20_10.yaml
diff --git a/launch/rpa_positioning_rosbag.launch b/launch/rpa_positioning_rosbag.launch
index 10e25af..e0b7d31 100644
--- a/launch/rpa_positioning_rosbag.launch
+++ b/launch/rpa_positioning_rosbag.launch
@@ -22,13 +22,13 @@
<!-- using "clock" option to use the simulated time. Rosbag launch as the first node -->
<param name="use_sim_time" value="true" />
- <arg name="bag_path" default="/home/joanvallve/bags/GAUSS/loring_tests"/>
+ <!--<arg name="bag_path" default="/home/joanvallve/bags/GAUSS/loring_tests"/>-->
<!--<arg name="bag_suffix" default="2021-02-16-15-49-27"/>-->
<!--<arg name="bag_suffix" default="2021-02-16-15-55-07"/>-->
- <arg name="bag_suffix" default="2021-02-16-14-39-56"/>
+ <!--<arg name="bag_suffix" default="2021-02-16-14-39-56"/>-->
- <!--<arg name="bag_path" default="$(find gauss_ros)/bags"/>-->
- <!--<arg name="bag_suffix" default="mini8"/>-->
+ <arg name="bag_path" default="$(find gauss_ros)/bags"/>
+ <arg name="bag_suffix" default="mini8"/>
<!--POSITIONING-->
<include file="$(find gauss_ros)/launch/rpa_positioning.launch">
diff --git a/yaml/IRI/params.yaml b/yaml/IRI/params.yaml
new file mode 100644
index 0000000..eed5cde
--- /dev/null
+++ b/yaml/IRI/params.yaml
@@ -0,0 +1,145 @@
+config:
+ debug:
+ profiling: true
+ profiling_file: "~/profiling_M600.txt"
+ print_problem: false
+ print_period: 1
+
+ problem:
+ frame_structure: "POV"
+ dimension: 3
+ tree_manager:
+ follow: "../tree_manager/sliding_window_20_0.yaml"
+ prior:
+ #follow: "../prior/POV_factor.yaml"
+ follow: "../prior/POV_fix.yaml"
+
+ 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: true
+ cov_enum: 3
+ cov_period: 1
+
+ sensors:
+ -
+ type: "SensorImu"
+ name: "IMU_odometer"
+ plugin: "imu"
+ follow: "../sensor_bno.yaml"
+ -
+ type: "SensorCompass"
+ name: "compass"
+ plugin: "imu"
+ follow: "../sensor_bno_compass.yaml"
+ -
+ type: "SensorGnss"
+ name: "gnss"
+ plugin: "gnss"
+ extrinsic:
+ pose: [1.36, -0.04, 0.280] # BNO055 in rosbag
+ follow: "../sensor_f9t.yaml"
+
+ processors:
+ -
+ type: "ProcessorImu"
+ name: "processor IMU"
+ sensor_name: "IMU_odometer"
+ plugin: "imu"
+ follow: "../processor_imu.yaml"
+ -
+ type: "ProcessorCompass"
+ name: "processor compass"
+ sensor_name: "compass"
+ plugin: "imu"
+ follow: "../processor_compass.yaml"
+ -
+ type: "ProcessorTrackerGnss"
+ name: "processor gnss"
+ sensor_name: "gnss"
+ plugin: "gnss"
+ follow: "processor_gnss.yaml"
+
+ ROS subscriber:
+ -
+ type: "SubscriberGnssUblox"
+ topic: "/ublox_gps/raw_data_stream"
+ sensor_name: "gnss"
+ package: "wolf_ros_gnss"
+ follow: "../subscriber_gnss_ublox.yaml"
+ -
+ type: "SubscriberImuEnableable"
+ topic: "/bno055_imu/imu"
+ sensor_name: "IMU_odometer"
+ package: "wolf_ros_imu"
+ follow: "subscriber_imu.yaml"
+ -
+ type: "SubscriberCompass"
+ topic: "/bno055_imu/magnetometer"
+ sensor_name: "compass"
+ package: "wolf_ros_imu"
+ follow: "subscriber_imu.yaml"
+ -
+ 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:
+ -
+ type: "PublisherRpaStateInfo"
+ topic: "rpa_state_info"
+ package: "wolf_ros_gauss"
+ period: 0.5
+ follow: "publisher_rpa_state_info.yaml"
+ -
+ type: "PublisherRpaPositioningInfo"
+ topic: "rpa_positioning_info"
+ package: "wolf_ros_gauss"
+ period: 0.5
+ sensor: "gnss"
+ -
+ type: "PublisherRpaImuGnssPositioning"
+ topic: "rpa_imu_gnss_positioning"
+ package: "wolf_ros_gauss"
+ period: 0.1
+ sensor: "gnss"
+ -
+ type: "PublisherPose"
+ topic: "current_pose"
+ package: "wolf_ros_node"
+ marker_color: [1,0,0,1]
+ period: 0.1
+ extrinsics: true
+ sensor: "gnss"
+ frame_id: "ENU"
+ -
+ type: "PublisherGnssTf"
+ topic: " "
+ package: "wolf_ros_gnss"
+ period: 0.01
+ sensor_gnss_name: "gnss"
+ -
+ type: "PublisherGraph"
+ topic: "graph"
+ package: "wolf_ros_node"
+ period: 0.2
+ -
+ type: "PublisherTrackerGnssInfo"
+ topic: "gnss_tracker"
+ package: "wolf_ros_gnss"
+ period: 0.2
+ processor_gnss: "processor gnss"
\ No newline at end of file
diff --git a/yaml/IRI/params_only_gnss.yaml b/yaml/IRI/params_only_gnss.yaml
new file mode 100644
index 0000000..27a9982
--- /dev/null
+++ b/yaml/IRI/params_only_gnss.yaml
@@ -0,0 +1,104 @@
+config:
+ debug:
+ profiling: true
+ profiling_file: "~/profiling_M600_only_GNSS.txt"
+ print_problem: false
+ print_period: 1
+
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ tree_manager:
+ follow: "../tree_manager/sliding_window_dual_20_10_10_0.yaml"
+ prior:
+ #follow: "../prior/PO_factor.yaml"
+ follow: "../prior/PO_fix.yaml"
+
+ 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
+
+ sensors:
+ -
+ type: "SensorGnss"
+ name: "gnss"
+ plugin: "gnss"
+ extrinsic:
+ pose: [0, 0, 0]
+ follow: "../sensor_f9t.yaml"
+
+ processors:
+ -
+ type: "ProcessorTrackerGnss"
+ name: "processor gnss"
+ sensor_name: "gnss"
+ plugin: "gnss"
+ follow: "processor_gnss.yaml"
+
+ ROS subscriber:
+ -
+ type: "SubscriberGnssUblox"
+ topic: "/ublox_gps/raw_data_stream"
+ sensor_name: "gnss"
+ package: "wolf_ros_gnss"
+ follow: "../subscriber_gnss_ublox.yaml"
+ -
+ type: "SubscriberGnssFixPublisherEcef"
+ topic: "/ublox_gps/fix"
+ sensor_name: "gnss"
+ package: "wolf_ros_gnss"
+ marker_color: [0,0,1,1]
+ period: 0.1
+
+ ROS publisher:
+ -
+ type: "PublisherRpaStateInfo"
+ topic: "rpa_state_info"
+ package: "wolf_ros_gauss"
+ period: 0.5
+ follow: "publisher_rpa_state_info.yaml"
+ -
+ type: "PublisherRpaPositioningInfo"
+ topic: "rpa_positioning_info"
+ package: "wolf_ros_gauss"
+ period: 0.5
+ sensor: "gnss"
+ -
+ type: "PublisherRpaImuGnssPositioning"
+ topic: "rpa_imu_gnss_positioning"
+ package: "wolf_ros_gauss"
+ period: 0.1
+ sensor: "gnss"
+ -
+ type: "PublisherPose"
+ topic: "current_pose"
+ package: "wolf_ros_node"
+ marker_color: [1,0,0,1]
+ period: 0.01
+ extrinsics: true
+ sensor: "gnss"
+ frame_id: "ENU"
+ -
+ type: "PublisherGnssTf"
+ topic: " "
+ package: "wolf_ros_gnss"
+ period: 0.01
+ sensor_gnss_name: "gnss"
+ -
+ type: "PublisherGraph"
+ topic: "graph"
+ package: "wolf_ros_node"
+ period: 0.2
+ -
+ type: "PublisherTrackerGnssInfo"
+ topic: "gnss_tracker"
+ package: "wolf_ros_gnss"
+ period: 0.2
+ processor_gnss: "processor gnss"
\ No newline at end of file
diff --git a/yaml/IRI/processor_gnss.yaml b/yaml/IRI/processor_gnss.yaml
new file mode 100644
index 0000000..6fa1d27
--- /dev/null
+++ b/yaml/IRI/processor_gnss.yaml
@@ -0,0 +1,42 @@
+time_tolerance: 0.1
+apply_loss_function: false
+max_new_features: -1 #unlimited
+remove_outliers: true
+remove_outliers_with_fix: true
+outlier_residual_th: 10
+init_frames: false
+pseudo_ranges: true
+enu_map_fix_dist: 5
+keyframe_vote:
+ voting_active: true
+ max_time_span: 0.01
+ min_features_for_keyframe: 0 #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(11):broadcast + SBAS(IONOOPT_SBAS if possible, otherwise IONOOPT_BRDC)
+ tropopt: 2 # troposphere option: (0:correction off,1:Saastamoinen model,2:SBAS model,3:troposphere option: ZTD estimation,4:ZTD+grad estimation,5:ZTD correction,6:ZTD+grad correction)
+ sbascorr: 15 # SBAS option (1:long term correction,2:fast correction,4:ionosphere correction,8:ranging)
+ raim: 1 # apply RAIM
+ elmin: 0.26 # elevation min (rad) = 15 degrees
+ maxgdop: 30 # reject threshold of gdop (ground dilusion of precision)
+ min_sbas_sats: 5 # used in case of EPHOPT_SBAS3, EPHOPT_SBAS4, IONOOPT_SBAS3, IONOOPT_SBAS4
+ constellations:
+ GPS: true
+ SBS: false
+ GLO: true
+ GAL: true
+ QZS: false
+ CMP: false
+ IRN: false
+ LEO: false
+ tdcp:
+ enabled: true
+ corr_iono: false
+ corr_tropo: false
+ loss_function: false
+ time_window: 10 # s
+ sigma_atm: 0.3 # m/sqrt(s)
+ sigma_carrier: 0.1 # m
+ multi_freq: false
+ remove_outliers: false
+ batch: false
\ No newline at end of file
diff --git a/yaml/IRI/publisher_rpa_state_info.yaml b/yaml/IRI/publisher_rpa_state_info.yaml
new file mode 100644
index 0000000..0e42fe5
--- /dev/null
+++ b/yaml/IRI/publisher_rpa_state_info.yaml
@@ -0,0 +1,7 @@
+topic_security: "/alarm_data"
+sensor: "gnss"
+icao: 3434000 # M600a
+k_H: 5.256522
+k_V: 4.891638
+HAL: 20 #20/10
+VAL: 30 #30/444
\ No newline at end of file
diff --git a/yaml/IRI/subscriber_imu.yaml b/yaml/IRI/subscriber_imu.yaml
new file mode 100644
index 0000000..72c7da2
--- /dev/null
+++ b/yaml/IRI/subscriber_imu.yaml
@@ -0,0 +1,16 @@
+enableable: true
+topic_enable: "gnss_available"
+
+imu_x_axis: -1
+imu_y_axis: 2
+imu_z_axis: -3
+
+cov_source: "msg"
+
+in_degrees: true
+
+static_init_duration: 30.0
+
+lowpass_filter: true
+lowpass_cutoff_freq: 10
+dt: 0.01
\ No newline at end of file
diff --git a/yaml/tree_manager/sliding_window_10_0.yaml b/yaml/tree_manager/sliding_window_10_0.yaml
new file mode 100644
index 0000000..26fa9e3
--- /dev/null
+++ b/yaml/tree_manager/sliding_window_10_0.yaml
@@ -0,0 +1,4 @@
+type: "TreeManagerSlidingWindow"
+n_frames: 10
+n_fix_first_frames: 0
+viral_remove_empty_parent: true
\ No newline at end of file
diff --git a/yaml/tree_manager/sliding_window_10_1.yaml b/yaml/tree_manager/sliding_window_10_1.yaml
new file mode 100644
index 0000000..ae15151
--- /dev/null
+++ b/yaml/tree_manager/sliding_window_10_1.yaml
@@ -0,0 +1,4 @@
+type: "TreeManagerSlidingWindow"
+n_frames: 10
+n_fix_first_frames: 1
+viral_remove_empty_parent: true
\ No newline at end of file
diff --git a/yaml/tree_manager/sliding_window_20_0.yaml b/yaml/tree_manager/sliding_window_20_0.yaml
new file mode 100644
index 0000000..1b98483
--- /dev/null
+++ b/yaml/tree_manager/sliding_window_20_0.yaml
@@ -0,0 +1,4 @@
+type: "TreeManagerSlidingWindow"
+n_frames: 20
+n_fix_first_frames: 0
+viral_remove_empty_parent: true
\ No newline at end of file
diff --git a/yaml/tree_manager/sliding_window_20_1.yaml b/yaml/tree_manager/sliding_window_20_1.yaml
new file mode 100644
index 0000000..cf53665
--- /dev/null
+++ b/yaml/tree_manager/sliding_window_20_1.yaml
@@ -0,0 +1,4 @@
+type: "TreeManagerSlidingWindow"
+n_frames: 20
+n_fix_first_frames: 1
+viral_remove_empty_parent: true
\ No newline at end of file
diff --git a/yaml/tree_manager/sliding_window_20_10.yaml b/yaml/tree_manager/sliding_window_20_10.yaml
new file mode 100644
index 0000000..3f9ada0
--- /dev/null
+++ b/yaml/tree_manager/sliding_window_20_10.yaml
@@ -0,0 +1,4 @@
+type: "TreeManagerSlidingWindow"
+n_frames: 20
+n_fix_first_frames: 10
+viral_remove_empty_parent: true
\ No newline at end of file
--
GitLab