[RM] Decouple read/write cycles of each component with mutex to not block other components

[saikishor]

Right now, how it is implemented if one HW component takes longer to activate/deactivate it will completely block the read and write cycles. If there are more critical components that expect command on every cycle, they would be affected with the current approach.

This part of the code is called with the non-RT thread using the CM services

ros2_control/hardware_interface/src/resource_manager.cpp

Lines 1724 to 1739 in 5cd1a43

	std::lock_guard<std::recursive_mutex> guard(resources_lock_);
	bool found = find_set_component_state(
	std::bind(&ResourceStorage::set_component_state<Actuator>, resource_storage_.get(), _1, _2),
	resource_storage_->actuators_);
	if (!found)
	{
	found = find_set_component_state(
	std::bind(&ResourceStorage::set_component_state<Sensor>, resource_storage_.get(), _1, _2),
	resource_storage_->sensors_);
	}
	if (!found)
	{
	found = find_set_component_state(
	std::bind(&ResourceStorage::set_component_state<System>, resource_storage_.get(), _1, _2),
	resource_storage_->systems_);
	}

We have this part of the code in the RT loop

ros2_control/hardware_interface/src/resource_manager.cpp

Lines 1770 to 1773 in 5cd1a43

	HardwareReadWriteStatus ResourceManager::read(
	const rclcpp::Time & time, const rclcpp::Duration & period)
	{
	std::lock_guard<std::recursive_mutex> guard(resources_lock_);

ros2_control/hardware_interface/src/resource_manager.cpp

Lines 1831 to 1834 in 5cd1a43

	HardwareReadWriteStatus ResourceManager::write(
	const rclcpp::Time & time, const rclcpp::Duration & period)
	{
	std::lock_guard<std::recursive_mutex> guard(resources_lock_);

This PR proposes to have it within the Hardware Component so it is local to it's own and while activating or deactivating, we simply skip the cycles of read/write of that particular component to avoid issues, and the rest of the components can continue with their read/write cycles

[codecov]

Codecov Report

Attention: Patch coverage is 70.00000% with 15 lines in your changes missing coverage. Please review.

Project coverage is 87.93%. Comparing base (a1ad523) to head (7fbf13e).

Files	Patch %	Lines
hardware_interface/src/actuator.cpp	66.66%	4 Missing and 2 partials ⚠️
hardware_interface/src/system.cpp	66.66%	4 Missing and 2 partials ⚠️
hardware_interface/src/sensor.cpp	78.57%	2 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1646      +/-   ##
==========================================
- Coverage   88.07%   87.93%   -0.15%     
==========================================
  Files         108      108              
  Lines       10023    10070      +47     
  Branches      892      897       +5     
==========================================
+ Hits         8828     8855      +27     
- Misses        876      888      +12     
- Partials      319      327       +8     

Flag	Coverage Δ
unittests	87.93% <70.00%> (-0.15%)	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files	Coverage Δ
..._interface/include/hardware_interface/actuator.hpp	100.00% <ø> (ø)
.../include/hardware_interface/actuator_interface.hpp	95.23% <ø> (-4.77%)	⬇️
...re_interface/include/hardware_interface/sensor.hpp	50.00% <ø> (-16.67%)	⬇️
...ce/include/hardware_interface/sensor_interface.hpp	94.11% <ø> (-5.89%)	⬇️
...re_interface/include/hardware_interface/system.hpp	100.00% <ø> (ø)
...ce/include/hardware_interface/system_interface.hpp	100.00% <ø> (ø)
hardware_interface/src/resource_manager.cpp	73.80% <ø> (-0.07%)	⬇️
hardware_interface/src/sensor.cpp	70.53% <78.57%> (+0.12%)	⬆️
hardware_interface/src/actuator.cpp	73.13% <66.66%> (-1.87%)	⬇️
hardware_interface/src/system.cpp	73.13% <66.66%> (-1.87%)	⬇️

[christophfroehlich]

seams reasonable

[christophfroehlich]

why don't we need this in the other interface.hpp files?

[saikishor]

I've added them in other interfaces.hpp as well

[christophfroehlich]

why don't we need this here? or why do we need this lock in other methods of this file?

[saikishor]

The mutex from here is from the Resource Manager, it is used along with non-RT threads, as it is used this way, there is most likely a chance that it can block the read and write cycles.

We have added in other methods because, when this happens, the read cycles of that particular interface can be skipped instead of waiting for the cycle as it is configuring itself to be activated or deactivated