ROS2 Generating Odometry with GPS and IMU Data and Visualizing in RViz

Question

I'm a new robot developer and I am learning ROS2, and I'm working on a project where I want to make a robot go straight to a specific area using GPS and IMU data. Since there won't be any obstacles around the robot, I don't need additional sensors like LIDAR or cameras. However, the biggest challenge I'm facing is that I can't generate odometry with GPS and IMU data, and I can't visualize this data in RViz.

Currently, I'm using GPS and IMU sensors to determine the robot's position and orientation. However, I'm struggling to convert this data into odometry and then visualize it using RViz. I need your help to figure out the steps to ensure the robot goes smoothly to a specific area without odometry.

I tried ekf with robot_localization but imu and gps data did not enter odometry. I tried to write it myself without success

ekf.yaml file:

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

ekf_filter_node_odom:
  ros__parameters:
    frequency: 30.0
    two_d_mode: true # Recommended to use 2d mode for nav2 in mostly planar environments
    print_diagnostics: true
    debug: false
    publish_tf: true

    map_frame: map
    odom_frame: odom
    base_link_frame: base_link # the frame id used by the turtlebot's diff drive plugin
    world_frame: odom

    odom0: gps
    odom0_config: [false, false, false,
                  false, false, false,
                  true,  true,  true,
                  false, false, true,
                  false, false, false]
    odom0_queue_size: 10
    odom0_differential: false
    odom0_relative: false

    imu0: imu
    imu0_config: [false, false, false,
                  false,  false,  true,
                  false, false, false,
                  false,  false,  false,
                  false,  false,  false]
    imu0_differential: false  # If using a real robot you might want to set this to true, since usually absolute measurements from real imu's are not very accurate
    imu0_relative: false
    imu0_queue_size: 10
    imu0_remove_gravitational_acceleration: true

    use_control: false

    process_noise_covariance: [1e-3, 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,    1e-3,  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,    1e-3,  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.3,   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.3,   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.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.5,    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.5,    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.1,   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.3,   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.3,   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.3,   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.3,   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.3,   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.3]

ekf_filter_node_map:
  ros__parameters:
    frequency: 30.0
    two_d_mode: true  # Recommended to use 2d mode for nav2 in mostly planar environments
    print_diagnostics: true
    debug: false
    publish_tf: true

    map_frame: map
    odom_frame: odom
    base_link_frame: base_link # the frame id used by the turtlebot's diff drive plugin
    world_frame: map

    # odom0: gps
    # odom0_config: [false, false, false,
    #               false, false, false,
    #               true,  true,  true,
    #               false, false, true,
    #               false, false, false]
    # odom0_queue_size: 10
    # odom0_differential: false
    # odom0_relative: false

    odom0: gps
    odom0_config: [true,  true,  false,
                  false, false, false,
                  true,  true,  true,
                  false, false, true,
                  false, false, false]
    odom0_queue_size: 10
    odom0_differential: false
    odom0_relative: false

    imu0: imu
    imu0_config: [false, false, false,
                  false,  false,  true,
                  false, false, false,
                  false,  false,  false,
                  false,  false,  false]
    imu0_differential: false  # If using a real robot you might want to set this to true, since usually absolute measurements from real imu's are not very accurate
    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.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,    0.0,    0.0,    0.0,    0.0,    0.0,
                              0.0,    0.0,    1e-3,   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.3,    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.3,    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.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.5,     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.5,     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.1,    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.3,    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.3,    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.3,    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.3,    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.3,    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.3]

navsat_transform:
  ros__parameters:
    frequency: 30.0
    delay: 3.0
    magnetic_declination_radians: 0.0
    yaw_offset: 0.0
    zero_altitude: true
    broadcast_utm_transform: true
    publish_filtered_gps: true
    use_odometry_yaw: true
    wait_for_datum: false 

and launch.py file:

import os
from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import Command, LaunchConfiguration

from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
import launch_ros

def generate_launch_description():
   
    start_ekf_local = Node(
        package='robot_localization',
        executable='ekf_node',
        name='ekf_filter_node_odom',
        output='screen',
        parameters=[os.path.join(pkg_share, 'config/dual_ekf_navsat.yaml')],
        remappings=[('odometry/filtered', 'odometry/local')]  
        )
    
    start_ekf_global = Node(
        package='robot_localization',
        executable='ekf_node',
        name='ekf_filter_node_map',
        output='screen',
        parameters=[os.path.join(pkg_share, 'config/dual_ekf_navsat.yaml')],
        remappings=[('odometry/filtered', 'odometry/global')]
        )
    
    navsat_transform= Node(
        package='robot_localization',
        executable='navsat_transform_node',
        name='navsat_transform',
        output='screen',
        parameters=[os.path.join(pkg_share, 'config/dual_ekf_navsat.yaml')],
        remappings=[('gps/fix', 'gps')]
        )

    return LaunchDescription([
        start_ekf_local,
        start_ekf_global,
        navsat_transform
    ])

rqt-graph

Answer 1

Welcome to ROS2!

nav2 alreay has a solution for estimating the position of a robot relative to a fixed starting position, which you use: robot localization Extended Kalman Filter.

Now, what this node does is estimating from your sensor input GPS/IMU/You_name_it the change in position relative to your starting position. This starting position then is given a coordinate system (or in another word frame).

This frame is usually called odom-frame, like you did in your parameter file

ekf_filter_node_odom:
  ros__parameters:
    .
    .
    .
    map_frame: map
    odom_frame: **odom**
    base_link_frame: base_link # the frame id used by the turtlebot's diff drive plugin
    world_frame: **odom**
    .
    .
    .

since your roboter probably has a fixed frame (usually called base_link or similar), all you need to do is visualize the change between those two coordinate systems (frames). In Rviz you want to see those two moving relative to each other. To do that all you need to do in rviz is to add tf topic on the left hand side:

https://answers.ros.org/question/11706/is-there-a-way-to-subscribe-to-tf-data/

https://docs.ros.org/en/humble/Tutorials/Intermediate/RViz/RViz-User-Guide/RViz-User-Guide.html:

which we know will work since your parameter file says EKF is publishing the transform (tf):

.
.
.
publish_tf: true
.
.
.