diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp index f12ab9e96b..20af889c81 100644 --- a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -22,7 +22,6 @@ #include "controller_manager_test_common.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" #include "test_controller/test_controller.hpp" using ::testing::_; @@ -163,6 +162,10 @@ class TestControllerManagerWithTestableCM {}, strictness); } + // values to set to hardware to simulate failure on read and write + static constexpr double READ_FAIL_VALUE = 28282828.0; + static constexpr double WRITE_FAIL_VALUE = 23232323.0; + static constexpr char TEST_CONTROLLER_ACTUATOR_NAME[] = "test_controller_actuator"; static constexpr char TEST_CONTROLLER_SYSTEM_NAME[] = "test_controller_system"; static constexpr char TEST_BROADCASTER_ALL_NAME[] = "test_broadcaster_all"; @@ -255,7 +258,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er // Simulate error in read() on TEST_ACTUATOR_HARDWARE_NAME by setting first command interface to // READ_FAIL_VALUE - test_controller_actuator->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE; + test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE; { auto new_counter = test_controller_actuator->internal_counter + 1; EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); @@ -324,8 +327,8 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er // Simulate error in read() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME // by setting first command interface to READ_FAIL_VALUE - test_controller_actuator->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE; - test_controller_system->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE; + test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE; + test_controller_system->set_first_command_interface_value_to = READ_FAIL_VALUE; { auto previous_counter_lower = test_controller_actuator->internal_counter + 1; auto previous_counter_higher = test_controller_system->internal_counter + 1; @@ -447,7 +450,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME by setting first command interface to // WRITE_FAIL_VALUE - test_controller_actuator->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE; + test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE; { auto new_counter = test_controller_actuator->internal_counter + 1; EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); @@ -516,8 +519,8 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME // by setting first command interface to WRITE_FAIL_VALUE - test_controller_actuator->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE; - test_controller_system->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE; + test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE; + test_controller_system->set_first_command_interface_value_to = WRITE_FAIL_VALUE; { auto previous_counter_lower = test_controller_actuator->internal_counter + 1; auto previous_counter_higher = test_controller_system->internal_counter + 1; diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 2108bab17c..939668593d 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -84,9 +84,7 @@ if(BUILD_TESTING) test/test_components/test_system.cpp) target_link_libraries(test_components hardware_interface) ament_target_dependencies(test_components - pluginlib - ros2_control_test_assets - ) + pluginlib) install(TARGETS test_components DESTINATION lib ) diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp index d39dee2c64..5c3ea22ca0 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp @@ -23,7 +23,6 @@ enum class return_type : std::uint8_t { OK = 0, ERROR = 1, - DEACTIVATE = 2, }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 5c5d5af3e6..3bb29d1d30 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1255,24 +1255,12 @@ HardwareReadWriteStatus ResourceManager::read( { for (auto & component : components) { - auto ret_val = component.read(time, period); - if (ret_val == return_type::ERROR) + if (component.read(time, period) != return_type::OK) { 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::DEACTIVATE) - { - resource_storage_->deactivate_hardware(component); - } - // If desired: automatic re-activation. We could add a flag for this... - // 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); - // } } }; @@ -1295,17 +1283,12 @@ HardwareReadWriteStatus ResourceManager::write( { for (auto & component : components) { - auto ret_val = component.write(time, period); - if (ret_val == return_type::ERROR) + if (component.write(time, period) != return_type::OK) { 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::DEACTIVATE) - { - resource_storage_->deactivate_hardware(component); - } } }; diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 7cf55b50d3..5f9c09e95e 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -16,7 +16,6 @@ #include #include "hardware_interface/actuator_interface.hpp" -#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" using hardware_interface::ActuatorInterface; using hardware_interface::CommandInterface; @@ -77,17 +76,12 @@ class TestActuator : public ActuatorInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on read - if (velocity_command_ == test_constants::READ_FAIL_VALUE) + if (velocity_command_ == 28282828.0) { // reset value to get out from error on the next call - simplifies CM tests velocity_command_ = 0.0; return return_type::ERROR; } - // simulate deactivate on read - if (velocity_command_ == test_constants::READ_DEACTIVATE_VALUE) - { - return return_type::DEACTIVATE; - } // The next line is for the testing purposes. We need value to be changed to be sure that // the feedback from hardware to controllers in the chain is working as it should. // This makes value checks clearer and confirms there is no "state = command" line or some @@ -99,17 +93,12 @@ class TestActuator : public ActuatorInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on write - if (velocity_command_ == test_constants::WRITE_FAIL_VALUE) + if (velocity_command_ == 23232323.0) { // reset value to get out from error on the next call - simplifies CM tests velocity_command_ = 0.0; return return_type::ERROR; } - // simulate deactivate on write - if (velocity_command_ == test_constants::WRITE_DEACTIVATE_VALUE) - { - return return_type::DEACTIVATE; - } return return_type::OK; } diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp index 3326cb893b..7477734cff 100644 --- a/hardware_interface/test/test_components/test_system.cpp +++ b/hardware_interface/test/test_components/test_system.cpp @@ -18,7 +18,6 @@ #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" using hardware_interface::CommandInterface; using hardware_interface::return_type; @@ -82,34 +81,24 @@ class TestSystem : public SystemInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on read - if (velocity_command_[0] == test_constants::READ_FAIL_VALUE) + if (velocity_command_[0] == 28282828) { // reset value to get out from error on the next call - simplifies CM tests velocity_command_[0] = 0.0; return return_type::ERROR; } - // simulate deactivate on read - if (velocity_command_[0] == test_constants::READ_DEACTIVATE_VALUE) - { - return return_type::DEACTIVATE; - } return return_type::OK; } return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on write - if (velocity_command_[0] == test_constants::WRITE_FAIL_VALUE) + if (velocity_command_[0] == 23232323) { // reset value to get out from error on the next call - simplifies CM tests velocity_command_[0] = 0.0; return return_type::ERROR; } - // simulate deactivate on write - if (velocity_command_[0] == test_constants::WRITE_DEACTIVATE_VALUE) - { - return return_type::DEACTIVATE; - } return return_type::OK; } diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index c80cdbb5bf..aedf2b862d 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -28,7 +28,6 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp_lifecycle/state.hpp" #include "ros2_control_test_assets/descriptions.hpp" -#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME; @@ -1539,122 +1538,6 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest } } - void check_read_or_write_deactivate( - FunctionT method_that_deactivates, FunctionT other_method, const double deactivate_value) - { - // define state to set components to - rclcpp_lifecycle::State state_active( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - hardware_interface::lifecycle_state_names::ACTIVE); - - // deactivate for TEST_ACTUATOR_HARDWARE_NAME - claimed_itfs[0].set_value(deactivate_value); - claimed_itfs[1].set_value(deactivate_value - 10.0); - { - // deactivate on error - auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); - EXPECT_TRUE(ok); - EXPECT_TRUE(failed_hardware_names.empty()); - auto status_map = rm->get_components_status(); - EXPECT_EQ( - status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - EXPECT_EQ( - status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - check_if_interface_available(true, true); - - // reactivate - rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active); - status_map = rm->get_components_status(); - EXPECT_EQ( - status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - EXPECT_EQ( - status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - check_if_interface_available(true, true); - } - // write is sill OK - { - auto [ok, failed_hardware_names] = other_method(time, duration); - EXPECT_TRUE(ok); - EXPECT_TRUE(failed_hardware_names.empty()); - check_if_interface_available(true, true); - } - - // deactivate for TEST_SYSTEM_HARDWARE_NAME - claimed_itfs[0].set_value(deactivate_value - 10.0); - claimed_itfs[1].set_value(deactivate_value); - { - // deactivate on flag - auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); - EXPECT_TRUE(ok); - EXPECT_TRUE(failed_hardware_names.empty()); - auto status_map = rm->get_components_status(); - EXPECT_EQ( - status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - EXPECT_EQ( - status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - check_if_interface_available(true, true); - // re-activate - rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active); - status_map = rm->get_components_status(); - EXPECT_EQ( - status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - EXPECT_EQ( - status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - check_if_interface_available(true, true); - } - // write is sill OK - { - auto [ok, failed_hardware_names] = other_method(time, duration); - EXPECT_TRUE(ok); - EXPECT_TRUE(failed_hardware_names.empty()); - check_if_interface_available(true, true); - } - - // deactivate both, TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME - claimed_itfs[0].set_value(deactivate_value); - claimed_itfs[1].set_value(deactivate_value); - { - // deactivate on flag - auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); - EXPECT_TRUE(ok); - EXPECT_TRUE(failed_hardware_names.empty()); - auto status_map = rm->get_components_status(); - EXPECT_EQ( - status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - EXPECT_EQ( - status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - check_if_interface_available(true, true); - // re-activate - rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active); - rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active); - status_map = rm->get_components_status(); - EXPECT_EQ( - status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - EXPECT_EQ( - status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - check_if_interface_available(true, true); - } - // write is sill OK - { - auto [ok, failed_hardware_names] = other_method(time, duration); - EXPECT_TRUE(ok); - EXPECT_TRUE(failed_hardware_names.empty()); - check_if_interface_available(true, true); - } - } - public: std::shared_ptr rm; std::vector claimed_itfs; @@ -1663,6 +1546,8 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest const rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.01); // values to set to hardware to simulate failure on read and write + static constexpr double READ_FAIL_VALUE = 28282828.0; + static constexpr double WRITE_FAIL_VALUE = 23232323.0; }; TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_read) @@ -1673,7 +1558,7 @@ TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_read) // check read methods failures check_read_or_write_failure( std::bind(&TestableResourceManager::read, rm, _1, _2), - std::bind(&TestableResourceManager::write, rm, _1, _2), test_constants::READ_FAIL_VALUE); + std::bind(&TestableResourceManager::write, rm, _1, _2), READ_FAIL_VALUE); } TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write) @@ -1684,29 +1569,7 @@ TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write) // check write methods failures check_read_or_write_failure( std::bind(&TestableResourceManager::write, rm, _1, _2), - std::bind(&TestableResourceManager::read, rm, _1, _2), test_constants::WRITE_FAIL_VALUE); -} - -TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_read) -{ - setup_resource_manager_and_do_initial_checks(); - - using namespace std::placeholders; - // check read methods failures - check_read_or_write_deactivate( - std::bind(&TestableResourceManager::read, rm, _1, _2), - std::bind(&TestableResourceManager::write, rm, _1, _2), test_constants::READ_DEACTIVATE_VALUE); -} - -TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_write) -{ - setup_resource_manager_and_do_initial_checks(); - - using namespace std::placeholders; - // check write methods failures - check_read_or_write_deactivate( - std::bind(&TestableResourceManager::write, rm, _1, _2), - std::bind(&TestableResourceManager::read, rm, _1, _2), test_constants::WRITE_DEACTIVATE_VALUE); + std::bind(&TestableResourceManager::read, rm, _1, _2), WRITE_FAIL_VALUE); } TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware) diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp deleted file mode 100644 index eb2bbf24c7..0000000000 --- a/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright (c) 2023, FZI Forschungszentrum Informatik -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -/// \author: Felix Exner - -#ifndef ROS2_CONTROL_TEST_ASSETS__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_ -#define ROS2_CONTROL_TEST_ASSETS__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_ -namespace test_constants -{ -/// Constants defining special values used inside tests to trigger things like deactivate or errors -/// on read/write. -constexpr double READ_FAIL_VALUE = 28282828.0; -constexpr double WRITE_FAIL_VALUE = 23232323.0; -constexpr double READ_DEACTIVATE_VALUE = 29292929.0; -constexpr double WRITE_DEACTIVATE_VALUE = 24242424.0; -} // namespace test_constants -#endif // ROS2_CONTROL_TEST_ASSETS__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_