Skip to content

Commit

Permalink
First attempt to deactivate hardware from read/write return value
Browse files Browse the repository at this point in the history 
This currently does not work as expected. The hardware interfaces get
available and unavailable as expected, but that seems not to be propagated
to the controllers. Maybe that would be a separate step inside the
controller_manager, but my current implementation still doesn't feel clean
as I feel to miss the overview at the moment.
  • Loading branch information
@fmauch
fmauch committed May 3, 2023
1 parent adb8897 commit d356f62
Show file tree
Hide file tree
Showing 2 changed files with 72 additions and 42 deletions.
1 change: 1 addition & 0 deletions hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@ enum class return_type : std::uint8_t
{
OK = 0,
ERROR = 1,
INACTIVE = 2,
};

} // namespace hardware_interface
Expand Down
113 changes: 71 additions & 42 deletions hardware_interface/src/resource_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -167,48 +167,28 @@ class ResourceStorage
}
}

// add command interfaces to available list
for (const auto & interface : hardware_info_map_[hardware.get_name()].command_interfaces)
{
// TODO(destogl): check if interface should be available on configure
auto found_it = std::find(
available_command_interfaces_.begin(), available_command_interfaces_.end(), interface);

if (found_it == available_command_interfaces_.end())
{
available_command_interfaces_.emplace_back(interface);
RCUTILS_LOG_DEBUG_NAMED(
"resource_manager", "(hardware '%s'): '%s' command interface added into available list",
hardware.get_name().c_str(), interface.c_str());
}
else
{
// TODO(destogl): do here error management if interfaces are only partially added into
// "available" list - this should never be the case!
RCUTILS_LOG_WARN_NAMED(
"resource_manager",
"(hardware '%s'): '%s' command interface already in available list."
" This can happen due to multiple calls to 'configure'",
hardware.get_name().c_str(), interface.c_str());
}
}
}
return result;
}

void remove_all_hardware_interfaces_from_available_list(const std::string & hardware_name)
{
// remove all command interfaces from available list
for (const auto & interface : hardware_info_map_[hardware_name].command_interfaces)
remove_all_state_interfaces_from_available_list(hardware_name);
remove_all_command_interfaces_from_available_list(hardware_name);
}

void remove_all_state_interfaces_from_available_list(const std::string & hardware_name)
{
for (const auto & interface : hardware_info_map_[hardware_name].state_interfaces)
{
auto found_it = std::find(
available_command_interfaces_.begin(), available_command_interfaces_.end(), interface);
available_state_interfaces_.begin(), available_state_interfaces_.end(), interface);

if (found_it != available_command_interfaces_.end())
if (found_it != available_state_interfaces_.end())
{
available_command_interfaces_.erase(found_it);
available_state_interfaces_.erase(found_it);
RCUTILS_LOG_DEBUG_NAMED(
"resource_manager", "(hardware '%s'): '%s' command interface removed from available list",
"resource_manager", "(hardware '%s'): '%s' state interface removed from available list",
hardware_name.c_str(), interface.c_str());
}
else
Expand All @@ -217,22 +197,25 @@ class ResourceStorage
// "available" list - this should never be the case!
RCUTILS_LOG_WARN_NAMED(
"resource_manager",
"(hardware '%s'): '%s' command interface not in available list. "
"(hardware '%s'): '%s' state interface not in available list. "
"This should not happen (hint: multiple cleanup calls).",
hardware_name.c_str(), interface.c_str());
}
}
// remove all state interfaces from available list
for (const auto & interface : hardware_info_map_[hardware_name].state_interfaces)
}

