0
$\begingroup$

Rosanswers logo

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
morten@agrirobot:~$ ros2 param get /camera_controller_middle use_sim_time 
Boolean value is: True
morten@agrirobot:~$ ros2 param get /controller_manager use_sim_time 
Boolean value is: True
morten@agrirobot:~$ ros2 param get /ekf_localization_odom use_sim_time 
Boolean value is: True
morten@agrirobot:~$ ros2 param get /gazebo use_sim_time 
Boolean value is: True
morten@agrirobot:~$ ros2 param get /gazebo_ros2_control use_sim_time 
Boolean value is: True
morten@agrirobot:~$ ros2 param get /velodyne_front/gazebo_ros_laser_controller use_sim_time 
Boolean value is: True
morten@agrirobot:~$ ros2 param get /gps use_sim_time 
Boolean value is: True
morten@agrirobot:~$ ros2 param get /imu use_sim_time 
Boolean value is: True
morten@agrirobot:~$ ros2 param get /joint_state_publisher use_sim_time 
Boolean value is: True
morten@agrirobot:~$ ros2 param get /robot_state_publisher use_sim_time 
Boolean value is: True

and everything is on use_sim_time True.

Originally posted by morten on ROS Answers with karma: 198 on 2021-11-24

Post score: 1

Original comments

Comment by osilva on 2021-11-24:
It's definitely an issue as you can see: https://github.com/ros-controls/ros2_control/issues/325

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/371458/should-i-manually-pass-use_sim_time-to-all-nodes-in-a-ros-2-launchfile/

Comment by morten on 2021-11-24:
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

Comment by osilva on 2021-11-24:
Pls read the full thread.

Comment by morten on 2021-11-24:
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.

Comment by osilva on 2021-11-24:
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.

Comment by osilva on 2021-11-24:
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.

Comment by morten on 2021-11-25:
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.

Comment by osilva on 2021-11-25:
Thank you @morten. I believe you are right to discard use_sim_time

Comment by morten on 2021-11-25:
While checking the use_sim_time params I noticed that the /joint_state was also publishing at 10Hz, perhaps this is the bottleneck

Comment by osilva on 2021-11-25:
Could be. Check this discussion: https://answers.ros.org/question/222074/joint_states-topic-and-publish-rate/

I haven’t encountered this but I’ll research more

Comment by ljaniec on 2021-11-25:
This issue seems to be relevant to your question

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

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

Originally posted by morten with karma: 198 on 2021-11-26

This answer was ACCEPTED on the original site

Post score: 3

Original comments

Comment by osilva on 2021-11-26:
Glad that you found the issue and posted a great answer that I’m sure will benefit others.

Comment by morten on 2021-11-26:
Me too, and thanks for the help along the way.

Improve this answer
$\endgroup$

Your Answer

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