|
sas
Modularised monitoring, logging, and control of robots.
|
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.
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.
| 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.
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.
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):
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.
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].