Allow running Sensors and Physics update in Lockstep mode

[YoshuaNava]

Desired behavior

Classic Gazebo provided a Lockstep mode that synchronized the update of physics and sensors -> https://classic.gazebosim.org/tutorials?tut=lockstep_physics_sensors

This mode allowed the simulation to better resemble a real-system, where sensors are sampled uniformly following a global clock, fitting the same timeline as the physics.

However, this feature does not seem to be present in New/Ignition Gazebo. Could it be implemented? Happy to help if so!

Alternatives considered

Not relying on the Lockstep mode, but this would make our simulation less representative of a real system.

Implementation suggestion

Implementing an equivalent of the Classic Gazebo Lockstep mode.

Additional context

None

[iche033]

This should be supported by default in gz-sim (without needing to specify any args on launch). If you set a camera sensor to run at 30 Hz, it should run at the specified rate in sim time. If the camera can not render fast enough, it would block the main / physics thread and hence lowering the sim RTF.

However, this feature does not seem to be present in New/Ignition Gazebo

Are you running into issues where you see that physics and sensors are not lockstepping?

[azeey]

Technically, it is possible for the next physics step to occur before the rendering/sensor system blocks, but this only introduces one extra time step in physics. If this is a concern for you, I'd like to know, but otherwise, as @iche033 said, Sensors and Physics do run in lockstep by default in the new Gazebo.

[YoshuaNava]

Hi @iche033 and @azeey,
First of all, thank you for your prompt reply.

Are you running into issues where you see that physics and sensors are not lockstepping?

We may have observed physics and sensors updating at different time frames.

To be more specific on the testing setup, my project partner @Magnusgaertner and I were running a Distrobox of Ubuntu 24.04 with ROS2 Jazzy. When running some of the single-camera examples from this repo, the /stats topic of the simulator indicated a higher update rate (>60%), than the frequency of the camera topic (it was set to 30Hz, but we could get at most 8 Hz).

Technically, it is possible for the next physics step to occur before the rendering/sensor system blocks, but this only introduces one extra time step in physics.

Out of curiosity, how is this mechanism for synchronizing and spinning a new physics iteration implemented?

Thank you in advance 🙏

[iche033]

is the 8 Hz output from ros topic hz or gz topic -f -t? I think they output the frequency in wall clock time instead of sim time. If your simulator is running at a lower RTF, the frequency output from those tools will be lower than expected. The cameras should still be updating at their target rate in sim time. If not, then there could be a bug. One way to double check this is to look at the timestamps of the received camera images - they should indicate that the images are published at fixed intervals.

[iche033]

I wrote a simple python program to verify camera timestamps some time ago, see osrf/mbzirc#147 (comment) in case it helps

[DavidG-Develop]

I have been experiencing a similar issue... I am running Gazebo Fortress with ROS2 Humble and have a robot with multiple senors. The senors currently in use are a Velodyne PUCK (gpu_lidar) and a camera. My laptop is powered by a i9 14900HX and an RTX 4070 with 32GB of ram. The RTF is constantly between 90 and 100% but the camera images are lagging behind. The camera is set to 30Hz but i receive only around 16-20Hz on the ros topic with the image header stamps acting as if were to be running at 30 Hz which than makes the timestamps fall behind the sim time by a large amount ever increasingly. Even visualising the image in RViZ the lagging behind is soon obvious (the longer you wait the bigger the discrepancy).

[iche033]

can you check to see if the camera images are also lagging in Gazebo's Image Display GUI plugin (top right menu, select Image Display, and choose topic)? This visualizes the camera images coming through gz topic instead of ROS topic. If the images from gz topic is not lagging like in rviz, then it suggests the issue might be in the ros_gz bridge.

[DavidG-Develop]

Thank you for the swift response. Indeed inside gazebo the image seems fine with the image on the ROS end lagging behind (see image below)

For the bridge setup I initially had a single bridge node bridging all topics that might be needed but after some digging I saw that it might improve performance splitting it that each sensor has its own bridge node, which is also the setup I am using atm (see code below)

main bridge:

main_bridge = Node(
    package='ros_gz_bridge',
    executable='parameter_bridge',
    namespace='dumper',
    name='main_bridge',
    arguments=[
        '/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock',
        'base_controller/cmd_vel_unstamped@geometry_msgs/msg/Twist[gz.msgs.Twist'
    ],
    parameters=[
        {'use_sim_time': True}
    ],
    output='screen'
)

sensor bridges (multiple different are used but here is one example):

