From ad416cea004b99db04525030e19dade41fc5e289 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 | 32 +- .../mock_components/generic_system.hpp | 67 +-- .../src/mock_components/generic_system.cpp | 528 ++++++++---------- .../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 +- .../test/test_components/test_actuator.cpp | 60 +- .../test/test_components/test_sensor.cpp | 12 - .../test/test_components/test_system.cpp | 62 +- .../test/test_resource_manager.cpp | 31 +- .../transmission_interface/accessor.hpp | 5 +- .../differential_transmission.hpp | 12 - .../four_bar_linkage_transmission.hpp | 12 - .../simple_transmission.hpp | 12 +- transmission_interface/test/utils_test.cpp | 7 +- 16 files changed, 406 insertions(+), 609 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 31b459177a..471258b759 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; @@ -79,9 +61,6 @@ class Handle virtual ~Handle() = default; - /// Returns true if handle references a value. - inline operator bool() const { return value_ptr_ != nullptr; } - const std::string get_name() const { return prefix_name_ + "/" + interface_name_; } const std::string & get_interface_name() const { return interface_name_; } @@ -183,7 +162,6 @@ class Handle std::string prefix_name_; std::string interface_name_; HANDLE_DATATYPE value_; - double * value_ptr_; }; class StateInterface : public Handle diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index fbb8547ab1..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,54 +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_; - /// 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_; - - 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_; + struct MimicJoint + { + std::string joint_name; + std::string mimic_joint_name; + double multiplier = 1.0; + }; + std::vector mimic_joints_; + + // 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_; + + // 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 2d8a01a34f..9d8ab9e582 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -59,7 +59,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()) @@ -119,49 +118,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++) - { - for (auto j = 0u; j < standard_interfaces_.size(); j++) - { - if (std::isnan(joint_states_[j][i])) - { - joint_states_[j][i] = 0.0; - } - } - } // 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_interfaces(joint.command_interfaces, non_standard_interfaces_); // populate non-standard state interfaces to other_interfaces_ - populate_non_standard_interfaces(joint.state_interfaces, other_interfaces_); + populate_non_standard_interfaces(joint.state_interfaces, non_standard_interfaces_); } - // Initialize storage for non-standard interfaces - initialize_storage_vectors(other_commands_, other_states_, other_interfaces_, info_.joints); + // 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 (const auto & state_inteface : joint.state_interfaces) + { + if ( + std::find(standard_interfaces_.begin(), standard_interfaces_.end(), state_inteface.name) == + standard_interfaces_.end()) + { + 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()); + } + } + + // 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) + { + if ( + std::find( + std_joint_state_interface_names_.begin(), std_joint_state_interface_names_.end(), + command_interface.name) == std_joint_state_interface_names_.end()) + { + std_joint_command_interface_names_.insert(joint.name + "/" + command_interface.name); + } + 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()) + { + 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"); + } + } + } // 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 { @@ -172,149 +212,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)) + for (const auto & gpio : info_.gpios) { - 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)) - { - 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( @@ -330,8 +300,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) { @@ -343,14 +313,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) { @@ -362,7 +332,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) { @@ -374,7 +344,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 @@ -388,7 +358,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!", @@ -398,13 +368,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; } } @@ -434,15 +404,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; } } } @@ -459,97 +429,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)); } } } @@ -558,136 +551,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 : info_.mimic_joints) + 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.offset + - 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_); } - if (use_mock_gpio_command_interfaces_) - { - mirror_command_to_state(gpio_states_, gpio_mock_commands_); - } - else - { - // do loopback on all gpio interfaces - mirror_command_to_state(gpio_states_, gpio_commands_); - } + // do loopback on all gpio interfaces + 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 47b19f9769..16ecb7649d 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 { @@ -50,54 +50,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 aba2f86fe5..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 fe06a64223..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 diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index a01ccd879f..c020f57e54 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -45,42 +45,29 @@ class TestActuator : public ActuatorInterface * CallbackReturn::ERROR;} */ - return CallbackReturn::SUCCESS; - } + pos_state_ = info_.joints[0].name + "/position"; + vel_state_ = info_.joints[0].name + "/velocity"; + vel_command_ = info_.joints[0].name + "/velocity"; - std::vector export_state_interfaces() override - { - std::vector state_interfaces; - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, info_.joints[0].state_interfaces[0].name, &position_state_)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, info_.joints[0].state_interfaces[1].name, &velocity_state_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(info_.joints[0].name, "some_unlisted_interface", nullptr)); - - return state_interfaces; + return CallbackReturn::SUCCESS; } - std::vector export_command_interfaces() override + std::vector export_state_interfaces_2() override { - std::vector command_interfaces; - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, info_.joints[0].command_interfaces[0].name, &velocity_command_)); - - if (info_.joints[0].command_interfaces.size() > 1) - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, info_.joints[0].command_interfaces[1].name, &max_velocity_command_)); - } + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "some_unlisted_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface(info_.joints[0].name, info); + interfaces.push_back(unlisted_state_interface); - return command_interfaces; + return interfaces; } hardware_interface::return_type prepare_command_mode_switch( const std::vector & /*start_interfaces*/, const std::vector & /*stop_interfaces*/) override { - position_state_ += 1.0; + set_state(pos_state_, get_state(pos_state_) + 1.0); return hardware_interface::return_type::OK; } @@ -88,22 +75,22 @@ class TestActuator : public ActuatorInterface const std::vector & /*start_interfaces*/, const std::vector & /*stop_interfaces*/) override { - position_state_ += 100.0; + set_state(pos_state_, get_state(pos_state_) + 100.0); return hardware_interface::return_type::OK; } return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on read - if (velocity_command_ == test_constants::READ_FAIL_VALUE) + if (get_command(vel_command_) == test_constants::READ_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM // tests - velocity_command_ = 0.0; + set_command(vel_command_, 0.0); return return_type::ERROR; } // simulate deactivate on read - if (velocity_command_ == test_constants::READ_DEACTIVATE_VALUE) + if (get_command(vel_command_) == test_constants::READ_DEACTIVATE_VALUE) { return return_type::DEACTIVATE; } @@ -112,22 +99,22 @@ class TestActuator : public ActuatorInterface // working as it should. This makes value checks clearer and confirms there // is no "state = command" line or some other mixture of interfaces // somewhere in the test stack. - velocity_state_ = velocity_command_ / 2; + set_state(vel_state_, get_command(vel_command_) / 2); return return_type::OK; } return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on write - if (velocity_command_ == test_constants::WRITE_FAIL_VALUE) + if (get_command(vel_command_) == test_constants::WRITE_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM // tests - velocity_command_ = 0.0; + set_command(vel_command_, 0.0); return return_type::ERROR; } // simulate deactivate on write - if (velocity_command_ == test_constants::WRITE_DEACTIVATE_VALUE) + if (get_command(vel_command_) == test_constants::WRITE_DEACTIVATE_VALUE) { return return_type::DEACTIVATE; } @@ -135,10 +122,9 @@ class TestActuator : public ActuatorInterface } private: - double position_state_ = 0.0; - double velocity_state_ = 0.0; - double velocity_command_ = 0.0; - double max_velocity_command_ = 0.0; + std::string pos_state_; + std::string vel_state_; + std::string vel_command_; }; class TestUninitializableActuator : public TestActuator diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index 2ea36ac5c1..254d2cec11 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -37,22 +37,10 @@ class TestSensor : public SensorInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override - { - std::vector state_interfaces; - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.sensors[0].name, info_.sensors[0].state_interfaces[0].name, &velocity_state_)); - - return state_interfaces; - } - return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { return return_type::OK; } - -private: - double velocity_state_ = 0.0; }; class TestUninitializableSensor : public TestSensor diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index 20e606ee6d..d6d0cad190 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -27,64 +27,18 @@ using hardware_interface::SystemInterface; class TestSystem : public SystemInterface { - std::vector export_state_interfaces() override - { - 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])); - } - - if (info_.gpios.size() > 0) - { - // Add configuration/max_tcp_jerk interface - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.gpios[0].name, info_.gpios[0].state_interfaces[0].name, &configuration_state_)); - } - - return state_interfaces; - } - - std::vector export_command_interfaces() override - { - 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_VELOCITY, &velocity_command_[i])); - } - // Add max_acceleration command interface - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, info_.joints[0].command_interfaces[1].name, - &max_acceleration_command_)); - - if (info_.gpios.size() > 0) - { - // Add configuration/max_tcp_jerk interface - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.gpios[0].name, info_.gpios[0].command_interfaces[0].name, &configuration_command_)); - } - - return command_interfaces; - } - return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on read - if (velocity_command_[0] == test_constants::READ_FAIL_VALUE) + if (get_command(info_.joints[0].name + "/velocity") == test_constants::READ_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM // tests - velocity_command_[0] = 0.0; + set_command(info_.joints[0].name + "/velocity", 0.0); return return_type::ERROR; } // simulate deactivate on read - if (velocity_command_[0] == test_constants::READ_DEACTIVATE_VALUE) + if (get_command(info_.joints[0].name + "/velocity") == test_constants::READ_DEACTIVATE_VALUE) { return return_type::DEACTIVATE; } @@ -94,15 +48,15 @@ class TestSystem : public SystemInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on write - if (velocity_command_[0] == test_constants::WRITE_FAIL_VALUE) + if (get_command(info_.joints[0].name + "/velocity") == test_constants::WRITE_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM // tests - velocity_command_[0] = 0.0; + set_command(info_.joints[0].name + "/velocity", 0.0); return return_type::ERROR; } // simulate deactivate on write - if (velocity_command_[0] == test_constants::WRITE_DEACTIVATE_VALUE) + if (get_command(info_.joints[0].name + "/velocity") == test_constants::WRITE_DEACTIVATE_VALUE) { return return_type::DEACTIVATE; } @@ -110,10 +64,6 @@ class TestSystem : public SystemInterface } private: - 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}}; double max_acceleration_command_ = 0.0; double configuration_state_ = 0.0; double configuration_command_ = 0.0; diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index c8b077229e..5bafdb7864 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -309,22 +309,26 @@ TEST_F(ResourceManagerTest, resource_claiming) class ExternalComponent : public hardware_interface::ActuatorInterface { - std::vector export_state_interfaces() override + std::vector export_state_interfaces_2() override { - std::vector state_interfaces; - state_interfaces.emplace_back( - hardware_interface::StateInterface("external_joint", "external_state_interface", nullptr)); + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "external_state_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface("external_joint", info); + interfaces.push_back(unlisted_state_interface); - return state_interfaces; + return interfaces; } - std::vector export_command_interfaces() override + std::vector export_command_interfaces_2() override { - std::vector command_interfaces; - command_interfaces.emplace_back(hardware_interface::CommandInterface( - "external_joint", "external_command_interface", nullptr)); + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "external_command_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface("external_joint", info); + interfaces.push_back(unlisted_state_interface); - return command_interfaces; + return interfaces; } std::string get_name() const override { return "ExternalComponent"; } @@ -1191,8 +1195,11 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) for (size_t i = 0; i < REFERENCE_INTERFACE_NAMES.size(); ++i) { - reference_interfaces.push_back(hardware_interface::CommandInterface( - CONTROLLER_NAME, REFERENCE_INTERFACE_NAMES[i], &(reference_interface_values[i]))); + hardware_interface::InterfaceInfo info; + info.name = REFERENCE_INTERFACE_NAMES[i]; + info.initial_value = std::to_string(reference_interface_values[i]); + hardware_interface::InterfaceDescription ref_interface(CONTROLLER_NAME, info); + reference_interfaces.push_back(hardware_interface::CommandInterface(ref_interface)); } rm.import_controller_reference_interfaces(CONTROLLER_NAME, reference_interfaces); diff --git a/transmission_interface/include/transmission_interface/accessor.hpp b/transmission_interface/include/transmission_interface/accessor.hpp index dd58e55744..64652f7802 100644 --- a/transmission_interface/include/transmission_interface/accessor.hpp +++ b/transmission_interface/include/transmission_interface/accessor.hpp @@ -60,10 +60,9 @@ std::vector get_ordered_handles( { std::copy_if( unordered_handles.cbegin(), unordered_handles.cend(), std::back_inserter(result), - [&](const auto & handle) - { + [&](const auto & handle) { return (handle.get_prefix_name() == name) && - (handle.get_interface_name() == interface_type) && handle; + (handle.get_interface_name() == interface_type); }); } return result; diff --git a/transmission_interface/include/transmission_interface/differential_transmission.hpp b/transmission_interface/include/transmission_interface/differential_transmission.hpp index 53a084fe76..5df12b8352 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.hpp +++ b/transmission_interface/include/transmission_interface/differential_transmission.hpp @@ -261,8 +261,6 @@ inline void DifferentialTransmission::actuator_to_joint() auto & joint_pos = joint_position_; if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints()) { - assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); - joint_pos[0].set_value( (act_pos[0].get_value() / ar[0] + act_pos[1].get_value() / ar[1]) / (2.0 * jr[0]) + @@ -277,8 +275,6 @@ inline void DifferentialTransmission::actuator_to_joint() auto & joint_vel = joint_velocity_; if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints()) { - assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); - joint_vel[0].set_value( (act_vel[0].get_value() / ar[0] + act_vel[1].get_value() / ar[1]) / (2.0 * jr[0])); @@ -291,8 +287,6 @@ inline void DifferentialTransmission::actuator_to_joint() auto & joint_eff = joint_effort_; if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints()) { - assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - joint_eff[0].set_value( jr[0] * (act_eff[0].get_value() * ar[0] + act_eff[1].get_value() * ar[1])); joint_eff[1].set_value( @@ -309,8 +303,6 @@ inline void DifferentialTransmission::joint_to_actuator() auto & joint_pos = joint_position_; if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints()) { - assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); - double joints_offset_applied[2] = { joint_pos[0].get_value() - joint_offset_[0], joint_pos[1].get_value() - joint_offset_[1]}; @@ -324,8 +316,6 @@ inline void DifferentialTransmission::joint_to_actuator() auto & joint_vel = joint_velocity_; if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints()) { - assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); - act_vel[0].set_value( (joint_vel[0].get_value() * jr[0] + joint_vel[1].get_value() * jr[1]) * ar[0]); @@ -338,8 +328,6 @@ inline void DifferentialTransmission::joint_to_actuator() auto & joint_eff = joint_effort_; if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints()) { - assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - act_eff[0].set_value( (joint_eff[0].get_value() / jr[0] + joint_eff[1].get_value() / jr[1]) / (2.0 * ar[0])); diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index 8b5fdd5433..e2ebcbeab6 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -259,8 +259,6 @@ inline void FourBarLinkageTransmission::actuator_to_joint() auto & joint_pos = joint_position_; if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints()) { - assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); - joint_pos[0].set_value(act_pos[0].get_value() / (jr[0] * ar[0]) + joint_offset_[0]); joint_pos[1].set_value( (act_pos[1].get_value() / ar[1] - act_pos[0].get_value() / (jr[0] * ar[0])) / @@ -273,8 +271,6 @@ inline void FourBarLinkageTransmission::actuator_to_joint() auto & joint_vel = joint_velocity_; if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints()) { - assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); - joint_vel[0].set_value(act_vel[0].get_value() / (jr[0] * ar[0])); joint_vel[1].set_value( (act_vel[1].get_value() / ar[1] - act_vel[0].get_value() / (jr[0] * ar[0])) / @@ -286,8 +282,6 @@ inline void FourBarLinkageTransmission::actuator_to_joint() auto & joint_eff = joint_effort_; if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints()) { - assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - joint_eff[0].set_value(jr[0] * act_eff[0].get_value() * ar[0]); joint_eff[1].set_value( jr[1] * @@ -305,8 +299,6 @@ inline void FourBarLinkageTransmission::joint_to_actuator() auto & joint_pos = joint_position_; if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints()) { - assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); - double joints_offset_applied[2] = { joint_pos[0].get_value() - joint_offset_[0], joint_pos[1].get_value() - joint_offset_[1]}; @@ -319,8 +311,6 @@ inline void FourBarLinkageTransmission::joint_to_actuator() auto & joint_vel = joint_velocity_; if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints()) { - assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); - act_vel[0].set_value(joint_vel[0].get_value() * jr[0] * ar[0]); act_vel[1].set_value( (joint_vel[0].get_value() + joint_vel[1].get_value() * jr[1]) * ar[1]); @@ -331,8 +321,6 @@ inline void FourBarLinkageTransmission::joint_to_actuator() auto & joint_eff = joint_effort_; if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints()) { - assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - act_eff[0].set_value(joint_eff[0].get_value() / (ar[0] * jr[0])); act_eff[1].set_value( (joint_eff[0].get_value() + joint_eff[1].get_value() / jr[1]) / ar[1]); diff --git a/transmission_interface/include/transmission_interface/simple_transmission.hpp b/transmission_interface/include/transmission_interface/simple_transmission.hpp index b240c1e489..91638060a2 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.hpp +++ b/transmission_interface/include/transmission_interface/simple_transmission.hpp @@ -128,13 +128,13 @@ class SimpleTransmission : public Transmission double reduction_; double jnt_offset_; - JointHandle joint_position_ = {"", "", nullptr}; - JointHandle joint_velocity_ = {"", "", nullptr}; - JointHandle joint_effort_ = {"", "", nullptr}; + JointHandle joint_position_(std::string(), std::string()); + JointHandle joint_velocity_(std::string(), std::string()); + JointHandle joint_effort_(std::string(), std::string()); - ActuatorHandle actuator_position_ = {"", "", nullptr}; - ActuatorHandle actuator_velocity_ = {"", "", nullptr}; - ActuatorHandle actuator_effort_ = {"", "", nullptr}; + ActuatorHandle actuator_position_(std::string(), std::string()); + ActuatorHandle actuator_velocity_(std::string(), std::string()); + ActuatorHandle actuator_effort_(std::string(), std::string()); }; inline SimpleTransmission::SimpleTransmission( diff --git a/transmission_interface/test/utils_test.cpp b/transmission_interface/test/utils_test.cpp index 9afaccaa47..808faa20d7 100644 --- a/transmission_interface/test/utils_test.cpp +++ b/transmission_interface/test/utils_test.cpp @@ -25,8 +25,11 @@ using transmission_interface::JointHandle; TEST(UtilsTest, AccessorTest) { const std::string NAME = "joint"; - double joint_value = 0.0; - const JointHandle joint_handle(NAME, HW_IF_POSITION, &joint_value); + hardware_interface::InterfaceInfo info; + info.name = HW_IF_POSITION; + info.initial_value = "0.0"; + hardware_interface::InterfaceDescription joint_handle_desc(NAME, info); + const JointHandle joint_handle(joint_handle_desc); const std::vector joint_handles = {joint_handle}; ASSERT_EQ(transmission_interface::get_names(joint_handles), std::vector{NAME});