Robot localization ekf capped at 10Hz
I'm using the ekf
in robot_localization
, for some reason it doesn't seem to want to go above the odd 10Hz. I've checked my imu and odometry rates, and they're both 50Hz as I've set them.
I launch the ekf node by ros2 launch tractor_gazebo ekf.launch.py use_sim_time:=true
and I check the individual frequencies:
$ ros2 param get /ekf_localization_odom frequency
Double value is: 30.0
$ ros2 topic hz /imu/data
average rate: 49.887
min: 0.020s max: 0.021s std dev: 0.00020s window: 51
$ ros2 topic hz /odometry/wheel
average rate: 49.910
min: 0.020s max: 0.021s std dev: 0.00018s window: 51
average rate: 49.853
min: 0.020s max: 0.021s std dev: 0.00020s window: 101
$ ros2 topic hz /odometry/filtered/odom
average rate: 9.956
min: 0.100s max: 0.101s std dev: 0.00033s window: 11
average rate: 9.961
min: 0.099s max: 0.101s std dev: 0.00043s window: 21
with the launch and config files added at the bottom.
Note: I have tried setting the ekf frequency to less than 10Hz, and this is can follow fine. So, e.g. changing the parameter in the yaml to 5Hz results in publishing to /odometry/filtered/odom
at 5Hz, setting it to 15Hz instead results in the topic publishing at 10Hz. This is the core of the problem
ekf.launch.py:
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 LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
# get directories
tractor_gazebo_dir = get_package_share_directory("tractor_gazebo")
# Create the launch configuration variables
use_sim_time = LaunchConfiguration("use_sim_time")
# declare launch arguments
declare_use_sim_time = DeclareLaunchArgument("use_sim_time")
# Specify the actions
robot_localization_odom = Node(
package="robot_localization",
executable="ekf_node",
name="ekf_localization_odom",
output="screen",
parameters=[
{"use_sim_time": use_sim_time},
os.path.join(
tractor_gazebo_dir,
"config",
"dual_ekf_navsat_toolcarrier.yaml",
),
],
remappings=[("odometry/filtered", "odometry/filtered/odom")],
)
return LaunchDescription(
[
declare_use_sim_time,
robot_localization_odom,
]
)
config file:
ekf_localization_odom:
ros__parameters:
frequency: 30.0
sensor_timeout: 0.0 #0.1
two_d_mode: true
transform_time_offset: 0.0 #0.01
transform_timeout: 0.0
print_diagnostics: true
debug: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
odom0: odometry/wheel
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: false
imu0: imu/data
imu0_config: [false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
use_control: false
Edit: Following @osilva's comments I have checked the sim time parameter of all the nodes:
morten@agrirobot:~$ ros2 param list
/ackermann_drive:
use_sim_time
/camera_controller_front:
use_sim_time
/camera_controller_middle:
use_sim_time
/controller_manager:
use_sim_time
/ekf_localization_odom:
use_sim_time
/gazebo:
use_sim_time
/gazebo_ros2_control:
use_sim_time
/velodyne_front/gazebo_ros_laser_controller:
use_sim_time
/gps:
use_sim_time
/imu:
use_sim_time
/joint_state_publisher:
use_sim_time
/robot_state_publisher:
use_sim_time
Note, I've trimmed the other parameters so as not to go on too long, I have checked each and every one of these:
morten@agrirobot:~$ ros2 param get /ackermann_drive use_sim_time
Boolean value is: True
morten@agrirobot:~$ ros2 param get /camera_controller_front use_sim_time
Boolean value is: True ...
It's definitely an issue as you can see: https://github.com/ros-controls/ros2_...
Root cause: Since there's no global param server anymore, all nodes need to manage their
use_sim_time
parameter.Please take a look at prior answer how it's solved. https://answers.ros.org/question/3714...
I called the launch file
use_sim_time:=true
, regardless I'm not really sure how this has to do with the frequency problem? The EKF runs, it just doesn't run at the requested frequency capping out at 10HzPls read the full thread.
I don't see how this is relevant? Everything in this sim is on sim time, and this isn't even a problem with
ros2_control
?I don't think it's relevant but my sim launch file also contains a line setting the
controller_manager
'suse_sim_time
to be true.Please explain your point more than "read this", I'm having a hard time seeing how this information is relevant to the questions on
robot_localization
.In the thread it states:
all nodes need to manage their use_sim_time parameter.
Failure to do so will lead to nodes publishing messages with different times from the rest of the system, different real time factors and other stuff.
The simple solution is to set it on each controller parameter file, as well as on the controller_manager node.
Since your issue is with Gazebo and you do not have issues with your hardware as you clearly shown in your question. The time factors get screwed up in ROS 2 as there is no longer a master node. I’m not sure this a solution that’s why I added as comment vs an answer.
My controllers yaml file starts with
controller_manager
...use_sim_time: True
, furthermore echoing the/imu/data
and/odometry/wheel
shows they run in sync on sim time, i.e. starting from 0 seconds and counting up as opposed to the current unix epoch.The issue should be with
robot_localization
, no? I've encountered theuse_sim_time
problem already in another context, I feel like everything here is using sim time though.Edit: I've appended some information to the original post.
Thank you @morten. I believe you are right to discard
use_sim_time