void remove_all_command_interfaces_from_available_list(const std::string & hardware_name)
{
for (const auto & interface : hardware_info_map_[hardware_name].command_interfaces)
{
auto found_it = std::find(
available_state_interfaces_.begin(), available_state_interfaces_.end(), interface);
available_command_interfaces_.begin(), available_command_interfaces_.end(), interface);

if (found_it != available_state_interfaces_.end())
if (found_it != available_command_interfaces_.end())
{
available_state_interfaces_.erase(found_it);
available_command_interfaces_.erase(found_it);
RCUTILS_LOG_DEBUG_NAMED(
"resource_manager", "(hardware '%s'): '%s' state interface removed from available list",
"resource_manager", "(hardware '%s'): '%s' command interface removed from available list",
hardware_name.c_str(), interface.c_str());
}
else
Expand All @@ -241,7 +224,7 @@ class ResourceStorage
// "available" list - this should never be the case!
RCUTILS_LOG_WARN_NAMED(
"resource_manager",
"(hardware '%s'): '%s' state interface not in available list. "
"(hardware '%s'): '%s' command interface not in available list. "
"This should not happen (hint: multiple cleanup calls).",
hardware_name.c_str(), interface.c_str());
}
Expand Down Expand Up @@ -288,7 +271,31 @@ class ResourceStorage

if (result)
{
// TODO(destogl): make all command interfaces available (currently are all available)
// add command interfaces to available list
for (const auto & interface : hardware_info_map_[hardware.get_name()].command_interfaces)
{
// TODO(destogl): check if interface should be available on configure
auto found_it = std::find(
available_command_interfaces_.begin(), available_command_interfaces_.end(), interface);

if (found_it == available_command_interfaces_.end())
{
available_command_interfaces_.emplace_back(interface);
RCUTILS_LOG_DEBUG_NAMED(
"resource_manager", "(hardware '%s'): '%s' command interface added into available list",
hardware.get_name().c_str(), interface.c_str());
}
else
{
// TODO(destogl): do here error management if interfaces are only partially added into
// "available" list - this should never be the case!
RCUTILS_LOG_WARN_NAMED(
"resource_manager",
"(hardware '%s'): '%s' command interface already in available list."
" This can happen due to multiple calls to 'configure'",
hardware.get_name().c_str(), interface.c_str());
}
}
}

return result;
Expand All @@ -303,6 +310,8 @@ class ResourceStorage

if (result)
{
RCUTILS_LOG_INFO_NAMED("resource_manager", "Hardware '%s' has been deactivated", hardware.get_name().c_str());
remove_all_command_interfaces_from_available_list(hardware.get_name());
// TODO(destogl): make all command interfaces unavailable that should be present only
// when active (currently are all available) also at inactive
}
Expand Down Expand Up @@ -1072,12 +1081,25 @@ HardwareReadWriteStatus ResourceManager::read(
{
for (auto & component : components)
{
if (component.read(time, period) != return_type::OK)
auto ret_val = component.read(time, period);
if (ret_val == return_type::ERROR)
{
read_write_status.ok = false;
read_write_status.failed_hardware_names.push_back(component.get_name());
resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name());
}
else if (ret_val == return_type::INACTIVE)
{
using lifecycle_msgs::msg::State;
rclcpp_lifecycle::State state(State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE);
set_component_state(component.get_name(), state);
}
else
{
using lifecycle_msgs::msg::State;
rclcpp_lifecycle::State state(State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE);
set_component_state(component.get_name(), state);
}
}
};

Expand All @@ -1100,12 +1122,19 @@ HardwareReadWriteStatus ResourceManager::write(
{
for (auto & component : components)
{
if (component.write(time, period) != return_type::OK)
auto ret_val = component.write(time, period);
if (ret_val == return_type::ERROR)
{
read_write_status.ok = false;
read_write_status.failed_hardware_names.push_back(component.get_name());
resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name());
}
else if (ret_val == return_type::INACTIVE)
{
using lifecycle_msgs::msg::State;
rclcpp_lifecycle::State state(State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE);
set_component_state(component.get_name(), state);
}
}
};

Expand Down

0 comments on commit d356f62

Please sign in to comment.