From 5b406aa7d6044771f4be94d85b2159b8e25cd374 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 14 Feb 2024 11:49:36 +0100 Subject: [PATCH] first version of transmissions for mock hardware --- hardware_interface/CMakeLists.txt | 25 -- mock_hardware/CMakeLists.txt | 67 ++++++ .../doc/mock_components_userdoc.rst | 0 .../mock_components/generic_system.hpp | 38 ++- .../mock_components/visibility_control.h | 0 .../mock_components_plugin_description.xml | 0 mock_hardware/package.xml | 27 +++ .../src}/generic_system.cpp | 226 +++++++++++++++--- .../test}/test_generic_system.cpp | 0 9 files changed, 324 insertions(+), 59 deletions(-) create mode 100644 mock_hardware/CMakeLists.txt rename {hardware_interface => mock_hardware}/doc/mock_components_userdoc.rst (100%) rename {hardware_interface => mock_hardware}/include/mock_components/generic_system.hpp (80%) rename {hardware_interface => mock_hardware}/include/mock_components/visibility_control.h (100%) rename {hardware_interface => mock_hardware}/mock_components_plugin_description.xml (100%) create mode 100644 mock_hardware/package.xml rename {hardware_interface/src/mock_components => mock_hardware/src}/generic_system.cpp (72%) rename {hardware_interface/test/mock_components => mock_hardware/test}/test_generic_system.cpp (100%) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 2613ba735a3..7f56dcca1fd 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -39,22 +39,6 @@ ament_target_dependencies(hardware_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPEN # which is appropriate when building the dll but not consuming it. target_compile_definitions(hardware_interface PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") -add_library(mock_components SHARED - src/mock_components/generic_system.cpp -) -target_compile_features(mock_components PUBLIC cxx_std_17) -target_include_directories(mock_components PUBLIC - $ - $ -) -ament_target_dependencies(mock_components PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(mock_components PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") - -pluginlib_export_plugin_description_file( - hardware_interface mock_components_plugin_description.xml) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) @@ -94,14 +78,6 @@ if(BUILD_TESTING) pluginlib_export_plugin_description_file( hardware_interface test/test_hardware_components/test_hardware_components.xml ) - - ament_add_gmock(test_generic_system test/mock_components/test_generic_system.cpp) - target_include_directories(test_generic_system PRIVATE include) - target_link_libraries(test_generic_system hardware_interface) - ament_target_dependencies(test_generic_system - pluginlib - ros2_control_test_assets - ) endif() install( @@ -110,7 +86,6 @@ install( ) install( TARGETS - mock_components hardware_interface EXPORT export_hardware_interface RUNTIME DESTINATION bin diff --git a/mock_hardware/CMakeLists.txt b/mock_hardware/CMakeLists.txt new file mode 100644 index 00000000000..2e82828c222 --- /dev/null +++ b/mock_hardware/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 3.16) +project(mock_hardware LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + hardware_interface + pluginlib + rclcpp_lifecycle + rcpputils + rcutils + transmission_interface +) + +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +add_library(mock_components SHARED + src/generic_system.cpp +) +target_compile_features(mock_components PUBLIC cxx_std_17) +target_include_directories(mock_components PUBLIC + $ + $ +) +ament_target_dependencies(mock_components PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(mock_components PRIVATE "MOCK_COMPONENTS_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + hardware_interface mock_components_plugin_description.xml) + +if(BUILD_TESTING) + + find_package(ament_cmake_gmock REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_generic_system test/test_generic_system.cpp) + target_include_directories(test_generic_system PRIVATE include) + target_link_libraries(test_generic_system mock_components) + ament_target_dependencies(test_generic_system + pluginlib + ros2_control_test_assets + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/mock_components +) +install( + TARGETS + mock_components + EXPORT export_mock_components + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_mock_components HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/mock_hardware/doc/mock_components_userdoc.rst similarity index 100% rename from hardware_interface/doc/mock_components_userdoc.rst rename to mock_hardware/doc/mock_components_userdoc.rst diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/mock_hardware/include/mock_components/generic_system.hpp similarity index 80% rename from hardware_interface/include/mock_components/generic_system.hpp rename to mock_hardware/include/mock_components/generic_system.hpp index e9b38de65dc..bc20fae9a97 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/mock_hardware/include/mock_components/generic_system.hpp @@ -17,6 +17,7 @@ #ifndef MOCK_COMPONENTS__GENERIC_SYSTEM_HPP_ #define MOCK_COMPONENTS__GENERIC_SYSTEM_HPP_ +#include #include #include @@ -25,6 +26,7 @@ #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "transmission_interface/transmission.hpp" using hardware_interface::return_type; @@ -55,10 +57,7 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; - return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - return return_type::OK; - } + return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; protected: /// Use standard interfaces for joints because they are relevant for dynamic behavior @@ -100,6 +99,37 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::vector> gpio_commands_; std::vector> gpio_states_; + double actuator_slowdown_; + + // used for the Transmission pass through. + // read: actuator_interface.state_->Transmission->joint_interface.state_ + // write: joint_interface.command_->Transmission->actuator_interface.command_ + // And StateInterface(joint_interface.state_) + struct InterfaceData + { + // TODO(Manuel) set initial to NaN and on_init initialize to given value in info or 0.0 + explicit InterfaceData(const std::string & name) + : name_(name), + command_(0.0), // command_(std::numeric_limits::quiet_NaN()), + state_(0.0), // state_(std::numeric_limits::quiet_NaN()), + transmission_passthrough_( + 0.0) // transmission_passthrough_(std::numeric_limits::quiet_NaN()) + { + } + + std::string name_; + double command_; + double state_; + // this is the "sink" that will be part of the transmission Joint/Actuator handles + double transmission_passthrough_; + }; + + std::vector joint_interfaces_; + std::vector actuator_interfaces_; + + // transmissions + std::vector> transmissions_; + private: template bool get_interface( diff --git a/hardware_interface/include/mock_components/visibility_control.h b/mock_hardware/include/mock_components/visibility_control.h similarity index 100% rename from hardware_interface/include/mock_components/visibility_control.h rename to mock_hardware/include/mock_components/visibility_control.h diff --git a/hardware_interface/mock_components_plugin_description.xml b/mock_hardware/mock_components_plugin_description.xml similarity index 100% rename from hardware_interface/mock_components_plugin_description.xml rename to mock_hardware/mock_components_plugin_description.xml diff --git a/mock_hardware/package.xml b/mock_hardware/package.xml new file mode 100644 index 00000000000..79909cbfef0 --- /dev/null +++ b/mock_hardware/package.xml @@ -0,0 +1,27 @@ + + + mock_hardware + 4.3.0 + ros2_control hardware interface + Bence Magyar + Denis Štogl + Apache License 2.0 + + ament_cmake + + hardware_interface + rclcpp_lifecycle + pluginlib + rcpputils + transmission_interface + + rcutils + rcutils + + ament_cmake_gmock + ros2_control_test_assets + + + ament_cmake + + diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/mock_hardware/src/generic_system.cpp similarity index 72% rename from hardware_interface/src/mock_components/generic_system.cpp rename to mock_hardware/src/generic_system.cpp index 22d8aa573ca..43b0bba5d92 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/mock_hardware/src/generic_system.cpp @@ -18,10 +18,14 @@ #include #include +#include #include #include #include +#include +#include #include +#include #include #include @@ -29,6 +33,8 @@ #include "hardware_interface/lexical_casts.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rcutils/logging_macros.h" +#include "transmission_interface/simple_transmission_loader.hpp" +#include "transmission_interface/transmission_interface_exception.hpp" namespace mock_components { @@ -237,6 +243,83 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i initialize_storage_vectors(gpio_commands_, gpio_states_, gpio_interfaces_, info_.gpios); } + actuator_slowdown_ = std::stod(info_.hardware_parameters["actuator_slowdown"]); + + const auto num_joints = std::accumulate( + info_.transmissions.begin(), info_.transmissions.end(), 0ul, + [](const auto & acc, const auto & trans_info) { return acc + trans_info.joints.size(); }); + + const auto num_actuators = std::accumulate( + info_.transmissions.begin(), info_.transmissions.end(), 0ul, + [](const auto & acc, const auto & trans_info) { return acc + trans_info.actuators.size(); }); + + // reserve the space needed for joint and actuator data structures + joint_interfaces_.reserve(num_joints); + actuator_interfaces_.reserve(num_actuators); + + // TODO(Manuel) Add all types of transmissions + // create transmissions, joint and actuator handles + auto transmission_loader = transmission_interface::SimpleTransmissionLoader(); + + for (const auto & transmission_info : info_.transmissions) + { + // only simple transmissions are supported in this demo + if (transmission_info.type != "transmission_interface/SimpleTransmission") + { + std::string msg( + "Transmission " + transmission_info.name + " of type " + transmission_info.type.c_str() + + " not supported"); + throw std::runtime_error(msg); + } + + std::shared_ptr transmission; + + transmission = transmission_loader.load(transmission_info); + + std::vector joint_handles; + for (const auto & joint_info : transmission_info.joints) + { + // this demo supports only one interface per joint + if (!(joint_info.state_interfaces.size() == 1 && + joint_info.state_interfaces[0] == hardware_interface::HW_IF_POSITION && + joint_info.command_interfaces.size() == 1 && + joint_info.command_interfaces[0] == hardware_interface::HW_IF_POSITION)) + { + std::string msg("Invalid transmission joint " + joint_info.name + " configuration"); + throw std::runtime_error(msg); + } + + const auto joint_interface = + joint_interfaces_.insert(joint_interfaces_.end(), InterfaceData(joint_info.name)); + + transmission_interface::JointHandle joint_handle( + joint_info.name, hardware_interface::HW_IF_POSITION, + &joint_interface->transmission_passthrough_); + joint_handles.push_back(joint_handle); + } + + std::vector actuator_handles; + for (const auto & actuator_info : transmission_info.actuators) + { + // no check for actuators types + + const auto actuator_interface = + actuator_interfaces_.insert(actuator_interfaces_.end(), InterfaceData(actuator_info.name)); + transmission_interface::ActuatorHandle actuator_handle( + actuator_info.name, hardware_interface::HW_IF_POSITION, + &actuator_interface->transmission_passthrough_); + actuator_handles.push_back(actuator_handle); + } + + // TODO(Manuel) set initial to NaN and on_init initialize to given value in info or 0.0 + + /// @note no need to store the joint and actuator handles, the transmission + /// will keep whatever info it needs after is done with them + transmission->configure(joint_handles, actuator_handles); + + transmissions_.push_back(transmission); + } + return CallbackReturn::SUCCESS; } @@ -245,24 +328,42 @@ std::vector GenericSystem::export_state_inte std::vector state_interfaces; // Joints' state interfaces - for (auto i = 0u; i < info_.joints.size(); i++) + // we manually use the loop_counter because we want to iterate over the interfaces themselves so + // we can use std::find them but still need the loop counter for our storage matrix + size_t i = 0; + for (const auto & joint : info_.joints) { - const auto & joint = info_.joints[i]; - for (const auto & interface : joint.state_interfaces) + /// @pre all joint interfaces exist, checked in on_init() + auto joint_interface = std::find_if( + joint_interfaces_.begin(), joint_interfaces_.end(), + [&](const InterfaceData & interface) { return interface.name_ == joint.name; }); + + // joint is not used by transmissions so we use the storage matrix for each StateInterface + if (joint_interface == joint_interfaces_.end()) { - // 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)) + 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, other_interfaces_, interface.name, i, other_states_, state_interfaces)) + joint.name, standard_interfaces_, interface.name, i, joint_states_, state_interfaces)) { - throw std::runtime_error( - "Interface is not found in the standard nor other list. " - "This should never happen!"); + 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!"); + } } } } + // joint is used by transmissions so we use the separate InterfaceData storage vector. + else + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint.name, hardware_interface::HW_IF_POSITION, &joint_interface->state_)); + } + ++i; } // Sensor state interfaces @@ -286,27 +387,45 @@ std::vector GenericSystem::export_command_ { std::vector command_interfaces; - // Joints' state interfaces - for (size_t i = 0; i < info_.joints.size(); ++i) + // Joints' command interfaces + // we manually use the loop_counter because we want to iterate over the interfaces themselves so + // we can use std::find them but still need the loop counter for our storage matrix + size_t i = 0; + for (const auto & joint : info_.joints) { - const auto & joint = info_.joints[i]; - for (const auto & interface : joint.command_interfaces) + /// @pre all joint interfaces exist, checked in on_init() + auto joint_interface = std::find_if( + joint_interfaces_.begin(), joint_interfaces_.end(), + [&](const InterfaceData & interface) { return interface.name_ == joint.name; }); + + // joint is not used by transmissions so we use the storage matrix for each CommandInterface + if (joint_interface == joint_interfaces_.end()) { - // 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)) + 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, other_interfaces_, interface.name, i, other_commands_, + joint.name, standard_interfaces_, interface.name, i, joint_commands_, command_interfaces)) { - throw std::runtime_error( - "Interface is not found in the standard nor other list. " - "This should never happen!"); + 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!"); + } } } } + // joint is used by transmissions so we use the separate InterfaceData storage vector. + else + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint.name, hardware_interface::HW_IF_POSITION, &joint_interface->command_)); + } + ++i; } // Set position control mode per default joint_control_mode_.resize(info_.joints.size(), POSITION_INTERFACE_INDEX); @@ -570,15 +689,12 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } else { - for (size_t k = 0; k < joint_states_[POSITION_INTERFACE_INDEX].size(); ++k) + if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) { - 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); - } + 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); } } } @@ -636,9 +752,59 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur mirror_command_to_state(gpio_states_, gpio_commands_); } + // do loopback over all transmission interface an update + // actuator: state -> transmission + std::for_each( + actuator_interfaces_.begin(), actuator_interfaces_.end(), + [](auto & actuator_interface) + { actuator_interface.transmission_passthrough_ = actuator_interface.state_; }); + + // transmission: actuator -> joint + std::for_each( + transmissions_.begin(), transmissions_.end(), + [](auto & transmission) { transmission->actuator_to_joint(); }); + + // joint: transmission -> state + std::for_each( + joint_interfaces_.begin(), joint_interfaces_.end(), + [](auto & joint_interface) + { joint_interface.state_ = joint_interface.transmission_passthrough_; }); + return return_type::OK; } +return_type GenericSystem::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // joint: command -> transmission + std::for_each( + joint_interfaces_.begin(), joint_interfaces_.end(), + [](auto & joint_interface) + { joint_interface.transmission_passthrough_ = joint_interface.command_; }); + + // transmission: joint -> actuator + std::for_each( + transmissions_.begin(), transmissions_.end(), + [](auto & transmission) { transmission->joint_to_actuator(); }); + + // actuator: transmission -> command + std::for_each( + actuator_interfaces_.begin(), actuator_interfaces_.end(), + [](auto & actuator_interface) + { actuator_interface.command_ = actuator_interface.transmission_passthrough_; }); + + // simulate motor motion + std::for_each( + actuator_interfaces_.begin(), actuator_interfaces_.end(), + [&](auto & actuator_interface) + { + actuator_interface.state_ = + actuator_interface.state_ + + (actuator_interface.command_ - actuator_interface.state_) / actuator_slowdown_; + }); + + return hardware_interface::return_type::OK; +} + // Private methods template bool GenericSystem::get_interface( diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/mock_hardware/test/test_generic_system.cpp similarity index 100% rename from hardware_interface/test/mock_components/test_generic_system.cpp rename to mock_hardware/test/test_generic_system.cpp