From afc5c268c450b74c4425b01dfea4972262adae94 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 19 Sep 2025 18:57:50 +0000 Subject: [PATCH 01/21] Change mock hardware to also save the joint names --- hardware_interface/include/hardware_interface/hardware_info.hpp | 2 ++ hardware_interface/src/component_parser.cpp | 2 ++ 2 files changed, 4 insertions(+) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 801dd0ff28..3941cf5361 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -55,7 +55,9 @@ struct InterfaceInfo /// @brief This structure stores information about a joint that is mimicking another joint struct MimicJoint { + std::string joint_name; std::size_t joint_index; + std::string mimicked_joint_name; std::size_t mimicked_joint_index; double multiplier = 1.0; double offset = 0.0; diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 70c0ad0185..586eb8c4d0 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -994,7 +994,9 @@ std::vector parse_control_resources_from_urdf(const std::string & }; MimicJoint mimic_joint; + mimic_joint.joint_name = joint.name; mimic_joint.joint_index = i; + mimic_joint.mimicked_joint_name = urdf_joint->mimic->joint_name; mimic_joint.mimicked_joint_index = find_joint(urdf_joint->mimic->joint_name); mimic_joint.multiplier = urdf_joint->mimic->multiplier; mimic_joint.offset = urdf_joint->mimic->offset; From 042cc99a56a4b3ef0dce4244850ca00b310a9922 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 19 Sep 2025 18:58:05 +0000 Subject: [PATCH 02/21] Initial rewrite of the system --- .../mock_components/generic_system.hpp | 39 +- .../src/mock_components/generic_system.cpp | 479 +++++++----------- 2 files changed, 197 insertions(+), 321 deletions(-) diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index 5919120f42..c654c020b4 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -45,9 +45,8 @@ class GenericSystem : public hardware_interface::SystemInterface CallbackReturn on_init( const hardware_interface::HardwareComponentInterfaceParams & params) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; + std::vector + export_unlisted_command_interface_descriptions() override; return_type prepare_command_mode_switch( const std::vector & start_interfaces, @@ -76,50 +75,20 @@ class GenericSystem : public hardware_interface::SystemInterface hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; - /// The size of this vector is (standard_interfaces_.size() x nr_joints) - std::vector> joint_command_values_; - std::vector> joint_state_values_; - std::vector other_interfaces_; - /// The size of this vector is (other_interfaces_.size() x nr_joints) - std::vector> other_command_values_; - std::vector> other_state_values_; - std::vector sensor_interfaces_; - /// The size of this vector is (sensor_interfaces_.size() x nr_joints) - std::vector> sensor_mock_command_values_; - std::vector> sensor_state_values_; - - std::vector gpio_interfaces_; - /// The size of this vector is (gpio_interfaces_.size() x nr_joints) - std::vector> gpio_mock_command_values_; - std::vector> gpio_command_values_; - std::vector> gpio_state_values_; + std::vector gpio_mock_interfaces_; private: - template - bool get_interface( - const std::string & name, const std::vector & interface_list, - const std::string & interface_name, const size_t vector_index, - std::vector> & values, std::vector & interfaces); - - void initialize_storage_vectors( - std::vector> & commands, std::vector> & states, - const std::vector & interfaces, - const std::vector & component_infos); - - template bool populate_interfaces( const std::vector & components, - std::vector & interfaces, std::vector> & storage, - std::vector & target_interfaces, bool using_state_interfaces); + std::vector & command_interface_descriptions) const; bool use_mock_gpio_command_interfaces_; bool use_mock_sensor_command_interfaces_; double position_state_following_offset_; std::string custom_interface_with_following_offset_; - size_t index_custom_interface_with_following_offset_; bool calculate_dynamics_; std::vector joint_control_mode_; diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index a05fc40142..bc26653479 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -17,6 +17,7 @@ #include "mock_components/generic_system.hpp" #include +#include #include #include #include @@ -118,23 +119,6 @@ CallbackReturn GenericSystem::on_init( custom_interface_with_following_offset_ = it->second; } } - // it's extremely improbable that std::distance results in this value - therefore default - index_custom_interface_with_following_offset_ = std::numeric_limits::max(); - - // Initialize storage for standard interfaces - initialize_storage_vectors( - joint_command_values_, joint_state_values_, standard_interfaces_, get_hardware_info().joints); - // set all values without initial values to 0 - for (auto i = 0u; i < get_hardware_info().joints.size(); i++) - { - for (auto j = 0u; j < standard_interfaces_.size(); j++) - { - if (std::isnan(joint_state_values_[j][i])) - { - joint_state_values_[j][i] = 0.0; - } - } - } // search for non-standard joint interfaces for (const auto & joint : get_hardware_info().joints) @@ -146,10 +130,6 @@ CallbackReturn GenericSystem::on_init( populate_non_standard_interfaces(joint.state_interfaces, other_interfaces_); } - // Initialize storage for non-standard interfaces - initialize_storage_vectors( - other_command_values_, other_state_values_, other_interfaces_, get_hardware_info().joints); - // when following offset is used on custom interface then find its index if (!custom_interface_with_following_offset_.empty()) { @@ -157,12 +137,9 @@ CallbackReturn GenericSystem::on_init( other_interfaces_.begin(), other_interfaces_.end(), custom_interface_with_following_offset_); if (if_it != other_interfaces_.end()) { - index_custom_interface_with_following_offset_ = - static_cast(std::distance(other_interfaces_.begin(), if_it)); RCLCPP_INFO( - get_logger(), "Custom interface with following offset '%s' found at index: %zu.", - custom_interface_with_following_offset_.c_str(), - index_custom_interface_with_following_offset_); + get_logger(), "Custom interface with following offset '%s' found.", + custom_interface_with_following_offset_.c_str()); } else { @@ -185,147 +162,27 @@ CallbackReturn GenericSystem::on_init( } } } - initialize_storage_vectors( - sensor_mock_command_values_, sensor_state_values_, sensor_interfaces_, - get_hardware_info().sensors); - - // search for gpio interfaces - for (const auto & gpio : get_hardware_info().gpios) - { - // populate non-standard command interfaces to gpio_interfaces_ - populate_non_standard_interfaces(gpio.command_interfaces, gpio_interfaces_); - - // populate non-standard state interfaces to gpio_interfaces_ - populate_non_standard_interfaces(gpio.state_interfaces, gpio_interfaces_); - } - - // Mock gpio command interfaces - if (use_mock_gpio_command_interfaces_) - { - initialize_storage_vectors( - gpio_mock_command_values_, gpio_state_values_, gpio_interfaces_, get_hardware_info().gpios); - } - // Real gpio command interfaces - else - { - initialize_storage_vectors( - gpio_command_values_, gpio_state_values_, gpio_interfaces_, get_hardware_info().gpios); - } return CallbackReturn::SUCCESS; } -std::vector GenericSystem::export_state_interfaces() +std::vector +GenericSystem::export_unlisted_command_interface_descriptions() { - std::vector state_interfaces; - - // Joints' state interfaces - for (auto i = 0u; i < get_hardware_info().joints.size(); i++) - { - const auto & joint = get_hardware_info().joints[i]; - for (const auto & interface : joint.state_interfaces) - { - // Add interface: if not in the standard list then use "other" interface list - if (!get_interface( - joint.name, standard_interfaces_, interface.name, i, joint_state_values_, - state_interfaces)) - { - if (!get_interface( - joint.name, other_interfaces_, interface.name, i, other_state_values_, - state_interfaces)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. " - "This should never happen!"); - } - } - } - } - - // Sensor state interfaces - if (!populate_interfaces( - get_hardware_info().sensors, sensor_interfaces_, sensor_state_values_, state_interfaces, - true)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. This should never happen!"); - }; - - // GPIO state interfaces - if (!populate_interfaces( - get_hardware_info().gpios, gpio_interfaces_, gpio_state_values_, state_interfaces, true)) - { - throw std::runtime_error("Interface is not found in the gpio list. This should never happen!"); - } - - return state_interfaces; -} - -std::vector GenericSystem::export_command_interfaces() -{ - std::vector command_interfaces; - - // Joints' state interfaces - for (size_t i = 0; i < get_hardware_info().joints.size(); ++i) - { - const auto & joint = get_hardware_info().joints[i]; - for (const auto & interface : joint.command_interfaces) - { - // Add interface: if not in the standard list than use "other" interface list - if (!get_interface( - joint.name, standard_interfaces_, interface.name, i, joint_command_values_, - command_interfaces)) - { - if (!get_interface( - joint.name, other_interfaces_, interface.name, i, other_command_values_, - command_interfaces)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. " - "This should never happen!"); - } - } - } - } - // Set position control mode per default - joint_control_mode_.resize(get_hardware_info().joints.size(), POSITION_INTERFACE_INDEX); - + std::vector command_interface_descriptions; // Mock sensor command interfaces if (use_mock_sensor_command_interfaces_) { - if (!populate_interfaces( - get_hardware_info().sensors, sensor_interfaces_, sensor_mock_command_values_, - command_interfaces, true)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. This should never happen!"); - } + populate_interfaces(get_hardware_info().sensors, command_interface_descriptions); } // Mock gpio command interfaces (consider all state interfaces for command interfaces) if (use_mock_gpio_command_interfaces_) { - if (!populate_interfaces( - get_hardware_info().gpios, gpio_interfaces_, gpio_mock_command_values_, - command_interfaces, true)) - { - throw std::runtime_error( - "Interface is not found in the gpio list. This should never happen!"); - } - } - // GPIO command interfaces (real command interfaces) - else - { - if (!populate_interfaces( - get_hardware_info().gpios, gpio_interfaces_, gpio_command_values_, command_interfaces, - false)) - { - throw std::runtime_error( - "Interface is not found in the gpio list. This should never happen!"); - } + populate_interfaces(get_hardware_info().gpios, command_interface_descriptions); } - return command_interfaces; + return command_interface_descriptions; } return_type GenericSystem::prepare_command_mode_switch( @@ -433,6 +290,9 @@ return_type GenericSystem::perform_command_mode_switch( return hardware_interface::return_type::OK; } + // Set position control mode per default + joint_control_mode_.resize(get_hardware_info().joints.size(), POSITION_INTERFACE_INDEX); + for (const auto & key : start_interfaces) { // check if interface is joint @@ -472,87 +332,146 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur return return_type::OK; } - auto mirror_command_to_state = - [](auto & states_, auto commands_, size_t start_index = 0) -> return_type + for (size_t j = 0; j < get_hardware_info().joints.size(); ++j) { - for (size_t i = start_index; i < states_.size(); ++i) + if (calculate_dynamics_) { - for (size_t j = 0; j < states_[i].size(); ++j) + std::array joint_state_values_ = { + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}; + std::array joint_command_values_ = { + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}; + const auto joint_name = get_hardware_info().joints[j].name; { - if (!std::isnan(commands_[i][j])) + auto it_pos = std::find_if( + joint_commands_.begin(), joint_commands_.end(), + [this, &joint_name](const auto & command) + { + return command.get()->get_name() == + joint_name + "/" + standard_interfaces_[POSITION_INTERFACE_INDEX]; + }); + if (it_pos != joint_commands_.end()) { - states_[i][j] = commands_[i][j]; + joint_command_values_[POSITION_INTERFACE_INDEX] = get_command(it_pos->get()->get_name()); } - if (std::isinf(commands_[i][j])) + auto it_vel = std::find_if( + joint_commands_.begin(), joint_commands_.end(), + [this, &joint_name](const auto & command) + { + return command.get()->get_name() == + joint_name + "/" + standard_interfaces_[VELOCITY_INTERFACE_INDEX]; + }); + if (it_vel != joint_commands_.end()) { - return return_type::ERROR; + joint_command_values_[VELOCITY_INTERFACE_INDEX] = get_command(it_vel->get()->get_name()); + } + auto it_acc = std::find_if( + joint_commands_.begin(), joint_commands_.end(), + [this, &joint_name](const auto & command) + { + return command.get()->get_name() == + joint_name + "/" + standard_interfaces_[ACCELERATION_INTERFACE_INDEX]; + }); + if (it_acc != joint_commands_.end()) + { + joint_command_values_[ACCELERATION_INTERFACE_INDEX] = + get_command(it_acc->get()->get_name()); + } + } + { + auto it_pos = std::find_if( + joint_states_.begin(), joint_states_.end(), + [this, &joint_name](const auto & command) + { + return command.get()->get_name() == + joint_name + "/" + standard_interfaces_[POSITION_INTERFACE_INDEX]; + }); + if (it_pos != joint_states_.end()) + { + joint_state_values_[POSITION_INTERFACE_INDEX] = get_state(it_pos->get()->get_name()); + } + auto it_vel = std::find_if( + joint_states_.begin(), joint_states_.end(), + [this, &joint_name](const auto & command) + { + return command.get()->get_name() == + joint_name + "/" + standard_interfaces_[VELOCITY_INTERFACE_INDEX]; + }); + if (it_vel != joint_states_.end()) + { + joint_state_values_[VELOCITY_INTERFACE_INDEX] = get_state(it_vel->get()->get_name()); + } + auto it_acc = std::find_if( + joint_states_.begin(), joint_states_.end(), + [this, &joint_name](const auto & command) + { + return command.get()->get_name() == + joint_name + "/" + standard_interfaces_[ACCELERATION_INTERFACE_INDEX]; + }); + if (it_acc != joint_states_.end()) + { + joint_state_values_[ACCELERATION_INTERFACE_INDEX] = get_state(it_acc->get()->get_name()); } } - } - return return_type::OK; - }; - for (size_t j = 0; j < joint_state_values_[POSITION_INTERFACE_INDEX].size(); ++j) - { - if (calculate_dynamics_) - { switch (joint_control_mode_[j]) { case ACCELERATION_INTERFACE_INDEX: { // currently we do backward integration - joint_state_values_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only - joint_state_values_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + + joint_state_values_[POSITION_INTERFACE_INDEX] += // apply offset to positions only + joint_state_values_[VELOCITY_INTERFACE_INDEX] * period.seconds() + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); - joint_state_values_[VELOCITY_INTERFACE_INDEX][j] += - joint_state_values_[ACCELERATION_INTERFACE_INDEX][j] * period.seconds(); + joint_state_values_[VELOCITY_INTERFACE_INDEX] += + joint_state_values_[ACCELERATION_INTERFACE_INDEX] * period.seconds(); - if (!std::isnan(joint_command_values_[ACCELERATION_INTERFACE_INDEX][j])) + if (!std::isnan(joint_command_values_[ACCELERATION_INTERFACE_INDEX])) { - joint_state_values_[ACCELERATION_INTERFACE_INDEX][j] = - joint_command_values_[ACCELERATION_INTERFACE_INDEX][j]; + joint_state_values_[ACCELERATION_INTERFACE_INDEX] = + joint_command_values_[ACCELERATION_INTERFACE_INDEX]; } break; } case VELOCITY_INTERFACE_INDEX: { // currently we do backward integration - joint_state_values_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only - joint_state_values_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + + joint_state_values_[POSITION_INTERFACE_INDEX] += // apply offset to positions only + joint_state_values_[VELOCITY_INTERFACE_INDEX] * period.seconds() + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); - if (!std::isnan(joint_command_values_[VELOCITY_INTERFACE_INDEX][j])) + if (!std::isnan(joint_command_values_[VELOCITY_INTERFACE_INDEX])) { - const double old_velocity = joint_state_values_[VELOCITY_INTERFACE_INDEX][j]; + const double old_velocity = joint_state_values_[VELOCITY_INTERFACE_INDEX]; - joint_state_values_[VELOCITY_INTERFACE_INDEX][j] = - joint_command_values_[VELOCITY_INTERFACE_INDEX][j]; + joint_state_values_[VELOCITY_INTERFACE_INDEX] = + joint_command_values_[VELOCITY_INTERFACE_INDEX]; - joint_state_values_[ACCELERATION_INTERFACE_INDEX][j] = - (joint_state_values_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + joint_state_values_[ACCELERATION_INTERFACE_INDEX] = + (joint_state_values_[VELOCITY_INTERFACE_INDEX] - old_velocity) / period.seconds(); } break; } case POSITION_INTERFACE_INDEX: { - if (!std::isnan(joint_command_values_[POSITION_INTERFACE_INDEX][j])) + if (!std::isnan(joint_command_values_[POSITION_INTERFACE_INDEX])) { - const double old_position = joint_state_values_[POSITION_INTERFACE_INDEX][j]; - const double old_velocity = joint_state_values_[VELOCITY_INTERFACE_INDEX][j]; + const double old_position = joint_state_values_[POSITION_INTERFACE_INDEX]; + const double old_velocity = joint_state_values_[VELOCITY_INTERFACE_INDEX]; - joint_state_values_[POSITION_INTERFACE_INDEX][j] = // apply offset to positions only - joint_command_values_[POSITION_INTERFACE_INDEX][j] + + joint_state_values_[POSITION_INTERFACE_INDEX] = // apply offset to positions only + joint_command_values_[POSITION_INTERFACE_INDEX] + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); - joint_state_values_[VELOCITY_INTERFACE_INDEX][j] = - (joint_state_values_[POSITION_INTERFACE_INDEX][j] - old_position) / period.seconds(); + joint_state_values_[VELOCITY_INTERFACE_INDEX] = + (joint_state_values_[POSITION_INTERFACE_INDEX] - old_position) / period.seconds(); - joint_state_values_[ACCELERATION_INTERFACE_INDEX][j] = - (joint_state_values_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + joint_state_values_[ACCELERATION_INTERFACE_INDEX] = + (joint_state_values_[VELOCITY_INTERFACE_INDEX] - old_velocity) / period.seconds(); } break; } @@ -560,145 +479,133 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } else { - for (size_t k = 0; k < joint_state_values_[POSITION_INTERFACE_INDEX].size(); ++k) + for (const auto & joint_command : joint_command_interfaces_) { - if (!std::isnan(joint_command_values_[POSITION_INTERFACE_INDEX][k])) + if ( + joint_command.second.get_interface_name() == + standard_interfaces_[POSITION_INTERFACE_INDEX]) { - joint_state_values_[POSITION_INTERFACE_INDEX][k] = // apply offset to positions only - joint_command_values_[POSITION_INTERFACE_INDEX][k] + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ - : 0.0); + const std::string & name = joint_command.second.get_name(); + auto it = joint_state_interfaces_.find(name); + if (it != joint_state_interfaces_.end()) + { + set_state( + name, get_command(name) + (custom_interface_with_following_offset_.empty() + ? position_state_following_offset_ + : 0.0)); + } } } } } - // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 3 are position, + // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 2 are position, // velocity, and acceleration interface - if ( - mirror_command_to_state( - joint_state_values_, joint_command_values_, calculate_dynamics_ ? 3 : 1) != return_type::OK) + // Create a subvector of standard_interfaces_ with the given indices + std::vector skip_interfaces; + for (size_t i = 0; i < (calculate_dynamics_ ? 3 : 1); ++i) { - return return_type::ERROR; + skip_interfaces.push_back(standard_interfaces_[i]); } - - for (const auto & mimic_joint : get_hardware_info().mimic_joints) + // TODO(anyone): optimize by using joint_command_interfaces_/joint_state_interfaces_ map + for (const auto & joint_command : joint_commands_) { - for (auto i = 0u; i < joint_state_values_.size(); ++i) + if ( + std::find( + skip_interfaces.begin(), skip_interfaces.end(), + joint_command.get()->get_interface_name()) != skip_interfaces.end()) { - joint_state_values_[i][mimic_joint.joint_index] = - mimic_joint.offset + - mimic_joint.multiplier * joint_state_values_[i][mimic_joint.mimicked_joint_index]; + continue; } - } - - for (size_t i = 0; i < other_state_values_.size(); ++i) - { - for (size_t j = 0; j < other_state_values_[i].size(); ++j) + const std::string & full_interface_name = joint_command.get()->get_name(); + auto it = std::find_if( + joint_states_.begin(), joint_states_.end(), [&full_interface_name](const auto & state) + { return state.get()->get_name() == full_interface_name; }); + if (it != joint_states_.end()) { if ( - i == index_custom_interface_with_following_offset_ && - !std::isnan(joint_command_values_[POSITION_INTERFACE_INDEX][j])) + joint_command.get()->get_interface_name() == + standard_interfaces_[POSITION_INTERFACE_INDEX] && + custom_interface_with_following_offset_ == full_interface_name) { - other_state_values_[i][j] = - joint_command_values_[POSITION_INTERFACE_INDEX][j] + position_state_following_offset_; + set_state( + full_interface_name, get_command(full_interface_name) + position_state_following_offset_); } - else if (!std::isnan(other_command_values_[i][j])) + else { - other_state_values_[i][j] = other_command_values_[i][j]; + set_state(full_interface_name, get_command(full_interface_name)); } } } - if (use_mock_sensor_command_interfaces_) - { - mirror_command_to_state(sensor_state_values_, sensor_mock_command_values_); - } - - if (use_mock_gpio_command_interfaces_) - { - mirror_command_to_state(gpio_state_values_, gpio_mock_command_values_); - } - else + for (const auto & mimic_joint : get_hardware_info().mimic_joints) { - // do loopback on all gpio interfaces - mirror_command_to_state(gpio_state_values_, gpio_command_values_); + set_state( + mimic_joint.joint_name, + mimic_joint.offset + mimic_joint.multiplier * get_state(mimic_joint.mimicked_joint_name)); } - return return_type::OK; -} - -// Private methods -template -bool GenericSystem::get_interface( - const std::string & name, const std::vector & interface_list, - const std::string & interface_name, const size_t vector_index, - std::vector> & values, std::vector & interfaces) -{ - auto it = std::find(interface_list.begin(), interface_list.end(), interface_name); - if (it != interface_list.end()) + if (use_mock_sensor_command_interfaces_) { - auto j = static_cast(std::distance(interface_list.begin(), it)); - interfaces.emplace_back(name, *it, &values[j][vector_index]); - return true; + // do loopback on all sensor interfaces as we have exported them all + for (const auto & sensor_state : sensor_states_) + { + const std::string & name = sensor_state.get()->get_name(); + set_state(name, get_command(name)); + } } - return false; -} -void GenericSystem::initialize_storage_vectors( - std::vector> & commands, std::vector> & states, - const std::vector & interfaces, - const std::vector & component_infos) -{ - // Initialize storage for all joints, regardless of their existence - commands.resize(interfaces.size()); - states.resize(interfaces.size()); - for (auto i = 0u; i < interfaces.size(); i++) + if (use_mock_gpio_command_interfaces_) { - commands[i].resize(component_infos.size(), std::numeric_limits::quiet_NaN()); - states[i].resize(component_infos.size(), std::numeric_limits::quiet_NaN()); + // do loopback on all gpio interfaces as we have exported them all + // TODO(anyone): how to pass data_type? + // from gpio_command_interfaces_ maybe? + for (const auto & gpio_command : gpio_commands_) + { + const std::string & name = gpio_command.get()->get_name(); + set_state(name, get_command(name)); + } } - - // Initialize with values from URDF - for (auto i = 0u; i < component_infos.size(); i++) + else { - const auto & component = component_infos[i]; - for (const auto & interface : component.state_interfaces) + // do loopback on all gpio interfaces, where they exist + for (const auto & gpio_command : gpio_commands_) { - auto it = std::find(interfaces.begin(), interfaces.end(), interface.name); - - // If interface name is found in the interfaces list - if (it != interfaces.end()) + // TODO(anyone): how to pass data_type? + // from gpio_command_interfaces_ maybe? + const std::string & name = gpio_command.get()->get_name(); + auto it = std::find_if( + gpio_states_.begin(), gpio_states_.end(), + [&name](const auto & state) { return state.get()->get_name() == name; }); + if (it != gpio_states_.end()) { - auto index = static_cast(std::distance(interfaces.begin(), it)); - - // Check the initial_value param is used - if (!interface.initial_value.empty()) - { - states[index][i] = hardware_interface::stod(interface.initial_value); - } + set_state(name, get_command(name)); } } } + + return return_type::OK; } -template +// Private methods bool GenericSystem::populate_interfaces( const std::vector & components, - std::vector & interface_names, std::vector> & storage, - std::vector & target_interfaces, bool using_state_interfaces) + std::vector & command_interface_descriptions) const { - for (auto i = 0u; i < components.size(); i++) + for (const auto & component : components) { - const auto & component = components[i]; - const auto interfaces = - (using_state_interfaces) ? component.state_interfaces : component.command_interfaces; - for (const auto & interface : interfaces) + for (const auto & interface : component.state_interfaces) { - if (!get_interface( - component.name, interface_names, interface.name, i, storage, target_interfaces)) + // add to list if not already there + if ( + std::find_if( + command_interface_descriptions.begin(), command_interface_descriptions.end(), + [&component, &interface](const auto & desc) + { return (desc.get_name() == (component.name + "/" + interface.name)); }) == + command_interface_descriptions.end()) { - return false; + hardware_interface::InterfaceDescription description(component.name, interface); + command_interface_descriptions.push_back(description); } } } From 97eb5163e76ca136982209f57d52a676eeb32bdb Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 19 Sep 2025 18:58:17 +0000 Subject: [PATCH 03/21] Switch some EXPECT/ASSERT macros --- .../mock_components/test_generic_system.cpp | 160 +++++++++--------- 1 file changed, 80 insertions(+), 80 deletions(-) diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index e88df213cd..1995af2d32 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -869,14 +869,14 @@ void generic_system_functional_test( hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); // State interfaces without initial value are set to 0 - ASSERT_EQ(3.45, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(2.78, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); - ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); - ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); - ASSERT_TRUE(std::isnan(j2p_c.get_optional().value())); - ASSERT_TRUE(std::isnan(j2v_c.get_optional().value())); + EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_EQ(0.0, j1v_s.get_optional().value()); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + EXPECT_EQ(0.0, j2v_s.get_optional().value()); + EXPECT_TRUE(std::isnan(j1p_c.get_optional().value())); + EXPECT_TRUE(std::isnan(j1v_c.get_optional().value())); + EXPECT_TRUE(std::isnan(j2p_c.get_optional().value())); + EXPECT_TRUE(std::isnan(j2v_c.get_optional().value())); // set some new values in commands ASSERT_TRUE(j1p_c.set_value(0.11)); @@ -885,36 +885,36 @@ void generic_system_functional_test( ASSERT_TRUE(j2v_c.set_value(0.44)); // State values should not be changed - ASSERT_EQ(3.45, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(2.78, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); - ASSERT_EQ(0.11, j1p_c.get_optional().value()); - ASSERT_EQ(0.22, j1v_c.get_optional().value()); - ASSERT_EQ(0.33, j2p_c.get_optional().value()); - ASSERT_EQ(0.44, j2v_c.get_optional().value()); + EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_EQ(0.0, j1v_s.get_optional().value()); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + EXPECT_EQ(0.0, j2v_s.get_optional().value()); + EXPECT_EQ(0.11, j1p_c.get_optional().value()); + EXPECT_EQ(0.22, j1v_c.get_optional().value()); + EXPECT_EQ(0.33, j2p_c.get_optional().value()); + EXPECT_EQ(0.44, j2v_c.get_optional().value()); // write() does not change values - EXPECT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); - ASSERT_EQ(3.45, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(2.78, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); - ASSERT_EQ(0.11, j1p_c.get_optional().value()); - ASSERT_EQ(0.22, j1v_c.get_optional().value()); - ASSERT_EQ(0.33, j2p_c.get_optional().value()); - ASSERT_EQ(0.44, j2v_c.get_optional().value()); + ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_EQ(0.0, j1v_s.get_optional().value()); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + EXPECT_EQ(0.0, j2v_s.get_optional().value()); + EXPECT_EQ(0.11, j1p_c.get_optional().value()); + EXPECT_EQ(0.22, j1v_c.get_optional().value()); + EXPECT_EQ(0.33, j2p_c.get_optional().value()); + EXPECT_EQ(0.44, j2v_c.get_optional().value()); // read() mirrors commands + offset to states - EXPECT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); - ASSERT_EQ(0.11 + offset, j1p_s.get_optional().value()); - ASSERT_EQ(0.22, j1v_s.get_optional().value()); - ASSERT_EQ(0.33 + offset, j2p_s.get_optional().value()); - ASSERT_EQ(0.44, j2v_s.get_optional().value()); - ASSERT_EQ(0.11, j1p_c.get_optional().value()); - ASSERT_EQ(0.22, j1v_c.get_optional().value()); - ASSERT_EQ(0.33, j2p_c.get_optional().value()); - ASSERT_EQ(0.44, j2v_c.get_optional().value()); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(0.11 + offset, j1p_s.get_optional().value()); + EXPECT_EQ(0.22, j1v_s.get_optional().value()); + EXPECT_EQ(0.33 + offset, j2p_s.get_optional().value()); + EXPECT_EQ(0.44, j2v_s.get_optional().value()); + EXPECT_EQ(0.11, j1p_c.get_optional().value()); + EXPECT_EQ(0.22, j1v_c.get_optional().value()); + EXPECT_EQ(0.33, j2p_c.get_optional().value()); + EXPECT_EQ(0.44, j2v_c.get_optional().value()); // set some new values in commands ASSERT_TRUE(j1p_c.set_value(0.55)); @@ -923,14 +923,14 @@ void generic_system_functional_test( ASSERT_TRUE(j2v_c.set_value(0.88)); // state values should not be changed - ASSERT_EQ(0.11 + offset, j1p_s.get_optional().value()); - ASSERT_EQ(0.22, j1v_s.get_optional().value()); - ASSERT_EQ(0.33 + offset, j2p_s.get_optional().value()); - ASSERT_EQ(0.44, j2v_s.get_optional().value()); - ASSERT_EQ(0.55, j1p_c.get_optional().value()); - ASSERT_EQ(0.66, j1v_c.get_optional().value()); - ASSERT_EQ(0.77, j2p_c.get_optional().value()); - ASSERT_EQ(0.88, j2v_c.get_optional().value()); + EXPECT_EQ(0.11 + offset, j1p_s.get_optional().value()); + EXPECT_EQ(0.22, j1v_s.get_optional().value()); + EXPECT_EQ(0.33 + offset, j2p_s.get_optional().value()); + EXPECT_EQ(0.44, j2v_s.get_optional().value()); + EXPECT_EQ(0.55, j1p_c.get_optional().value()); + EXPECT_EQ(0.66, j1v_c.get_optional().value()); + EXPECT_EQ(0.77, j2p_c.get_optional().value()); + EXPECT_EQ(0.88, j2v_c.get_optional().value()); deactivate_components(rm, {component_name}); status_map = rm.get_components_status(); @@ -975,10 +975,10 @@ void generic_system_error_group_test( hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); // State interfaces without initial value are set to 0 - ASSERT_EQ(3.45, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(2.78, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); + EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_EQ(0.0, j1v_s.get_optional().value()); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + EXPECT_EQ(0.0, j2v_s.get_optional().value()); ASSERT_TRUE(std::isnan(j1p_c.get_optional().value())); ASSERT_TRUE(std::isnan(j1v_c.get_optional().value())); ASSERT_TRUE(std::isnan(j2p_c.get_optional().value())); @@ -991,36 +991,36 @@ void generic_system_error_group_test( ASSERT_TRUE(j2v_c.set_value(0.44)); // State values should not be changed - ASSERT_EQ(3.45, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(2.78, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); - ASSERT_EQ(0.11, j1p_c.get_optional().value()); - ASSERT_EQ(0.22, j1v_c.get_optional().value()); - ASSERT_EQ(0.33, j2p_c.get_optional().value()); - ASSERT_EQ(0.44, j2v_c.get_optional().value()); + EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_EQ(0.0, j1v_s.get_optional().value()); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + EXPECT_EQ(0.0, j2v_s.get_optional().value()); + EXPECT_EQ(0.11, j1p_c.get_optional().value()); + EXPECT_EQ(0.22, j1v_c.get_optional().value()); + EXPECT_EQ(0.33, j2p_c.get_optional().value()); + EXPECT_EQ(0.44, j2v_c.get_optional().value()); // write() does not change values - EXPECT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); - ASSERT_EQ(3.45, j1p_s.get_optional().value()); - ASSERT_EQ(0.0, j1v_s.get_optional().value()); - ASSERT_EQ(2.78, j2p_s.get_optional().value()); - ASSERT_EQ(0.0, j2v_s.get_optional().value()); - ASSERT_EQ(0.11, j1p_c.get_optional().value()); - ASSERT_EQ(0.22, j1v_c.get_optional().value()); - ASSERT_EQ(0.33, j2p_c.get_optional().value()); - ASSERT_EQ(0.44, j2v_c.get_optional().value()); + ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_EQ(0.0, j1v_s.get_optional().value()); + EXPECT_EQ(2.78, j2p_s.get_optional().value()); + EXPECT_EQ(0.0, j2v_s.get_optional().value()); + EXPECT_EQ(0.11, j1p_c.get_optional().value()); + EXPECT_EQ(0.22, j1v_c.get_optional().value()); + EXPECT_EQ(0.33, j2p_c.get_optional().value()); + EXPECT_EQ(0.44, j2v_c.get_optional().value()); // read() mirrors commands to states - EXPECT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); - ASSERT_EQ(0.11, j1p_s.get_optional().value()); - ASSERT_EQ(0.22, j1v_s.get_optional().value()); - ASSERT_EQ(0.33, j2p_s.get_optional().value()); - ASSERT_EQ(0.44, j2v_s.get_optional().value()); - ASSERT_EQ(0.11, j1p_c.get_optional().value()); - ASSERT_EQ(0.22, j1v_c.get_optional().value()); - ASSERT_EQ(0.33, j2p_c.get_optional().value()); - ASSERT_EQ(0.44, j2v_c.get_optional().value()); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(0.11, j1p_s.get_optional().value()); + EXPECT_EQ(0.22, j1v_s.get_optional().value()); + EXPECT_EQ(0.33, j2p_s.get_optional().value()); + EXPECT_EQ(0.44, j2v_s.get_optional().value()); + EXPECT_EQ(0.11, j1p_c.get_optional().value()); + EXPECT_EQ(0.22, j1v_c.get_optional().value()); + EXPECT_EQ(0.33, j2p_c.get_optional().value()); + EXPECT_EQ(0.44, j2v_c.get_optional().value()); // set some new values in commands ASSERT_TRUE(j1p_c.set_value(0.55)); @@ -1029,14 +1029,14 @@ void generic_system_error_group_test( ASSERT_TRUE(j2v_c.set_value(0.88)); // state values should not be changed - ASSERT_EQ(0.11, j1p_s.get_optional().value()); - ASSERT_EQ(0.22, j1v_s.get_optional().value()); - ASSERT_EQ(0.33, j2p_s.get_optional().value()); - ASSERT_EQ(0.44, j2v_s.get_optional().value()); - ASSERT_EQ(0.55, j1p_c.get_optional().value()); - ASSERT_EQ(0.66, j1v_c.get_optional().value()); - ASSERT_EQ(0.77, j2p_c.get_optional().value()); - ASSERT_EQ(0.88, j2v_c.get_optional().value()); + EXPECT_EQ(0.11, j1p_s.get_optional().value()); + EXPECT_EQ(0.22, j1v_s.get_optional().value()); + EXPECT_EQ(0.33, j2p_s.get_optional().value()); + EXPECT_EQ(0.44, j2v_s.get_optional().value()); + EXPECT_EQ(0.55, j1p_c.get_optional().value()); + EXPECT_EQ(0.66, j1v_c.get_optional().value()); + EXPECT_EQ(0.77, j2p_c.get_optional().value()); + EXPECT_EQ(0.88, j2v_c.get_optional().value()); // Error testing ASSERT_TRUE(j1p_c.set_value(std::numeric_limits::infinity())); From f13cf89a3edc30fa716501cc29f70cc07e7d126b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 19 Sep 2025 19:15:24 +0000 Subject: [PATCH 04/21] Parse initial values in on_activate --- .../mock_components/generic_system.hpp | 3 +++ .../src/mock_components/generic_system.cpp | 21 +++++++++++++++++++ 2 files changed, 24 insertions(+) diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index c654c020b4..2c3219f8f6 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -45,6 +45,9 @@ class GenericSystem : public hardware_interface::SystemInterface CallbackReturn on_init( const hardware_interface::HardwareComponentInterfaceParams & params) override; + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + std::vector export_unlisted_command_interface_descriptions() override; diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index bc26653479..7d1fd4fd3b 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -324,6 +324,27 @@ return_type GenericSystem::perform_command_mode_switch( return hardware_interface::return_type::OK; } +hardware_interface::CallbackReturn GenericSystem::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + for (const auto & joint_state : joint_state_interfaces_) + { + const std::string & name = joint_state.second.get_name(); + // TODO(anyone): consider initial value for non-double interfaces as well + double val = joint_state.second.interface_info.initial_value.empty() + ? 0.0 + : hardware_interface::stod(joint_state.second.interface_info.initial_value); + if ( + joint_state.second.get_interface_name() == standard_interfaces_[POSITION_INTERFACE_INDEX] && + custom_interface_with_following_offset_.empty()) + { + val += position_state_following_offset_; + } + set_state(name, val); + } + return hardware_interface::CallbackReturn::SUCCESS; +} + return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { if (command_propagation_disabled_) From bba6291fcbb87abe9575599cd62ebb6158dfdeb4 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 19 Sep 2025 19:15:37 +0000 Subject: [PATCH 05/21] Some more asserts --- .../test/mock_components/test_generic_system.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 1995af2d32..c197815a8d 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -843,18 +843,18 @@ void generic_system_functional_test( TestableResourceManager rm(node, urdf); // check is hardware is configured auto status_map = rm.get_components_status(); - EXPECT_EQ( + ASSERT_EQ( status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::UNCONFIGURED); EXPECT_EQ(status_map[component_name].rw_rate, 100u); configure_components(rm, {component_name}); status_map = rm.get_components_status(); - EXPECT_EQ( + ASSERT_EQ( status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); EXPECT_EQ(status_map[component_name].rw_rate, 100u); activate_components(rm, {component_name}); status_map = rm.get_components_status(); - EXPECT_EQ( + ASSERT_EQ( status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); EXPECT_EQ(status_map[component_name].rw_rate, 100u); @@ -949,17 +949,17 @@ void generic_system_error_group_test( auto status_map = rm.get_components_status(); for (auto component : {component1, component2}) { - EXPECT_EQ( + ASSERT_EQ( status_map[component].state.label(), hardware_interface::lifecycle_state_names::UNCONFIGURED); EXPECT_EQ(status_map[component].rw_rate, 200u); configure_components(rm, {component}); status_map = rm.get_components_status(); - EXPECT_EQ( + ASSERT_EQ( status_map[component].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); EXPECT_EQ(status_map[component].rw_rate, 200u); activate_components(rm, {component}); status_map = rm.get_components_status(); - EXPECT_EQ( + ASSERT_EQ( status_map[component].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); EXPECT_EQ(status_map[component].rw_rate, 200u); } From 942071283b8a4f77aeb3065853729dea2b084791 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 19 Sep 2025 19:40:48 +0000 Subject: [PATCH 06/21] Fix mimicked interfaces and standard interfaces if isnan/isinf --- .../src/mock_components/generic_system.cpp | 58 +++++++++++++++---- 1 file changed, 47 insertions(+), 11 deletions(-) diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 7d1fd4fd3b..25fd634610 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -523,6 +523,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 2 are position, // velocity, and acceleration interface // Create a subvector of standard_interfaces_ with the given indices + // TODO(anyone): preallocate this std::vector skip_interfaces; for (size_t i = 0; i < (calculate_dynamics_ ? 3 : 1); ++i) { @@ -544,26 +545,61 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur { return state.get()->get_name() == full_interface_name; }); if (it != joint_states_.end()) { - if ( - joint_command.get()->get_interface_name() == - standard_interfaces_[POSITION_INTERFACE_INDEX] && - custom_interface_with_following_offset_ == full_interface_name) + auto cmd = get_command(full_interface_name); + if (std::isinf(cmd)) { - set_state( - full_interface_name, get_command(full_interface_name) + position_state_following_offset_); + return return_type::ERROR; } - else + if (std::isfinite(cmd)) { - set_state(full_interface_name, get_command(full_interface_name)); + if ( + joint_command.get()->get_interface_name() == + standard_interfaces_[POSITION_INTERFACE_INDEX] && + custom_interface_with_following_offset_ == full_interface_name) + { + cmd += position_state_following_offset_; + } + set_state(full_interface_name, cmd); } } } for (const auto & mimic_joint : get_hardware_info().mimic_joints) { - set_state( - mimic_joint.joint_name, - mimic_joint.offset + mimic_joint.multiplier * get_state(mimic_joint.mimicked_joint_name)); + if ( + joint_state_interfaces_.find( + mimic_joint.joint_name + "/" + standard_interfaces_[POSITION_INTERFACE_INDEX]) != + joint_state_interfaces_.end()) + { + set_state( + mimic_joint.joint_name + "/" + standard_interfaces_[POSITION_INTERFACE_INDEX], + mimic_joint.offset + + mimic_joint.multiplier * get_state( + mimic_joint.mimicked_joint_name + "/" + + standard_interfaces_[POSITION_INTERFACE_INDEX])); + } + if ( + joint_state_interfaces_.find( + mimic_joint.joint_name + "/" + standard_interfaces_[VELOCITY_INTERFACE_INDEX]) != + joint_state_interfaces_.end()) + { + set_state( + mimic_joint.joint_name + "/" + standard_interfaces_[VELOCITY_INTERFACE_INDEX], + mimic_joint.multiplier * get_state( + mimic_joint.mimicked_joint_name + "/" + + standard_interfaces_[VELOCITY_INTERFACE_INDEX])); + } + if ( + joint_state_interfaces_.find( + mimic_joint.joint_name + "/" + standard_interfaces_[ACCELERATION_INTERFACE_INDEX]) != + joint_state_interfaces_.end()) + { + set_state( + mimic_joint.joint_name + "/" + standard_interfaces_[ACCELERATION_INTERFACE_INDEX], + mimic_joint.multiplier * get_state( + mimic_joint.mimicked_joint_name + "/" + + standard_interfaces_[ACCELERATION_INTERFACE_INDEX])); + } } if (use_mock_sensor_command_interfaces_) From a941b8f0265c7f8425df6b2c04d5f78ae16f81f8 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 19 Sep 2025 20:19:01 +0000 Subject: [PATCH 07/21] Fix logic for custom_interface_with_following_offset_ --- .../doc/mock_components_userdoc.rst | 6 +++-- .../src/mock_components/generic_system.cpp | 27 ++++++++++--------- .../mock_components/test_generic_system.cpp | 12 ++++----- 3 files changed, 24 insertions(+), 21 deletions(-) diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index 8eca65c52b..f5cd6b1196 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -76,7 +76,7 @@ calculate_dynamics (optional; boolean; default: false) Calculation of states from commands by using Euler-forward integration or finite differences. custom_interface_with_following_offset (optional; string; default: "") - Mapping of offsetted commands to a custom interface. + Mapping of offsetted commands to a custom interface (see ``position_state_following_offset``). disable_commands (optional; boolean; default: false) Disables mirroring commands to states. @@ -92,7 +92,9 @@ mock_sensor_commands (optional; boolean; default: false) Those interfaces are usually used by a :ref:`forward controller ` to provide access from ROS-world. position_state_following_offset (optional; double; default: 0.0) - Following offset added to the commanded values when mirrored to states. Only applied, if ``custom_interface_with_following_offset`` is false. + Following offset added to the state values when commands are mirrored to states. + If ``custom_interface_with_following_offset`` is empty, the offset is applied to the ``position`` state interface. + If a custom interface is set, the ``position`` state value + offset is applied to that interface. Per-Interface Parameters ######################## diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 25fd634610..46ee4cb7f9 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -530,20 +530,20 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur skip_interfaces.push_back(standard_interfaces_[i]); } // TODO(anyone): optimize by using joint_command_interfaces_/joint_state_interfaces_ map - for (const auto & joint_command : joint_commands_) + for (const auto & joint_state : joint_states_) { if ( std::find( - skip_interfaces.begin(), skip_interfaces.end(), - joint_command.get()->get_interface_name()) != skip_interfaces.end()) + skip_interfaces.begin(), skip_interfaces.end(), joint_state.get()->get_interface_name()) != + skip_interfaces.end()) { continue; } - const std::string & full_interface_name = joint_command.get()->get_name(); + const std::string & full_interface_name = joint_state.get()->get_name(); auto it = std::find_if( - joint_states_.begin(), joint_states_.end(), [&full_interface_name](const auto & state) + joint_commands_.begin(), joint_commands_.end(), [&full_interface_name](const auto & state) { return state.get()->get_name() == full_interface_name; }); - if (it != joint_states_.end()) + if (it != joint_commands_.end()) { auto cmd = get_command(full_interface_name); if (std::isinf(cmd)) @@ -552,16 +552,17 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } if (std::isfinite(cmd)) { - if ( - joint_command.get()->get_interface_name() == - standard_interfaces_[POSITION_INTERFACE_INDEX] && - custom_interface_with_following_offset_ == full_interface_name) - { - cmd += position_state_following_offset_; - } set_state(full_interface_name, cmd); } } + if (custom_interface_with_following_offset_ == joint_state.get()->get_interface_name()) + { + const auto cmd = get_command( + joint_state.get()->get_prefix_name() + "/" + + standard_interfaces_[POSITION_INTERFACE_INDEX]) + + position_state_following_offset_; + set_state(full_interface_name, cmd); + } } for (const auto & mimic_joint : get_hardware_info().mimic_joints) diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index c197815a8d..9671477e1a 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -869,9 +869,9 @@ void generic_system_functional_test( hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); // State interfaces without initial value are set to 0 - EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_EQ(3.45 + offset, j1p_s.get_optional().value()); EXPECT_EQ(0.0, j1v_s.get_optional().value()); - EXPECT_EQ(2.78, j2p_s.get_optional().value()); + EXPECT_EQ(2.78 + offset, j2p_s.get_optional().value()); EXPECT_EQ(0.0, j2v_s.get_optional().value()); EXPECT_TRUE(std::isnan(j1p_c.get_optional().value())); EXPECT_TRUE(std::isnan(j1v_c.get_optional().value())); @@ -885,9 +885,9 @@ void generic_system_functional_test( ASSERT_TRUE(j2v_c.set_value(0.44)); // State values should not be changed - EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_EQ(3.45 + offset, j1p_s.get_optional().value()); EXPECT_EQ(0.0, j1v_s.get_optional().value()); - EXPECT_EQ(2.78, j2p_s.get_optional().value()); + EXPECT_EQ(2.78 + offset, j2p_s.get_optional().value()); EXPECT_EQ(0.0, j2v_s.get_optional().value()); EXPECT_EQ(0.11, j1p_c.get_optional().value()); EXPECT_EQ(0.22, j1v_c.get_optional().value()); @@ -896,9 +896,9 @@ void generic_system_functional_test( // write() does not change values ASSERT_EQ(rm.write(TIME, PERIOD).result, hardware_interface::return_type::OK); - EXPECT_EQ(3.45, j1p_s.get_optional().value()); + EXPECT_EQ(3.45 + offset, j1p_s.get_optional().value()); EXPECT_EQ(0.0, j1v_s.get_optional().value()); - EXPECT_EQ(2.78, j2p_s.get_optional().value()); + EXPECT_EQ(2.78 + offset, j2p_s.get_optional().value()); EXPECT_EQ(0.0, j2v_s.get_optional().value()); EXPECT_EQ(0.11, j1p_c.get_optional().value()); EXPECT_EQ(0.22, j1v_c.get_optional().value()); From 17df7308c674cb9d8e2645fc1e47845413b5340a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 19 Sep 2025 20:21:12 +0000 Subject: [PATCH 08/21] Fix gpio mock --- .../src/mock_components/generic_system.cpp | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 46ee4cb7f9..a02814d28b 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -618,9 +618,9 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur // do loopback on all gpio interfaces as we have exported them all // TODO(anyone): how to pass data_type? // from gpio_command_interfaces_ maybe? - for (const auto & gpio_command : gpio_commands_) + for (const auto & gpio_state : gpio_states_) { - const std::string & name = gpio_command.get()->get_name(); + const std::string & name = gpio_state.get()->get_name(); set_state(name, get_command(name)); } } @@ -652,17 +652,25 @@ bool GenericSystem::populate_interfaces( { for (const auto & component : components) { - for (const auto & interface : component.state_interfaces) + for (const auto & state_interface : component.state_interfaces) { - // add to list if not already there + // add to state interface to command interface list if not already there if ( std::find_if( command_interface_descriptions.begin(), command_interface_descriptions.end(), - [&component, &interface](const auto & desc) - { return (desc.get_name() == (component.name + "/" + interface.name)); }) == - command_interface_descriptions.end()) + [&component, &state_interface](const auto & desc) + { return (desc.get_name() == (component.name + "/" + state_interface.name)); }) == + command_interface_descriptions.end() && + std::find_if( + component.command_interfaces.begin(), component.command_interfaces.end(), + [&component, &state_interface](const auto & cmd_if) + { + return ( + (component.name + "/" + cmd_if.name) == + (component.name + "/" + state_interface.name)); + }) == component.command_interfaces.end()) { - hardware_interface::InterfaceDescription description(component.name, interface); + hardware_interface::InterfaceDescription description(component.name, state_interface); command_interface_descriptions.push_back(description); } } From 25d4da5d8b9f45714323f5ff632cd58b38bc9b2d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 19 Sep 2025 20:33:18 +0000 Subject: [PATCH 09/21] Fix calculate dynamics --- .../src/mock_components/generic_system.cpp | 39 ++++++++++++++++++- 1 file changed, 38 insertions(+), 1 deletion(-) diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index a02814d28b..7f8d0f5f0d 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -305,18 +305,26 @@ return_type GenericSystem::perform_command_mode_switch( { const size_t joint_index = static_cast(std::distance(info.joints.begin(), joint_it_found)); - if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) { joint_control_mode_[joint_index] = POSITION_INTERFACE_INDEX; + RCLCPP_INFO( + get_logger(), "Joint '%s' switched to position control mode.", + info.joints[joint_index].name.c_str()); } if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) { joint_control_mode_[joint_index] = VELOCITY_INTERFACE_INDEX; + RCLCPP_INFO( + get_logger(), "Joint '%s' switched to velocity control mode.", + info.joints[joint_index].name.c_str()); } if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) { joint_control_mode_[joint_index] = ACCELERATION_INTERFACE_INDEX; + RCLCPP_INFO( + get_logger(), "Joint '%s' switched to acceleration control mode.", + info.joints[joint_index].name.c_str()); } } } @@ -440,6 +448,13 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur { case ACCELERATION_INTERFACE_INDEX: { + RCLCPP_INFO( + get_logger(), + "Processing joint '%s' in acceleration control mode with commands: " + "pos: %f, vel: %f, acc: %f", + joint_name.c_str(), joint_command_values_[POSITION_INTERFACE_INDEX], + joint_command_values_[VELOCITY_INTERFACE_INDEX], + joint_command_values_[ACCELERATION_INTERFACE_INDEX]); // currently we do backward integration joint_state_values_[POSITION_INTERFACE_INDEX] += // apply offset to positions only joint_state_values_[VELOCITY_INTERFACE_INDEX] * period.seconds() + @@ -458,6 +473,13 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } case VELOCITY_INTERFACE_INDEX: { + RCLCPP_INFO( + get_logger(), + "Processing joint '%s' in velocity control mode with commands: " + "pos: %f, vel: %f, acc: %f", + joint_name.c_str(), joint_command_values_[POSITION_INTERFACE_INDEX], + joint_command_values_[VELOCITY_INTERFACE_INDEX], + joint_command_values_[ACCELERATION_INTERFACE_INDEX]); // currently we do backward integration joint_state_values_[POSITION_INTERFACE_INDEX] += // apply offset to positions only joint_state_values_[VELOCITY_INTERFACE_INDEX] * period.seconds() + @@ -478,6 +500,13 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } case POSITION_INTERFACE_INDEX: { + RCLCPP_INFO( + get_logger(), + "Processing joint '%s' in position control mode with commands: " + "pos: %f, vel: %f, acc: %f", + joint_name.c_str(), joint_command_values_[POSITION_INTERFACE_INDEX], + joint_command_values_[VELOCITY_INTERFACE_INDEX], + joint_command_values_[ACCELERATION_INTERFACE_INDEX]); if (!std::isnan(joint_command_values_[POSITION_INTERFACE_INDEX])) { const double old_position = joint_state_values_[POSITION_INTERFACE_INDEX]; @@ -497,6 +526,14 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur break; } } + // mirror them back + for (size_t i = 0; i < 3; ++i) + { + if (!std::isnan(joint_state_values_[i])) + { + set_state(joint_name + "/" + standard_interfaces_[i], joint_state_values_[i]); + } + } } else { From 17c723bd4d8753b16eea520ea221e2a8c2f0ebef Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 19 Sep 2025 20:43:11 +0000 Subject: [PATCH 10/21] Support boolean --- .../src/mock_components/generic_system.cpp | 77 +++++++++++++++---- 1 file changed, 63 insertions(+), 14 deletions(-) diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 7f8d0f5f0d..a0dd3304f1 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -338,17 +338,37 @@ hardware_interface::CallbackReturn GenericSystem::on_activate( for (const auto & joint_state : joint_state_interfaces_) { const std::string & name = joint_state.second.get_name(); - // TODO(anyone): consider initial value for non-double interfaces as well - double val = joint_state.second.interface_info.initial_value.empty() - ? 0.0 - : hardware_interface::stod(joint_state.second.interface_info.initial_value); - if ( - joint_state.second.get_interface_name() == standard_interfaces_[POSITION_INTERFACE_INDEX] && - custom_interface_with_following_offset_.empty()) + switch (joint_state.second.get_data_type()) { - val += position_state_following_offset_; + case hardware_interface::HandleDataType::DOUBLE: + { + double val = joint_state.second.interface_info.initial_value.empty() + ? 0.0 + : hardware_interface::stod(joint_state.second.interface_info.initial_value); + if ( + joint_state.second.get_interface_name() == + standard_interfaces_[POSITION_INTERFACE_INDEX] && + custom_interface_with_following_offset_.empty()) + { + val += position_state_following_offset_; + } + set_state(name, val); + break; + } + case hardware_interface::HandleDataType::BOOL: + { + bool bval = + joint_state.second.interface_info.initial_value.empty() + ? false + : hardware_interface::parse_bool(joint_state.second.interface_info.initial_value); + set_state(name, bval); + break; + } + default: + { + } + // not handling other types } - set_state(name, val); } return hardware_interface::CallbackReturn::SUCCESS; } @@ -653,12 +673,27 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur if (use_mock_gpio_command_interfaces_) { // do loopback on all gpio interfaces as we have exported them all - // TODO(anyone): how to pass data_type? // from gpio_command_interfaces_ maybe? for (const auto & gpio_state : gpio_states_) { const std::string & name = gpio_state.get()->get_name(); - set_state(name, get_command(name)); + switch (gpio_state.get()->get_data_type()) + { + case hardware_interface::HandleDataType::DOUBLE: + { + set_state(name, get_command(name)); + break; + } + case hardware_interface::HandleDataType::BOOL: + { + set_state(name, get_command(name)); + break; + } + default: + { + } + // not handling other types + } } } else @@ -666,15 +701,29 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur // do loopback on all gpio interfaces, where they exist for (const auto & gpio_command : gpio_commands_) { - // TODO(anyone): how to pass data_type? - // from gpio_command_interfaces_ maybe? const std::string & name = gpio_command.get()->get_name(); auto it = std::find_if( gpio_states_.begin(), gpio_states_.end(), [&name](const auto & state) { return state.get()->get_name() == name; }); if (it != gpio_states_.end()) { - set_state(name, get_command(name)); + switch (gpio_command.get()->get_data_type()) + { + case hardware_interface::HandleDataType::DOUBLE: + { + set_state(name, get_command(name)); + break; + } + case hardware_interface::HandleDataType::BOOL: + { + set_state(name, get_command(name)); + break; + } + default: + { + } + // not handling other types + } } } } From 1d9fcdd64013c12aab4c773d817505f2dfe7b453 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 19 Sep 2025 21:02:01 +0000 Subject: [PATCH 11/21] Support boolean for joints --- .../src/mock_components/generic_system.cpp | 81 +++++++++---------- 1 file changed, 39 insertions(+), 42 deletions(-) diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index a0dd3304f1..a0aa8c85a1 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -366,8 +366,10 @@ hardware_interface::CallbackReturn GenericSystem::on_activate( } default: { + RCLCPP_WARN( + get_logger(), "Data type of joint state interface '%s' will not be handled.", + joint_state.second.get_interface_name().c_str()); } - // not handling other types } } return hardware_interface::CallbackReturn::SUCCESS; @@ -381,6 +383,36 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur return return_type::OK; } + auto mirror_command_to_state = [this](const auto & name, const auto & data_type) -> return_type + { + switch (data_type) + { + case hardware_interface::HandleDataType::DOUBLE: + { + auto cmd = get_command(name); + if (std::isinf(cmd)) + { + return return_type::ERROR; + } + if (std::isfinite(cmd)) + { + set_state(name, cmd); + } + break; + } + case hardware_interface::HandleDataType::BOOL: + { + set_state(name, get_command(name)); + break; + } + default: + { + } + // not handling other types + } + return return_type::OK; + }; + for (size_t j = 0; j < get_hardware_info().joints.size(); ++j) { if (calculate_dynamics_) @@ -602,15 +634,12 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur { return state.get()->get_name() == full_interface_name; }); if (it != joint_commands_.end()) { - auto cmd = get_command(full_interface_name); - if (std::isinf(cmd)) + if ( + mirror_command_to_state(full_interface_name, joint_state.get()->get_data_type()) != + return_type::OK) { return return_type::ERROR; } - if (std::isfinite(cmd)) - { - set_state(full_interface_name, cmd); - } } if (custom_interface_with_following_offset_ == joint_state.get()->get_interface_name()) { @@ -666,7 +695,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur for (const auto & sensor_state : sensor_states_) { const std::string & name = sensor_state.get()->get_name(); - set_state(name, get_command(name)); + mirror_command_to_state(name, sensor_state.get()->get_data_type()); } } @@ -677,23 +706,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur for (const auto & gpio_state : gpio_states_) { const std::string & name = gpio_state.get()->get_name(); - switch (gpio_state.get()->get_data_type()) - { - case hardware_interface::HandleDataType::DOUBLE: - { - set_state(name, get_command(name)); - break; - } - case hardware_interface::HandleDataType::BOOL: - { - set_state(name, get_command(name)); - break; - } - default: - { - } - // not handling other types - } + mirror_command_to_state(name, gpio_state.get()->get_data_type()); } } else @@ -707,23 +720,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur [&name](const auto & state) { return state.get()->get_name() == name; }); if (it != gpio_states_.end()) { - switch (gpio_command.get()->get_data_type()) - { - case hardware_interface::HandleDataType::DOUBLE: - { - set_state(name, get_command(name)); - break; - } - case hardware_interface::HandleDataType::BOOL: - { - set_state(name, get_command(name)); - break; - } - default: - { - } - // not handling other types - } + mirror_command_to_state(name, gpio_command.get()->get_data_type()); } } } From a008bfb30c192d55269b80b11b43ae25b53fb993 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 19 Sep 2025 21:03:21 +0000 Subject: [PATCH 12/21] Remove debug output --- .../src/mock_components/generic_system.cpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index a0aa8c85a1..b9c48a9350 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -308,23 +308,14 @@ return_type GenericSystem::perform_command_mode_switch( if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) { joint_control_mode_[joint_index] = POSITION_INTERFACE_INDEX; - RCLCPP_INFO( - get_logger(), "Joint '%s' switched to position control mode.", - info.joints[joint_index].name.c_str()); } if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) { joint_control_mode_[joint_index] = VELOCITY_INTERFACE_INDEX; - RCLCPP_INFO( - get_logger(), "Joint '%s' switched to velocity control mode.", - info.joints[joint_index].name.c_str()); } if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) { joint_control_mode_[joint_index] = ACCELERATION_INTERFACE_INDEX; - RCLCPP_INFO( - get_logger(), "Joint '%s' switched to acceleration control mode.", - info.joints[joint_index].name.c_str()); } } } From 67c6baefee39f16a5a65ca57f8c92f80ccca916c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 19 Sep 2025 21:04:57 +0000 Subject: [PATCH 13/21] Remove debug output --- .../src/mock_components/generic_system.cpp | 21 ------------------- 1 file changed, 21 deletions(-) diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index b9c48a9350..f5868b5057 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -491,13 +491,6 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur { case ACCELERATION_INTERFACE_INDEX: { - RCLCPP_INFO( - get_logger(), - "Processing joint '%s' in acceleration control mode with commands: " - "pos: %f, vel: %f, acc: %f", - joint_name.c_str(), joint_command_values_[POSITION_INTERFACE_INDEX], - joint_command_values_[VELOCITY_INTERFACE_INDEX], - joint_command_values_[ACCELERATION_INTERFACE_INDEX]); // currently we do backward integration joint_state_values_[POSITION_INTERFACE_INDEX] += // apply offset to positions only joint_state_values_[VELOCITY_INTERFACE_INDEX] * period.seconds() + @@ -516,13 +509,6 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } case VELOCITY_INTERFACE_INDEX: { - RCLCPP_INFO( - get_logger(), - "Processing joint '%s' in velocity control mode with commands: " - "pos: %f, vel: %f, acc: %f", - joint_name.c_str(), joint_command_values_[POSITION_INTERFACE_INDEX], - joint_command_values_[VELOCITY_INTERFACE_INDEX], - joint_command_values_[ACCELERATION_INTERFACE_INDEX]); // currently we do backward integration joint_state_values_[POSITION_INTERFACE_INDEX] += // apply offset to positions only joint_state_values_[VELOCITY_INTERFACE_INDEX] * period.seconds() + @@ -543,13 +529,6 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } case POSITION_INTERFACE_INDEX: { - RCLCPP_INFO( - get_logger(), - "Processing joint '%s' in position control mode with commands: " - "pos: %f, vel: %f, acc: %f", - joint_name.c_str(), joint_command_values_[POSITION_INTERFACE_INDEX], - joint_command_values_[VELOCITY_INTERFACE_INDEX], - joint_command_values_[ACCELERATION_INTERFACE_INDEX]); if (!std::isnan(joint_command_values_[POSITION_INTERFACE_INDEX])) { const double old_position = joint_state_values_[POSITION_INTERFACE_INDEX]; From d56c8185f58954589bd5b8719bdb6b1d17ddf3d2 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 21 Sep 2025 19:38:56 +0000 Subject: [PATCH 14/21] Fix missing braces --- hardware_interface/src/mock_components/generic_system.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index f5868b5057..bc8bbfb33a 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -409,11 +409,11 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur if (calculate_dynamics_) { std::array joint_state_values_ = { - std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()}; + {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}}; std::array joint_command_values_ = { - std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()}; + {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}}; const auto joint_name = get_hardware_info().joints[j].name; { auto it_pos = std::find_if( From 381e79f66386e307bd44921085f856221a2f168d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 23 Sep 2025 06:26:46 +0000 Subject: [PATCH 15/21] Use the mimic joint index instead --- .../src/mock_components/generic_system.cpp | 31 ++++++++++--------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index bc8bbfb33a..1297ad88b5 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -621,41 +621,42 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } } + const auto joints = get_hardware_info().joints; for (const auto & mimic_joint : get_hardware_info().mimic_joints) { + 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.joint_name + "/" + standard_interfaces_[POSITION_INTERFACE_INDEX]) != + mimic_joint_name + "/" + standard_interfaces_[POSITION_INTERFACE_INDEX]) != joint_state_interfaces_.end()) { set_state( - mimic_joint.joint_name + "/" + standard_interfaces_[POSITION_INTERFACE_INDEX], + mimic_joint_name + "/" + standard_interfaces_[POSITION_INTERFACE_INDEX], mimic_joint.offset + - mimic_joint.multiplier * get_state( - mimic_joint.mimicked_joint_name + "/" + - standard_interfaces_[POSITION_INTERFACE_INDEX])); + mimic_joint.multiplier * + get_state(mimicked_joint_name + "/" + standard_interfaces_[POSITION_INTERFACE_INDEX])); } if ( joint_state_interfaces_.find( - mimic_joint.joint_name + "/" + standard_interfaces_[VELOCITY_INTERFACE_INDEX]) != + mimic_joint_name + "/" + standard_interfaces_[VELOCITY_INTERFACE_INDEX]) != joint_state_interfaces_.end()) { set_state( - mimic_joint.joint_name + "/" + standard_interfaces_[VELOCITY_INTERFACE_INDEX], - mimic_joint.multiplier * get_state( - mimic_joint.mimicked_joint_name + "/" + - standard_interfaces_[VELOCITY_INTERFACE_INDEX])); + mimic_joint_name + "/" + standard_interfaces_[VELOCITY_INTERFACE_INDEX], + mimic_joint.multiplier * + get_state(mimicked_joint_name + "/" + standard_interfaces_[VELOCITY_INTERFACE_INDEX])); } if ( joint_state_interfaces_.find( - mimic_joint.joint_name + "/" + standard_interfaces_[ACCELERATION_INTERFACE_INDEX]) != + mimic_joint_name + "/" + standard_interfaces_[ACCELERATION_INTERFACE_INDEX]) != joint_state_interfaces_.end()) { set_state( - mimic_joint.joint_name + "/" + standard_interfaces_[ACCELERATION_INTERFACE_INDEX], - mimic_joint.multiplier * get_state( - mimic_joint.mimicked_joint_name + "/" + - standard_interfaces_[ACCELERATION_INTERFACE_INDEX])); + mimic_joint_name + "/" + standard_interfaces_[ACCELERATION_INTERFACE_INDEX], + mimic_joint.multiplier * + get_state( + mimicked_joint_name + "/" + standard_interfaces_[ACCELERATION_INTERFACE_INDEX])); } } From 0d0d5b2dc4c94201bc6eaf4f2e9a130260339f58 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 23 Sep 2025 06:27:30 +0000 Subject: [PATCH 16/21] Revert "Change mock hardware to also save the joint names" This reverts commit afc5c268c450b74c4425b01dfea4972262adae94. --- hardware_interface/include/hardware_interface/hardware_info.hpp | 2 -- hardware_interface/src/component_parser.cpp | 2 -- 2 files changed, 4 deletions(-) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 3941cf5361..801dd0ff28 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -55,9 +55,7 @@ struct InterfaceInfo /// @brief This structure stores information about a joint that is mimicking another joint struct MimicJoint { - std::string joint_name; std::size_t joint_index; - std::string mimicked_joint_name; std::size_t mimicked_joint_index; double multiplier = 1.0; double offset = 0.0; diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 586eb8c4d0..70c0ad0185 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -994,9 +994,7 @@ std::vector parse_control_resources_from_urdf(const std::string & }; MimicJoint mimic_joint; - mimic_joint.joint_name = joint.name; mimic_joint.joint_index = i; - mimic_joint.mimicked_joint_name = urdf_joint->mimic->joint_name; mimic_joint.mimicked_joint_index = find_joint(urdf_joint->mimic->joint_name); mimic_joint.multiplier = urdf_joint->mimic->multiplier; mimic_joint.offset = urdf_joint->mimic->offset; From b8a6fdc5818f785ab6eb96bb3a7cbfc6b76df520 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 23 Sep 2025 08:11:37 +0000 Subject: [PATCH 17/21] Add test for boolean data_type --- .../mock_components/test_generic_system.cpp | 111 ++++++++++++++++-- 1 file changed, 100 insertions(+), 11 deletions(-) diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 9671477e1a..d69b901630 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -46,7 +46,7 @@ class TestGenericSystem : public ::testing::Test void test_generic_system_with_mock_sensor_commands( std::string & urdf, const std::string & component_name); void test_generic_system_with_mock_gpio_commands( - std::string & urdf, const std::string & component_name); + std::string & urdf, const std::string & component_name, const bool data_type_bool = false); protected: void SetUp() override @@ -425,6 +425,42 @@ class TestGenericSystem : public ::testing::Test )"; + valid_urdf_ros2_control_system_robot_with_gpio_bool_mock_command_ = + R"( + + + mock_components/GenericSystem + true + + + + + + 3.45 + + + + + + + + 2.78 + + + + + + + + + + + + + + +)"; + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ = R"( @@ -661,6 +697,7 @@ class TestGenericSystem : public ::testing::Test 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_mock_command_; + std::string valid_urdf_ros2_control_system_robot_with_gpio_bool_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_; @@ -1751,7 +1788,7 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) } void TestGenericSystem::test_generic_system_with_mock_gpio_commands( - std::string & urdf, const std::string & component_name) + std::string & urdf, const std::string & component_name, const bool data_type_bool) { TestableResourceManager rm(node_, urdf); @@ -1812,49 +1849,90 @@ void TestGenericSystem::test_generic_system_with_mock_gpio_commands( EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_optional().value())); EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_optional().value())); EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_optional().value())); - EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); + if (data_type_bool) + { + // for bool data type initial value is false + EXPECT_FALSE(gpio2_vac_s.get_optional().value()); + EXPECT_FALSE(gpio2_vac_c.get_optional().value()); + } + else + { + EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); + EXPECT_TRUE(std::isnan(gpio2_vac_c.get_optional().value())); + } EXPECT_TRUE(std::isnan(gpio1_a_o1_c.get_optional().value())); EXPECT_TRUE(std::isnan(gpio1_a_i1_c.get_optional().value())); EXPECT_TRUE(std::isnan(gpio1_a_i2_c.get_optional().value())); - EXPECT_TRUE(std::isnan(gpio2_vac_c.get_optional().value())); // set some new values in commands ASSERT_TRUE(gpio1_a_o1_c.set_value(0.11)); ASSERT_TRUE(gpio1_a_i1_c.set_value(0.33)); ASSERT_TRUE(gpio1_a_i2_c.set_value(1.11)); - ASSERT_TRUE(gpio2_vac_c.set_value(2.22)); + if (data_type_bool) + { + ASSERT_TRUE(gpio2_vac_c.set_value(true)); + } + else + { + ASSERT_TRUE(gpio2_vac_c.set_value(2.22)); + } // State values should not be changed EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_optional().value())); EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_optional().value())); EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_optional().value())); - EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); + if (data_type_bool) + { + EXPECT_FALSE(gpio2_vac_s.get_optional().value()); + EXPECT_TRUE(gpio2_vac_c.get_optional().value()); + } + else + { + EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); + ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value()); + } ASSERT_EQ(0.11, gpio1_a_o1_c.get_optional().value()); ASSERT_EQ(0.33, gpio1_a_i1_c.get_optional().value()); ASSERT_EQ(1.11, gpio1_a_i2_c.get_optional().value()); - ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value()); // write() does not change values rm.write(TIME, PERIOD); EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_optional().value())); EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_optional().value())); EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_optional().value())); - EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); + if (data_type_bool) + { + // for bool data type initial value is false + EXPECT_FALSE(gpio2_vac_s.get_optional().value()); + EXPECT_TRUE(gpio2_vac_c.get_optional().value()); + } + else + { + EXPECT_TRUE(std::isnan(gpio2_vac_s.get_optional().value())); + ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value()); + } ASSERT_EQ(0.11, gpio1_a_o1_c.get_optional().value()); ASSERT_EQ(0.33, gpio1_a_i1_c.get_optional().value()); ASSERT_EQ(1.11, gpio1_a_i2_c.get_optional().value()); - ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value()); // read() mirrors commands to states rm.read(TIME, PERIOD); ASSERT_EQ(0.11, gpio1_a_o1_s.get_optional().value()); ASSERT_EQ(0.33, gpio1_a_i1_s.get_optional().value()); ASSERT_EQ(1.11, gpio1_a_o2_s.get_optional().value()); - ASSERT_EQ(2.22, gpio2_vac_s.get_optional().value()); + if (data_type_bool) + { + EXPECT_TRUE(gpio2_vac_s.get_optional().value()); + EXPECT_TRUE(gpio2_vac_c.get_optional().value()); + } + else + { + ASSERT_EQ(2.22, gpio2_vac_s.get_optional().value()); + ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value()); + } ASSERT_EQ(0.11, gpio1_a_o1_c.get_optional().value()); ASSERT_EQ(0.33, gpio1_a_i1_c.get_optional().value()); ASSERT_EQ(1.11, gpio1_a_i2_c.get_optional().value()); - ASSERT_EQ(2.22, gpio2_vac_c.get_optional().value()); } TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command) @@ -1866,6 +1944,15 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_co test_generic_system_with_mock_gpio_commands(urdf, "MockHardwareSystem"); } +TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_bool_mock_command) +{ + auto urdf = ros2_control_test_assets::urdf_head + + valid_urdf_ros2_control_system_robot_with_gpio_bool_mock_command_ + + ros2_control_test_assets::urdf_tail; + + test_generic_system_with_mock_gpio_commands(urdf, "MockHardwareSystem", true); +} + TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True) { auto urdf = ros2_control_test_assets::urdf_head + @@ -2198,6 +2285,8 @@ TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tag ASSERT_TRUE(check_prepare_command_mode_switch(valid_urdf_ros2_control_system_robot_with_gpio_)); ASSERT_TRUE(check_prepare_command_mode_switch( valid_urdf_ros2_control_system_robot_with_gpio_mock_command_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + valid_urdf_ros2_control_system_robot_with_gpio_bool_mock_command_)); ASSERT_TRUE(check_prepare_command_mode_switch( valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_)); ASSERT_TRUE(check_prepare_command_mode_switch(sensor_with_initial_value_)); From 73812bb73aa203b48b4e9c685a58b9cc2429dc85 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 23 Sep 2025 12:48:07 +0000 Subject: [PATCH 18/21] Preallocate skip_interfaces --- .../include/mock_components/generic_system.hpp | 1 + .../src/mock_components/generic_system.cpp | 14 ++++++-------- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index 2c3219f8f6..14e041f6d1 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -77,6 +77,7 @@ class GenericSystem : public hardware_interface::SystemInterface const std::vector standard_interfaces_ = { hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; + std::vector skip_interfaces_; std::vector other_interfaces_; std::vector sensor_interfaces_; diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 1297ad88b5..d847134857 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -104,6 +104,10 @@ CallbackReturn GenericSystem::on_init( { calculate_dynamics_ = false; } + for (size_t i = 0; i < (calculate_dynamics_ ? 3 : 1); ++i) + { + skip_interfaces_.push_back(standard_interfaces_[i]); + } // process parameters about state following position_state_following_offset_ = 0.0; @@ -582,19 +586,13 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 2 are position, // velocity, and acceleration interface // Create a subvector of standard_interfaces_ with the given indices - // TODO(anyone): preallocate this - std::vector skip_interfaces; - for (size_t i = 0; i < (calculate_dynamics_ ? 3 : 1); ++i) - { - skip_interfaces.push_back(standard_interfaces_[i]); - } // TODO(anyone): optimize by using joint_command_interfaces_/joint_state_interfaces_ map for (const auto & joint_state : joint_states_) { if ( std::find( - skip_interfaces.begin(), skip_interfaces.end(), joint_state.get()->get_interface_name()) != - skip_interfaces.end()) + skip_interfaces_.begin(), skip_interfaces_.end(), + joint_state.get()->get_interface_name()) != skip_interfaces_.end()) { continue; } From a4ebdca452c4d4d8fcf720db47dd0424acd5ef5d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 23 Sep 2025 12:50:49 +0000 Subject: [PATCH 19/21] Use some references instead of copies --- hardware_interface/src/mock_components/generic_system.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index d847134857..324fbe065e 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -619,11 +619,11 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } } - const auto joints = get_hardware_info().joints; + const auto & joints = get_hardware_info().joints; for (const auto & mimic_joint : get_hardware_info().mimic_joints) { - 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; + 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 + "/" + standard_interfaces_[POSITION_INTERFACE_INDEX]) != From 25461613a70e5f52bcf69221314a1607770a7521 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 23 Sep 2025 12:52:05 +0000 Subject: [PATCH 20/21] Move comments --- hardware_interface/src/mock_components/generic_system.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 324fbe065e..f0aabb4f81 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -104,6 +104,9 @@ CallbackReturn GenericSystem::on_init( { calculate_dynamics_ = false; } + // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 2 are position, + // velocity, and acceleration interface + // Create a subvector of standard_interfaces_ with the given indices for (size_t i = 0; i < (calculate_dynamics_ ? 3 : 1); ++i) { skip_interfaces_.push_back(standard_interfaces_[i]); @@ -583,10 +586,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } } - // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 2 are position, - // velocity, and acceleration interface - // Create a subvector of standard_interfaces_ with the given indices - // TODO(anyone): optimize by using joint_command_interfaces_/joint_state_interfaces_ map + // do loopback on all other interfaces for (const auto & joint_state : joint_states_) { if ( From eda82f4e6371177e2acb5a1fc4fd991923761a90 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 23 Sep 2025 13:04:04 +0000 Subject: [PATCH 21/21] Use unordered maps to improve efficiency --- .../src/mock_components/generic_system.cpp | 94 +++---------------- 1 file changed, 15 insertions(+), 79 deletions(-) diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index f0aabb4f81..6b44780b34 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -421,76 +421,18 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur std::array joint_command_values_ = { {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}}; - const auto joint_name = get_hardware_info().joints[j].name; - { - auto it_pos = std::find_if( - joint_commands_.begin(), joint_commands_.end(), - [this, &joint_name](const auto & command) - { - return command.get()->get_name() == - joint_name + "/" + standard_interfaces_[POSITION_INTERFACE_INDEX]; - }); - if (it_pos != joint_commands_.end()) - { - joint_command_values_[POSITION_INTERFACE_INDEX] = get_command(it_pos->get()->get_name()); - } - auto it_vel = std::find_if( - joint_commands_.begin(), joint_commands_.end(), - [this, &joint_name](const auto & command) - { - return command.get()->get_name() == - joint_name + "/" + standard_interfaces_[VELOCITY_INTERFACE_INDEX]; - }); - if (it_vel != joint_commands_.end()) - { - joint_command_values_[VELOCITY_INTERFACE_INDEX] = get_command(it_vel->get()->get_name()); - } - auto it_acc = std::find_if( - joint_commands_.begin(), joint_commands_.end(), - [this, &joint_name](const auto & command) - { - return command.get()->get_name() == - joint_name + "/" + standard_interfaces_[ACCELERATION_INTERFACE_INDEX]; - }); - if (it_acc != joint_commands_.end()) - { - joint_command_values_[ACCELERATION_INTERFACE_INDEX] = - get_command(it_acc->get()->get_name()); - } - } + const auto & joint_name = get_hardware_info().joints[j].name; + for (size_t i = 0; i < 3; ++i) { - auto it_pos = std::find_if( - joint_states_.begin(), joint_states_.end(), - [this, &joint_name](const auto & command) - { - return command.get()->get_name() == - joint_name + "/" + standard_interfaces_[POSITION_INTERFACE_INDEX]; - }); - if (it_pos != joint_states_.end()) + const auto full_name = joint_name + "/" + standard_interfaces_[i]; + if (joint_command_interfaces_.find(full_name) != joint_command_interfaces_.end()) { - joint_state_values_[POSITION_INTERFACE_INDEX] = get_state(it_pos->get()->get_name()); + joint_command_values_[i] = + get_command(joint_command_interfaces_.at(full_name).get_name()); } - auto it_vel = std::find_if( - joint_states_.begin(), joint_states_.end(), - [this, &joint_name](const auto & command) - { - return command.get()->get_name() == - joint_name + "/" + standard_interfaces_[VELOCITY_INTERFACE_INDEX]; - }); - if (it_vel != joint_states_.end()) - { - joint_state_values_[VELOCITY_INTERFACE_INDEX] = get_state(it_vel->get()->get_name()); - } - auto it_acc = std::find_if( - joint_states_.begin(), joint_states_.end(), - [this, &joint_name](const auto & command) - { - return command.get()->get_name() == - joint_name + "/" + standard_interfaces_[ACCELERATION_INTERFACE_INDEX]; - }); - if (it_acc != joint_states_.end()) + if (joint_state_interfaces_.find(full_name) != joint_state_interfaces_.end()) { - joint_state_values_[ACCELERATION_INTERFACE_INDEX] = get_state(it_acc->get()->get_name()); + joint_state_values_[i] = get_state(joint_state_interfaces_.at(full_name).get_name()); } } @@ -566,15 +508,14 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } else { - for (const auto & joint_command : joint_command_interfaces_) + for (const auto & joint_command : joint_commands_) { if ( - joint_command.second.get_interface_name() == + joint_command.get()->get_interface_name() == standard_interfaces_[POSITION_INTERFACE_INDEX]) { - const std::string & name = joint_command.second.get_name(); - auto it = joint_state_interfaces_.find(name); - if (it != joint_state_interfaces_.end()) + const std::string & name = joint_command.get()->get_name(); + if (joint_state_interfaces_.find(name) != joint_state_interfaces_.end()) { set_state( name, get_command(name) + (custom_interface_with_following_offset_.empty() @@ -597,10 +538,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur continue; } const std::string & full_interface_name = joint_state.get()->get_name(); - auto it = std::find_if( - joint_commands_.begin(), joint_commands_.end(), [&full_interface_name](const auto & state) - { return state.get()->get_name() == full_interface_name; }); - if (it != joint_commands_.end()) + if (joint_command_interfaces_.find(full_interface_name) != joint_command_interfaces_.end()) { if ( mirror_command_to_state(full_interface_name, joint_state.get()->get_data_type()) != @@ -619,6 +557,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } } + // Update mimic joints const auto & joints = get_hardware_info().joints; for (const auto & mimic_joint : get_hardware_info().mimic_joints) { @@ -684,10 +623,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(); - auto it = std::find_if( - gpio_states_.begin(), gpio_states_.end(), - [&name](const auto & state) { return state.get()->get_name() == name; }); - if (it != gpio_states_.end()) + if (gpio_state_interfaces_.find(name) != gpio_state_interfaces_.end()) { mirror_command_to_state(name, gpio_command.get()->get_data_type()); }