robot_localization no output (using only VO, Compass, and GPS)

Question

I'm using robot_localization (ROS2 Humble on Ubuntu) for a boat integration and want to fuse my LiDAR odometry (ideally to be used more in the harbors etc) with GPS data (ideally to be used on open water). However as of now I cannot get any outputs from either the navsat_transform_node or ekf_global_odom. I use a rosbag with captured data.

As of now the only data I have access to is the compass (heading added to a IMU-msg), a LiDAR (using KISS-ICP for LiDAR odometry) and GPS.

Below I have listed my launch files and msgs:

EKF global launch:

def generate_launch_description():
package_dir = get_package_share_directory('odometry_localization')
config_file = os.path.join(package_dir, 'config', 'ekf_global.yaml')

use_sim_time = LaunchConfiguration('use_sim_time')

return LaunchDescription([
    DeclareLaunchArgument(
        'use_sim_time', default_value='true',
        description='Use clock topic (from bagfile) if true'),

    # EKF node for global odometry
    Node(
        name='ekf_global_odom',
        package='robot_localization',
        executable='ekf_node',
        parameters=[config_file,
                    {'use_sim_time': True}],
        # Subscriptions (in yaml):
        #   odom0: odometry/gps (from navsat node)
        #   odom1: kiss/odometry
        #   imu0:  heading/imu
        # Publishes odometry/global
        remappings=[('odometry/filtered', 'odometry/global')]
    ),

    # Turn GPS fix into odometry message for EKF
    Node(
        name='navsat_transform_node',
        package='robot_localization',
        executable='navsat_transform_node',
        # yaw_offset - corrects to make IMU read 0 when facing East
        # magnetic_declination - see http://www.ngdc.noaa.gov/geomag-web
        # zero_altitude - pretends the world is flat
        # use_odometry_yaw - false, use IMU yaw
        parameters=[{'yaw_offset': pi / 2,
                     'magnetic_declination_radians': 0.086219265,
                     'zero_altitude': True,
                     'publish_filtered_gps': True,
                     'frequency': 10.0,
                     'use_odometry_yaw': False}],
        # Subscribes to imu, gps/fix, odometry/global (by default - can be remapped)
        remappings=[('odometry/filtered', 'odometry/global'),
                    ('/imu', '/heading/imu'),
                    ('/gps/fix', '/gps')
                    ]
        # Publishes to odometry/gps, gps/filtered (if chosen)
    )
])

EKF global yaml:

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

two_d_mode: true    # 2D mode (x, y, yaw)
frequency: 50.0

odom0: odometry/gps
odom0_config: [true,  true,  false,   # x,   y,   z
               false, false, false,   # r,   p,   w (yaw)
               false, false, false,   # dx,  dy,  dz
               false, false, false,   # dr,  dp,  dw
               false, false, false]   # ddx, ddy, ddz
odom0_differential: false
odom0_queue_size: 10

odom1: kiss/odometry
odom1_config: [true, true, true,   # x,   y,   z
               true, true, true,   # r,   p,   w (yaw)
               false,  false,  false,    # dx,  dy,  dz
               false, false, false,    # dr,  dp,  dw
               false, false, false]   # ddx, ddy, ddz
odom1_differential: false
odom1_queue_size: 10

# (note this is not raw IMU, but output from madgwick filter)
imu0: heading/imu
imu0_config: [false, false, false,   # x,   y,   z
              true,  true,  true,    # r,   p,   w (yaw)
              false, false, false,   # dx,  dy,  dz
              false,  false,  false,    # dr,  dp,  dw
              false,  false,  false,]   # ddx, ddy, ddz
imu0_differential: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true

I'm running the KISS-odometry node as well as my static transforms from this launch:

    Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='tf_base_gps',
        output='screen',
        arguments = ['0', '0', '0', '0', '0', '0', 'base_link', 'gps_frame']
    ),

    Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='tf_base_compass',
        output='screen',
        arguments = ['0', '0', '0', '0', '0', '0', 'base_link', 'compass_frame']
    ),


    Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='tf_base_lidar',
        output='screen',
        arguments = ['0', '0', '0', '0', '0', '0', 'base_link', 'lidar_frame'] # f"{pi}"
    ),


    # Run kiss odometry launch file
    IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            os.path.join(get_package_share_directory("kiss_icp"), 'launch', 'odometry.launch.py'),
        ]),
        # add arguments to the launched file
        launch_arguments={'topic': '/lidar/filtered',
                          'visualize': 'true',
                          'odom_frame': 'odom',
                          'base_frame': 'base_link',
                          'publish_odom_tf': 'true',
                          }.items()
    ),

