Skip to content

Cleanup GenericSystem component code 🧹 #2706

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
christophfroehlich merged 6 commits into from
Oct 20, 2025
Merged

Cleanup GenericSystem component code 🧹 #2706

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
364b150
Add teardown and remove unnecessary friending
christophfroehlich Oct 18, 2025
d357c46
Remove unaccessible code
christophfroehlich Oct 18, 2025
2645b29
Remove unneeded variables
christophfroehlich Oct 18, 2025
1a51218
Use has_command/state helper more often
christophfroehlich Oct 18, 2025
ce6fe00
other_interfaces is only needed during on_init
christophfroehlich Oct 18, 2025
9300df6
Merge branch 'master' into cleanup/mock
christophfroehlich Oct 19, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 0 additions & 4 deletions hardware_interface/include/mock_components/generic_system.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,10 +82,6 @@ class GenericSystem : public hardware_interface::SystemInterface
hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT};
std::vector<std::string> skip_interfaces_;

std::vector<std::string> other_interfaces_;
std::vector<std::string> sensor_interfaces_;
std::vector<std::string> gpio_mock_interfaces_;

private:
bool populate_interfaces(
const std::vector<hardware_interface::ComponentInfo> & components,
Expand Down
115 changes: 40 additions & 75 deletions hardware_interface/src/mock_components/generic_system.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,27 +39,6 @@ CallbackReturn GenericSystem::on_init(
return CallbackReturn::ERROR;
}

auto populate_non_standard_interfaces =
[this](auto interface_list, auto & non_standard_interfaces)
{
for (const auto & interface : interface_list)
{
// add to list if non-standard interface
if (
std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface.name) ==
standard_interfaces_.end())
{
if (
std::find(
non_standard_interfaces.begin(), non_standard_interfaces.end(), interface.name) ==
non_standard_interfaces.end())
{
non_standard_interfaces.emplace_back(interface.name);
}
}
}
};

// check if to create mock command interface for sensor
auto it = get_hardware_info().hardware_parameters.find("mock_sensor_commands");
if (it != get_hardware_info().hardware_parameters.end())
Expand Down Expand Up @@ -128,21 +107,44 @@ CallbackReturn GenericSystem::on_init(
}

// search for non-standard joint interfaces
std::vector<std::string> other_interfaces;
auto populate_non_standard_interfaces =
[this](auto interface_list, auto & non_standard_interfaces)
{
for (const auto & interface : interface_list)
{
// add to list if non-standard interface
if (
std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface.name) ==
standard_interfaces_.end())
{
// and does not exist yet
if (
std::find(
non_standard_interfaces.begin(), non_standard_interfaces.end(), interface.name) ==
non_standard_interfaces.end())
{
non_standard_interfaces.emplace_back(interface.name);
}
}
}
};
for (const auto & joint : get_hardware_info().joints)
{
// populate non-standard command interfaces to other_interfaces_
populate_non_standard_interfaces(joint.command_interfaces, other_interfaces_);
// populate non-standard command interfaces to other_interfaces
populate_non_standard_interfaces(joint.command_interfaces, other_interfaces);

// populate non-standard state interfaces to other_interfaces_
populate_non_standard_interfaces(joint.state_interfaces, other_interfaces_);
// populate non-standard state interfaces to other_interfaces
populate_non_standard_interfaces(joint.state_interfaces, other_interfaces);
}

