Robotics StackExchange | Archived questions

navsat_transform_node outputs zero pose on /odometry/gps

Hi all,

I'am trying to get robotlocalization to work with imu and GPS (no real odometry) using the navsattransform_node node with Gazebo.

I have my first ekf node, running with imu0 only as input, to obtain the odom frame. Then I try to fuse GPS information using the navsattransformnode, but this node only outputs zero odometry on /odometry/gps:

header: 
   seq: 194
   stamp: 
   secs: 40
   nsecs: 200000000
   frame_id: "odom"
   child_frame_id: ''
pose: 
   pose: 
     position: 
        x: 0.0
        y: 0.0
        z: 0.0
     orientation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
     covariance: [1.0099999999999998, -8.788794008375919e-25, 0.0, 0.0, 0.0, 0.0, -8.271806125530277e-25, 1.0099999999999998, -1.6940658945086007e-21, 0.0, 0.0, 0.0, 6.776263578034403e-21, 0.0, 1.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
     linear: 
        x: 0.0
        y: 0.0
        z: 0.0
    angular: 
       x: 0.0
       y: 0.0
       z: 0.0
 covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

Is there something obvious that I am missing?

My launch file for the robot_localization components is the following

<launch>

<rosparam command="load" file="$(find symeter2)/config/ekf_config.yaml" />

<!-- Launch Symeter 2 gui-->
<node pkg="symeter2_gui" type="symeter2_gui" name="symeter2_gui" output="screen" />

<!-- Launch the robot_localization node -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true">
    <rosparam param="initial_state">[0.0,  0.0,  0.0,
                             0.0,  0.0,  0.0,
                             0.0,  0.0,  0.0,
                             0.0,  0.0,  0.0,
                             0.0,  0.0,  0.0]</rosparam>

</node>

<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" respawn="true" output="log">

    <rosparam param="datum">[49.9, 8.9, 0.0, map, base_link]</rosparam>

    <remap from="/imu/data" to="/symeter2/imu0" />
    <remap from="/gps/fix" to="/symeter2/fix/" />
    <remap from="/odometry/filtered" to="/odometry/filtered" />


</node>

   </launch> 

and the ekf_config.yaml file is the following:

ekf_se_odom:

  frequency: 30
  sensor_timeout: 1
  two_d_mode: false
  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
  base_link_frame: base_link
  world_frame: odom

  imu0: symeter2/imu0
  imu0_config: [false, false, false, #x, y, z
            true,  true,  true,  #roll, pitch, yaw
            false, false, false, #vx, vy, vz,
            true,  true, true,  #vroll, vpitch, vyaw
            true,  true,  true]  #ax, ay, az
  imu0_nodelay: false
  imu0_differential: false
  imu0_relative: false
  imu0_queue_size: 10
  imu0_remove_gravitational_acceleration: true

  use_control: false

  process_noise_covariance: [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,   #x
                         0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,   #y
                         0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,   #z
                         0,    0,    0,    0.004, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0, #roll
                         0,    0,    0,    0,    0.004, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0, #pitch
                         0,    0,    0,    0,    0,    0.004, 0,     0,     0,    0,    0,    0,    0,    0,    0, #yaw
                         0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,   #vx
                         0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,   #vy
                         0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,   #vz
                         0,    0,    0,    0,    0,    0,    0,     0,     0,    4e-6, 0,    0,    0,    0,    0, #vroll
                         0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    4e-6, 0,    0,    0,    0, #vpitch
                         0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    4e-6, 0,    0,    0, #vyaw
                         0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.004, 0,    0, #ax
                         0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.004, 0, #ay
                         0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.004] #az

   initial_estimate_covariance: [1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,     #x
                            0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,     #y
                            0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,     #z
                            0,    0,    0,    1,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,     #roll
                            0,    0,    0,    0,    1,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,     #pitch
                            0,    0,    0,    0,    0,    1,    0,    0,    0,    0,     0,     0,     0,    0,    0,     #yaw
                            0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,     #vx
                            0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,     #vy
                            0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,     #vz
                            0,    0,    0,    0,    0,    0,    0,    0,    0,    1,     0,     0,     0,    0,    0,     #vroll
                            0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1,     0,     0,    0,    0,     #vpitch
                            0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1,     0,    0,    0,     #vyaw
                            0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1,    0,    0,     #ax
                            0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1,    0,     #ay
                            0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1]     #az


ekf_se_map:
  frequency: 30
  sensor_timeout: 0.1 
  two_d_mode: false
  transform_time_offset: 0.0
  transform_timeout: 0.0
 print_diagnostics: true
 debug: false
 publish_tf: true

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

 odom1: odometry/gps
 odom1_config: [true,  true,  true,
             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

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

 use_control: false

 process_noise_covariance: [1.0,  0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                         0,    1.0,  0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                         0,    0,    1.0,  0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                         0,    0,    0,    0.3,  0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                         0,    0,    0,    0,    0.3,  0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                         0,    0,    0,    0,    0,    0.01, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                         0,    0,    0,    0,    0,    0,    0.5,   0,     0,    0,    0,    0,    0,    0,    0,
                         0,    0,    0,    0,    0,    0,    0,     0.5,   0,    0,    0,    0,    0,    0,    0,
                         0,    0,    0,    0,    0,    0,    0,     0,     0.1,  0,    0,    0,    0,    0,    0,
                         0,    0,    0,    0,    0,    0,    0,     0,     0,    0.3,  0,    0,    0,    0,    0,
                         0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.3,  0,    0,    0,    0,
                         0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.3,  0,    0,    0,
                         0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.3,  0,    0,
                         0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.3,  0,
                         0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.3]

  initial_estimate_covariance: [1.0,  0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                            0,    1.0,  0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                            0,    0,    1.0, 0,     0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                            0,    0,    0,    1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                            0,    0,    0,    0,    1.0,  0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                            0,    0,    0,    0,    0,    1.0 , 0,    0,    0,    0,     0,     0,     0,    0,    0,
                            0,    0,    0,    0,    0,    0,    1.0,  0,    0,    0,     0,     0,     0,    0,    0,
                            0,    0,    0,    0,    0,    0,    0,    1.0,  0,    0,     0,     0,     0,    0,    0,
                            0,    0,    0,    0,    0,    0,    0,    0,    1.0,  0,     0,     0,     0,    0,    0,
                            0,    0,    0,    0,    0,    0,    0,    0,    0,    1.0,   0,     0,     0,    0,    0,
                            0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1.0,   0,     0,    0,    0,
                            0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1.0,   0,    0,    0,
                            0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1.0,  0,    0,
                            0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1.0,  0,
                            0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1.0]

navsat_transform:
  frequency: 30
  delay: 3.0
  #magnetic_declination_radians: 0.0429351  # For lat/long 55.944831, -3.186998
  yaw_offset: 0
  zero_altitude: false
  broadcast_utm_transform: true
  publish_filtered_gps: true
  use_odometry_yaw: false
  wait_for_datum: false

Asked by sebh on 2018-06-12 08:58:47 UTC

Comments

After reading https://answers.ros.org/question/200071/how-to-fuse-imu-gps-using-robot_localization/#200087, it seems that I do not need to have two ekf nodes when I only have IMU to fuse with GPS.

But I would still have my problem with navsat_transform_node generating 0 pose in /odometry/gps.

Asked by sebh on 2018-06-12 09:55:36 UTC

I'am probably missing something quite simple, but it still eludes me...

Asked by sebh on 2018-06-12 09:56:31 UTC

It turns out that I have the exact same behavior as described in https://answers.ros.org/question/282246/no-pose-update-in-navsat_transform_node-topic/

I also have an empty child_frame_id

Asked by sebh on 2018-06-13 03:36:20 UTC

Answers

It turns out all my problems came out of a faulty configuration of my gazebo hector gps plugin.

I had to specify in the <frameId> tag and the <referenceHeading> tag in order to get the navsat_transform_node working correctly.

navsat_transform_node needs the <frameId> of the GPS link to perform the transform from the GPS link to the base_link, in my case:

<frameId>gps_link</frameId>

And by default the hector gazebo gps plugin mesures yaw from the NORTH. So when we move along the x-axis in gazebo, the navsat_transform_node returns a gps odometry moving along the y-axis. In order to compensate we need to configure the <referenceHeading> to 90 in the hector gazebo gps plugin:

<referenceHeading>90</referenceHeading>

Now everything works!

Asked by sebh on 2018-06-13 06:13:56 UTC

Comments