Skip to content

Commit

Permalink
Fixup some details
Browse files Browse the repository at this point in the history
  • Loading branch information
@destogl
destogl committed Jun 15, 2023
1 parent eddd465 commit 65f7a54
Showing 1 changed file with 142 additions and 24 deletions.
166 changes: 142 additions & 24 deletions hardware_interface/test/mock_components/test_generic_system.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,8 +40,9 @@ const auto COMPARE_DELTA = 0.0001;
class TestGenericSystem : public ::testing::Test
{
public:
void test_generic_system_with_mock_sensor_commands(std::string & urdf);
void test_generic_system_with_mimic_joint(std::string & urdf);
void test_generic_system_with_mock_sensor_commands(std::string & urdf);
void test_generic_system_with_mock_gpio_commands(std::string & urdf);

protected:
void SetUp() override
Expand Down Expand Up @@ -186,7 +187,7 @@ class TestGenericSystem : public ::testing::Test
</ros2_control>
)";

hardware_system_2dof_with_sensor_fake_command_ =
hardware_system_2dof_with_sensor_mock_command_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
Expand Down Expand Up @@ -215,7 +216,7 @@ class TestGenericSystem : public ::testing::Test
</ros2_control>
)";

hardware_system_2dof_with_sensor_fake_command_True_ =
hardware_system_2dof_with_sensor_mock_command_True_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
Expand Down Expand Up @@ -382,7 +383,41 @@ class TestGenericSystem : public ::testing::Test
</ros2_control>
)";

valid_urdf_ros2_control_system_robot_with_gpio_fake_command_ =
valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="mock_gpio_commands">true</param>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<param name="initial_position">3.45</param>
</joint>
<joint name="joint2">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<param name="initial_position">2.78</param>
</joint>
<gpio name="flange_analog_IOs">
<command_interface name="analog_output1" data_type="double"/>
<state_interface name="analog_output1"/>
<state_interface name="analog_input1"/>
<state_interface name="analog_input2"/>
</gpio>
<gpio name="flange_vacuum">
<command_interface name="vacuum"/>
<state_interface name="vacuum" data_type="double"/>
</gpio>
</ros2_control>
)";

valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
Expand Down Expand Up @@ -450,7 +485,7 @@ class TestGenericSystem : public ::testing::Test
</ros2_control>
)";

hardware_system_2dof_standard_interfaces_with_different_controli_modes_ =
hardware_system_2dof_standard_interfaces_with_different_control_modes_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
Expand Down Expand Up @@ -482,6 +517,24 @@ class TestGenericSystem : public ::testing::Test
</gpio>
</ros2_control>
)";

disabled_commands_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
<plugin>fake_components/GenericSystem</plugin>
<param name="disable_commands">True</param>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position">
<param name="initial_value">3.45</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
</ros2_control>
)";
}

std::string hardware_robot_2dof_;
Expand All @@ -491,17 +544,19 @@ class TestGenericSystem : public ::testing::Test
std::string hardware_system_2dof_standard_interfaces_;
std::string hardware_system_2dof_with_other_interface_;
std::string hardware_system_2dof_with_sensor_;
std::string hardware_system_2dof_with_sensor_fake_command_;
std::string hardware_system_2dof_with_sensor_fake_command_True_;
std::string hardware_system_2dof_with_sensor_mock_command_;
std::string hardware_system_2dof_with_sensor_mock_command_True_;
std::string hardware_system_2dof_with_mimic_joint_;
std::string hardware_system_2dof_standard_interfaces_with_offset_;
std::string hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_;
std::string hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_;
std::string valid_urdf_ros2_control_system_robot_with_gpio_;
std::string valid_urdf_ros2_control_system_robot_with_gpio_fake_command_;
std::string valid_urdf_ros2_control_system_robot_with_gpio_mock_command_;
std::string valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_;
std::string sensor_with_initial_value_;
std::string gpio_with_initial_value_;
std::string hardware_system_2dof_standard_interfaces_with_different_controli_modes_;
std::string hardware_system_2dof_standard_interfaces_with_different_control_modes_;
std::string disabled_commands_;
};

// Forward declaration
Expand All @@ -520,12 +575,12 @@ class TestableResourceManager : public hardware_interface::ResourceManager
FRIEND_TEST(TestGenericSystem, generic_system_2dof_asymetric_interfaces);
FRIEND_TEST(TestGenericSystem, generic_system_2dof_other_interfaces);
FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor);
FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_fake_command);
FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_fake_command_True);
FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_mock_command);
FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_mock_command_True);
FRIEND_TEST(TestGenericSystem, hardware_system_2dof_with_mimic_joint);
FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio);
FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command);
FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command);
FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command);
FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True);

TestableResourceManager() : hardware_interface::ResourceManager() {}

Expand Down Expand Up @@ -1105,18 +1160,18 @@ void TestGenericSystem::test_generic_system_with_mock_sensor_commands(std::strin
ASSERT_EQ(4.44, sty_c.get_value());
}

