I'm using version 2.7.4 of the robot_localization package for ROS Noetic.
I'm currently utilizing two nodes of the package:
- EKF Local Node: Fuses data from an IMU (100Hz) and wheel encoders (4Hz).
- EKF Global Node: Fuses the output of the EKF Local Node with GPS data (4Hz).
Despite these frequencies, the ekf_global node's frequency appears bounded between 60-65 Hz (asking for 100). And the ekf_local node's frequency appears bounded between 80-85 Hz(asking for 100). This restriction remains even when altering the input frequencies. with a lot of "Failed to meet update rate! Took 0.02000X" warnings in the console.
Configuration:
launch file:
<launch>
<arg name="namespace" default="fjcruiser" />
<arg name="base_link_frame" value="$(arg namespace)/base_footprint" />
<arg name="use_legacy_odom" default="false" />
<!-- Load conf and parameters from yaml files -->
<rosparam command="load" file="$(find ugv_odometry)/config/origins.yaml"/>
<rosparam command="load" file="$(find ugv_odometry)/params/$(arg namespace)/localization.yaml" />
<!-- Set the origin as a rosparam -->
<node pkg="ugv_odometry" type="initializer_node" name="initializer_node" output="screen" />
<!-- GNSS velocity converter-->
<node name="gnss_velocity_converter" pkg="ugv_odometry" type="gnss_velocity_converter" output="screen" />
<!-- EKF LOCAL-->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_local" respawn="false" output="screen">
<rosparam command="load" file="$(find ugv_odometry)/config/$(arg namespace)/ekf_local.yaml"/>
<param name="base_link_frame" value="$(arg base_link_frame)"/>
<remap from="odometry/filtered" to="relative_odom"/>
</node>
<!-- EKF Global -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_global" respawn="true" output="screen">
<rosparam command="load" file="$(find ugv_odometry)/config/common/ekf_global.yaml"/>
<param name="base_link_frame" value="$(arg base_link_frame)" />
<param name="odom0" value="/$(arg namespace)/relative_odom" />
<param name="odom1" value="/$(arg namespace)/navsat/odometry" />
<remap from="odometry/filtered" to="odom" />
</node>
<!-- Navsat Transform Node -->
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true">
<rosparam command="load" file="$(find ugv_odometry)/config/common/navsat_transform.yaml"/>
<remap from="imu/data" to="imu" />
<remap from="gps/fix" to="fix" />
<remap from="odometry/filtered" to="odom" />
<remap from="odometry/gps" to="navsat/odometry" />
</node>
<launch>
ekf_local.yaml
frequency: 100
sensor_timeout: 0.1
two_d_mode: true
world_frame: odom
odom_frame: odom
debug: false
debug_out_file: /home/developer/workspace/rosbag/ekf_local_debug_file
reset_on_time_jump: true
publish_tf: true
dynamic_process_noise_covariance: true
#transform_time_offset: 0.05
# -------------------------------------
# IMU configuration:
# -------------------------------------
imu0: /fjcruiser/imu
imu0_config: [false, false, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]
imu0_differential: false
imu0_queue_size: 50
imu0_remove_gravitational_acceleration: true
imu0_nodelay: true
# -------------------------------------
# GNSS Doppler velocity configuration:
# -------------------------------------
odom0: /fjcruiser/converted_gnss_velocity
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, false,
false, false, false]
odom0_differential: false
odom0_queue_size: 4
odom0_nodelay: true
# [ADVANCED] This matrix represents the noise we add to the total error after each prediction step.
process_noise_covariance: [0.005, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.005, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.006, 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.03, 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.5, 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.5, 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.01, 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.01, 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.015]
# [ADVANCED] This represents the initial value for the state estimate error covariance matrix.
initial_estimate_covariance: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 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-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]
ekf_global.yaml
# The frequency of the output
frequency: 50
sensor_timeout: 0.1
two_d_mode: true
debug: false
debug_out_file: /home/developer/workspace/rosbag/ekf_global_debug_file
reset_on_time_jump: true
publish_tf: true
dynamic_process_noise_covariance: true
#transform_time_offset: 0.05
# The frame IDs
map_frame: map
odom_frame: odom
world_frame: map
# Sensor configurations
# Local EKF odometry settings
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
odom0_differential: false
odom0_queue_size: 50
odom0_nodelay: true
# GNSS settings
odom1_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom1_differential: false
odom1_queue_size: 4
odom1_nodelay: true
# [ADVANCED] This matrix represents the noise we add to the total error after each prediction step.
process_noise_covariance: [0.005, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.005, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.006, 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.03, 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.025, 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.04, 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.01, 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.01, 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.015]
# [ADVANCED] This represents the initial value for the state estimate error covariance matrix.
initial_estimate_covariance: [1, 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, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 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, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]
As requested you will find below a sample message from each sensor input:
Fix
header:
seq: 4502
stamp:
secs: 1691504843
nsecs: 100990753
frame_id: "fjcruiser/gnss1_antenna_wgs84"
status:
status: 0
service: 1
latitude: 24.4363592
longitude: 54.6110056
altitude: -19.088
position_covariance: [0.4342809729757313, 0.0, 0.0, 0.0, 0.4342809729757313, 0.0, 0.0, 0.0, 0.7259039967498779]
position_covariance_type: 2
imu
header:
seq: 136324
stamp:
secs: 1691505083
nsecs: 75626252
frame_id: "fjcruiser/sensor_wgs84"
orientation:
x: -0.42456197949594277
y: 0.9053296474668617
z: -0.004811025879026105
w: -0.010112218289144895
orientation_covariance: [1.2389542403262732e-06, 0.0, 0.0, 0.0, 1.2375152844005589e-06, 0.0, 0.0, 0.0, 1.5143168398652242e-05]
angular_velocity:
x: -0.00975768268108368
y: 0.01087676640599966
z: 0.0530206598341465
angular_velocity_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
linear_acceleration:
x: 0.7374916076660156
y: 0.49138200283050537
z: -10.263421058654785
linear_acceleration_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
Wheel_encoder:
header:
seq: 62
stamp:
secs: 1691504859
nsecs: 601004771
frame_id: "fjcruiser/base_footprint"
child_frame_id: "fjcruiser/base_footprint"
pose:
pose:
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist:
twist:
linear:
x: 0.00014493087447644442
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 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.0, 0.0, 0.0, 0.0, 0.0, 0.0]
And as requested this is my TF launch file:
<launch>
<arg name="namespace" default="" />
<arg name="robot_tf_prefix" default="$(arg namespace)/"/>
<arg name="type" default="$(arg type)/" /> <!-- real, simulation or rosbag-->
<group if="$(eval arg('namespace') == 'sbuggy')">
<node pkg="tf2_ros" type="static_transform_publisher" name="$(arg namespace)_link_broadcaster" args="0.375 0 2.29 0 0 0 $(arg namespace)/base_footprint $(arg namespace)/base_link" />
<node pkg="tf2_ros" type="static_transform_publisher" name="$(arg namespace)_linkcenter_broadcaster" args="0 0 0 0 0 0 $(arg namespace)/base_footprint $(arg namespace)/center_link" />
<node pkg="tf2_ros" type="static_transform_publisher" name="$(arg namespace)_link1_broadcaster" args="0 0 0 0 0 0 $(arg namespace)/base_link $(arg namespace)/os1/os_sensor" />
<node pkg="tf2_ros" type="static_transform_publisher" name="$(arg namespace)_link2_broadcaster" args="1.10 -0.48 -0.71 0.01151917 0.02600541 0.02600541 $(arg namespace)/base_link $(arg namespace)/os2/os_sensor" />
<node pkg="tf2_ros" type="static_transform_publisher" name="$(arg namespace)_link3_broadcaster" args="1.10 0.56 -0.71 0.02722714 0.02234021 0.04258603 $(arg namespace)/base_link $(arg namespace)/os3/os_sensor" />
<node pkg="tf2_ros" type="static_transform_publisher" name="$(arg namespace)_link4_broadcaster" args="0 0 0 0 0 0 $(arg namespace)/base_link $(arg namespace)/gnss1_antenna_wgs84" />
<node pkg="tf2_ros" type="static_transform_publisher" name="$(arg namespace)_link5_broadcaster" args="0 0 0 0 3.14 0 $(arg namespace)/base_link $(arg namespace)/sensor_wgs84" />
</group>
</launch>
Observations and Testing:
- CPU Load: Always remains below 10% across all cores.
- EKF local Frequency: The limiting frequency of around 80/85 hz remains consistent, even when input frequencies are varied.
- Compilation and Debugging: The package is compiled in release mode, and the debug parameter is set to false.
- Input Queue Sizes: Altering the sizes doesn't have a discernable impact on frequency.
- EKF Dependency: The frequency of ekf_global always matches the maximum frequency of ekf_local. If I set the ekf_local to 10 Hz, I will have also 10Hz on the ekf_global regardless of the asked frequency.
- TF Frequency: The frequency of the /tf topic is the combined frequency of the odom and relative_odom topics.
- TF Interactions: Both EKF nodes have interdependent TF requirements. However, when I replaced the TF produced by ekf_global with a static TF, there was no improvement on frequency.
Request: I'm keen on understanding the reasons behind the frequency limitation of the ekf_local nodes and any potential solutions or workarounds that can be recommended. thank you.