From 67d9c5463a73363c2ce5b94c898be3a296e2fa96 Mon Sep 17 00:00:00 2001 From: Maximilian Schik Date: Thu, 4 Jan 2024 16:57:08 +0100 Subject: [PATCH] [ResourceManager] adds test for uninitialized hardware (#1243) (cherry picked from commit 64c9e2ab9636c98e0383e6dba4fb431f8e0948d5) --- .../test/test_components/test_actuator.cpp | 10 ++++ .../test/test_components/test_components.xml | 18 +++++++ .../test/test_components/test_sensor.cpp | 10 ++++ .../test/test_components/test_system.cpp | 10 ++++ .../test/test_resource_manager.cpp | 40 +++++++++++++++ .../ros2_control_test_assets/descriptions.hpp | 51 +++++++++++++++++++ 6 files changed, 139 insertions(+) diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 7cf55b50d3..2f606b74cb 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -120,5 +120,15 @@ class TestActuator : public ActuatorInterface double max_velocity_command_ = 0.0; }; +class TestUnitilizableActuator : public TestActuator +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + ActuatorInterface::on_init(info); + return CallbackReturn::ERROR; + } +}; + #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestActuator, hardware_interface::ActuatorInterface) +PLUGINLIB_EXPORT_CLASS(TestUnitilizableActuator, hardware_interface::ActuatorInterface) diff --git a/hardware_interface/test/test_components/test_components.xml b/hardware_interface/test/test_components/test_components.xml index 235fec668f..f029d3dd37 100644 --- a/hardware_interface/test/test_components/test_components.xml +++ b/hardware_interface/test/test_components/test_components.xml @@ -17,4 +17,22 @@ Test System + + + + Test Unitilizable Actuator + + + + + + Test Unitilizable Sensor + + + + + + Test Unitilizable System + + diff --git a/hardware_interface/test/test_components/test_sensor.cpp b/hardware_interface/test/test_components/test_sensor.cpp index ae31c73436..4811244138 100644 --- a/hardware_interface/test/test_components/test_sensor.cpp +++ b/hardware_interface/test/test_components/test_sensor.cpp @@ -55,5 +55,15 @@ class TestSensor : public SensorInterface double velocity_state_ = 0.0; }; +class TestUnitilizableSensor : public TestSensor +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + SensorInterface::on_init(info); + return CallbackReturn::ERROR; + } +}; + #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestSensor, hardware_interface::SensorInterface) +PLUGINLIB_EXPORT_CLASS(TestUnitilizableSensor, hardware_interface::SensorInterface) diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp index 3326cb893b..bc7a75df6f 100644 --- a/hardware_interface/test/test_components/test_system.cpp +++ b/hardware_interface/test/test_components/test_system.cpp @@ -123,5 +123,15 @@ class TestSystem : public SystemInterface double configuration_command_ = 0.0; }; +class TestUnitilizableSystem : public TestSystem +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + SystemInterface::on_init(info); + return CallbackReturn::ERROR; + } +}; + #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestSystem, hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(TestUnitilizableSystem, hardware_interface::SystemInterface) diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index c80cdbb5bf..1bb3eb5362 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -69,6 +69,7 @@ class TestableResourceManager : public hardware_interface::ResourceManager FRIEND_TEST(ResourceManagerTest, post_initialization_add_components); FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces); FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle); + FRIEND_TEST(ResourceManagerTest, test_unitilizable_hardware_no_validation); TestableResourceManager() : hardware_interface::ResourceManager() {} @@ -154,6 +155,45 @@ TEST_F(ResourceManagerTest, post_initialization_with_urdf) ASSERT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf)); } +TEST_F(ResourceManagerTest, test_unitilizable_hardware_validation) +{ + // If the the hardware can not be initialized and load_urdf tried to validate the interfaces a + // runtime exception is thrown + TestableResourceManager rm; + ASSERT_THROW( + rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, true), + std::runtime_error); +} + +TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) +{ + // If the the hardware can not be initialized and load_urdf didn't try to validate the interfaces, + // the interface should not show up + TestableResourceManager rm; + EXPECT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); + + // test actuator + EXPECT_FALSE(rm.state_interface_exists("joint1/position")); + EXPECT_FALSE(rm.state_interface_exists("joint1/velocity")); + EXPECT_FALSE(rm.command_interface_exists("joint1/position")); + EXPECT_FALSE(rm.command_interface_exists("joint1/max_velocity")); + + // test sensor + EXPECT_FALSE(rm.state_interface_exists("sensor1/velocity")); + + // test system + EXPECT_FALSE(rm.state_interface_exists("joint2/position")); + EXPECT_FALSE(rm.state_interface_exists("joint2/velocity")); + EXPECT_FALSE(rm.state_interface_exists("joint2/acceleration")); + EXPECT_FALSE(rm.command_interface_exists("joint2/velocity")); + EXPECT_FALSE(rm.command_interface_exists("joint2/max_acceleration")); + EXPECT_FALSE(rm.state_interface_exists("joint3/position")); + EXPECT_FALSE(rm.state_interface_exists("joint3/velocity")); + EXPECT_FALSE(rm.state_interface_exists("joint3/acceleration")); + EXPECT_FALSE(rm.command_interface_exists("joint3/velocity")); + EXPECT_FALSE(rm.command_interface_exists("joint3/max_acceleration")); +} + TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) { // we validate the results manually diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 05ffad00d6..a2f6195c67 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -176,6 +176,55 @@ const auto hardware_resources = )"; +const auto unitilizable_hardware_resources = + R"( + + + test_unitilizable_actuator + + + + + + + + + + + test_unitilizable_sensor + 2 + 2 + + + + + + + + test_unitilizable_system + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; + const auto hardware_resources_missing_state_keys = R"( @@ -406,6 +455,8 @@ const auto diffbot_urdf = const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); +const auto minimal_unitilizable_robot_urdf = + std::string(urdf_head) + std::string(unitilizable_hardware_resources) + std::string(urdf_tail); const auto minimal_robot_missing_state_keys_urdf = std::string(urdf_head) + std::string(hardware_resources_missing_state_keys) +