TEST_F(TestGenericSystem, generic_system_2dof_sensor_fake_command)
TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command)
{
auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_fake_command_ +
auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_mock_command_ +
ros2_control_test_assets::urdf_tail;

test_generic_system_with_mock_sensor_commands(urdf);
}

TEST_F(TestGenericSystem, generic_system_2dof_sensor_fake_command_True)
TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command_True)
{
auto urdf = ros2_control_test_assets::urdf_head +
hardware_system_2dof_with_sensor_fake_command_True_ +
hardware_system_2dof_with_sensor_mock_command_True_ +
ros2_control_test_assets::urdf_tail;

test_generic_system_with_mock_sensor_commands(urdf);
Expand Down Expand Up @@ -1431,11 +1486,8 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio)
generic_system_functional_test(urdf);
}

TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command)
void TestGenericSystem::test_generic_system_with_mock_gpio_commands(std::string & urdf)
{
auto urdf = ros2_control_test_assets::urdf_head +
valid_urdf_ros2_control_system_robot_with_gpio_fake_command_ +
ros2_control_test_assets::urdf_tail;
TestableResourceManager rm(urdf);

// check is hardware is started
Expand Down Expand Up @@ -1542,7 +1594,25 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_co
ASSERT_EQ(2.22, gpio2_vac_c.get_value());
}

TEST_F(TestGenericSystem, sensor_with_initial_value_)
TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command)
{
auto urdf = ros2_control_test_assets::urdf_head +
valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ +
ros2_control_test_assets::urdf_tail;

test_generic_system_with_mock_gpio_commands(urdf);
}

TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True)
{
auto urdf = ros2_control_test_assets::urdf_head +
valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ +
ros2_control_test_assets::urdf_tail;

test_generic_system_with_mock_gpio_commands(urdf);
}

TEST_F(TestGenericSystem, sensor_with_initial_value)
{
auto urdf = ros2_control_test_assets::urdf_head + sensor_with_initial_value_ +
ros2_control_test_assets::urdf_tail;
Expand Down Expand Up @@ -1570,7 +1640,7 @@ TEST_F(TestGenericSystem, sensor_with_initial_value_)
ASSERT_EQ(0.0, force_z_s.get_value());
}

TEST_F(TestGenericSystem, gpio_with_initial_value_)
TEST_F(TestGenericSystem, gpio_with_initial_value)
{
auto urdf = ros2_control_test_assets::urdf_head + gpio_with_initial_value_ +
ros2_control_test_assets::urdf_tail;
Expand All @@ -1592,8 +1662,9 @@ TEST_F(TestGenericSystem, gpio_with_initial_value_)
TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces)
{
auto urdf = ros2_control_test_assets::urdf_head +
hardware_system_2dof_standard_interfaces_with_different_controli_modes_ +
hardware_system_2dof_standard_interfaces_with_different_control_modes_ +
ros2_control_test_assets::urdf_tail;

TestableResourceManager rm(urdf);
// Activate components to get all interfaces available
activate_components(rm);
Expand Down Expand Up @@ -1783,3 +1854,50 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces)
ASSERT_EQ(2.0, j2v_c.get_value());
ASSERT_EQ(3.5, j2a_c.get_value());
}

TEST_F(TestGenericSystem, disabled_commands_flag_is_active)
{
auto urdf =
ros2_control_test_assets::urdf_head + disabled_commands_ + ros2_control_test_assets::urdf_tail;
TestableResourceManager rm(urdf);
// Activate components to get all interfaces available
activate_components(rm);

// Check interfaces
EXPECT_EQ(1u, rm.system_components_size());
ASSERT_EQ(2u, rm.state_interface_keys().size());
EXPECT_TRUE(rm.state_interface_exists("joint1/position"));
EXPECT_TRUE(rm.state_interface_exists("joint1/velocity"));

ASSERT_EQ(2u, rm.command_interface_keys().size());
EXPECT_TRUE(rm.command_interface_exists("joint1/position"));
EXPECT_TRUE(rm.command_interface_exists("joint1/velocity"));
// Check initial values
hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position");
hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface("joint1/velocity");
hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position");

ASSERT_EQ(3.45, j1p_s.get_value());
ASSERT_EQ(0.0, j1v_s.get_value());
ASSERT_TRUE(std::isnan(j1p_c.get_value()));

// set some new values in commands
j1p_c.set_value(0.11);

// State values should not be changed
ASSERT_EQ(3.45, j1p_s.get_value());
ASSERT_EQ(0.0, j1v_s.get_value());
ASSERT_EQ(0.11, j1p_c.get_value());

// write() does not change values
rm.write(TIME, PERIOD);
ASSERT_EQ(3.45, j1p_s.get_value());
ASSERT_EQ(0.0, j1v_s.get_value());
ASSERT_EQ(0.11, j1p_c.get_value());

// read() also does not change values
rm.read(TIME, PERIOD);
ASSERT_EQ(3.45, j1p_s.get_value());
ASSERT_EQ(0.0, j1v_s.get_value());
ASSERT_EQ(0.11, j1p_c.get_value());
}

0 comments on commit 65f7a54

Please sign in to comment.