Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add additional checks for non existing and not available interfaces. #1218

Merged
Show file tree
Hide file tree
Changes from 9 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions hardware_interface/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -113,6 +113,10 @@ if(BUILD_TESTING)
target_link_libraries(test_resource_manager hardware_interface)
ament_target_dependencies(test_resource_manager ros2_control_test_assets)

ament_add_gmock(test_resource_manager_prepare_perform_switch test/test_resource_manager_prepare_perform_switch.cpp)
target_link_libraries(test_resource_manager_prepare_perform_switch hardware_interface)
ament_target_dependencies(test_resource_manager_prepare_perform_switch ros2_control_test_assets)

ament_add_gmock(test_generic_system test/mock_components/test_generic_system.cpp)
target_include_directories(test_generic_system PRIVATE include)
target_link_libraries(test_generic_system hardware_interface)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -331,7 +331,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
* by default
* \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
* \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping.
* \return true if switch can be prepared, false if a component rejects switch request.
* \return true if switch can be prepared; false if a component rejects switch request, and if
* at least one of the input interfaces are not existing or not available (i.e., component is not
* in ACTIVE or INACTIVE state).
*/
bool prepare_command_mode_switch(
const std::vector<std::string> & start_interfaces,
Expand All @@ -344,6 +346,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
* \note this is intended for mode-switching when a hardware interface needs to change
* control mode depending on which command interface is claimed.
* \note this is for realtime switching of the command interface.
* \note is is assumed that `prepare_command_mode_switch` is called just before this methods
destogl marked this conversation as resolved.
Show resolved Hide resolved
* with the same input arguments.
* \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
* \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping.
* \return true if switch is performed, false if a component rejects switching.
Expand Down
135 changes: 106 additions & 29 deletions hardware_interface/src/resource_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -287,6 +287,7 @@ class ResourceStorage

if (result)
{
remove_all_hardware_interfaces_from_available_list(hardware.get_name());
async_component_threads_.erase(hardware.get_name());
// TODO(destogl): change this - deimport all things if there is there are interfaces there
// deimport_non_movement_command_interfaces(hardware);
bmagyar marked this conversation as resolved.
Show resolved Hide resolved
Expand Down Expand Up @@ -1072,6 +1073,12 @@ bool ResourceManager::prepare_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces)
{
// When only broadcaster is activated then this lists are empty
if (start_interfaces.empty() && stop_interfaces.empty())
{
return true;
}

auto interfaces_to_string = [&]()
{
std::stringstream ss;
Expand All @@ -1090,55 +1097,125 @@ bool ResourceManager::prepare_command_mode_switch(
return ss.str();
};

for (auto & component : resource_storage_->actuators_)
// Check if interface exists
std::stringstream ss_not_existing;
ss_not_existing << "Not existing: " << std::endl << "[" << std::endl;
auto check_exist = [&](const std::vector<std::string> & list_to_check)
{
if (return_type::OK != component.prepare_command_mode_switch(start_interfaces, stop_interfaces))
bool all_exist = true;
for (const auto & interface : list_to_check)
{
RCUTILS_LOG_ERROR_NAMED(
"resource_manager", "Component '%s' did not accept new command resource combination: \n %s",
component.get_name().c_str(), interfaces_to_string().c_str());
return false;
if (!command_interface_exists(interface))
{
all_exist = false;
ss_not_existing << " " << interface << std::endl;
}
}
return all_exist;
};
if (!(check_exist(start_interfaces) && check_exist(stop_interfaces)))
{
ss_not_existing << "]" << std::endl;
RCUTILS_LOG_ERROR_NAMED(
"resource_manager", "Not acceptable command interfaces combination: \n%s%s",
interfaces_to_string().c_str(), ss_not_existing.str().c_str());
return false;
}
for (auto & component : resource_storage_->systems_)

// Check if interfaces are available
std::stringstream ss_not_available;
ss_not_available << "Not available: " << std::endl << "[" << std::endl;
auto check_available = [&](const std::vector<std::string> & list_to_check)
{
if (return_type::OK != component.prepare_command_mode_switch(start_interfaces, stop_interfaces))
bool all_available = true;
for (const auto & interface : list_to_check)
{
RCUTILS_LOG_ERROR_NAMED(
"resource_manager", "Component '%s' did not accept new command resource combination: \n %s",
component.get_name().c_str(), interfaces_to_string().c_str());
return false;
if (!command_interface_is_available(interface))
{
all_available = false;
ss_not_available << " " << interface << std::endl;
}
}
return all_available;
};
if (!(check_available(start_interfaces) && check_available(stop_interfaces)))
{
ss_not_available << "]" << std::endl;
RCUTILS_LOG_ERROR_NAMED(
"resource_manager", "Not acceptable command interfaces combination: \n%s%s",
interfaces_to_string().c_str(), ss_not_available.str().c_str());
return false;
}
return true;

auto call_method_on_components =
destogl marked this conversation as resolved.
Show resolved Hide resolved
[&start_interfaces, &stop_interfaces, &interfaces_to_string](auto & components)
{
bool ret = true;
for (auto & component : components)
{
if (
component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
if (
return_type::OK !=
component.prepare_command_mode_switch(start_interfaces, stop_interfaces))
{
RCUTILS_LOG_ERROR_NAMED(
"resource_manager",
"Component '%s' did not accept command interfaces combination: \n%s",
component.get_name().c_str(), interfaces_to_string().c_str());
ret = false;
}
}
}
return ret;
};

const bool actuators_result = call_method_on_components(resource_storage_->actuators_);
const bool systems_result = call_method_on_components(resource_storage_->systems_);
destogl marked this conversation as resolved.
Show resolved Hide resolved

return actuators_result && systems_result;
}

// CM API: Called in "update"-thread
bool ResourceManager::perform_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces)
{
for (auto & component : resource_storage_->actuators_)
// When only broadcaster is activated then this lists are empty
if (start_interfaces.empty() && stop_interfaces.empty())
{
if (return_type::OK != component.perform_command_mode_switch(start_interfaces, stop_interfaces))
{
RCUTILS_LOG_ERROR_NAMED(
"resource_manager", "Component '%s' could not perform switch",
component.get_name().c_str());
return false;
}
return true;
}
for (auto & component : resource_storage_->systems_)

auto call_method_on_components = [&start_interfaces, &stop_interfaces](auto & components)
destogl marked this conversation as resolved.
Show resolved Hide resolved
{
if (return_type::OK != component.perform_command_mode_switch(start_interfaces, stop_interfaces))
bool ret = true;
for (auto & component : components)
{
RCUTILS_LOG_ERROR_NAMED(
"resource_manager", "Component '%s' could not perform switch",
component.get_name().c_str());
return false;
if (
component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
if (
return_type::OK !=
component.perform_command_mode_switch(start_interfaces, stop_interfaces))
{
RCUTILS_LOG_ERROR_NAMED(
"resource_manager", "Component '%s' could not perform switch",
component.get_name().c_str());
ret = false;
}
}
}
}
return true;
return ret;
};

const bool actuators_result = call_method_on_components(resource_storage_->actuators_);
const bool systems_result = call_method_on_components(resource_storage_->systems_);
destogl marked this conversation as resolved.
Show resolved Hide resolved

return actuators_result && systems_result;
}

// CM API: Called in "callback/slow"-thread
Expand Down
Loading
Loading