zed_rgb_bridge = Node(
    package='ros_gz_bridge',
    executable='parameter_bridge',
    namespace='dumper',
    name='zed_rgb_bridge',
    arguments=[
        'zed2i/zed_node/rgb/image_rect_color@sensor_msgs/msg/Image[gz.msgs.Image',
        'zed2i/zed_node/rgb/camera_info@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo'
    ],
    remappings=[
        ('zed2i/zed_node/rgb/camera_info', 'zed2i/zed_node/camera_info')
    ],
    parameters=[
        {'use_sim_time': True}
    ],
    output='screen'
)

edit:
I have been doing a couple of further tests and can conclude it has to be a bridge issue... My camera is currently running on 15hz with a RTF locked to 0.5 so i expect to have a 7.5Hz frequency on ROS end. I have made a script which echos the gazebo camera topic and extrapolates a frequency from the received message stamps (Stamps frequency - expected 15Hz) and from the amount of messages received measures the real frequency (expected 7.5Hz) and shows it together with the last stamp it has (top right of the image). Bottom right also shows the current sim and real time gotten from gazebo. On the left side there is the data from ROS topics with the top showing header stamps of the image messages and bottom showing the frequency of the image ROS topic.

[iche033]

ah yes for image / point cloud topics, it's recommended to separate them out into it's own bridge process. You can also enable lazy subscriber to save some compute resources: parameters=[{'lazy':True}],

If you able to use harmonic, you can take advantage of node composition for further speedup

[DavidG-Develop]

I have tried lazy=true didnt help much... Now I have quickly built a docker container which than runs ROS2 Jazzy with Harmonic (I could not have used humble as ros2 control for gazebo does not support that configuration)... I have now implemented GZ server + 3 bridges as compostable nodes in one container and the problem remains the same but different :)...
Before I saw that clock topic from gazebo would match clock topic on ros2 with lidar and/or camera topic falling behind (img 1) while now I see that the camera stamps match ros2 clock time but fall behind the clock time of sim time from gazebo.
Right side are ros data while on the left is the gazebo data.

The large change in difference between times is due to different sensor setups (img 1 rtf locked to 0.5, both sensors reduced quaility, while img 2 is running both sensor at real life quality with rtf 1).

I hope this can help you have a better insight into what might be causing the issue as well as step how could I improve it. The only main improvement untill now was to reduce the quality on both sensors (resolution, range, ...) as well as locking the rtf to a value with which the sensor can keep up, but I find that to be a sub optimal solution as I lose the senor quality I expect to have on the real robot. I am looking forward to any possible tips or tricks and am at your disposal if you need any further info.

Here is also my composable node setup if it helps:

gz_server_params = [
    {'use_sim_time': True}, 
    {'on_exit_shutdown': True},
    {'world_sdf_file': LaunchConfiguration('path_to_world')},
]

gz_server_node = ComposableNode(
    package='ros_gz_sim',
    plugin='ros_gz_sim::GzServer',
    name='gz_server',
    namespace='dumper',
    parameters=gz_server_params,
    extra_arguments=[{'use_intra_process_comms': True}],
)

composable_nodes = []
composable_nodes.append(gz_server_node)

main_bridge_config = os.path.join(get_package_share_directory("dumper_bringup"), 'launch', 'sim', 'harmonic', 'bridge_params', 'main_bridge.yaml')
main_bridge = ComposableNode(
    package='ros_gz_bridge',
    plugin='ros_gz_bridge::RosGzBridge',
    namespace='dumper',
    name='main_bridge',
    parameters=[{'config_file': main_bridge_config}],
    extra_arguments=[{'use_intra_process_comms': True}]
)
composable_nodes.append(main_bridge)

container=ComposableNodeContainer(
    name='dumper_sim_container',
    namespace='dumper',
    package='rclcpp_components',
    executable='component_container',
    composable_node_descriptions=composable_nodes,
    output='screen'
)

- ros_topic_name: "/clock"
  gz_topic_name: "/clock"
  ros_type_name: "rosgraph_msgs/msg/Clock"
  gz_type_name: "gz.msgs.Clock"
  lazy: true
  direction: GZ_TO_ROS

- ros_topic_name: "base_controller/cmd_vel"
  gz_topic_name: "base_controller/cmd_vel"
  ros_type_name: "geometry_msgs/msg/TwistStamped"
  gz_type_name: "gz.msgs.Twist"
  lazy: true
  direction: GZ_TO_ROS

Further 2 bridges are made in the same way.