// when following offset is used on custom interface then find its index
if (!custom_interface_with_following_offset_.empty())
{
auto if_it = std::find(
other_interfaces_.begin(), other_interfaces_.end(), custom_interface_with_following_offset_);
if (if_it != other_interfaces_.end())
if (
std::find(
other_interfaces.begin(), other_interfaces.end(),
custom_interface_with_following_offset_) != other_interfaces.end())
{
RCLCPP_INFO(
get_logger(), "Custom interface with following offset '%s' found.",
Expand All @@ -157,19 +159,6 @@ CallbackReturn GenericSystem::on_init(
}
}

for (const auto & sensor : get_hardware_info().sensors)
{
for (const auto & interface : sensor.state_interfaces)
{
if (
std::find(sensor_interfaces_.begin(), sensor_interfaces_.end(), interface.name) ==
sensor_interfaces_.end())
{
sensor_interfaces_.emplace_back(interface.name);
}
}
}

return CallbackReturn::SUCCESS;
}

Expand Down Expand Up @@ -231,26 +220,10 @@ return_type GenericSystem::prepare_command_mode_switch(
}
if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY)
{
if (!calculate_dynamics_)
{
RCLCPP_WARN(
get_logger(),
"Requested velocity mode for joint '%s' without dynamics calculation enabled - this "
"might lead to wrong feedback and unexpected behavior.",
info.joints[joint_index].name.c_str());
}
joint_found_in_x_requests_[joint_index] += 1;
}
if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION)
{
if (!calculate_dynamics_)
{
RCLCPP_WARN(
get_logger(),
"Requested acceleration mode for joint '%s' without dynamics calculation enabled - "
"this might lead to wrong feedback and unexpected behavior.",
info.joints[joint_index].name.c_str());
}
joint_found_in_x_requests_[joint_index] += 1;
}
}
Expand Down Expand Up @@ -412,14 +385,13 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
for (size_t i = 0; i < 3; ++i)
{
const auto full_name = joint_name + "/" + standard_interfaces_[i];
if (joint_command_interfaces_.find(full_name) != joint_command_interfaces_.end())
if (has_command(full_name))
{
joint_command_values_[i] =
get_command(joint_command_interfaces_.at(full_name).get_name());
joint_command_values_[i] = get_command(full_name);
}
if (joint_state_interfaces_.find(full_name) != joint_state_interfaces_.end())
if (has_state(full_name))
{
joint_state_values_[i] = get_state(joint_state_interfaces_.at(full_name).get_name());
joint_state_values_[i] = get_state(full_name);
}
}

Expand Down Expand Up @@ -513,7 +485,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
if (joint_command.get()->get_interface_name() == hardware_interface::HW_IF_POSITION)
{
const std::string & name = joint_command.get()->get_name();
if (joint_state_interfaces_.find(name) != joint_state_interfaces_.end())
if (has_state(name))
{
set_state(
name, get_command(name) + (custom_interface_with_following_offset_.empty()
Expand All @@ -536,7 +508,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
continue;
}
const std::string & full_interface_name = joint_state.get()->get_name();
if (joint_command_interfaces_.find(full_interface_name) != joint_command_interfaces_.end())
if (has_command(full_interface_name))
{
if (
mirror_command_to_state(full_interface_name, joint_state.get()->get_data_type()) !=
Expand All @@ -561,29 +533,22 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
{
const auto & mimic_joint_name = joints.at(mimic_joint.joint_index).name;
const auto & mimicked_joint_name = joints.at(mimic_joint.mimicked_joint_index).name;
if (
joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION) !=
joint_state_interfaces_.end())
if (has_state(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION))
{
set_state(
mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION,
mimic_joint.offset +
mimic_joint.multiplier *
get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_POSITION));
}
if (
joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY) !=
joint_state_interfaces_.end())
if (has_state(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY))
{
set_state(
mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY,
mimic_joint.multiplier *
get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_VELOCITY));
}
if (
joint_state_interfaces_.find(
mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) !=
joint_state_interfaces_.end())
if (has_state(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION))
{
set_state(
mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION,
Expand Down Expand Up @@ -624,7 +589,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur
for (const auto & gpio_command : gpio_commands_)
{
const std::string & name = gpio_command.get()->get_name();
if (gpio_state_interfaces_.find(name) != gpio_state_interfaces_.end())
if (has_state(name))
{
if (mirror_command_to_state(name, gpio_command.get()->get_data_type()) != return_type::OK)
{
Expand Down
15 changes: 2 additions & 13 deletions hardware_interface/test/mock_components/test_generic_system.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -808,6 +808,8 @@ class TestGenericSystem : public ::testing::Test
hardware_interface::DEFAULT_REGISTRY_KEY);
}

void TearDown() override { node_.reset(); }

std::string hardware_system_2dof_;
std::string hardware_system_2dof_asymetric_;
std::string hardware_system_2dof_standard_interfaces_;
Expand Down Expand Up @@ -847,19 +849,6 @@ class ResourceStorage;
class TestableResourceManager : public hardware_interface::ResourceManager
{
public:
friend TestGenericSystem;

FRIEND_TEST(TestGenericSystem, generic_system_2dof_symetric_interfaces);
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_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_mock_command);
FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True);

explicit TestableResourceManager(rclcpp::Node::SharedPtr node)
: hardware_interface::ResourceManager(
node->get_node_clock_interface(), node->get_node_logging_interface())
Expand Down
Loading