Skip to content

Commit

Permalink
[MockHardware] Enable disabling of command to simulate HW failures. (#…
Browse files Browse the repository at this point in the history
  • Loading branch information
destogl authored Jun 4, 2023
1 parent e89fbe7 commit bc8052a
Show file tree
Hide file tree
Showing 4 changed files with 93 additions and 0 deletions.
5 changes: 5 additions & 0 deletions hardware_interface/doc/mock_components_userdoc.rst
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,11 @@ Features:
Parameters
,,,,,,,,,,

disable_commands (optional; boolean; default: false)
Disables mirroring commands to states.
This option is helpful to simulate an erroneous connection to the hardware when nothing breaks, but suddenly there is no feedback from a hardware interface.
Or it can help you to test your setup when the hardware is running without feedback, i.e., in open loop configuration.

mock_sensor_commands (optional; boolean; default: false)
Creates fake command interfaces for faking sensor measurements with an external command.
Those interfaces are usually used by a :ref:`forward controller <forward_command_controller_userdoc>` to provide access from ROS-world.
Expand Down
2 changes: 2 additions & 0 deletions hardware_interface/include/mock_components/generic_system.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -114,6 +114,8 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste
double position_state_following_offset_;
std::string custom_interface_with_following_offset_;
size_t index_custom_interface_with_following_offset_;

bool command_propagation_disabled_;
};

typedef GenericSystem GenericRobot;
Expand Down
19 changes: 19 additions & 0 deletions hardware_interface/src/mock_components/generic_system.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -119,6 +119,18 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i
}
}

// check if there is parameter that disables commands
// this way we simulate disconnected driver
it = info_.hardware_parameters.find("disable_commands");
if (it != info.hardware_parameters.end())
{
command_propagation_disabled_ = hardware_interface::parse_bool(it->second);
}
else
{
command_propagation_disabled_ = false;
}

// process parameters about state following
position_state_following_offset_ = 0.0;
custom_interface_with_following_offset_ = "";
Expand Down Expand Up @@ -359,6 +371,13 @@ std::vector<hardware_interface::CommandInterface> GenericSystem::export_command_

return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
if (command_propagation_disabled_)
{
RCUTILS_LOG_WARN_NAMED(
"mock_generic_system", "Command propagation is disabled - no values will be returned!");
return return_type::OK;
}

auto mirror_command_to_state = [](auto & states_, auto commands_, size_t start_index = 0)
{
for (size_t i = start_index; i < states_.size(); ++i)
Expand Down
67 changes: 67 additions & 0 deletions hardware_interface/test/mock_components/test_generic_system.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -483,6 +483,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 @@ -503,6 +521,7 @@ class TestGenericSystem : public ::testing::Test
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 disabled_commands_;
};

// Forward declaration
Expand Down Expand Up @@ -1604,3 +1623,51 @@ TEST_F(TestGenericSystem, gpio_with_initial_value)

ASSERT_EQ(1, state.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 bc8052a

Please sign in to comment.