From 9fbd1d68bc23bcc95159f4778f17bd7bf5e3db1e Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 2 Feb 2024 13:07:35 +0100 Subject: [PATCH] tmp commit! remove deprecated handle constructor -> adjust generic system ! not compiling not all tests have been adjusted still wip! --- .../include/hardware_interface/handle.hpp | 28 +- .../mock_components/generic_system.hpp | 61 +- .../src/mock_components/generic_system.cpp | 541 +++++++----------- .../test_force_torque_sensor.cpp | 53 +- .../test_single_joint_actuator.cpp | 39 +- .../test_system_with_command_modes.cpp | 55 +- .../test_two_joint_system.cpp | 28 +- 7 files changed, 331 insertions(+), 474 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 31b459177a..f85b5b4dcd 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -36,13 +36,12 @@ class Handle public: [[deprecated("Use InterfaceDescription for initializing the Interface")]] - Handle( - const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) + Handle(const std::string & prefix_name, const std::string & interface_name) + : prefix_name_(prefix_name), interface_name_(interface_name) { - // default to double - value_ = std::numeric_limits::quiet_NaN(); + // init to default value defined by init_handle_value() + InterfaceInfo info; + init_handle_value(info); } explicit Handle(const InterfaceDescription & interface_description) @@ -52,23 +51,6 @@ class Handle init_handle_value(interface_description.interface_info); } - [[deprecated("Use InterfaceDescription for initializing the Interface")]] - - explicit Handle(const std::string & interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) - { - // default to double - value_ = std::numeric_limits::quiet_NaN(); - } - - [[deprecated("Use InterfaceDescription for initializing the Interface")]] - - explicit Handle(const char * interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) - { // default to double - value_ = std::numeric_limits::quiet_NaN(); - } - Handle(const Handle & other) = default; Handle(Handle && other) = default; diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index e9b38de65d..addc8f21f5 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -18,6 +18,8 @@ #define MOCK_COMPONENTS__GENERIC_SYSTEM_HPP_ #include +#include +#include #include #include "hardware_interface/handle.hpp" @@ -41,9 +43,7 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste public: CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; + std::vector export_command_interfaces_2() override; return_type prepare_command_mode_switch( const std::vector & start_interfaces, @@ -71,62 +71,43 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste 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}; + // added dynamically during on_init + std::vector non_standard_interfaces_; struct MimicJoint { - std::size_t joint_index; - std::size_t mimicked_joint_index; + std::string joint_name; + std::string mimic_joint_name; double multiplier = 1.0; }; std::vector mimic_joints_; - /// The size of this vector is (standard_interfaces_.size() x nr_joints) - std::vector> joint_commands_; - std::vector> joint_states_; - - std::vector other_interfaces_; - /// The size of this vector is (other_interfaces_.size() x nr_joints) - std::vector> other_commands_; - std::vector> other_states_; - - std::vector sensor_interfaces_; - /// The size of this vector is (sensor_interfaces_.size() x nr_joints) - std::vector> sensor_mock_commands_; - std::vector> sensor_states_; + // All the joints that are of type defined by standard_interfaces_ vector -> In {pos, vel, acc, + // effort} + std::vector std_joint_names_; + std::unordered_set std_joint_command_interface_names_; + std::unordered_set std_joint_state_interface_names_; - std::vector gpio_interfaces_; - /// The size of this vector is (gpio_interfaces_.size() x nr_joints) - std::vector> gpio_mock_commands_; - std::vector> gpio_commands_; - std::vector> gpio_states_; + // All the joints that are of not of a type defined by standard_interfaces_ vector -> Not in {pos, + // vel, acc, effort} + std::vector other_joint_names_; + std::unordered_set other_joint_command_interface_names_; + std::unordered_set other_joint_state_interface_names_; 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( + void search_and_add_interface_names( const std::vector & components, - std::vector & interfaces, std::vector> & storage, - std::vector & target_interfaces, bool using_state_interfaces); + const std::vector & interface_list, std::vector & vector_to_add); 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_; + std::string custom_interface_name_with_following_offset_; bool calculate_dynamics_; - std::vector joint_control_mode_; + std::unordered_map joint_control_mode_; bool command_propagation_disabled_; }; diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 22d8aa573c..96ba6f40e3 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -60,7 +60,6 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } }; - // check if to create mock command interface for sensor auto it = info_.hardware_parameters.find("mock_sensor_commands"); if (it != info_.hardware_parameters.end()) @@ -120,77 +119,90 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i custom_interface_with_following_offset_ = it->second; } } - // it's extremely improbable that std::distance results int this value - therefore default - index_custom_interface_with_following_offset_ = std::numeric_limits::max(); - // Initialize storage for standard interfaces - initialize_storage_vectors(joint_commands_, joint_states_, standard_interfaces_, info_.joints); - // set all values without initial values to 0 - for (auto i = 0u; i < info_.joints.size(); i++) + // search for non-standard joint interfaces + for (const auto & joint : info_.joints) + { + // populate non-standard command interfaces to other_interfaces_ + populate_non_standard_interfaces(joint.command_interfaces, non_standard_interfaces_); + + // populate non-standard state interfaces to other_interfaces_ + populate_non_standard_interfaces(joint.state_interfaces, non_standard_interfaces_); + } + + // search for standard joint interfaces and add them to std_joint_names_ and + // search for non-standard joint interfaces and add them to other_joint_names_ + for (const auto & joint : info.joints) { - for (auto j = 0u; j < standard_interfaces_.size(); j++) + for (const auto & state_inteface : joint.state_interfaces) { - if (std::isnan(joint_states_[j][i])) + if ( + std::find(standard_interfaces_.begin(), standard_interfaces_.end(), state_inteface.name) == + standard_interfaces_.end()) { - joint_states_[j][i] = 0.0; + std_joint_names_.push_back(joint.name); + std_joint_state_interface_names_.insert(joint.name + "/" + state_inteface.name); + } + else if ( + std::find( + non_standard_interfaces_.begin(), non_standard_interfaces_.end(), state_inteface.name) == + non_standard_interfaces_.end()) + { + other_joint_names_.push_back(joint.name); + other_joint_state_interface_names_.insert(joint.name + "/" + state_inteface.name); + } + else + { + RCUTILS_LOG_WARN_NAMED( + "The state_interface: '%s' of the joint: '%s' is neither part of the std_interfaces " + "(pos, vel, acc, eff), nor of any other use " + "defined.", + state_inteface.name.c_str(), joint.name.c_str()); } } - } - // Search for mimic joints - for (auto i = 0u; i < info_.joints.size(); ++i) - { - const auto & joint = info_.joints.at(i); - if (joint.parameters.find("mimic") != joint.parameters.cend()) + // Check that for all the available state_interfaces a command_interface exists + // We don't need to add name of the joint again since it has already been added for the + // state_interface + for (const auto & command_interface : joint.command_interfaces) { - const auto mimicked_joint_it = std::find_if( - info_.joints.begin(), info_.joints.end(), - [&mimicked_joint = - joint.parameters.at("mimic")](const hardware_interface::ComponentInfo & joint_info) - { return joint_info.name == mimicked_joint; }); - if (mimicked_joint_it == info_.joints.cend()) + if ( + std::find( + std_joint_state_interface_names_.begin(), std_joint_state_interface_names_.end(), + command_interface.name) == std_joint_state_interface_names_.end()) { - throw std::runtime_error( - std::string("Mimicked joint '") + joint.parameters.at("mimic") + "' not found"); + std_joint_command_interface_names_.insert(joint.name + "/" + command_interface.name); } - MimicJoint mimic_joint; - mimic_joint.joint_index = i; - mimic_joint.mimicked_joint_index = std::distance(info_.joints.begin(), mimicked_joint_it); - auto param_it = joint.parameters.find("multiplier"); - if (param_it != joint.parameters.end()) + else if ( + std::find( + other_joint_state_interface_names_.begin(), other_joint_state_interface_names_.end(), + command_interface.name) == other_joint_state_interface_names_.end()) { - mimic_joint.multiplier = hardware_interface::stod(joint.parameters.at("multiplier")); + other_joint_command_interface_names_.insert(joint.name + "/" + command_interface.name); + } + else + { + throw std::runtime_error( + std::string("For command_interface: '") + command_interface.name + "' of the joint: '" + + joint.name + "' exists no state_interface"); } - mimic_joints_.push_back(mimic_joint); } } - // search for non-standard joint interfaces - for (const auto & joint : info_.joints) - { - // populate non-standard command interfaces to other_interfaces_ - populate_non_standard_interfaces(joint.command_interfaces, other_interfaces_); - - // populate non-standard state interfaces to other_interfaces_ - populate_non_standard_interfaces(joint.state_interfaces, other_interfaces_); - } - - // Initialize storage for non-standard interfaces - initialize_storage_vectors(other_commands_, other_states_, other_interfaces_, info_.joints); - // when following offset is used on custom interface then find its index + custom_interface_name_with_following_offset_ = ""; if (!custom_interface_with_following_offset_.empty()) { auto if_it = std::find( - other_interfaces_.begin(), other_interfaces_.end(), custom_interface_with_following_offset_); - if (if_it != other_interfaces_.end()) + non_standard_interfaces_.begin(), non_standard_interfaces_.end(), + custom_interface_with_following_offset_); + if (if_it != non_standard_interfaces_.end()) { - index_custom_interface_with_following_offset_ = - std::distance(other_interfaces_.begin(), if_it); + custom_interface_name_with_following_offset_ = *if_it; RCUTILS_LOG_INFO_NAMED( - "mock_generic_system", "Custom interface with following offset '%s' found at index: %zu.", + "mock_generic_system", "Custom interface with following offset '%s' found at index: %s.", custom_interface_with_following_offset_.c_str(), - index_custom_interface_with_following_offset_); + custom_interface_name_with_following_offset_.c_str()); } else { @@ -201,149 +213,79 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } - for (const auto & sensor : info_.sensors) + // Search for mimic joints + for (auto i = 0u; i < info_.joints.size(); ++i) { - for (const auto & interface : sensor.state_interfaces) + const auto & joint = info_.joints.at(i); + if (joint.parameters.find("mimic") != joint.parameters.cend()) { - if ( - std::find(sensor_interfaces_.begin(), sensor_interfaces_.end(), interface.name) == - sensor_interfaces_.end()) + const auto mimicked_joint_it = std::find_if( + info_.joints.begin(), info_.joints.end(), + [&mimicked_joint = + joint.parameters.at("mimic")](const hardware_interface::ComponentInfo & joint_info) + { return joint_info.name == mimicked_joint; }); + if (mimicked_joint_it == info_.joints.cend()) { - sensor_interfaces_.emplace_back(interface.name); + throw std::runtime_error( + std::string("Mimicked joint '") + joint.parameters.at("mimic") + "' not found"); } - } - } - initialize_storage_vectors( - sensor_mock_commands_, sensor_states_, sensor_interfaces_, info_.sensors); - - // search for gpio interfaces - for (const auto & gpio : 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_commands_, gpio_states_, gpio_interfaces_, info_.gpios); - } - // Real gpio command interfaces - else - { - initialize_storage_vectors(gpio_commands_, gpio_states_, gpio_interfaces_, info_.gpios); - } - - return CallbackReturn::SUCCESS; -} - -std::vector GenericSystem::export_state_interfaces() -{ - std::vector state_interfaces; - - // Joints' state interfaces - for (auto i = 0u; i < info_.joints.size(); i++) - { - const auto & joint = 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_states_, state_interfaces)) + MimicJoint mimic_joint; + mimic_joint.joint_name = joint.name; + mimic_joint.mimic_joint_name = mimicked_joint_it->name; + auto param_it = joint.parameters.find("multiplier"); + if (param_it != joint.parameters.end()) { - if (!get_interface( - joint.name, other_interfaces_, interface.name, i, other_states_, state_interfaces)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. " - "This should never happen!"); - } + mimic_joint.multiplier = hardware_interface::stod(joint.parameters.at("multiplier")); } + mimic_joints_.push_back(mimic_joint); } } - // Sensor state interfaces - if (!populate_interfaces( - info_.sensors, sensor_interfaces_, sensor_states_, 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(info_.gpios, gpio_interfaces_, gpio_states_, state_interfaces, true)) + // set all values without initial values to 0 + for (auto [name, descr] : joint_state_interfaces_) { - throw std::runtime_error("Interface is not found in the gpio list. This should never happen!"); + if (std::isnan(get_state(name))) + { + set_state(name, 0.0); + } } - return state_interfaces; + return CallbackReturn::SUCCESS; } -std::vector GenericSystem::export_command_interfaces() +std::vector GenericSystem::export_command_interfaces_2() { - std::vector command_interfaces; - - // Joints' state interfaces - for (size_t i = 0; i < info_.joints.size(); ++i) - { - const auto & joint = 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_commands_, - command_interfaces)) - { - if (!get_interface( - joint.name, other_interfaces_, interface.name, i, other_commands_, - 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(info_.joints.size(), POSITION_INTERFACE_INDEX); - + // we check if we should mock command interfaces or not. After they have been exported we can then + // use them as we would normally via (set/get)_(state/command) + std::vector descriptions; // Mock sensor command interfaces if (use_mock_sensor_command_interfaces_) { - if (!populate_interfaces( - info_.sensors, sensor_interfaces_, sensor_mock_commands_, command_interfaces, true)) + for (const auto & sensor : info_.sensors) { - throw std::runtime_error( - "Interface is not found in the standard nor other list. This should never happen!"); + for (const auto & state_interface : sensor.state_interfaces) + { + hardware_interface::InterfaceInfo info = state_interface; + hardware_interface::InterfaceDescription descr(sensor.name, info); + descriptions.push_back(descr); + } } } // Mock gpio command interfaces (consider all state interfaces for command interfaces) if (use_mock_gpio_command_interfaces_) { - if (!populate_interfaces( - info_.gpios, gpio_interfaces_, gpio_mock_commands_, 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( - info_.gpios, gpio_interfaces_, gpio_commands_, command_interfaces, false)) + for (const auto & gpio : info_.gpios) { - throw std::runtime_error( - "Interface is not found in the gpio list. This should never happen!"); + for (const auto & state_interface : gpio.state_interfaces) + { + hardware_interface::InterfaceInfo info = state_interface; + hardware_interface::InterfaceDescription descr(gpio.name, info); + descriptions.push_back(descr); + } } } - - return command_interfaces; + return descriptions; } return_type GenericSystem::prepare_command_mode_switch( @@ -359,8 +301,8 @@ return_type GenericSystem::prepare_command_mode_switch( const size_t FOUND_ONCE_FLAG = 1000000; - std::vector joint_found_in_x_requests_; - joint_found_in_x_requests_.resize(info_.joints.size(), 0); + std::vector joint_found_in_x_requests; + joint_found_in_x_requests.resize(info_.joints.size(), 0); for (const auto & key : start_interfaces) { @@ -372,14 +314,14 @@ return_type GenericSystem::prepare_command_mode_switch( if (joint_it_found != info_.joints.end()) { const size_t joint_index = std::distance(info_.joints.begin(), joint_it_found); - if (joint_found_in_x_requests_[joint_index] == 0) + if (joint_found_in_x_requests[joint_index] == 0) { - joint_found_in_x_requests_[joint_index] = FOUND_ONCE_FLAG; + joint_found_in_x_requests[joint_index] = FOUND_ONCE_FLAG; } if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) { - joint_found_in_x_requests_[joint_index] += 1; + joint_found_in_x_requests[joint_index] += 1; } if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) { @@ -391,7 +333,7 @@ return_type GenericSystem::prepare_command_mode_switch( "might lead to wrong feedback and unexpected behavior.", info_.joints[joint_index].name.c_str()); } - joint_found_in_x_requests_[joint_index] += 1; + joint_found_in_x_requests[joint_index] += 1; } if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) { @@ -403,7 +345,7 @@ return_type GenericSystem::prepare_command_mode_switch( "this might lead to wrong feedback and unexpected behavior.", info_.joints[joint_index].name.c_str()); } - joint_found_in_x_requests_[joint_index] += 1; + joint_found_in_x_requests[joint_index] += 1; } } else @@ -417,7 +359,7 @@ return_type GenericSystem::prepare_command_mode_switch( for (size_t i = 0; i < info_.joints.size(); ++i) { // There has to always be at least one control mode from the above three set - if (joint_found_in_x_requests_[i] == FOUND_ONCE_FLAG) + if (joint_found_in_x_requests[i] == FOUND_ONCE_FLAG) { RCUTILS_LOG_ERROR_NAMED( "mock_generic_system", "Joint '%s' has to have '%s', '%s', or '%s' interface!", @@ -427,13 +369,13 @@ return_type GenericSystem::prepare_command_mode_switch( } // Currently we don't support multiple interface request - if (joint_found_in_x_requests_[i] > (FOUND_ONCE_FLAG + 1)) + if (joint_found_in_x_requests[i] > (FOUND_ONCE_FLAG + 1)) { RCUTILS_LOG_ERROR_NAMED( "mock_generic_system", "Got multiple (%zu) starting interfaces for joint '%s' - this is not " "supported!", - joint_found_in_x_requests_[i] - FOUND_ONCE_FLAG, info_.joints[i].name.c_str()); + joint_found_in_x_requests[i] - FOUND_ONCE_FLAG, info_.joints[i].name.c_str()); ret_val = hardware_interface::return_type::ERROR; } } @@ -463,15 +405,15 @@ 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; + joint_control_mode_[key] = POSITION_INTERFACE_INDEX; } if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) { - joint_control_mode_[joint_index] = VELOCITY_INTERFACE_INDEX; + joint_control_mode_[key] = VELOCITY_INTERFACE_INDEX; } if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) { - joint_control_mode_[joint_index] = ACCELERATION_INTERFACE_INDEX; + joint_control_mode_[key] = ACCELERATION_INTERFACE_INDEX; } } } @@ -488,97 +430,120 @@ 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) + auto mirror_command_to_state = [this](const auto & joint_names, const auto & interface_names) { - for (size_t i = start_index; i < states_.size(); ++i) + for (const auto & joint_name : joint_names) { - for (size_t j = 0; j < states_[i].size(); ++j) + for (const auto & interface_name : interface_names) { - if (!std::isnan(commands_[i][j])) + const auto & joint_state_name = joint_name + "/" + interface_name; + if ( + this->std_joint_command_interface_names_.find(joint_state_name) != + this->std_joint_command_interface_names_.end()) { - states_[i][j] = commands_[i][j]; + this->set_state(joint_state_name, this->get_command(joint_state_name)); } } } }; - for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) + auto mirror_all_available_commands_to_states = [this](const auto & interfaces) + { + for (const auto & [name, descr] : interfaces) + { + // TODO(Manuel) should this be checked if interface exists??? + this->set_state(name, this->get_command(name)); + } + }; + + if (calculate_dynamics_) { - if (calculate_dynamics_) + for (const auto joint_name : std_joint_names_) { - switch (joint_control_mode_[j]) + const auto joint_pos = joint_name + "/" + hardware_interface::HW_IF_POSITION; + const auto joint_vel = joint_name + "/" + hardware_interface::HW_IF_VELOCITY; + const auto joint_acc = joint_name + "/" + hardware_interface::HW_IF_ACCELERATION; + const auto joint_eff = joint_name + "/" + hardware_interface::HW_IF_EFFORT; + + switch (joint_control_mode_.at(joint_name)) { case ACCELERATION_INTERFACE_INDEX: { // currently we do backward integration - joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only - joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + + // apply offset to positions only + const auto old_pos = get_state(joint_pos); + const auto new_pos = + get_state(joint_vel) * period.seconds() + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); + set_state(joint_pos, old_pos + new_pos); - joint_states_[VELOCITY_INTERFACE_INDEX][j] += - joint_states_[ACCELERATION_INTERFACE_INDEX][j] * period.seconds(); + const auto old_vel = get_state(joint_vel); + const auto new_vel = get_state(joint_acc) * period.seconds(); + set_state(joint_vel, old_vel + new_vel); - if (!std::isnan(joint_commands_[ACCELERATION_INTERFACE_INDEX][j])) + if ( + std_joint_command_interface_names_.find(joint_acc) != + std_joint_command_interface_names_.end()) { - joint_states_[ACCELERATION_INTERFACE_INDEX][j] = - joint_commands_[ACCELERATION_INTERFACE_INDEX][j]; + set_state(joint_acc, get_command(joint_acc)); } break; } case VELOCITY_INTERFACE_INDEX: { // currently we do backward integration - joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only - joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + + const auto old_pos = get_state(joint_pos); + const auto new_pos = + get_state(joint_vel) * period.seconds() + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); + set_state(joint_pos, old_pos + new_pos); - if (!std::isnan(joint_commands_[VELOCITY_INTERFACE_INDEX][j])) + if ( + std_joint_command_interface_names_.find(joint_vel) != + std_joint_command_interface_names_.end()) { - const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; - - joint_states_[VELOCITY_INTERFACE_INDEX][j] = - joint_commands_[VELOCITY_INTERFACE_INDEX][j]; - - joint_states_[ACCELERATION_INTERFACE_INDEX][j] = - (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + const auto old_vel = get_state(joint_vel); + set_state(joint_vel, get_command(joint_vel)); + set_state(joint_acc, (get_state(joint_vel) - old_vel) / period.seconds()); } break; } case POSITION_INTERFACE_INDEX: { - if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + if ( + std_joint_command_interface_names_.find(joint_pos) != + std_joint_command_interface_names_.end()) { - const double old_position = joint_states_[POSITION_INTERFACE_INDEX][j]; - const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; - - joint_states_[POSITION_INTERFACE_INDEX][j] = // apply offset to positions only - joint_commands_[POSITION_INTERFACE_INDEX][j] + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ - : 0.0); - - joint_states_[VELOCITY_INTERFACE_INDEX][j] = - (joint_states_[POSITION_INTERFACE_INDEX][j] - old_position) / period.seconds(); - - joint_states_[ACCELERATION_INTERFACE_INDEX][j] = - (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + const double old_pos = get_state(joint_pos); + const double old_vel = get_state(joint_vel); + + set_state( + joint_pos, get_command(joint_pos) + (custom_interface_with_following_offset_.empty() + ? position_state_following_offset_ + : 0.0)); + set_state(joint_vel, (get_state(joint_pos) - old_pos) / period.seconds()); + set_state(joint_acc, (get_state(joint_vel) - old_vel) / period.seconds()); } break; } } } - else + } + else + { + for (const auto joint_name : std_joint_names_) { - for (size_t k = 0; k < joint_states_[POSITION_INTERFACE_INDEX].size(); ++k) + const auto joint_pos = joint_name + "/" + hardware_interface::HW_IF_POSITION; + if ( + std_joint_command_interface_names_.find(joint_pos) != + std_joint_command_interface_names_.end()) { - if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][k])) - { - joint_states_[POSITION_INTERFACE_INDEX][k] = // apply offset to positions only - joint_commands_[POSITION_INTERFACE_INDEX][k] + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ - : 0.0); - } + set_state( + joint_pos, get_command(joint_pos) + (custom_interface_with_following_offset_.empty() + ? position_state_following_offset_ + : 0.0)); } } } @@ -587,135 +552,57 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur // velocity, and acceleration interface if (calculate_dynamics_) { - mirror_command_to_state(joint_states_, joint_commands_, 3); + std::vector interfaces( + standard_interfaces_.begin() + 3, standard_interfaces_.end()); + mirror_command_to_state(std_joint_names_, interfaces); } else { - mirror_command_to_state(joint_states_, joint_commands_, 1); + std::vector interfaces( + standard_interfaces_.begin() + 1, standard_interfaces_.end()); + mirror_command_to_state(std_joint_names_, interfaces); } for (const auto & mimic_joint : mimic_joints_) { - for (auto i = 0u; i < joint_states_.size(); ++i) - { - joint_states_[i][mimic_joint.joint_index] = - mimic_joint.multiplier * joint_states_[i][mimic_joint.mimicked_joint_index]; - } + set_state( + mimic_joint.joint_name, get_state(mimic_joint.mimic_joint_name) * mimic_joint.multiplier); } - for (size_t i = 0; i < other_states_.size(); ++i) + for (const auto joint_name : other_joint_names_) { - for (size_t j = 0; j < other_states_[i].size(); ++j) + for (const auto interface_name : non_standard_interfaces_) { + const auto joint_inteface = joint_name + "/" + interface_name; + const auto joint_pos = joint_name + "/" + hardware_interface::HW_IF_POSITION; if ( - i == index_custom_interface_with_following_offset_ && - !std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + interface_name == custom_interface_name_with_following_offset_ && + (std_joint_command_interface_names_.find(joint_pos) != + std_joint_command_interface_names_.end())) { - other_states_[i][j] = - joint_commands_[POSITION_INTERFACE_INDEX][j] + position_state_following_offset_; + set_state(joint_inteface, get_command(joint_pos) + position_state_following_offset_); } - else if (!std::isnan(other_commands_[i][j])) + else if ( + other_joint_command_interface_names_.find(joint_inteface) != + other_joint_command_interface_names_.end()) { - other_states_[i][j] = other_commands_[i][j]; + set_state(joint_inteface, get_command(joint_inteface)); } } } + // do loopback on all sensor interfaces if (use_mock_sensor_command_interfaces_) { - mirror_command_to_state(sensor_states_, sensor_mock_commands_); + mirror_all_available_commands_to_states(sensor_state_interfaces_); } // do loopback on all gpio interfaces - if (use_mock_gpio_command_interfaces_) - { - mirror_command_to_state(gpio_states_, gpio_mock_commands_); - } - else - { - mirror_command_to_state(gpio_states_, gpio_commands_); - } + mirror_all_available_commands_to_states(gpio_state_interfaces_); 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()) - { - auto j = std::distance(interface_list.begin(), it); - interfaces.emplace_back(name, *it, &values[j][vector_index]); - return true; - } - 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++) - { - commands[i].resize(component_infos.size(), std::numeric_limits::quiet_NaN()); - states[i].resize(component_infos.size(), std::numeric_limits::quiet_NaN()); - } - - // Initialize with values from URDF - for (auto i = 0u; i < component_infos.size(); i++) - { - const auto & component = component_infos[i]; - for (const auto & interface : component.state_interfaces) - { - auto it = std::find(interfaces.begin(), interfaces.end(), interface.name); - - // If interface name is found in the interfaces list - if (it != interfaces.end()) - { - auto index = 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); - } - } - } - } -} - -template -bool GenericSystem::populate_interfaces( - const std::vector & components, - std::vector & interface_names, std::vector> & storage, - std::vector & target_interfaces, bool using_state_interfaces) -{ - for (auto i = 0u; i < components.size(); i++) - { - const auto & component = components[i]; - const auto interfaces = - (using_state_interfaces) ? component.state_interfaces : component.command_interfaces; - for (const auto & interface : interfaces) - { - if (!get_interface( - component.name, interface_names, interface.name, i, storage, target_interfaces)) - { - return false; - } - } - } - - return true; -} } // namespace mock_components #include "pluginlib/class_list_macros.hpp" diff --git a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp index d226e08fd9..7ffbf7fa5d 100644 --- a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp @@ -19,9 +19,9 @@ #include "hardware_interface/sensor_interface.hpp" +using hardware_interface::InterfaceDescription; using hardware_interface::return_type; using hardware_interface::SensorInterface; -using hardware_interface::StateInterface; namespace test_hardware_components { @@ -51,54 +51,41 @@ class TestForceTorqueSensor : public SensorInterface } } + sensor_name_ = info_.sensors[0].name; fprintf(stderr, "TestForceTorqueSensor configured successfully.\n"); return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector export_state_interfaces_2() override { - std::vector state_interfaces; + std::vector state_interfaces; - const auto & sensor_name = info_.sensors[0].name; - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "fx", &values_.fx)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "fy", &values_.fy)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "fz", &values_.fz)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "tx", &values_.tx)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "ty", &values_.ty)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "tz", &values_.tz)); + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; + + for (const auto & interface_name : inteface_names_) + { + info.name = interface_name; + state_interfaces.push_back(hardware_interface::InterfaceDescription(sensor_name_, info)); + } return state_interfaces; } return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { - values_.fx = fmod((values_.fx + 1.0), 10); - values_.fy = fmod((values_.fy + 1.0), 10); - values_.fz = fmod((values_.fz + 1.0), 10); - values_.tx = fmod((values_.tx + 1.0), 10); - values_.ty = fmod((values_.ty + 1.0), 10); - values_.tz = fmod((values_.tz + 1.0), 10); + for (const auto & interface_name : inteface_names_) + { + const auto name = sensor_name_ + "/" + interface_name; + + set_state(name, fmod((get_state(name) + 1.0), 10)); + } return return_type::OK; } private: - struct FTValues - { - double fx = 0.0; - double fy = 0.0; - double fz = 0.0; - double tx = 0.0; - double ty = 0.0; - double tz = 0.0; - }; - - FTValues values_; + std::vector inteface_names_{"fx", "fy", "fz", "tx", "ty", "tz"}; + std::string sensor_name_; }; } // namespace test_hardware_components diff --git a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp index 390056d990..c4b64f9a3f 100644 --- a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp +++ b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp @@ -64,30 +64,35 @@ class TestSingleJointActuator : public ActuatorInterface return CallbackReturn::ERROR; } } + joint_name_ = info_.joints[0].name; + joint_pos_ = joint_name_ + "/" + hardware_interface::HW_IF_POSITION; + joint_vel_ = joint_name_ + "/" + hardware_interface::HW_IF_VELOCITY; fprintf(stderr, "TestSingleJointActuator configured successfully.\n"); return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector export_state_interfaces_2() override { - std::vector state_interfaces; + std::vector state_interfaces; + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; - const auto & joint_name = info_.joints[0].name; - state_interfaces.emplace_back(hardware_interface::StateInterface( - joint_name, hardware_interface::HW_IF_POSITION, &position_state_)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - joint_name, hardware_interface::HW_IF_VELOCITY, &velocity_state_)); + info.name = hardware_interface::HW_IF_POSITION; + state_interfaces.push_back(hardware_interface::InterfaceDescription(joint_name_, info)); + info.name = hardware_interface::HW_IF_VELOCITY; + state_interfaces.push_back(hardware_interface::InterfaceDescription(joint_name_, info)); return state_interfaces; } - std::vector export_command_interfaces() override + std::vector export_command_interfaces_2() override { - std::vector command_interfaces; + std::vector command_interfaces; + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; - const auto & joint_name = info_.joints[0].name; - command_interfaces.emplace_back(hardware_interface::CommandInterface( - joint_name, hardware_interface::HW_IF_POSITION, &position_command_)); + info.name = hardware_interface::HW_IF_POSITION; + command_interfaces.push_back(hardware_interface::InterfaceDescription(joint_name_, info)); return command_interfaces; } @@ -99,15 +104,15 @@ class TestSingleJointActuator : public ActuatorInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { - velocity_state_ = position_command_ - position_state_; - position_state_ = position_command_; + set_state(joint_vel_, get_command(joint_pos_) - get_state(joint_pos_)); + set_state(joint_pos_, get_command(joint_pos_)); return return_type::OK; } private: - double position_state_ = 0.0; - double velocity_state_ = 0.0; - double position_command_ = 0.0; + std::string joint_name_; + std::string joint_pos_; + std::string joint_vel_; }; } // namespace test_hardware_components diff --git a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp index a594d3b70a..f9cba77394 100644 --- a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp +++ b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp @@ -74,31 +74,42 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector export_state_interfaces_2() override { - std::vector state_interfaces; + std::vector state_interfaces; for (auto i = 0u; i < info_.joints.size(); i++) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; + + info.name = hardware_interface::HW_IF_POSITION; + state_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); + info.name = hardware_interface::HW_IF_VELOCITY; + state_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); + info.name = hardware_interface::HW_IF_ACCELERATION; + state_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); } return state_interfaces; } - std::vector export_command_interfaces() override + std::vector export_command_interfaces_2() override { - std::vector command_interfaces; + std::vector command_interfaces; for (auto i = 0u; i < info_.joints.size(); i++) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_command_[i])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; + + info.name = hardware_interface::HW_IF_POSITION; + command_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); + info.name = hardware_interface::HW_IF_VELOCITY; + command_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); } return command_interfaces; @@ -120,7 +131,10 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface const std::vector & start_interfaces, const std::vector & stop_interfaces) override { - acceleration_state_[0] += 1.0; + const auto first_joint_acc = + info_.joints[0].name + "/" + hardware_interface::HW_IF_ACCELERATION; + const auto old_acc = get_state(first_joint_acc); + set_state(first_joint_acc, old_acc + 1.0); // Starting interfaces start_modes_.clear(); @@ -168,7 +182,10 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface const std::vector & start_interfaces, const std::vector & /*stop_interfaces*/) override { - acceleration_state_[0] += 100.0; + const auto first_joint_acc = + info_.joints[0].name + "/" + hardware_interface::HW_IF_ACCELERATION; + const auto old_acc = get_state(first_joint_acc); + set_state(first_joint_acc, old_acc + 100.0); // Test of failure in perform command mode switch // Fail if given an empty list. // This should never occur in a real system as the same start_interfaces list is sent to both @@ -183,12 +200,6 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface private: std::vector start_modes_ = {"position", "position"}; std::vector stop_modes_ = {false, false}; - - std::array position_command_ = {0.0, 0.0}; - std::array velocity_command_ = {0.0, 0.0}; - std::array position_state_ = {0.0, 0.0}; - std::array velocity_state_ = {0.0, 0.0}; - std::array acceleration_state_ = {0.0, 0.0}; }; } // namespace test_hardware_components diff --git a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp index b597804104..5e612abe39 100644 --- a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp +++ b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp @@ -68,25 +68,33 @@ class TestTwoJointSystem : public SystemInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector export_state_interfaces_2() override { - std::vector state_interfaces; + std::vector state_interfaces; for (auto i = 0u; i < info_.joints.size(); ++i) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; + + info.name = hardware_interface::HW_IF_POSITION; + state_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); } return state_interfaces; } - std::vector export_command_interfaces() override + std::vector export_command_interfaces_2() override { - std::vector command_interfaces; + std::vector command_interfaces; for (auto i = 0u; i < info_.joints.size(); ++i) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_command_[i])); + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; + + info.name = hardware_interface::HW_IF_POSITION; + command_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); } return command_interfaces; @@ -101,10 +109,6 @@ class TestTwoJointSystem : public SystemInterface { return return_type::OK; } - -private: - std::array position_command_ = {0.0, 0.0}; - std::array position_state_ = {0.0, 0.0}; }; } // namespace test_hardware_components