GPS move base waypoint

edit

I have a mobile robot and I am using a double ekf from robot localization. The first one fusing continuous data while the second one has the GPS information. I think this part of the integration is well implemented. Now, I want to publish a GPS goal to the robot, basically implement waypoint navigation with GPS coordinates. To do this I am using geodesy and converting google maps coordinates to the utm frame and then to map/odom frame.

Whenever I publish the goal in "odom" my robot moves to the goal position (gives the goal reached message) but it is not the position from the google coordinates. When I see the bags in Rviz with world frame as odom the robot goes indeed to the correct goal but if I set map frame as the world frame it never reaches the goal. From what I have seen, the map frame is the one I want the robot to go which is only possible if both my goal (from geodesy conversion) is in map frame and move_base/current_goal is in the map frame. From what I read this is not the normal behavior and I can only achieve it by changing parameters of the move base, such as the frame for the laser scan.

My question is: what is the correct setup of parameters in order to achieve gps waypoint navigation with dual ekf from robot_localization + move_base. There is very few information detailing this and I am having trouble figuring it out. Any help is welcome!

EKF #1 continuous data

frequency: 20   
sensor_timeout: 0.2
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
publish_tf: true

publish_acceleration: false

map_frame: map
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: odom       # Defaults to the value of odom_frame if unspecified

odom0: odom_calibration/odom_calibration_node/odom
odom0_config: [false, false, false,   #[     x    ,     y     ,    z    ]
      false, false, false,    #[  roll    ,   pitch   ,   yaw   ]
      true, true, false,      #[  x_vel   ,   y_vel   ,   z_vel ]
      false, false, false,    #[ roll_vel , pitch_vel , yaw_vel ]
      false, false, false]    #[ x_accel  , y_accel   , z_accel ]
odom0_queue_size: 5
odom0_differential: false
odom0_relative: false

imu0: mavros/imu/data
imu0_config: [false, false, false,   #[     x    ,     y     ,    z    ]
     false, false, true,    #[  roll    ,   pitch   ,   yaw   ]
     false, false, false,   #[  x_vel   ,   y_vel   ,   z_vel ]
     false, false, true,    #[ roll_vel , pitch_vel , yaw_vel ]
     false, false, false]   #[ x_accel  , y_accel   , z_accel ]
imu0_differential: false
imu0_queue_size: 5

EKF #2 gps data

frequency: 20  
sensor_timeout: 0.2
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
publish_tf: true
publish_acceleration: false

map_frame: map
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: map       # Defaults to the value of odom_frame if unspecified


odom0: odom_calibration/odom_calibration_node/odom
odom0_config: [false, false, false,   #[     x    ,     y     ,    z    ]
     false, false, false,    #[  roll    ,   pitch   ,   yaw   ]
     true, true, false,      #[  x_vel   ,   y_vel   ,   z_vel ]
     false, false, false,    #[ roll_vel , pitch_vel , yaw_vel ]
     false, false, false]    #[ x_accel  , y_accel   , z_accel ]
odom0_queue_size: 5
odom0_differential: false
odom0_relative: false


imu0: mavros/imu/data
imu0_config: [false, false, false,   #[     x    ,     y     ,    z    ]
    false, false, true,    #[  roll    ,   pitch   ,   yaw   ]
    false, false, false,   #[  x_vel ...