Skip to content
Draft
Show file tree
Hide file tree
Changes from all 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
96 changes: 69 additions & 27 deletions hardware_interface/test/test_component_interfaces.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/executors/multi_threaded_executor.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "ros2_control_test_assets/components_urdfs.hpp"
Expand Down Expand Up @@ -778,18 +779,33 @@ class DummySystemPreparePerform : public hardware_interface::SystemInterface
};

} // namespace test_components
class TestComponentInterfaces : public ::testing::Test
{
protected:
void SetUp() override
{
if (!rclcpp::ok())
{
rclcpp::init(0, nullptr);
}
}

void TearDown() override { rclcpp::shutdown(); }
std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor_ = nullptr;
};
// BEGIN (Handle export change): for backward compatibility
TEST(TestComponentInterfaces, dummy_actuator)
TEST_F(TestComponentInterfaces, dummy_actuator)
{
hardware_interface::Actuator actuator_hw(std::make_unique<test_components::DummyActuator>());

hardware_interface::HardwareInfo mock_hw_info{};
hardware_interface::HardwareInfo mock_hw_info;
mock_hw_info.name = "mock_hw";
rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("test_actuator_components");
hardware_interface::HardwareComponentParams params;
params.hardware_info = mock_hw_info;
params.clock = node->get_clock();
params.logger = node->get_logger();
params.executor = executor_;
auto state = actuator_hw.initialize(params);
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id());
EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label());
Expand Down Expand Up @@ -879,7 +895,7 @@ TEST(TestComponentInterfaces, dummy_actuator)
}
// END

TEST(TestComponentInterfaces, dummy_actuator_default)
TEST_F(TestComponentInterfaces, dummy_actuator_default)
{
hardware_interface::Actuator actuator_hw(
std::make_unique<test_components::DummyActuatorDefault>());
Expand All @@ -895,6 +911,7 @@ TEST(TestComponentInterfaces, dummy_actuator_default)
params.hardware_info = dummy_actuator;
params.clock = node->get_clock();
params.logger = node->get_logger();
params.executor = executor_;
auto state = actuator_hw.initialize(params);

EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id());
Expand Down Expand Up @@ -1008,16 +1025,18 @@ TEST(TestComponentInterfaces, dummy_actuator_default)
}

// BEGIN (Handle export change): for backward compatibility
TEST(TestComponentInterfaces, dummy_sensor)
TEST_F(TestComponentInterfaces, dummy_sensor)
{
hardware_interface::Sensor sensor_hw(std::make_unique<test_components::DummySensor>());

hardware_interface::HardwareInfo mock_hw_info{};
hardware_interface::HardwareInfo mock_hw_info;
mock_hw_info.name = "mock_hw";
rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("test_sensor_components");
hardware_interface::HardwareComponentParams params;
params.hardware_info = mock_hw_info;
params.clock = node->get_clock();
params.logger = node->get_logger();
params.executor = executor_;
auto state = sensor_hw.initialize(params);
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id());
EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label());
Expand Down Expand Up @@ -1045,7 +1064,7 @@ TEST(TestComponentInterfaces, dummy_sensor)
}
// END

TEST(TestComponentInterfaces, dummy_sensor_default)
TEST_F(TestComponentInterfaces, dummy_sensor_default)
{
hardware_interface::Sensor sensor_hw(std::make_unique<test_components::DummySensorDefault>());

Expand All @@ -1061,6 +1080,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default)
params.hardware_info = voltage_sensor_res;
params.clock = node->get_clock();
params.logger = node->get_logger();
params.executor = executor_;
auto state = sensor_hw.initialize(params);
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id());
EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label());
Expand Down Expand Up @@ -1092,7 +1112,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default)
EXPECT_EQ(0x666, state_interfaces[si_sens1_vol]->get_optional().value());
}

TEST(TestComponentInterfaces, dummy_sensor_default_joint)
TEST_F(TestComponentInterfaces, dummy_sensor_default_joint)
{
hardware_interface::Sensor sensor_hw(
std::make_unique<test_components::DummySensorJointDefault>());
Expand All @@ -1109,6 +1129,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_joint)
params.hardware_info = sensor_res;
params.clock = node->get_clock();
params.logger = node->get_logger();
params.executor = executor_;
auto state = sensor_hw.initialize(params);
ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id());
ASSERT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label());
Expand Down Expand Up @@ -1151,16 +1172,18 @@ TEST(TestComponentInterfaces, dummy_sensor_default_joint)
}