As mentioned I have the following msgs:

Heading as IMU-msg (to be used for navsat_transform_node):

header:
stamp:
sec: 1695561038
nanosec: 453170061
frame_id: compass_frame
orientation:
   x: 0.0
   y: 0.0
   z: 0.6387678175155977
   w: 0.7693995550468951
orientation_covariance:
- 0.5
- 0.5 (for now I've just added 0.5 to all these as placeholder) 

LiDAR odometry:

header:
stamp:
sec: 1695561040
nanosec: 965941429
frame_id: odom
child_frame_id: ''
pose:
pose:
position:
  x: 31.813855359987997
  y: 1.885727513672836
  z: -2.2876716010826703
orientation:
  x: 0.016624870352472804
  y: 0.01141136892666701
  z: 0.006816351310623348
  w: 0.9997734401852219
covariance:
- 0.0
- 0.0 (etc)

NO twist output - maybe this is causing some problems??

GPS msg:

header:
stamp:
sec: 1695561036
nanosec: 619042634
frame_id: gps_frame
status:
 status: 0
 service: 0
latitude: 55.707455
longitude: 12.606366666666666
altitude: 0.0
position_covariance:
- 0.5
- 0.5 (have also just added 0.5 to these a of now)

Attached you see the tf tree and node/topic graph from rqt.

So... Do you see any reasons? I expect it's simply some data missing, some tfs blocking or something related, but I feel like I've tried most things and read most questions from in here.

I suspect that it's the VO causing some problems and have been thinking of the following:

• Could it be due to the VO generating odometry from 0 and it seems like the navsat_transform_node uses an absolute 0?
• Could it be due to not having any velocity data (e.g. should I make a node to calculate this from the VO and publish that?
• Is there anything with the tf's that should be adjusted (e.g. the odom->base_link tf is now being published by the kiss_icp but I have tried changing this (e.g. using odom1 instead) without luck.

I hope that any of you can help me out. Thanks a lot!

EDIT: Looks like the nav_sat_transform doesn't get the initial odometry pose correctly:

[INFO] [1701859939.480960082]: Datum (latitude, longitude, altitude) is (55.71, 12.61, 0.00)

[INFO] [1701859939.481115586]: Datum UTM coordinate is (33U, 349586.86, 6176202.22)

... And not anything with Initial odometry position

Answer 1

I use a rosbag with captured data.

And something is publishing on /clock? If you have use_sim_time set to true, then something has to publish the clock.

Also, you have two_d_mode set to true, but are attempting to fuse a bunch of 3D variables (Z, Roll, and Pitch) in your state estimate. Not going to cause problems per se, but those 3D state variables will be ignored.

But one thing you definitely need to fix is your lidar odometry. Here is the config you have:

odom1: kiss/odometry
odom1_config: [true, true, true,   # x,   y,   z
               true, true, true,   # r,   p,   w (yaw)
               false,  false,  false,    # dx,  dy,  dz
               false, false, false,    # dr,  dp,  dw
               false, false, false]   # ddx, ddy, ddz
odom1_differential: false
odom1_queue_size: 10

So you are telling the filter that, from your lidar odometry data, you want to fuse X, Y, Z, Roll, Pitch, and Yaw. Your EKF world frame is set to map. However, the lidar odometry message is given in the odom frame:

frame_id: odom
child_frame_id: ''

In order for the filter to use that data, it's going to need a transform from map->odom, but that is the very transform you need the EKF to provide!

To get around this, you can do a number of things:

• Change the laser odometry node so it outputs velocities instead of just pose data, and fuse that instead
• Turn on differential mode for the laser sensor. That will cause the filter to differentiate consecutive measurements and fuse the resulting velocity instead.
• If you don't need a map frame, you can stop whatever node you have publishing odom->base_link and change your EKF to operate in the odom frame.

Likely unrelated, but just as an FYI, whatever node you have publishing the odom->base_link transform should be doing so at a higher rate than 5 Hz. Your map-frame EKF config is publishing at 50.

Finally, I'll tell you what I often do with these setups: start with one input. Get the filter behaving as you want, and then start adding the others.