Ask Your Question
1

Robot localization ekf capped at 10Hz

asked 2021-11-24 07:29:28 -0600

morten gravatar image

updated 2021-11-25 03:48:30 -0600

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 ...
(more)
edit retag flag offensive close merge delete

Comments

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...

osilva gravatar image osilva  ( 2021-11-24 12:35:20 -0600 )edit

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 10Hz

morten gravatar image morten  ( 2021-11-24 14:58:34 -0600 )edit

Pls read the full thread.

osilva gravatar image osilva  ( 2021-11-24 15:49:24 -0600 )edit

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's use_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.

morten gravatar image morten  ( 2021-11-24 16:02:04 -0600 )edit

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.

osilva gravatar image osilva  ( 2021-11-24 16:09:56 -0600 )edit

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.

osilva gravatar image osilva  ( 2021-11-24 16:11:09 -0600 )edit

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 the use_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.

morten gravatar image morten  ( 2021-11-25 03:21:54 -0600 )edit

Thank you @morten. I believe you are right to discard use_sim_time

osilva gravatar image osilva  ( 2021-11-25 03:58:49 -0600 )edit

1 Answer

Sort by » oldest newest most voted
2

answered 2021-11-26 02:59:34 -0600

morten gravatar image

updated 2021-11-26 03:02:40 -0600

Credit to @ljaniec for referring me to the relevant issue.

The problem was with gazebo's /clock publishing frequency which is, by default, set to 10Hz. There is a corresponding issue topic on this problem here.

The issue can be addressed by creating a gazebo.yaml params file including

gazebo:
  ros__parameters:
    publish_rate: 100.0

and changing the launch file to

gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(get_package_share_directory("gazebo_ros"), "launch", "gazebo.launch.py")
        ),
        launch_arguments={
            "world": world,
            "extra_gazebo_args": ["--ros-args --params-file ", gazebo_params],
        }.items(),

Where gazebo_params is a LaunchConfiguration corresponding to the full path to the gazebo.yaml config file. Doing so enables the ekf node to run at the commanded frequency (30Hz):

$ ros2 topic hz /odometry/filtered/odom
average rate: 29.787
    min: 0.030s max: 0.044s std dev: 0.00379s window: 31
average rate: 29.811
    min: 0.030s max: 0.044s std dev: 0.00393s window: 62
average rate: 29.856
    min: 0.030s max: 0.044s std dev: 0.00385s window: 93
edit flag offensive delete link more

Comments

2

Glad that you found the issue and posted a great answer that I’m sure will benefit others.

osilva gravatar image osilva  ( 2021-11-26 05:50:27 -0600 )edit
1

Me too, and thanks for the help along the way.

morten gravatar image morten  ( 2021-11-26 06:41:38 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2021-11-24 07:29:28 -0600

Seen: 37 times

Last updated: Nov 26 '21