Robot localization package to perform sensor fusion, odometry/filtered not publishing any data

Question

I am working on a 6-wheeled autonomous vehicle to make it autonomously navigate to the desired location.

Using robot localization package I tried to perform sensor fusion of the Lidar and IMU data. I am not using wheel encoders since the vehicle has skid steering it will be very noisy. Following the steps, I first created a launch file and the configuration file for launching the Robot localization package and adding sensor configuration respectively. But when I look for odometry filtered data published on odometry/filtered topic by the Robot localization package I cannot see any results or sensor data. Even though individual sensors like IMU and Lidar are publishing the result.

Config File:

### ekf config file ###
ekf_filter_node:
    ros__parameters:
        frequency: 30.0
        sensor_timeout: 0.1
        two_d_mode: false
        transform_time_offset: 0.0
        transform_timeout: 0.0
        print_diagnostics: true
        debug: false
        debug_out_file: /path/to/debug/file.txt
        permit_corrected_publication: false
        publish_acceleration: false
        publish_tf: true

        map_frame: map              # Defaults to "map" if unspecified
        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

        # default values, and must be specified.
        #odom0: velodyne_points/point_cloud
        #odom0_config: [true,  true,  false,
        #               false, false, false,
        #               false, false, false,
        #               false, false, true,
        #               false, false, false]

        #odom0_queue_size: 2
        #odom0_differential: false
        #odom0_relative: false
        #odom0_pose_use_child_frame: false
        #odom0_pose_rejection_threshold: 5.0
        #odom0_twist_rejection_threshold: 1.0


        imu0: imu/applanix
        imu0_config: [false, false, false,
                      false, false, false,
                      false, false, false,
                      false,  false,  false,
                      true,  false,  false]
        imu0_differential: false
        imu0_relative: true
        imu0_queue_size: 5
        imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
        imu0_twist_rejection_threshold: 0.8                #
        imu0_linear_acceleration_rejection_threshold: 0.8  #

        imu0_remove_gravitational_acceleration: true

        use_control: true

        stamped_control: false

        control_timeout: 0.2

        control_config: [true, false, false, false, false, true]

        acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]

        deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]

        acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]

        deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]

        process_noise_covariance: [0.05, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.05, 0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.06, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.03, 0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.03, 0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.06, 0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.025, 0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.025, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.04, 0.0,    0.0,    0.0,    0.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.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.02, 0.0,    0.0,    0.0,
                                   0.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.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.015]

        initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9]

Launch File:

from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    # Load parameters from a YAML file
    parameters_file_dir = os.path.join(get_package_share_directory('impala_odometry'), 'config', 'ekf_localization.yaml')
    
    return LaunchDescription([
        Node(
            package='robot_localization',
            executable='ekf_node',
            name='ekf_localization',
            parameters=[parameters_file_dir],
        ),
    ])

Answer 1

You appear to be passing a point cloud as an input to the EKF. You've commented that out, so now you are just fusing an IMU. Three things:

• The filter doesn't fuse lidar or point cloud data directly. You need a node in your system that performs scan matching to generate, e.g., a geometry_msgs/TwistWithCovarianceStamped message.
• If you just fuse an IMU by itself, you are not providing a measurement reference for X velocity. Acceleration data alone is far too noisy and will be integrated twice before it produces pose estimate data. That's going to make your state estimate shoot off in some direction.
• Please use your wheel encoder odometry. If the data is noisy, characterise that noise via the covariance in the ROS message.

I would strongly recommend reading the package documentation, as all of these points are called out in there in some fashion, I believe. The docs are for ROS 1, but it's the same code under the hood, so it'll all be relevant.