diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp
index 57b5796d0d9..2996e748fcb 100644
--- a/hardware_interface/test/mock_components/test_generic_system.cpp
+++ b/hardware_interface/test/mock_components/test_generic_system.cpp
@@ -142,12 +142,12 @@ class TestGenericSystem : public ::testing::Test
0.2
-
+
0.5
-
+
)";
diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp
index 3326cb893b8..8e3de8919ec 100644
--- a/hardware_interface/test/test_components/test_system.cpp
+++ b/hardware_interface/test/test_components/test_system.cpp
@@ -32,22 +32,19 @@ class TestSystem : public SystemInterface
std::vector state_interfaces;
for (auto i = 0u; i < info_.joints.size(); ++i)
{
- if (info_.joints[i].name != "configuration")
- {
- state_interfaces.emplace_back(hardware_interface::StateInterface(
- info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i]));
- state_interfaces.emplace_back(hardware_interface::StateInterface(
- info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i]));
- state_interfaces.emplace_back(hardware_interface::StateInterface(
- info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i]));
- }
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
+ info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i]));
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
+ info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i]));
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
+ info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i]));
}
- if (info_.joints.size() > 2)
+ if (info_.gpios.size() > 0)
{
// Add configuration/max_tcp_jerk interface
state_interfaces.emplace_back(hardware_interface::StateInterface(
- info_.joints[2].name, info_.joints[2].state_interfaces[0].name, &configuration_state_));
+ info_.gpios[0].name, info_.gpios[0].state_interfaces[0].name, &configuration_state_));
}
return state_interfaces;
@@ -58,22 +55,19 @@ class TestSystem : public SystemInterface
std::vector command_interfaces;
for (auto i = 0u; i < info_.joints.size(); ++i)
{
- if (info_.joints[i].name != "configuration")
- {
- command_interfaces.emplace_back(hardware_interface::CommandInterface(
- info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i]));
- }
+ command_interfaces.emplace_back(hardware_interface::CommandInterface(
+ info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i]));
}
// Add max_acceleration command interface
command_interfaces.emplace_back(hardware_interface::CommandInterface(
info_.joints[0].name, info_.joints[0].command_interfaces[1].name,
&max_acceleration_command_));
- if (info_.joints.size() > 2)
+ if (info_.gpios.size() > 0)
{
// Add configuration/max_tcp_jerk interface
command_interfaces.emplace_back(hardware_interface::CommandInterface(
- info_.joints[2].name, info_.joints[2].command_interfaces[0].name, &configuration_command_));
+ info_.gpios[0].name, info_.gpios[0].command_interfaces[0].name, &configuration_command_));
}
return command_interfaces;
diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp
index 8163d31a10a..bbdf8b10daf 100644
--- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp
+++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp
@@ -113,6 +113,34 @@ const auto urdf_head =
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -169,10 +197,10 @@ const auto hardware_resources =
-
+
-
+
)";
@@ -807,10 +835,10 @@ const auto minimal_robot_missing_command_keys_urdf =
[[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_TYPE = "system";
[[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_PLUGIN_NAME = "test_system";
[[maybe_unused]] const std::vector TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES = {
- "joint2/velocity", "joint2/max_acceleration", "joint3/velocity", "configuration/max_tcp_jerk"};
+ "joint2/velocity", "joint2/max_acceleration", "joint3/velocity"};
[[maybe_unused]] const std::vector TEST_SYSTEM_HARDWARE_STATE_INTERFACES = {
- "joint2/position", "joint2/velocity", "joint2/acceleration", "joint3/position",
- "joint3/velocity", "joint3/acceleration", "configuration/max_tcp_jerk"};
+ "joint2/position", "joint2/velocity", "joint2/acceleration",
+ "joint3/position", "joint3/velocity", "joint3/acceleration"};
} // namespace ros2_control_test_assets