different configs comparative

1cb496cb · Joan Vallvé Navarro · 0a98df4f · 1cb496cb · 1cb496cb · 1cb496cb
Commit 1cb496cb authored Sep 29, 2021 by Joan Vallvé Navarro
--- a/launch/rpa_positioning_rosbag_second_field.launch
+++ b/launch/rpa_positioning_rosbag_second_field.launch
+<!-- -->
+<launch>
+    <arg name="rviz" default="true" /> 
+    
+    <arg name="record_output" 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" 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" />
+    
+    <arg name="bag_path" default="/media/joanvallve/second_field_inta/selected"/>
+
+    <arg name="flight" default="atlantic_16" />
+    
+	<arg name="bag_suffix"	value="atlantic_2021-09-16-09-04-43"	if="$(eval flight == 'atlantic_16')" />
+	<arg name="bag_suffix"	value="atlantic_2021-09-17-09-22-47"	if="$(eval flight == 'atlantic_17')" />
+	<arg name="bag_suffix"	value="M600b_2021-09-16-12-25-14"		if="$(eval flight == 'M600b_16_1')" />
+	<arg name="bag_suffix"	value="M600b_2021-09-16-14-41-46"		if="$(eval flight == 'M600b_16_2')" />
+	<arg name="bag_suffix"	value="M600b_2021-09-17-11-00-51"		if="$(eval flight == 'M600b_17_1')" />
+	<arg name="bag_suffix"	value="M600b_2021-09-17-11-31-55"		if="$(eval flight == 'M600b_17_2')" />
+	<arg name="bag_suffix"	value="scrab_2021-09-16-11-25-23"		if="$(eval flight == 'scrab_16')" />
+	<arg name="bag_suffix"	value="scrab_2021-09-17-09-59-32"		if="$(eval flight == 'scrab_17')" />
+    
+    <arg name="platform"    value="atlantic" 	if="$(eval flight == 'atlantic_16' or flight == 'atlantic_17')" />
+    <arg name="platform"    value="M600b" 		if="$(eval flight == 'M600b_16_1' or flight == 'M600b_16_2' or flight == 'M600b_17_1' or flight == 'M600b_17_2')" />
+    <arg name="platform"    value="scrab"	 	if="$(eval flight == 'scrab_16' or flight == 'scrab_17')" />
+    
+    <arg name="conf_gnss" default="1" />
+    
+	<arg name="yaml"	value="params"		if="$(eval conf_gnss == 1)" />
+	<arg name="yaml"	value="params_2"	if="$(eval conf_gnss == 2)" />
+	<arg name="yaml"	value="params_3"	if="$(eval conf_gnss == 3)" />
+	<arg name="yaml"	value="params_4"	if="$(eval conf_gnss == 4)" />
+    
+	<!--POSITIONING-->
+    <include file="$(find gauss_ros)/launch/rpa_positioning.launch">
+    	<arg name="platform"        value="$(arg platform)"/>
+    	<arg name="launch_pref"     value="$(arg launch_pref)"/>
+    	<arg name="yaml"     		value="$(arg yaml)"/>
+    </include>
+   	
+    <!--VISUALIZATION-->
+    <group if="$(arg rviz)">
+        <node name="rviz"
+             pkg="rviz"
+             type="rviz" 
+             args="-d $(find gauss_ros)/rviz/positioning_rosbag.rviz" />
+    </group>
+
+    <!--ROSBAG PLAY-->
+    <node pkg="rosbag" 
+          type="play" 
+          name="player"
+          required="true"
+          args="-r $(arg speed) 
+                -s $(arg sec) 
+                --clock 
+                $(arg bag_path)/rpa_sensors_$(arg bag_suffix).bag">
+    </node>
+
+    <!--ROSBAG RECORD OUTPUT-->
+    <group if="$(arg record_output)">
+        <node pkg="rosbag" 
+              type="record" 
+              name="recorder"
+              required="true"
+              args="/clock
+                    /wolf_ros_node/rpa_imu_gnss_positioning
+                    /wolf_ros_node/rpa_positioning_info
+                    /wolf_ros_node/rpa_state_info
+                    /tf
+                    /tf_static
+                    /alarm_data
+                    -O $(arg bag_path)/rpa_output_$(arg bag_suffix)_conf$(arg conf_gnss).bag">
+                    <!--/wolf_ros_node/current_pose_marker
+                    /wolf_ros_node/graph_factors
+                    /wolf_ros_node/graph_landmarks
+                    /wolf_ros_node/graph_trajectory-->
+        </node>
+    </group>
+
+</launch>
--- a/yaml/M600a/processor_gnss.yaml
+++ b/yaml/M600a/processor_gnss.yaml
@@ -24,7 +24,7 @@ gnss:
  constellations:
    GPS: true
    SBS: false
