Franka robot state broadcaster frequently fails to update

[scastro-bdai]

We've been using franka_ros2 for a while -- in the most recent Humble release of ros2_control, ros-controls/ros2_control#1969 added some error logs when a controller fails to update.

[controller_manager]: The update call of the following controller returned an error: 'franka_robot_state_broadcaster'

This has exposed a potential issue in the franka_robot_state_broadcaster, since this is now spamming the new error log above to our console; however, the controller performance remains the same. It's likely that this issue has been around for a while, but it simply wasn't being logged before.

We've isolated the failure to this line:

franka_ros2/franka_robot_state_broadcaster/src/franka_robot_state_broadcaster.cpp

Line 128 in 9b0f584

if (realtime_franka_state_publisher && realtime_franka_state_publisher->trylock()) {

Any suggestions on what to look at, or ideas on what may be going on? Thanks!

[bjin-bdai]

I'll add that the control flow logic after realtime_franka_state_publisher->trylock() seems slightly flawed. realtime_franka_state_publisher is a realtime_tools:RealtimePublisher object from the ros2 realtime tool box. The try_lock implementation is ultimately using std::mutex.try_lock() function, which is designed intentionally to fail spuriously sometimes for the purpose of runtime optimization (or it might actually be a valid failure because the publisher thread is owning the lock).

Ultimately I think this is a problem with ROS 2 realtime_toolbox, but knowing this is how it's implemented means that the franka_robot_state_broadcaster ideally has alternative logic when the try_lock() fails (i.e. add some kind of consecutive failure threshold before returning controller_interface::return_type::ERROR;), or just not use the realtime tools all together

Update: clarified mutex comment

[BarisYazici]

I noticed an issue with this when I was writing the unit-tests for the franka_robot_state_broadcaster. https://github.com/frankaemika/franka_ros2/blob/humble/franka_robot_state_broadcaster/test/test_franka_robot_state_broadcaster.cpp#L82
I kept the test as disabled.
Without looking in detail, this issues might be related.