navSat with hectorMapping for outdoor navigation

edit

Hello,

I'm trying to configure a robot with GPS for outdoor navigation. For that I'm using robot_localization package with the navSat transform and I want to fuse hectorMapping for pathPlanning and obsctacle avoindance. I've seen this package I tried to replicate for my purposes, so I have this localization.yaml file:

# For parameter descriptions, please refer to the template parameter files for each node.

ekf_se_odom: # Used only for broadcasting odom to base_link transforms
  frequency: 30
  sensor_timeout: 0.1
  two_d_mode: true
  transform_time_offset: 0.0
  transform_timeout: 0.0
  print_diagnostics: true
  debug: false

  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: odom

# -------------------------------------
# Wheel odometry:

  odom0: /husky_velocity_controller/odom
  odom0_config: [false, false, false,
                 false, false, false,
                 true,  true,  true,
                 false, false, false,
                 false, false, false]
  odom0_queue_size: 10
  odom0_nodelay: true
  odom0_differential: false
  odom0_relative: false

# -------------------------------------
# Laser scanmatching odometry:

  odom1: scanmatch_odom
  odom1_config: [false, false, false,
                 false, false, false,
                 true,  true,  true,
                 false, false, true,
                 false, false, false]
  odom1_queue_size: 10
  odom1_nodelay: true
  odom1_differential: false
  odom1_relative: false

# --------------------------------------
# imu configure:

  imu0: /imu/data
  imu0_config: [false, false, false,
                true,  true,  false,
                false, false, false,
                true,  true,  true,
                true,  true,  true]
  imu0_nodelay: false
  imu0_differential: false
  imu0_relative: false
  imu0_queue_size: 10
  imu0_remove_gravitational_acceleration: true

  use_control: false

  process_noise_covariance: [--- ]
  initial_estimate_covariance: [  ----]

ekf_se_map: # Used only for broadcasting MAP to ODOM transforms
  frequency: 30
  sensor_timeout: 0.1
  two_d_mode: true
  transform_time_offset: 0.0
  transform_timeout: 0.0
  print_diagnostics: true
  debug: false

  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: map

# -------------------------------------
# Wheel odometry:

  odom0: /husky_velocity_controller/odom
  odom0_config: [false, false, false,
                 false, false, false,
                 true,  true,  true,
                 false, false, true,
                 false, false, false]
  odom0_queue_size: 10
  odom0_nodelay: true
  odom0_differential: false
  odom0_relative: false

# -------------------------------------
# GPS odometry:

  odom1: /gps/rtkfix
  odom1_config: [true,  true,  false,
                 false, false, false,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  odom1_queue_size: 10
  odom1_nodelay: true
  odom1_differential: false
  odom1_relative: false

# -------------------------------------
# Laser scanmatching odometry:

  odom2: scanmatch_odom
  odom2_config: [false, false, false,
                 false, false, false,
                 true,  true,  true,
                false, false, true,
                false, false, false]
  odom2_queue_size: 10
  odom2_nodelay: true
  odom2_differential: false
  odom2_relative: false

# --------------------------------------
# imu configure: (IMU is too noisy for this)

  imu0: /imu_um7/data
  imu0_config: [false, false, false,
                false,  false, false,
                false, false, false,
                false,  false,  false,
                false,  false,  false]
  imu0_nodelay: true
  imu0_differential: false
  imu0_relative: false
  imu0_queue_size: 10
  imu0_remove_gravitational_acceleration: true

  use_control: false

  process_noise_covariance: [...]
   initial_estimate_covariance: [...]

My launch file for localization:

<?xml version="1.0"?>
<launch>

  <rosparam command="load" file="$(find agrob_gps_nav_stack)/params/localization.yaml" />
  <rosparam command="load" file="$(find agrob_gps_nav_stack)/params/navsat.yaml" />

  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true"/>

  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true">
    <remap from="odometry/filtered" to="odometry/filtered_map"/>
  </node>

  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true" > <!-- output="screen" -->
      <remap from="odometry/filtered" to="odometry/filtered_map"/>
      <remap from="gps/fix" to="gps/fix"/>
      <remap from="imu/data" to="/imu_um7/data"/>
  </node>

</launch>

Here's the rviz image with the result after a rotation with the robot. The laser drifts from the map. Such thing does not happen if I use hector map to publish the tf between map and odom (but I don't do that because I want to use GPS)

https://imgur.com/a/w8TCBZJ

EDIT ... (more)