|
32 | 32 | #include "hardware_interface/types/hardware_interface_type_values.hpp" |
33 | 33 | #include "hardware_interface/types/lifecycle_state_names.hpp" |
34 | 34 | #include "lifecycle_msgs/msg/state.hpp" |
| 35 | +#include "rclcpp/executors/single_threaded_executor.hpp" |
35 | 36 | #include "rclcpp/node.hpp" |
36 | 37 | #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" |
37 | 38 | #include "ros2_control_test_assets/components_urdfs.hpp" |
@@ -769,12 +770,15 @@ TEST(TestComponentInterfaces, dummy_actuator) |
769 | 770 | { |
770 | 771 | hardware_interface::Actuator actuator_hw(std::make_unique<test_components::DummyActuator>()); |
771 | 772 |
|
772 | | - hardware_interface::HardwareInfo mock_hw_info{}; |
| 773 | + hardware_interface::HardwareInfo mock_hw_info; |
| 774 | + mock_hw_info.name = "mock_hw"; |
773 | 775 | rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("test_actuator_components"); |
774 | 776 | hardware_interface::HardwareComponentParams params; |
775 | 777 | params.hardware_info = mock_hw_info; |
776 | 778 | params.clock = node->get_clock(); |
777 | 779 | params.logger = node->get_logger(); |
| 780 | + auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 781 | + params.executor = executor; |
778 | 782 | auto state = actuator_hw.initialize(params); |
779 | 783 | EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); |
780 | 784 | EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); |
@@ -880,6 +884,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default) |
880 | 884 | params.hardware_info = dummy_actuator; |
881 | 885 | params.clock = node->get_clock(); |
882 | 886 | params.logger = node->get_logger(); |
| 887 | + auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 888 | + params.executor = executor; |
883 | 889 | auto state = actuator_hw.initialize(params); |
884 | 890 |
|
885 | 891 | EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); |
@@ -997,12 +1003,15 @@ TEST(TestComponentInterfaces, dummy_sensor) |
997 | 1003 | { |
998 | 1004 | hardware_interface::Sensor sensor_hw(std::make_unique<test_components::DummySensor>()); |
999 | 1005 |
|
1000 | | - hardware_interface::HardwareInfo mock_hw_info{}; |
| 1006 | + hardware_interface::HardwareInfo mock_hw_info; |
| 1007 | + mock_hw_info.name = "mock_hw"; |
1001 | 1008 | rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("test_sensor_components"); |
1002 | 1009 | hardware_interface::HardwareComponentParams params; |
1003 | 1010 | params.hardware_info = mock_hw_info; |
1004 | 1011 | params.clock = node->get_clock(); |
1005 | 1012 | params.logger = node->get_logger(); |
| 1013 | + auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 1014 | + params.executor = executor; |
1006 | 1015 | auto state = sensor_hw.initialize(params); |
1007 | 1016 | EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); |
1008 | 1017 | EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); |
@@ -1046,6 +1055,8 @@ TEST(TestComponentInterfaces, dummy_sensor_default) |
1046 | 1055 | params.hardware_info = voltage_sensor_res; |
1047 | 1056 | params.clock = node->get_clock(); |
1048 | 1057 | params.logger = node->get_logger(); |
| 1058 | + auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 1059 | + params.executor = executor; |
1049 | 1060 | auto state = sensor_hw.initialize(params); |
1050 | 1061 | EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); |
1051 | 1062 | EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); |
@@ -1094,6 +1105,8 @@ TEST(TestComponentInterfaces, dummy_sensor_default_joint) |
1094 | 1105 | params.hardware_info = sensor_res; |
1095 | 1106 | params.clock = node->get_clock(); |
1096 | 1107 | params.logger = node->get_logger(); |
| 1108 | + auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 1109 | + params.executor = executor; |
1097 | 1110 | auto state = sensor_hw.initialize(params); |
1098 | 1111 | ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); |
1099 | 1112 | ASSERT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); |
@@ -1140,12 +1153,15 @@ TEST(TestComponentInterfaces, dummy_system) |
1140 | 1153 | { |
1141 | 1154 | hardware_interface::System system_hw(std::make_unique<test_components::DummySystem>()); |
1142 | 1155 |
|
1143 | | - hardware_interface::HardwareInfo mock_hw_info{}; |
| 1156 | + hardware_interface::HardwareInfo mock_hw_info; |
| 1157 | + mock_hw_info.name = "mock_hw"; |
1144 | 1158 | rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("test_system_components"); |
1145 | 1159 | hardware_interface::HardwareComponentParams params; |
1146 | 1160 | params.hardware_info = mock_hw_info; |
1147 | 1161 | params.clock = node->get_clock(); |
1148 | 1162 | params.logger = node->get_logger(); |
| 1163 | + auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 1164 | + params.executor = executor; |
1149 | 1165 | auto state = system_hw.initialize(params); |
1150 | 1166 | EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); |
1151 | 1167 | EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); |
@@ -1285,6 +1301,8 @@ TEST(TestComponentInterfaces, dummy_system_default) |
1285 | 1301 | params.hardware_info = dummy_system; |
1286 | 1302 | params.clock = node->get_clock(); |
1287 | 1303 | params.logger = node->get_logger(); |
| 1304 | + auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 1305 | + params.executor = executor; |
1288 | 1306 | auto state = system_hw.initialize(params); |
1289 | 1307 | EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); |
1290 | 1308 | EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); |
@@ -1489,12 +1507,16 @@ TEST(TestComponentInterfaces, dummy_command_mode_system) |
1489 | 1507 | { |
1490 | 1508 | hardware_interface::System system_hw( |
1491 | 1509 | std::make_unique<test_components::DummySystemPreparePerform>()); |
1492 | | - hardware_interface::HardwareInfo mock_hw_info{}; |
| 1510 | + |
| 1511 | + hardware_interface::HardwareInfo mock_hw_info; |
| 1512 | + mock_hw_info.name = "mock_hw"; |
1493 | 1513 | rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("test_system_components"); |
1494 | 1514 | hardware_interface::HardwareComponentParams params; |
1495 | 1515 | params.hardware_info = mock_hw_info; |
1496 | 1516 | params.clock = node->get_clock(); |
1497 | 1517 | params.logger = node->get_logger(); |
| 1518 | + auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 1519 | + params.executor = executor; |
1498 | 1520 | auto state = system_hw.initialize(params); |
1499 | 1521 | EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); |
1500 | 1522 | EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); |
@@ -1526,12 +1548,15 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) |
1526 | 1548 | { |
1527 | 1549 | hardware_interface::Actuator actuator_hw(std::make_unique<test_components::DummyActuator>()); |
1528 | 1550 |
|
1529 | | - hardware_interface::HardwareInfo mock_hw_info{}; |
| 1551 | + hardware_interface::HardwareInfo mock_hw_info; |
| 1552 | + mock_hw_info.name = "mock_hw"; |
1530 | 1553 | rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("test_actuator_components"); |
1531 | 1554 | hardware_interface::HardwareComponentParams params; |
1532 | 1555 | params.hardware_info = mock_hw_info; |
1533 | 1556 | params.clock = node->get_clock(); |
1534 | 1557 | params.logger = node->get_logger(); |
| 1558 | + auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 1559 | + params.executor = executor; |
1535 | 1560 | auto state = actuator_hw.initialize(params); |
1536 | 1561 | EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); |
1537 | 1562 | EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); |
@@ -1604,6 +1629,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) |
1604 | 1629 | params.hardware_info = dummy_actuator; |
1605 | 1630 | params.clock = node->get_clock(); |
1606 | 1631 | params.logger = node->get_logger(); |
| 1632 | + auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 1633 | + params.executor = executor; |
1607 | 1634 | auto state = actuator_hw.initialize(params); |
1608 | 1635 |
|
1609 | 1636 | EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); |
@@ -1667,12 +1694,15 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) |
1667 | 1694 | { |
1668 | 1695 | hardware_interface::Actuator actuator_hw(std::make_unique<test_components::DummyActuator>()); |
1669 | 1696 |
|
1670 | | - hardware_interface::HardwareInfo mock_hw_info{}; |
| 1697 | + hardware_interface::HardwareInfo mock_hw_info; |
| 1698 | + mock_hw_info.name = "mock_hw"; |
1671 | 1699 | rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("test_actuator_components"); |
1672 | 1700 | hardware_interface::HardwareComponentParams params; |
1673 | 1701 | params.hardware_info = mock_hw_info; |
1674 | 1702 | params.clock = node->get_clock(); |
1675 | 1703 | params.logger = node->get_logger(); |
| 1704 | + auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 1705 | + params.executor = executor; |
1676 | 1706 | auto state = actuator_hw.initialize(params); |
1677 | 1707 | EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); |
1678 | 1708 | EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); |
@@ -1745,6 +1775,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) |
1745 | 1775 | params.hardware_info = dummy_actuator; |
1746 | 1776 | params.clock = node->get_clock(); |
1747 | 1777 | params.logger = node->get_logger(); |
| 1778 | + auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 1779 | + params.executor = executor; |
1748 | 1780 | auto state = actuator_hw.initialize(params); |
1749 | 1781 | EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); |
1750 | 1782 | EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); |
@@ -1807,12 +1839,15 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) |
1807 | 1839 | { |
1808 | 1840 | hardware_interface::Sensor sensor_hw(std::make_unique<test_components::DummySensor>()); |
1809 | 1841 |
|
1810 | | - hardware_interface::HardwareInfo mock_hw_info{}; |
| 1842 | + hardware_interface::HardwareInfo mock_hw_info; |
| 1843 | + mock_hw_info.name = "mock_hw"; |
1811 | 1844 | rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("test_sensor_components"); |
1812 | 1845 | hardware_interface::HardwareComponentParams params; |
1813 | 1846 | params.hardware_info = mock_hw_info; |
1814 | 1847 | params.clock = node->get_clock(); |
1815 | 1848 | params.logger = node->get_logger(); |
| 1849 | + auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 1850 | + params.executor = executor; |
1816 | 1851 | auto state = sensor_hw.initialize(params); |
1817 | 1852 |
|
1818 | 1853 | auto state_interfaces = sensor_hw.export_state_interfaces(); |
@@ -1889,6 +1924,8 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) |
1889 | 1924 | params.hardware_info = voltage_sensor_res; |
1890 | 1925 | params.clock = node->get_clock(); |
1891 | 1926 | params.logger = node->get_logger(); |
| 1927 | + auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 1928 | + params.executor = executor; |
1892 | 1929 | auto state = sensor_hw.initialize(params); |
1893 | 1930 |
|
1894 | 1931 | auto state_interfaces = sensor_hw.export_state_interfaces(); |
@@ -1942,12 +1979,15 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) |
1942 | 1979 | { |
1943 | 1980 | hardware_interface::System system_hw(std::make_unique<test_components::DummySystem>()); |
1944 | 1981 |
|
1945 | | - hardware_interface::HardwareInfo mock_hw_info{}; |
| 1982 | + hardware_interface::HardwareInfo mock_hw_info; |
| 1983 | + mock_hw_info.name = "mock_hw"; |
1946 | 1984 | rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("test_system_components"); |
1947 | 1985 | hardware_interface::HardwareComponentParams params; |
1948 | 1986 | params.hardware_info = mock_hw_info; |
1949 | 1987 | params.clock = node->get_clock(); |
1950 | 1988 | params.logger = node->get_logger(); |
| 1989 | + auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 1990 | + params.executor = executor; |
1951 | 1991 | auto state = system_hw.initialize(params); |
1952 | 1992 | EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); |
1953 | 1993 | EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); |
@@ -2024,6 +2064,8 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) |
2024 | 2064 | params.hardware_info = dummy_system; |
2025 | 2065 | params.clock = node->get_clock(); |
2026 | 2066 | params.logger = node->get_logger(); |
| 2067 | + auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 2068 | + params.executor = executor; |
2027 | 2069 | auto state = system_hw.initialize(params); |
2028 | 2070 | EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); |
2029 | 2071 | EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); |
@@ -2088,12 +2130,15 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) |
2088 | 2130 | { |
2089 | 2131 | hardware_interface::System system_hw(std::make_unique<test_components::DummySystem>()); |
2090 | 2132 |
|
2091 | | - hardware_interface::HardwareInfo mock_hw_info{}; |
| 2133 | + hardware_interface::HardwareInfo mock_hw_info; |
| 2134 | + mock_hw_info.name = "mock_hw"; |
2092 | 2135 | rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("test_system_components"); |
2093 | 2136 | hardware_interface::HardwareComponentParams params; |
2094 | 2137 | params.hardware_info = mock_hw_info; |
2095 | 2138 | params.clock = node->get_clock(); |
2096 | 2139 | params.logger = node->get_logger(); |
| 2140 | + auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 2141 | + params.executor = executor; |
2097 | 2142 | auto state = system_hw.initialize(params); |
2098 | 2143 | EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); |
2099 | 2144 | EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); |
@@ -2170,6 +2215,8 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) |
2170 | 2215 | params.hardware_info = dummy_system; |
2171 | 2216 | params.clock = node->get_clock(); |
2172 | 2217 | params.logger = node->get_logger(); |
| 2218 | + auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); |
| 2219 | + params.executor = executor; |
2173 | 2220 | auto state = system_hw.initialize(params); |
2174 | 2221 | EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); |
2175 | 2222 | EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); |
|
0 commit comments