// BEGIN (Handle export change): for backward compatibility
TEST(TestComponentInterfaces, dummy_system)
TEST_F(TestComponentInterfaces, dummy_system)
{
hardware_interface::System system_hw(std::make_unique<test_components::DummySystem>());

hardware_interface::HardwareInfo mock_hw_info{};
hardware_interface::HardwareInfo mock_hw_info;
mock_hw_info.name = "mock_hw";
rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("test_system_components");
hardware_interface::HardwareComponentParams params;
params.hardware_info = mock_hw_info;
params.clock = node->get_clock();
params.logger = node->get_logger();
params.executor = executor_;
auto state = system_hw.initialize(params);
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id());
EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label());
Expand Down Expand Up @@ -1284,7 +1307,7 @@ TEST(TestComponentInterfaces, dummy_system)
}
// END

TEST(TestComponentInterfaces, dummy_system_default)
TEST_F(TestComponentInterfaces, dummy_system_default)
{
hardware_interface::System system_hw(std::make_unique<test_components::DummySystemDefault>());

Expand All @@ -1300,6 +1323,7 @@ TEST(TestComponentInterfaces, dummy_system_default)
params.hardware_info = dummy_system;
params.clock = node->get_clock();
params.logger = node->get_logger();
params.executor = executor_;
auto state = system_hw.initialize(params);
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id());
EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label());
Expand Down Expand Up @@ -1500,16 +1524,19 @@ TEST(TestComponentInterfaces, dummy_system_default)
EXPECT_EQ(hardware_interface::return_type::OK, system_hw.perform_command_mode_switch({}, {}));
}

TEST(TestComponentInterfaces, dummy_command_mode_system)
TEST_F(TestComponentInterfaces, dummy_command_mode_system)
{
hardware_interface::System system_hw(
std::make_unique<test_components::DummySystemPreparePerform>());
hardware_interface::HardwareInfo mock_hw_info{};

hardware_interface::HardwareInfo mock_hw_info;
mock_hw_info.name = "mock_hw";
rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("test_system_components");
hardware_interface::HardwareComponentParams params;
params.hardware_info = mock_hw_info;
params.clock = node->get_clock();
params.logger = node->get_logger();
params.executor = executor_;
auto state = system_hw.initialize(params);
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id());
EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label());
Expand Down Expand Up @@ -1537,16 +1564,18 @@ TEST(TestComponentInterfaces, dummy_command_mode_system)
}

// BEGIN (Handle export change): for backward compatibility
TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior)
TEST_F(TestComponentInterfaces, dummy_actuator_read_error_behavior)
{
hardware_interface::Actuator actuator_hw(std::make_unique<test_components::DummyActuator>());

hardware_interface::HardwareInfo mock_hw_info{};
hardware_interface::HardwareInfo mock_hw_info;
mock_hw_info.name = "mock_hw";
rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("test_actuator_components");
hardware_interface::HardwareComponentParams params;
params.hardware_info = mock_hw_info;
params.clock = node->get_clock();
params.logger = node->get_logger();
params.executor = executor_;
auto state = actuator_hw.initialize(params);
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id());
EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label());
Expand Down Expand Up @@ -1602,7 +1631,7 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior)
}
// END

TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior)
TEST_F(TestComponentInterfaces, dummy_actuator_default_read_error_behavior)
{
hardware_interface::Actuator actuator_hw(
std::make_unique<test_components::DummyActuatorDefault>());
Expand All @@ -1619,6 +1648,7 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior)
params.hardware_info = dummy_actuator;
params.clock = node->get_clock();
params.logger = node->get_logger();
params.executor = executor_;
auto state = actuator_hw.initialize(params);

EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id());
Expand Down Expand Up @@ -1678,16 +1708,18 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior)
}

// BEGIN (Handle export change): for backward compatibility
TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior)
TEST_F(TestComponentInterfaces, dummy_actuator_write_error_behavior)
{
hardware_interface::Actuator actuator_hw(std::make_unique<test_components::DummyActuator>());

hardware_interface::HardwareInfo mock_hw_info{};
hardware_interface::HardwareInfo mock_hw_info;
mock_hw_info.name = "mock_hw";
rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("test_actuator_components");
hardware_interface::HardwareComponentParams params;
params.hardware_info = mock_hw_info;
params.clock = node->get_clock();
params.logger = node->get_logger();
params.executor = executor_;
auto state = actuator_hw.initialize(params);
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id());
EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label());
Expand Down Expand Up @@ -1743,7 +1775,7 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior)
}
// END

TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior)
TEST_F(TestComponentInterfaces, dummy_actuator_default_write_error_behavior)
{
hardware_interface::Actuator actuator_hw(
std::make_unique<test_components::DummyActuatorDefault>());
Expand All @@ -1760,6 +1792,7 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior)
params.hardware_info = dummy_actuator;
params.clock = node->get_clock();
params.logger = node->get_logger();
params.executor = executor_;
auto state = actuator_hw.initialize(params);
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id());
EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label());
Expand Down Expand Up @@ -1818,16 +1851,18 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior)
}

// BEGIN (Handle export change): for backward compatibility
TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior)
TEST_F(TestComponentInterfaces, dummy_sensor_read_error_behavior)
{
hardware_interface::Sensor sensor_hw(std::make_unique<test_components::DummySensor>());

hardware_interface::HardwareInfo mock_hw_info{};
hardware_interface::HardwareInfo mock_hw_info;
mock_hw_info.name = "mock_hw";
rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("test_sensor_components");
hardware_interface::HardwareComponentParams params;
params.hardware_info = mock_hw_info;
params.clock = node->get_clock();
params.logger = node->get_logger();
params.executor = executor_;
auto state = sensor_hw.initialize(params);

auto state_interfaces = sensor_hw.export_state_interfaces();
Expand Down Expand Up @@ -1888,7 +1923,7 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior)
}
// END

TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior)
TEST_F(TestComponentInterfaces, dummy_sensor_default_read_error_behavior)
{
hardware_interface::Sensor sensor_hw(std::make_unique<test_components::DummySensorDefault>());

Expand All @@ -1904,6 +1939,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior)
params.hardware_info = voltage_sensor_res;
params.clock = node->get_clock();
params.logger = node->get_logger();
params.executor = executor_;
auto state = sensor_hw.initialize(params);

auto state_interfaces = sensor_hw.export_state_interfaces();
Expand Down Expand Up @@ -1953,16 +1989,18 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior)
}

// BEGIN (Handle export change): for backward compatibility
TEST(TestComponentInterfaces, dummy_system_read_error_behavior)
TEST_F(TestComponentInterfaces, dummy_system_read_error_behavior)
{
hardware_interface::System system_hw(std::make_unique<test_components::DummySystem>());

hardware_interface::HardwareInfo mock_hw_info{};
hardware_interface::HardwareInfo mock_hw_info;
mock_hw_info.name = "mock_hw";
rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("test_system_components");
hardware_interface::HardwareComponentParams params;
params.hardware_info = mock_hw_info;
params.clock = node->get_clock();
params.logger = node->get_logger();
params.executor = executor_;
auto state = system_hw.initialize(params);
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id());
EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label());
Expand Down Expand Up @@ -2023,7 +2061,7 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior)
}
// END

TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior)
TEST_F(TestComponentInterfaces, dummy_system_default_read_error_behavior)
{
hardware_interface::System system_hw(std::make_unique<test_components::DummySystemDefault>());

Expand All @@ -2039,6 +2077,7 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior)
params.hardware_info = dummy_system;
params.clock = node->get_clock();
params.logger = node->get_logger();
params.executor = executor_;
auto state = system_hw.initialize(params);
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id());
EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label());
Expand Down Expand Up @@ -2099,16 +2138,18 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior)
}

// BEGIN (Handle export change): for backward compatibility
TEST(TestComponentInterfaces, dummy_system_write_error_behavior)
TEST_F(TestComponentInterfaces, dummy_system_write_error_behavior)
{
hardware_interface::System system_hw(std::make_unique<test_components::DummySystem>());

hardware_interface::HardwareInfo mock_hw_info{};
hardware_interface::HardwareInfo mock_hw_info;
mock_hw_info.name = "mock_hw";
rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("test_system_components");
hardware_interface::HardwareComponentParams params;
params.hardware_info = mock_hw_info;
params.clock = node->get_clock();
params.logger = node->get_logger();
params.executor = executor_;
auto state = system_hw.initialize(params);
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id());
EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label());
Expand Down Expand Up @@ -2169,7 +2210,7 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior)
}
// END

TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior)
TEST_F(TestComponentInterfaces, dummy_system_default_write_error_behavior)
{
hardware_interface::System system_hw(std::make_unique<test_components::DummySystemDefault>());

Expand All @@ -2185,6 +2226,7 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior)
params.hardware_info = dummy_system;
params.clock = node->get_clock();
params.logger = node->get_logger();
params.executor = executor_;
auto state = system_hw.initialize(params);
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id());
EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label());
Expand Down
Loading
Loading