diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e51b95040c..5eeb8509cf 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1352,9 +1352,11 @@ void ResourceManager::publish_all_interface_values() const interface_values.states = state_interface_values; interface_values.commands = command_interface_values; - rt_interface_values_publisher_->trylock(); - rt_interface_values_publisher_->msg_ = interface_values; - rt_interface_values_publisher_->unlockAndPublish(); + if(rt_interface_values_publisher_->trylock()) + { + rt_interface_values_publisher_->msg_ = interface_values; + rt_interface_values_publisher_->unlockAndPublish(); + } } // CM API: Called in "update"-thread