0
$\begingroup$

I have a differential robot with poor grip as you can see in the picture below, with the following devices:

enter image description here

-Motor controller - 100Hz, encoder and publishing in my topic /diff_cont/odom (frame id: odom)

-IMU with magnetometer publishing imu messages on /imu/data (frame id: imu_link) (I think EKF filter is ignoring totally the magnetic field information because I didnt declare it yet (but is being publish also in imu_link frame)

-GNSS publishing fix messages on /fix topic, with more or less 1m of covariance (I'll use RTK later) (frame id: ubx)

The robot is intended to use outdoor but I'll want to navigate avoiding obstacles (will add Lidar later) and going trough GPS poses, doing tasks on each position, etc.

For all of this, I want to use nav_sat_transform node following the instructions for each launch file + config file. My problem is that I'm getting a weird behavior on the estimations. The following image is from RVIZ, 2 wheels and "map" frame are jumping, appearing and disappearing.

enter image description here

I believe I have the TF and the nodes correctly described, take a look of my frames and topics.

enter image description here

Frame: enter image description here

Also, please check my config and launch file.

`
ekf_filter_node_odom:
  ros__parameters:
    frequency: 100.0
    sensor_timeout: 0.1
    two_d_mode: true
    transform_time_offset: 0.0
    transform_timeout: 0.0
    print_diagnostics: true
    debug: false

    use_control: true
    permit_corrected_publication: false
    control_config: [true, false, false, false, false, true]


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

    odom0: diff_cont/odom
    odom0_config: [false, false, false,
              false, false, false,
              true,  true,  true,
              false, false, true,
              false, false, false]
    odom0_queue_size: 10
    odom0_differential: true
    odom0_relative: false

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

    use_control: false

    process_noise_covariance: [0.001, 0.001, 0.1, 0.3, 0.3, 0.01, 0.05, 0.05, 0.01, 0.03, 0.03, 0.03, 0.03, 0.03, 0.03]
  #process_noise_covariance: [0.01, 0.01, 0.01, 0.03, 0.03, 0.01, 0.01, 0.01, 0.01, 0.03, 0.03, 0.03, 0.03, 0.03, 0.03]
    
    initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1.0, 1.0, 1e-9, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]

ekf_filter_node_map:
  ros__parameters:
    frequency: 100.0
    sensor_timeout: 0.1
    two_d_mode: true
    transform_time_offset: 0.0
    transform_timeout: 0.0
    print_diagnostics: true
    debug: false

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

    odom0: diff_cont/odom
    odom0_config: [false, false, false,
              false, false, false,
              true,  true,  true,
              false, false, true,
              false, false, false]
    odom0_queue_size: 10
    odom0_differential: true
    odom0_relative: false


 # odom1: odometry/gps
 # odom1_config: [true,  true,  false,
 #               false, false, false,
 #               false, false, false,
 #               false, false, false,
 #               false, false, false]

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

    process_noise_covariance: [0.001,0.001, 1e-3, 0.1, 0.1, 0.1, 0.01, 0.01, 0.01, 0.03, 0.03, 0.03, 0.03, 0.03, 0.03]
    #process_noise_covariance: [0.01, 0.01, 0.01, 0.03, 0.03, 0.01, 0.01, 0.01, 0.01, 0.03, 0.03, 0.03, 0.03, 0.03, 0.03]
    initial_estimate_covariance: [1.0, 1.0, 1e-9, 1.0, 1.0, 1e-9, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]

navsat_transform:
  ros__parameters:
    frequency: 100.0
    delay: 3.0
    magnetic_declination_radians: -0.02704614569 # For lat/long 55.944831, -3.186998
    yaw_offset:  0.0 #1.570796327  # IMU reads 0 facing magnetic north, not east
    zero_altitude: true
    broadcast_utm_transform: false
    publish_filtered_gps: true
    use_odometry_yaw: true
    wait_for_datum: false`

And Launch file:

`
def generate_launch_description():
    robot_localization_dir = get_package_share_directory('collie')
    parameters_file_dir = os.path.join(robot_localization_dir, 'config')
    parameters_file_path = os.path.join(get_package_share_directory('collie'),'config','dual_nav_sat.yaml')
    #os.environ['FILE_PATH'] = str(parameters_file_dir)
    return LaunchDescription([
        launch.actions.DeclareLaunchArgument(
            'output_final_position',
            default_value='false'),
        launch.actions.DeclareLaunchArgument(
            'output_location',
        default_value='~/dual_ekf_navsat_example_debug.txt'),
    
    launch_ros.actions.Node(
            package='robot_localization', 
            executable='ekf_node', 
            name='ekf_filter_node_odom',
            output='screen',
            parameters=[parameters_file_path],
            remappings=[('odometry/filtered', 'odometry/local')]           
           ),
    launch_ros.actions.Node(
            package='robot_localization', 
            executable='ekf_node', 
            name='ekf_filter_node_map',
            output='screen',
            parameters=[parameters_file_path],
            remappings=[('odometry/filtered', 'odometry/global')]
           ),           
    launch_ros.actions.Node(
            package='robot_localization', 
            executable='navsat_transform_node', 
            name='navsat_transform',
            output='screen',
            parameters=[parameters_file_path],
            remappings=[('imu/data', 'imu/filtered'),
                        ('gps/fix', 'fix'), 
                        ('gps/filtered', 'gps/filtered'),
                        ('odometry/gps', 'odometry/gps'),
                        ('odometry/filtered', 'odometry/global')]           

           )           
])`

The gps is publishing correctly into the filter as you can see on the RQT-topic-view and in the following pic from foxglove (on the picture you can see the robot appearing on odom position and map position. (same as Rviz)

enter image description here

And shitty pic with slow-camera-mode with my phone:

enter image description here

So any idea what can be causing the estimation to be 0 smooth. I understand that the robot could be moving caused by the GNSS measuring moving, but at least should be constant and smooth, trying to "follow" the /fix topic.

Anything else you guys think you need to see in order to help me out

$\endgroup$

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.