-    GLO: true
+    GLO: false
    GAL: true
    QZS: false
    CMP: false

--- a/yaml/M600b/params_2.yaml
+++ b/yaml/M600b/params_2.yaml
+config:
+  debug: 
+    profiling: true
+    profiling_file: "~/profiling_M600b.txt"
+    print_problem: false
+    print_period: 1
+    print_depth: 4
+    print_constr_by: false
+    print_metric: true
+    print_state_blocks: false
+    
+  problem:
+    frame_structure: "POV"
+    dimension: 3
+    tree_manager:
+      follow: "../tree_manager/sliding_window_20_0.yaml" 
+    prior:
+      follow: "../prior/POV_fix.yaml"
+      
+  solver:
+    max_num_iterations: 20
+    verbose: 0
+    period: 0.0
+    n_threads: 2
+    update_immediately: false
+    min_num_iterations: 5 #if update immediately
+    compute_cov: true
+    cov_enum: 3
+    cov_period: 1
+      
+  map:
+    type: "MapBase"
+    plugin: "core"
+    
+  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: [0.0, 0.0, 0.28] # M600
+      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_2.yaml"
+    -
+      type: "ProcessorFixWingModel"
+      name: "processor fix wing"
+      sensor_name: "IMU_odometer"
+      plugin: "core"
+      follow: "processor_fix_wing.yaml"
+      
+  ROS subscriber:
+    -
+      type: "SubscriberGnssUblox"
+      topic: "/ublox_gps/raw_data_stream"
+      sensor_name: "gnss"
+      package: "wolf_ros_gnss"
+      follow: "../subscriber_gnss_ublox_2.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]
+      line_size: 0.2
+      period: 0.2
+      max_points: 10000
+      
+  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]
+      line_size: 0.2
+      period: 0.2
+      extrinsics: true
+      sensor: "gnss"
+      frame_id: "ENU"
+      max_points: 10000
+    -
+      type: "PublisherGnssTf"
+      topic: " "
+      package: "wolf_ros_gnss"
+      period: 0.01
+      sensor_gnss_name: "gnss"
+    -
+      type: "PublisherTf"
+      topic: " "
+      package: "wolf_ros_node"
+      period: 0.2
+      map_frame_id: "map"
+      odom_frame_id: "odom"
+      base_frame_id: "base"
+      publish_odom_tf: false
+    -
+      type: "PublisherGraph"
+      topic: "graph"
+      package: "wolf_ros_node"
+      period: 0.2
+      text_scale: 2
+      viz_scale: 2
+    -
+      type: "PublisherTrackerGnssInfo"
+      topic: "gnss_tracker"
+      package: "wolf_ros_gnss"
+      period: 0.2
+      processor_gnss: "processor gnss"
\ No newline at end of file
--- a/yaml/M600b/params_3.yaml
+++ b/yaml/M600b/params_3.yaml
+config:
+  debug: 
+    profiling: true
+    profiling_file: "~/profiling_M600b.txt"
+    print_problem: false
+    print_period: 1
+    print_depth: 4
+    print_constr_by: false
+    print_metric: true
+    print_state_blocks: false
+    
+  problem:
+    frame_structure: "POV"
+    dimension: 3
+    tree_manager:
+      follow: "../tree_manager/sliding_window_20_0.yaml" 
+    prior:
+      follow: "../prior/POV_fix.yaml"
+      
+  solver:
+    max_num_iterations: 20
+    verbose: 0
+    period: 0.0
+    n_threads: 2
+    update_immediately: false
+    min_num_iterations: 5 #if update immediately
+    compute_cov: true
+    cov_enum: 3
+    cov_period: 1
+      
+  map:
+    type: "MapBase"
+    plugin: "core"
+    
+  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: [0.0, 0.0, 0.28] # M600
+      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_3.yaml"
+    -
+      type: "ProcessorFixWingModel"
+      name: "processor fix wing"
+      sensor_name: "IMU_odometer"
+      plugin: "core"
+      follow: "processor_fix_wing.yaml"
+      
+  ROS subscriber:
+    -
+      type: "SubscriberGnssUblox"
+      topic: "/ublox_gps/raw_data_stream"
+      sensor_name: "gnss"
+      package: "wolf_ros_gnss"
+      follow: "../subscriber_gnss_ublox_3.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]
+      line_size: 0.2
+      period: 0.2
+      max_points: 10000
+      
+  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]
+      line_size: 0.2
+      period: 0.2
+      extrinsics: true
+      sensor: "gnss"
+      frame_id: "ENU"
+      max_points: 10000
+    -
+      type: "PublisherGnssTf"
+      topic: " "
+      package: "wolf_ros_gnss"
+      period: 0.01
+      sensor_gnss_name: "gnss"
+    -
+      type: "PublisherTf"
+      topic: " "
+      package: "wolf_ros_node"
+      period: 0.2
+      map_frame_id: "map"
+      odom_frame_id: "odom"
+      base_frame_id: "base"
+      publish_odom_tf: false
+    -
+      type: "PublisherGraph"
+      topic: "graph"
+      package: "wolf_ros_node"
+      period: 0.2
+      text_scale: 2
+      viz_scale: 2
+    -
+      type: "PublisherTrackerGnssInfo"
+      topic: "gnss_tracker"
+      package: "wolf_ros_gnss"
+      period: 0.2
+      processor_gnss: "processor gnss"
\ No newline at end of file
--- a/yaml/M600b/params_4.yaml
+++ b/yaml/M600b/params_4.yaml
+config:
+  debug: 
+    profiling: true
+    profiling_file: "~/profiling_M600b.txt"
+    print_problem: false
+    print_period: 1
+    print_depth: 4
+    print_constr_by: false
+    print_metric: true
+    print_state_blocks: false
+    
+  problem:
+    frame_structure: "POV"
+    dimension: 3
+    tree_manager:
+      follow: "../tree_manager/sliding_window_20_0.yaml" 
+    prior:
+      follow: "../prior/POV_fix.yaml"
+      
+  solver:
+    max_num_iterations: 20
+    verbose: 0
+    period: 0.0
+    n_threads: 2
+    update_immediately: false
+    min_num_iterations: 5 #if update immediately
+    compute_cov: true
+    cov_enum: 3
+    cov_period: 1
+      
+  map:
+    type: "MapBase"
+    plugin: "core"
+    
+  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: [0.0, 0.0, 0.28] # M600
+      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_4.yaml"
+    -
+      type: "ProcessorFixWingModel"
+      name: "processor fix wing"
+      sensor_name: "IMU_odometer"
+      plugin: "core"
+      follow: "processor_fix_wing.yaml"
+      
+  ROS subscriber:
+    -
+      type: "SubscriberGnssUblox"
+      topic: "/ublox_gps/raw_data_stream"
+      sensor_name: "gnss"
+      package: "wolf_ros_gnss"
+      follow: "../subscriber_gnss_ublox_4.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]
+      line_size: 0.2
+      period: 0.2
+      max_points: 10000
+      
+  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]
+      line_size: 0.2
+      period: 0.2
+      extrinsics: true
+      sensor: "gnss"
+      frame_id: "ENU"
+      max_points: 10000
+    -
+      type: "PublisherGnssTf"
+      topic: " "
+      package: "wolf_ros_gnss"
+      period: 0.01
+      sensor_gnss_name: "gnss"
+    -
+      type: "PublisherTf"
+      topic: " "
+      package: "wolf_ros_node"
+      period: 0.2
+      map_frame_id: "map"
+      odom_frame_id: "odom"
+      base_frame_id: "base"
+      publish_odom_tf: false
+    -
+      type: "PublisherGraph"
+      topic: "graph"
+      package: "wolf_ros_node"
+      period: 0.2
+      text_scale: 2
+      viz_scale: 2
+    -
+      type: "PublisherTrackerGnssInfo"
+      topic: "gnss_tracker"
+      package: "wolf_ros_gnss"
+      period: 0.2
+      processor_gnss: "processor gnss"
\ No newline at end of file
--- a/yaml/M600b/processor_gnss.yaml
+++ b/yaml/M600b/processor_gnss.yaml
@@ -24,7 +24,7 @@ gnss:
  constellations:
    GPS: true
    SBS: false
-    GLO: true
+    GLO: false
    GAL: true
    QZS: false
    CMP: false

--- a/yaml/M600b/processor_gnss_2.yaml
+++ b/yaml/M600b/processor_gnss_2.yaml
+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
+fix: false