Skip to content

Commit 7b96ec2

Browse files
saikishorsaikishor
committed
Add async read and write tests for the test components with ResourceManager
1 parent 3cd4cb1 commit 7b96ec2

File tree

2 files changed

+249
-0
lines changed

2 files changed

+249
-0
lines changed

‎hardware_interface_testing/test/test_resource_manager.cpp

Lines changed: 200 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1688,3 +1688,203 @@ TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware)
16881688
{TEST_BROADCASTER_SENSOR_NAME, TEST_BROADCASTER_ALL_NAME})));
16891689
}
16901690
}
1691+
1692+
class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest
1693+
{
1694+
public:
1695+
void setup_resource_manager_and_do_initial_checks()
1696+
{
1697+
const auto minimal_robot_urdf_async =
1698+
std::string(ros2_control_test_assets::urdf_head) +
1699+
std::string(ros2_control_test_assets::hardware_resources_with_async) +
1700+
std::string(ros2_control_test_assets::urdf_tail);
1701+
rm = std::make_shared<TestableResourceManager>(minimal_robot_urdf_async, false);
1702+
activate_components(*rm);
1703+
1704+
auto status_map = rm->get_components_status();
1705+
EXPECT_EQ(
1706+
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
1707+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
1708+
EXPECT_EQ(
1709+
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
1710+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
1711+
EXPECT_EQ(
1712+
status_map[TEST_SENSOR_HARDWARE_NAME].state.id(),
1713+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
1714+
1715+
// Check that all the components are async
1716+
ASSERT_TRUE(status_map[TEST_ACTUATOR_HARDWARE_NAME].is_async);
1717+
ASSERT_TRUE(status_map[TEST_SYSTEM_HARDWARE_NAME].is_async);
1718+
ASSERT_TRUE(status_map[TEST_SENSOR_HARDWARE_NAME].is_async);
1719+
1720+
claimed_itfs.push_back(
1721+
rm->claim_command_interface(TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES[0]));
1722+
claimed_itfs.push_back(rm->claim_command_interface(TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES[0]));
1723+
1724+
state_itfs.push_back(rm->claim_state_interface(TEST_ACTUATOR_HARDWARE_STATE_INTERFACES[1]));
1725+
state_itfs.push_back(rm->claim_state_interface(TEST_SYSTEM_HARDWARE_STATE_INTERFACES[1]));
1726+
1727+
check_if_interface_available(true, true);
1728+
// with default values read and write should run without any problems
1729+
{
1730+
auto [ok, failed_hardware_names] = rm->read(time, duration);
1731+
EXPECT_TRUE(ok);
1732+
EXPECT_TRUE(failed_hardware_names.empty());
1733+
}
1734+
{
1735+
// claimed_itfs[0].set_value(10.0);
1736+
// claimed_itfs[1].set_value(20.0);
1737+
auto [ok, failed_hardware_names] = rm->write(time, duration);
1738+
EXPECT_TRUE(ok);
1739+
EXPECT_TRUE(failed_hardware_names.empty());
1740+
}
1741+
time = time + duration;
1742+
check_if_interface_available(true, true);
1743+
}
1744+
1745+
// check if all interfaces are available
1746+
void check_if_interface_available(const bool actuator_interfaces, const bool system_interfaces)
1747+
{
1748+
for (const auto & interface : TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES)
1749+
{
1750+
EXPECT_EQ(rm->command_interface_is_available(interface), actuator_interfaces);
1751+
}
1752+
for (const auto & interface : TEST_ACTUATOR_HARDWARE_STATE_INTERFACES)
1753+
{
1754+
EXPECT_EQ(rm->state_interface_is_available(interface), actuator_interfaces);
1755+
}
1756+
for (const auto & interface : TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES)
1757+
{
1758+
EXPECT_EQ(rm->command_interface_is_available(interface), system_interfaces);
1759+
}
1760+
for (const auto & interface : TEST_SYSTEM_HARDWARE_STATE_INTERFACES)
1761+
{
1762+
EXPECT_EQ(rm->state_interface_is_available(interface), system_interfaces);
1763+
}
1764+
};
1765+
1766+
void check_read_and_write_cycles(bool check_for_updated_values)
1767+
{
1768+
double prev_act_state_value = state_itfs[0].get_value();
1769+
double prev_system_state_value = state_itfs[1].get_value();
1770+
const double actuator_increment = 10.0;
1771+
const double system_increment = 20.0;
1772+
for (size_t i = 1; i < 100; i++)
1773+
{
1774+
auto [ok, failed_hardware_names] = rm->read(time, duration);
1775+
EXPECT_TRUE(ok);
1776+
EXPECT_TRUE(failed_hardware_names.empty());
1777+
std::this_thread::sleep_for(std::chrono::milliseconds(1));
1778+
// The values are computations exactly within the test_components
1779+
if (check_for_updated_values)
1780+
{
1781+
prev_system_state_value = claimed_itfs[1].get_value() / 2.0;
1782+
prev_act_state_value = claimed_itfs[0].get_value() / 2.0;
1783+
}
1784+
claimed_itfs[0].set_value(claimed_itfs[0].get_value() + actuator_increment);
1785+
claimed_itfs[1].set_value(claimed_itfs[1].get_value() + system_increment);
1786+
// This is needed to account for any missing hits to the read and write cycles as the tests
1787+
// are going to be run on a non-RT operating system
1788+
ASSERT_NEAR(state_itfs[0].get_value(), prev_act_state_value, actuator_increment / 2.0);
1789+
ASSERT_NEAR(state_itfs[1].get_value(), prev_system_state_value, system_increment / 2.0);
1790+
auto [ok_write, failed_hardware_names_write] = rm->write(time, duration);
1791+
std::this_thread::sleep_for(std::chrono::milliseconds(1));
1792+
EXPECT_TRUE(ok_write);
1793+
EXPECT_TRUE(failed_hardware_names_write.empty());
1794+
time = time + duration;
1795+
}
1796+
}
1797+
1798+
public:
1799+
std::shared_ptr<TestableResourceManager> rm;
1800+
unsigned int actuator_rw_rate_, system_rw_rate_, cm_update_rate_;
1801+
std::vector<hardware_interface::LoanedCommandInterface> claimed_itfs;
1802+
std::vector<hardware_interface::LoanedStateInterface> state_itfs;
1803+
1804+
rclcpp::Time time = rclcpp::Time(1657232, 0);
1805+
const rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.01);
1806+
};
1807+
1808+
TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_activate)
1809+
{
1810+
setup_resource_manager_and_do_initial_checks();
1811+
check_read_and_write_cycles(true);
1812+
}
1813+
1814+
TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_deactivate)
1815+
{
1816+
setup_resource_manager_and_do_initial_checks();
1817+
1818+
// Now deactivate all the components and test the same as above
1819+
rclcpp_lifecycle::State state_inactive(
1820+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
1821+
hardware_interface::lifecycle_state_names::INACTIVE);
1822+
rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_inactive);
1823+
rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_inactive);
1824+
rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_inactive);
1825+
1826+
auto status_map = rm->get_components_status();
1827+
EXPECT_EQ(
1828+
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
1829+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
1830+
EXPECT_EQ(
1831+
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
1832+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
1833+
EXPECT_EQ(
1834+
status_map[TEST_SENSOR_HARDWARE_NAME].state.id(),
1835+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
1836+
1837+
check_read_and_write_cycles(true);
1838+
}
1839+
1840+
TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_unconfigured)
1841+
{
1842+
setup_resource_manager_and_do_initial_checks();
1843+
1844+
// Now deactivate all the components and test the same as above
1845+
rclcpp_lifecycle::State state_unconfigured(
1846+
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
1847+
hardware_interface::lifecycle_state_names::UNCONFIGURED);
1848+
rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_unconfigured);
1849+
rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_unconfigured);
1850+
rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_unconfigured);
1851+
1852+
auto status_map = rm->get_components_status();
1853+
EXPECT_EQ(
1854+
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
1855+
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
1856+
EXPECT_EQ(
1857+
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
1858+
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
1859+
EXPECT_EQ(
1860+
status_map[TEST_SENSOR_HARDWARE_NAME].state.id(),
1861+
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
1862+
1863+
check_read_and_write_cycles(false);
1864+
}
1865+
1866+
TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_finalized)
1867+
{
1868+
setup_resource_manager_and_do_initial_checks();
1869+
1870+
// Now deactivate all the components and test the same as above
1871+
rclcpp_lifecycle::State state_finalized(
1872+
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
1873+
hardware_interface::lifecycle_state_names::FINALIZED);
1874+
rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_finalized);
1875+
rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_finalized);
1876+
rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_finalized);
1877+
1878+
auto status_map = rm->get_components_status();
1879+
EXPECT_EQ(
1880+
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
1881+
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
1882+
EXPECT_EQ(
1883+
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
1884+
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
1885+
EXPECT_EQ(
1886+
status_map[TEST_SENSOR_HARDWARE_NAME].state.id(),
1887+
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
1888+
1889+
check_read_and_write_cycles(false);
1890+
}

‎ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -660,6 +660,55 @@ const auto hardware_resources =
660660
</ros2_control>
661661
)";
662662

663+
const auto hardware_resources_with_async =
664+
R"(
665+
<ros2_control name="TestActuatorHardware" type="actuator" is_async="true">
666+
<hardware>
667+
<plugin>test_actuator</plugin>
668+
</hardware>
669+
<joint name="joint1">
670+
<command_interface name="position"/>
671+
<state_interface name="position"/>
672+
<state_interface name="velocity"/>
673+
<command_interface name="max_velocity" />
674+
</joint>
675+
</ros2_control>
676+
<ros2_control name="TestSensorHardware" type="sensor" is_async="true">
677+
<hardware>
678+
<plugin>test_sensor</plugin>
679+
<param name="example_param_write_for_sec">2</param>
680+
<param name="example_param_read_for_sec">2</param>
681+
</hardware>
682+
<sensor name="sensor1">
683+
<state_interface name="velocity"/>
684+
</sensor>true
685+
</ros2_control>
686+
<ros2_control name="TestSystemHardware" type="system" is_async="true">
687+
<hardware>
688+
<plugin>test_system</plugin>
689+
<param name="example_param_write_for_sec">2</param>
690+
<param name="example_param_read_for_sec">2</param>
691+
</hardware>
692+
<joint name="joint2">
693+
<command_interface name="velocity"/>
694+
<state_interface name="position"/>
695+
<state_interface name="velocity"/>
696+
<state_interface name="acceleration"/>
697+
<command_interface name="max_acceleration" />
698+
</joint>
699+
<joint name="joint3">
700+
<command_interface name="velocity"/>
701+
<state_interface name="position"/>
702+
<state_interface name="velocity"/>
703+
<state_interface name="acceleration"/>
704+
</joint>
705+
<gpio name="configuration">
706+
<command_interface name="max_tcp_jerk"/>
707+
<state_interface name="max_tcp_jerk"/>
708+
</gpio>
709+
</ros2_control>
710+
)";
711+
663712
const auto uninitializable_hardware_resources =
664713
R"(
665714
<ros2_control name="TestUninitializableActuatorHardware" type="actuator">

0 commit comments

Comments
 (0)