sas
Modularised monitoring, logging, and control of robots.
Loading...
Searching...
No Matches
Benchmark: how late the robot driver bridge reports Gazebo's joints

The bridge (sas_robot_driver_ros_gazebo_node, or the script sas_robot_driver_ros_gazebo.py) forwards target joint positions to Gazebo and reports Gazebo's joint positions on <robot_name>/get/joint_states. This benchmark measures how late, and how far off, those reports are, with the former pure-Python RobotDriverGazebo and with the C++ one that replaced it, and profiles the Python bridge to find why it fell behind.

Setup

  • ghcr.io/marinholab/sas-full:jazzy (ROS 2 Jazzy, Gazebo Harmonic: gz-sim 8.15, gz-transport 13.6, Python 3.12) under Docker Desktop on an arm64 Mac; this package built with colcon on top of it.
  • ur3e_world.sdf (physics and joint states every 2 ms), config/config.yaml (ur3e_1), robot_driver_server_launch.py.
  • Load: joint_interface_example.py --ros-args -p robot_topic_name:=/ur3e_1 (each joint ±10° about its start, sinusoids, 100 Hz commands).
  • Measurement: scripts/bridge_lag_monitor.py, 45 s; the analysis skips the first 10 s.
  • CPU: the bridge process's CPU from top -b -d 1, mean after the first 5 s.

The monitor

bridge_lag_monitor.py samples, every 20 ms, the joint positions Gazebo publishes (gz-transport) and those the bridge reports (ROS 2), and prints the shift that best aligns the two series (the delay) and their largest gap.

The monitor is itself a Python process with a Gazebo callback, so it can fall behind in the same way as the bridge. A first version polled ROS 2 in a tight loop (rclpy.spin_once(timeout_sec=0)) and starved its own Gazebo callback: its "Gazebo" positions were stale, which hid most of the Python bridge's lag. The figures in #20's description came from it. The monitor now throttles its Gazebo subscription (SubscribeOptions.msgs_per_sec = 50) and waits in its ROS 2 spin (timeout_sec=0.005). With it, Gazebo's joints show the full ±10° motion in every run below.

Results

Bridge thread_sampling_time_sec Reported vs Gazebo Largest gap CPU (bridge)
Python driver (former) 0.002 no update during the run (≈ 18 s late) 10° (the whole motion) 26–28 %
Python driver (former) 0.01 93–135 ms late 1.2–1.9° 19 %
C++ driver, from sas_robot_driver_ros_gazebo.py 0.002 no measurable lag 0.17° 32 %
C++ driver, from sas_robot_driver_ros_gazebo.py 0.001 no measurable lag 0.17° 43 %
C++ node 0.002 no measurable lag 0.18° 28 %
C++ node 0.001 no measurable lag 0.17° 39 %
C++ node, default launch (this branch) 0.002 0 ms 0.16°
implementation:=python (script, C++ driver through the package), this branch 0.002 0 ms 0.16°

"No measurable lag": the best alignment is no shift; a gap of 0.17° is the monitor's 20 ms sampling of the ±10° motion. With the Python driver at 2 ms, the reported positions stayed at their initial values for the whole run in one measurement, and began to move after about 20 s, about 17 s late, in another.

Two composed bridges

The R820 + UR3e of mmmarinho/random (2024_revolute_curve, curve_revolution_vfi.gazebo.sas_driver_loop): one 13-joint model in Gazebo, two bridges (r820_sim, ur3e_sim) composed by sas_robot_driver_ros_composer_node (longboy_sim), a client sending targets every 2 ms along a 26 s passage that drives several joints at their velocity limits.

Bridges thread_sampling_time_sec Reported vs Gazebo
Python drivers (former) 0.002 seconds late, growing while the robot moves
Python drivers (former) 0.01 12–20 ms late
C++ nodes 0.002 2–4 ms late, within 0.6°

These are qualitative: Gazebo's joints were read by the client itself, a Python process with its own Gazebo callback, which may fall behind as the first monitor did; the single-bridge table above is the reference.

Profile of the Python bridge

py-spy record --native on the bridge (former Python driver, 2 ms), 10 s at 100 Hz:

Thread Samples Where
main (RobotDriverROS.control_loop()) 82 % mostly clock_nanosleep (the loop's sleep between periods)
gz-transport callback (cb_deserialize → joint_states_callback) 3 % parsing the joint-state message
other (waits) 15 % pthread_cond_timedwait

py-spy record --gil --native (only the thread holding the Python interpreter lock): 67 % of the samples are the main thread in clock_nanosleep, inside control_loop().

So sas_robot_driver.RobotDriverROS.control_loop(), a C++ loop called from Python, holds the interpreter lock while it sleeps between periods. The gz-transport thread that delivers Gazebo's joint states to the Python callback needs that lock, gets it rarely, and the driver's cached joint positions fall behind or stop. What did not help (former Python driver, 2 ms):

  • keeping only the latest message in the callback and parsing it when read: no update during the run;
  • throttling the joint-state subscription (msgs_per_sec = 100): ≈ 21 s late;
  • publishing joint states at 50 Hz instead of 500 Hz (JointStatePublisher update_rate): ≈ 13 s late.

A slower loop (0.01 s) gives the callback more chances (93–135 ms). The C++ driver's callback takes no interpreter lock, so it runs as soon as a message arrives, from the C++ node and from Python alike.

Reproducing

# Build (this branch), in sas-full:jazzy, with the submodule
git submodule update --init
colcon build --packages-select sas_robot_driver_gazebo
source install/setup.bash
# Gazebo, the bridge (C++ node; implementation:=python for the script) and the load
cd $(ros2 pkg prefix sas_robot_driver_gazebo)/share/sas_robot_driver_gazebo/sdf
gz sim -s -r ur3e_world.sdf &
ros2 launch sas_robot_driver_gazebo robot_driver_server_launch.py &
ros2 run sas_robot_driver_gazebo joint_interface_example.py --ros-args -p robot_topic_name:=/ur3e_1 &
# The measurement
ros2 run sas_robot_driver_gazebo bridge_lag_monitor.py \
/world/ur3e_world/model/ur3e/model/ur3e_position_controller/model/ur3e/joint_state \
/ur3e_1/get/joint_states 45 \
shoulder_pan_joint shoulder_lift_joint elbow_joint wrist_1_joint wrist_2_joint wrist_3_joint

The former Python driver is sas_robot_driver_gazebo/sas_robot_driver_gazebo.py at commit 2bb09a7 (jazzy before this change); to measure it, run that commit's sas_robot_driver_ros_gazebo.py with the monitor of this branch. For the thread rate, set thread_sampling_time_sec in the configuration file. The profile: pip install py-spy, run the container with --cap-add SYS_PTRACE, then py-spy record -p <bridge pid> -d 10 -r 100 -f raw --native [--gil].