ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

robot_localization posting datum messages and never starting map localization

asked 2022-01-04 05:48:00 -0500

morten gravatar image

updated 2022-01-04 05:52:33 -0500

I ma trying to use the robot_localization package to fuse odometry, imu, and gps data according to link.

As such I have two ekf nodes and a navsat transform node. In the documentation it says that the navsat transform node can work by using the first GPS reading as the datum, as far as I am aware this then means that the wait_for_datum parameter should be set to false.

For me, this results in a neverending loop where ekf_localization_map waits on /odometry_gps from navsat_transform, navsat_transform then waits on /odometry/filtered/map from ekf_localization_map. The whole time navsat_transform spams with messages of

[navsat_transform_node-8] [INFO] [1641296576.110339410] [navsat_transform]: Datum (latitude, longitude, altitude) is (37.50, -122.00, 0.03)
[navsat_transform_node-8] [INFO] [1641296576.110379312] [navsat_transform]: Datum UTM coordinate is (10S, 588391.24, 4150810.87)

I'm not really sure how to set this up properly? Note, I think my setup more or less corresponds to the example param and launch files from the robot_localization github repo.

My config and launch files: config file

# For parameter descriptions, please refer to the template parameter files for each node.
ekf_localization_odom:
  ros__parameters:
    frequency: 30.0
    sensor_timeout: 0.0 #0.1
    two_d_mode: true
    transform_time_offset: 0.0 #0.01
    transform_timeout: 0.0
    print_diagnostics: true
    debug: false

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

    odom0: odometry/wheel
    odom0_config: [false, false, false,
                   false, false, false,
                   true,  true,  false,
                   false, false, true,
                   false, false, false]
    odom0_queue_size: 10
    odom0_nodelay: true
    odom0_differential: false
    odom0_relative: false

    imu0: imu/data
    imu0_config: [false, false, false,
                  true,  true,  true,
                  false, false, false,
                  true,  true,  true,
                  true,  true,  true]
    imu0_nodelay: true # false
    imu0_differential: false
    imu0_relative: false
    imu0_queue_size: 10
    imu0_remove_gravitational_acceleration: true # trying false with 2d_mode: true

    use_control: false

ekf_localization_map:
  ros__parameters:
    frequency: 30.0
    sensor_timeout: 0.0 #0.1
    two_d_mode: true
    transform_time_offset: 0.0 #0.01
    transform_timeout: 0.0
    print_diagnostics: true
    debug: false

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

    odom0: odometry/wheel
    odom0_config: [false, false, false,
                   false, false, false,
                   true,  true,  false,
                   false, false, true,
                   false, false, false]
    odom0_queue_size: 10
    odom0_nodelay: true
    odom0_differential: false
    odom0_relative: false

    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,  true,
                  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 # trying false with 2d_mode: true

    use_control: false

navsat_transform:
  ros__parameters:
# Frequency of the main run loop
        frequency: 30.0

# Delay time, in seconds, before we calculate the transform from the UTM frame to your world frame. This is especially
# important if you have use_odometry_yaw set to true. Defaults to 0.
        delay: 3.0

# PLEASE READ: Like all nodes in robot_localization, this node assumes that your IMU data is reported in the ENU frame.
# Many IMUs report data in the NED frame, so you'll want to verify that your data is in the correct frame before using
# it. 

# If your IMU does not account for magnetic declination ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2022-01-14 03:34:54 -0500

Tom Moore gravatar image

updated 2022-01-14 03:35:16 -0500

For me, this results in a neverending loop where ekf_localization_map waits on /odometry_gps from navsat_transform, navsat_transform then waits on /odometry/filtered/map from ekf_localization_map.

You should still be getting publications on /odometry/filtered/map. That EKF instance will start publishing as soon as it receives a single sensor message from _any_ sensor input. So it's not waiting on anything from navsat_transform, but navsat_transform _is_ waiting for /odometry/filtered/map. So you should just echo /odometry/filtered/map. I suspect it actually is publishing.

But I don't think this is your issue. The message you see spamming will keep printing every time we receive a GPS message, but we have _not_ yet received all the other data required to compute the transform. The criteria to get past that condition are here:

https://github.com/cra-ros-pkg/robot_...

You can see that it needs IMU data, odometry data (which you are suspecting), and GPS data. In this case, your IMU appears to be publishing on imu/data, but navsat_transform_node is subscribing to imu:

https://github.com/cra-ros-pkg/robot_...

I think you need a remap. I'm thinking you can confirm this by looking at the node and topic info for navsat_transform_node and the imu/data topic, respectively.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2022-01-04 05:48:00 -0500

Seen: 115 times

Last updated: Jan 14 '22