From a2dfd7696a95813d76f9ddf014da235fd2c5e138 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 16 Aug 2024 12:54:52 +0200 Subject: [PATCH 01/12] split hw related changes from #1240 --- doc/Iron.rst | 92 ++ hardware_interface/CMakeLists.txt | 5 + .../doc/writing_new_hardware_component.rst | 114 +- .../include/hardware_interface/actuator.hpp | 4 +- .../hardware_interface/actuator_interface.hpp | 212 ++- .../hardware_interface/component_parser.hpp | 18 + .../include/hardware_interface/handle.hpp | 35 +- .../hardware_interface/hardware_info.hpp | 30 +- .../hardware_interface/resource_manager.hpp | 5 +- .../include/hardware_interface/sensor.hpp | 2 +- .../hardware_interface/sensor_interface.hpp | 112 +- .../include/hardware_interface/system.hpp | 4 +- .../hardware_interface/system_interface.hpp | 256 +++- hardware_interface/src/actuator.cpp | 53 +- hardware_interface/src/component_parser.cpp | 34 + hardware_interface/src/resource_manager.cpp | 133 +- hardware_interface/src/sensor.cpp | 28 +- hardware_interface/src/system.cpp | 55 +- .../mock_components/test_generic_system.cpp | 3 - .../test/test_component_interfaces.cpp | 1203 +++++++++++++++-- ...est_component_interfaces_custom_export.cpp | 377 ++++++ .../test/test_component_parser.cpp | 110 ++ hardware_interface/test/test_components.hpp | 39 + hardware_interface/test/test_handle.cpp | 31 + .../test/test_resource_manager.cpp | 4 +- .../components_urdfs.hpp | 75 + 26 files changed, 2795 insertions(+), 239 deletions(-) create mode 100644 doc/Iron.rst create mode 100644 hardware_interface/test/test_component_interfaces_custom_export.cpp create mode 100644 hardware_interface/test/test_components.hpp diff --git a/doc/Iron.rst b/doc/Iron.rst new file mode 100644 index 0000000000..323d5ecffe --- /dev/null +++ b/doc/Iron.rst @@ -0,0 +1,92 @@ +Iron to Jazzy +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Adaption of Command-/StateInterfaces +*************************************** +Changes from `(PR #1240) `__ + +* ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which gets parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp `__). +* The memory for storing the value of a ``Command-/StateInterfaces`` is no longer allocated in the hardware but instead in the ``Command-/StateInterfaces`` itself. +* To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function by creating some custom ``Command-/StateInterfaces``. + +Migration of Command-/StateInterfaces +------------------------------------- +To adapt to the new way of creating and exporting ``Command-/StateInterfaces`` follow those steps: + +1. Delete the ``std::vector export_command_interfaces() override`` and ``std::vector export_state_interfaces() override``. +2. Delete allocated memory for any ``Command-/StateInterfaces``, e.g.: + + * If you have a ``std::vector hw_commands_;`` for joints' ``CommandInterfaces`` delete the definition and any usage/appearance. + * Wherever you iterated over a state/command or accessed commands like this: + +.. code-block:: c++ + + // states + for (uint i = 0; i < hw_states_.size(); i++) + { + hw_states_[i] = 0; + auto some_state = hw_states_[i]; + } + + // commands + for (uint i = 0; i < hw_commands_.size(); i++) + { + hw_commands_[i] = 0; + auto some_command = hw_commands_[i]; + } + + // specific state/command + hw_commands_[x] = hw_states_[y]; + +replace it with + +.. code-block:: c++ + + // states replace with this + for (const auto & [name, descr] : joint_state_interfaces_) + { + set_state(name, 0.0); + auto some_state = get_state(name); + } + + //commands replace with this + for (const auto & [name, descr] : joint_commands_interfaces_) + { + set_command(name, 0.0); + auto some_command = get_command(name); + } + + // replace specific state/command, for this you need to store the names which are std::strings + // somewhere or know them. However be careful since the names are fully qualified names which + // means that the prefix is included for the name: E.g.: prefix/joint_1/velocity + set_command(name_of_command_interface_x, get_state(name_of_state_interface_y)); + +Migration of unlisted Command-/StateInterfaces not defined in ``ros2_control`` XML-tag +-------------------------------------------------------------------------------------- +If you want some unlisted ``Command-/StateInterfaces`` not included in the ``ros2_control`` XML-tag you can follow those steps: + +1. Override the ``virtual std::vector export_command_interfaces_2()`` or ``virtual std::vector export_state_interfaces_2()`` +2. Create the InterfaceDescription for each of the interfaces you want to create in the override ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function, add it to a vector and return the vector: + + .. code-block:: c++ + + std::vector my_unlisted_interfaces; + + InterfaceInfo unlisted_interface; + unlisted_interface.name = "some_unlisted_interface"; + unlisted_interface.min = "-5.0"; + unlisted_interface.data_type = "double"; + my_unlisted_interfaces.push_back(InterfaceDescription(info_.name, unlisted_interface)); + + return my_unlisted_interfaces; + +3. The unlisted interface will then be stored in either the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_`` map depending in which function they are created. +4. You can access it like any other interface with the ``get_state(name)``, ``set_state(name, value)``, ``get_command(name)`` or ``set_command(name, value)``. E.g. ``get_state("some_unlisted_interface")``. + +Custom exportation of Command-/StateInterfaces +---------------------------------------------- +In case the default implementation (``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` ) for exporting the ``Command-/StateInterfaces`` is not enough you can override them. You should however consider the following things: + +* If you want to have unlisted interfaces available you need to call the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. +* Don't forget to store the created ``Command-/StateInterfaces`` internally as you only return ``shared_ptrs`` and the resource manager will not provide access to the created ``Command-/StateInterfaces`` for the hardware. So you must take care of storing them yourself. +* Names must be unique! diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index d2f480e2f3..385ae96fb1 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -78,6 +78,11 @@ if(BUILD_TESTING) ament_add_gmock(test_component_interfaces test/test_component_interfaces.cpp) target_link_libraries(test_component_interfaces hardware_interface) + ament_target_dependencies(test_component_interfaces ros2_control_test_assets) + + ament_add_gmock(test_component_interfaces_custom_export test/test_component_interfaces_custom_export.cpp) + target_link_libraries(test_component_interfaces_custom_export hardware_interface) + ament_target_dependencies(test_component_interfaces_custom_export ros2_control_test_assets) ament_add_gmock(test_component_parser test/test_component_parser.cpp) target_link_libraries(test_component_parser hardware_interface) diff --git a/hardware_interface/doc/writing_new_hardware_component.rst b/hardware_interface/doc/writing_new_hardware_component.rst index 698f6cf6e2..ad3728c21a 100644 --- a/hardware_interface/doc/writing_new_hardware_component.rst +++ b/hardware_interface/doc/writing_new_hardware_component.rst @@ -8,7 +8,7 @@ Writing a Hardware Component In ros2_control hardware system components are libraries, dynamically loaded by the controller manager using the `pluginlib `_ interface. The following is a step-by-step guide to create source files, basic tests, and compile rules for a new hardware interface. -1. **Preparing package** +#. **Preparing package** If the package for the hardware interface does not exist, then create it first. The package should have ``ament_cmake`` as a build type. @@ -17,7 +17,7 @@ The following is a step-by-step guide to create source files, basic tests, and c Use the ``--help`` flag for more information on how to use it. There is also an option to create library source files and compile rules to help you in the following steps. -2. **Preparing source files** +#. **Preparing source files** After creating the package, you should have at least ``CMakeLists.txt`` and ``package.xml`` files in it. Create also ``include//`` and ``src`` folders if they do not already exist. @@ -25,7 +25,7 @@ The following is a step-by-step guide to create source files, basic tests, and c Optionally add ``visibility_control.h`` with the definition of export rules for Windows. You can copy this file from an existing controller package and change the name prefix to the ````. -3. **Adding declarations into header file (.hpp)** +#. **Adding declarations into header file (.hpp)** 1. Take care that you use header guards. ROS2-style is using ``#ifndef`` and ``#define`` preprocessor directives. (For more information on this, a search engine is your friend :) ). @@ -39,98 +39,126 @@ The following is a step-by-step guide to create source files, basic tests, and c class HardwareInterfaceName : public hardware_interface::$InterfaceType$Interface 5. Add a constructor without parameters and the following public methods implementing ``LifecycleNodeInterface``: ``on_configure``, ``on_cleanup``, ``on_shutdown``, ``on_activate``, ``on_deactivate``, ``on_error``; and overriding ``$InterfaceType$Interface`` definition: ``on_init``, ``export_state_interfaces``, ``export_command_interfaces``, ``prepare_command_mode_switch`` (optional), ``perform_command_mode_switch`` (optional), ``read``, ``write``. - For further explanation of hardware-lifecycle check the `pull request `_ and for exact definitions of methods check the ``"hardware_interface/$interface_type$_interface.hpp"`` header or `doxygen documentation `_ for *Actuator*, *Sensor* or *System*. -4. **Adding definitions into source file (.cpp)** + For further explanation of hardware-lifecycle check the `pull request `_ and for exact definitions of methods check the ``"hardware_interface/$interface_type$_interface.hpp"`` header or `doxygen documentation `_ for *Actuator*, *Sensor* or *System*. - 1. Include the header file of your hardware interface and add a namespace definition to simplify further development. +#. **Adding definitions into source file (.cpp)** - 2. Implement ``on_init`` method. Here, you should initialize all member variables and process the parameters from the ``info`` argument. + #. Include the header file of your hardware interface and add a namespace definition to simplify further development. + + #. Implement ``on_init`` method. Here, you should initialize all member variables and process the parameters from the ``info`` argument. In the first line usually the parents ``on_init`` is called to process standard values, like name. This is done using: ``hardware_interface::(Actuator|Sensor|System)Interface::on_init(info)``. If all required parameters are set and valid and everything works fine return ``CallbackReturn::SUCCESS`` or ``return CallbackReturn::ERROR`` otherwise. - 4. Write the ``on_configure`` method where you usually setup the communication to the hardware and set everything up so that the hardware can be activated. + #. Write the ``on_configure`` method where you usually setup the communication to the hardware and set everything up so that the hardware can be activated. + + #. Implement ``on_cleanup`` method, which does the opposite of ``on_configure``. + #. ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which gets parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp `__). + + * To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_command_interface_descriptions()`` or ``export_state_interface_descriptions()`` function by creating some custom ``Command-/StateInterfaces``. + * For the ``Sensor``-type hardware interface there is no ``export_command_interfaces`` method. + * As a reminder, the full interface names have structure ``/``. + + #. (optional) If you want some unlisted ``Command-/StateInterfaces`` not included in the ``ros2_control`` XML-tag you can follow those steps: + + #. Override the ``virtual std::vector export_command_interface_descriptions()`` or ``virtual std::vector export_state_interface_descriptions()`` + #. Create the InterfaceDescription for each of the interfaces you want to create in the override ``export_command_interface_descriptions()`` or ``export_state_interface_descriptions()`` function, add it to a vector and return the vector: + + .. code-block:: c++ + + std::vector my_unlisted_interfaces; + + InterfaceInfo unlisted_interface; + unlisted_interface.name = "some_unlisted_interface"; + unlisted_interface.min = "-5.0"; + unlisted_interface.data_type = "double"; + my_unlisted_interfaces.push_back(InterfaceDescription(info_.name, unlisted_interface)); + + return my_unlisted_interfaces; + + #. The unlisted interface will then be stored in either the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_`` map depending in which function they are created. + #. You can access it like any other interface with the ``get_state(name)``, ``set_state(name, value)``, ``get_command(name)`` or ``set_command(name, value)``. E.g. ``get_state("some_unlisted_interface")``. - 5. Implement ``on_cleanup`` method, which does the opposite of ``on_configure``. + #. (optional) In case the default implementation (``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` ) for exporting the ``Command-/StateInterfaces`` is not enough you can override them. You should however consider the following things: - 6. Implement ``export_state_interfaces`` and ``export_command_interfaces`` methods where interfaces that hardware offers are defined. - For the ``Sensor``-type hardware interface there is no ``export_command_interfaces`` method. - As a reminder, the full interface names have structure ``/``. + * If you want to have unlisted interfaces available you need to call the ``export_command_interface_descriptions()`` or ``export_state_interface_descriptions()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. + * Don't forget to store the created ``Command-/StateInterfaces`` internally as you only return shared_ptrs and the resource manager will not provide access to the created ``Command-/StateInterfaces`` for the hardware. So you must take care of storing them yourself. + * Names must be unique! - 7. (optional) For *Actuator* and *System* types of hardware interface implement ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` if your hardware accepts multiple control modes. + #. (optional) For *Actuator* and *System* types of hardware interface implement ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` if your hardware accepts multiple control modes. - 8. Implement the ``on_activate`` method where hardware "power" is enabled. + #. Implement the ``on_activate`` method where hardware "power" is enabled. - 9. Implement the ``on_deactivate`` method, which does the opposite of ``on_activate``. + #. Implement the ``on_deactivate`` method, which does the opposite of ``on_activate``. - 10. Implement ``on_shutdown`` method where hardware is shutdown gracefully. + #. Implement ``on_shutdown`` method where hardware is shutdown gracefully. - 11. Implement ``on_error`` method where different errors from all states are handled. + #. Implement ``on_error`` method where different errors from all states are handled. - 12. Implement the ``read`` method getting the states from the hardware and storing them to internal variables defined in ``export_state_interfaces``. + #. Implement the ``read`` method getting the states from the hardware and storing them to internal variables defined in ``export_state_interfaces``. - 13. Implement ``write`` method that commands the hardware based on the values stored in internal variables defined in ``export_command_interfaces``. + #. Implement ``write`` method that commands the hardware based on the values stored in internal variables defined in ``export_command_interfaces``. - 14. IMPORTANT: At the end of your file after the namespace is closed, add the ``PLUGINLIB_EXPORT_CLASS`` macro. + #. IMPORTANT: At the end of your file after the namespace is closed, add the ``PLUGINLIB_EXPORT_CLASS`` macro. For this you will need to include the ``"pluginlib/class_list_macros.hpp"`` header. As first parameters you should provide exact hardware interface class, e.g., ``::``, and as second the base class, i.e., ``hardware_interface::(Actuator|Sensor|System)Interface``. -5. **Writing export definition for pluginlib** +#. **Writing export definition for pluginlib** - 1. Create the ``.xml`` file in the package and add a definition of the library and hardware interface's class which has to be visible for the pluginlib. + #. Create the ``.xml`` file in the package and add a definition of the library and hardware interface's class which has to be visible for the pluginlib. The easiest way to do that is to check definition for mock components in the :ref:`hardware_interface mock_components ` section. - 2. Usually, the plugin name is defined by the package (namespace) and the class name, e.g., + #. Usually, the plugin name is defined by the package (namespace) and the class name, e.g., ``/``. This name defines the hardware interface's type when the resource manager searches for it. The other two parameters have to correspond to the definition done in the macro at the bottom of the ``.cpp`` file. -6. **Writing a simple test to check if the controller can be found and loaded** +#. **Writing a simple test to check if the controller can be found and loaded** - 1. Create the folder ``test`` in your package, if it does not exist already, and add a file named ``test_load_.cpp``. + #. Create the folder ``test`` in your package, if it does not exist already, and add a file named ``test_load_.cpp``. - 2. You can copy the ``load_generic_system_2dof`` content defined in the `test_generic_system.cpp `_ package. + #. You can copy the ``load_generic_system_2dof`` content defined in the `test_generic_system.cpp `_ package. - 3. Change the name of the copied test and in the last line, where hardware interface type is specified put the name defined in ``.xml`` file, e.g., ``/``. + #. Change the name of the copied test and in the last line, where hardware interface type is specified put the name defined in ``.xml`` file, e.g., ``/``. -7. **Add compile directives into ``CMakeLists.txt`` file** +#. **Add compile directives into ``CMakeLists.txt`` file** - 1. Under the line ``find_package(ament_cmake REQUIRED)`` add further dependencies. + #. Under the line ``find_package(ament_cmake REQUIRED)`` add further dependencies. Those are at least: ``hardware_interface``, ``pluginlib``, ``rclcpp`` and ``rclcpp_lifecycle``. - 2. Add a compile directive for a shared library providing the ``.cpp`` file as the source. + #. Add a compile directive for a shared library providing the ``.cpp`` file as the source. - 3. Add targeted include directories for the library. This is usually only ``include``. + #. Add targeted include directories for the library. This is usually only ``include``. - 4. Add ament dependencies needed by the library. You should add at least those listed under 1. + #. Add ament dependencies needed by the library. You should add at least those listed under 1. - 5. Export for pluginlib description file using the following command: + #. Export for pluginlib description file using the following command: .. code:: cmake pluginlib_export_plugin_description_file(hardware_interface .xml) - 6. Add install directives for targets and include directory. + #. Add install directives for targets and include directory. - 7. In the test section add the following dependencies: ``ament_cmake_gmock``, ``hardware_interface``. + #. In the test section add the following dependencies: ``ament_cmake_gmock``, ``hardware_interface``. - 8. Add compile definitions for the tests using the ``ament_add_gmock`` directive. + #. Add compile definitions for the tests using the ``ament_add_gmock`` directive. For details, see how it is done for mock hardware in the `ros2_control `_ package. - 9. (optional) Add your hardware interface`s library into ``ament_export_libraries`` before ``ament_package()``. + #. (optional) Add your hardware interface`s library into ``ament_export_libraries`` before ``ament_package()``. -8. **Add dependencies into ``package.xml`` file** +#. **Add dependencies into ``package.xml`` file** - 1. Add at least the following packages into ```` tag: ``hardware_interface``, ``pluginlib``, ``rclcpp``, and ``rclcpp_lifecycle``. + #. Add at least the following packages into ```` tag: ``hardware_interface``, ``pluginlib``, ``rclcpp``, and ``rclcpp_lifecycle``. - 2. Add at least the following package into ```` tag: ``ament_add_gmock`` and ``hardware_interface``. + #. Add at least the following package into ```` tag: ``ament_add_gmock`` and ``hardware_interface``. -9. **Compiling and testing the hardware component** +#. **Compiling and testing the hardware component** - 1. Now everything is ready to compile the hardware component using the ``colcon build `` command. + #. Now everything is ready to compile the hardware component using the ``colcon build `` command. Remember to go into the root of your workspace before executing this command. - 2. If compilation was successful, source the ``setup.bash`` file from the install folder and execute ``colcon test `` to check if the new controller can be found through ``pluginlib`` library and be loaded by the controller manager. + #. If compilation was successful, source the ``setup.bash`` file from the install folder and execute ``colcon test `` to check if the new controller can be found through ``pluginlib`` library and be loaded by the controller manager. That's it! Enjoy writing great controllers! diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index bb91b62057..e14c2fcc58 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -71,10 +71,10 @@ class Actuator final const rclcpp_lifecycle::State & error(); HARDWARE_INTERFACE_PUBLIC - std::vector export_state_interfaces(); + std::vector> export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC - std::vector export_command_interfaces(); + std::vector> export_command_interfaces(); HARDWARE_INTERFACE_PUBLIC return_type prepare_command_mode_switch( diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index a74d2a4964..9d2c8e032a 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -15,9 +15,14 @@ #ifndef HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_ #define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_ +#include +#include #include +#include +#include #include +#include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" @@ -115,33 +120,196 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. * \returns CallbackReturn::ERROR if any error happens or data are missing. */ - virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/) + virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { + info_ = hardware_info; + import_state_interface_descriptions(info_); + import_command_interface_descriptions(info_); return CallbackReturn::SUCCESS; }; + /** + * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo. + * Separate them into the possible types: Joint and store them. + */ + virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) + { + auto joint_state_interface_descriptions = + parse_state_interface_descriptions_from_hardware_info(hardware_info.joints); + for (const auto & description : joint_state_interface_descriptions) + { + joint_state_interfaces_.insert(std::make_pair(description.get_name(), description)); + } + } + + /** + * Import the InterfaceDescription for the CommandInterfaces from the HardwareInfo. + * Separate them into the possible types: Joint and store them. + */ + virtual void import_command_interface_descriptions(const HardwareInfo & hardware_info) + { + auto joint_command_interface_descriptions = + parse_command_interface_descriptions_from_hardware_info(hardware_info.joints); + for (const auto & description : joint_command_interface_descriptions) + { + joint_command_interfaces_.insert(std::make_pair(description.get_name(), description)); + } + } + /// Exports all state interfaces for this hardware interface. /** - * The state interfaces have to be created and transferred according - * to the hardware info passed in for the configuration. + * Old way of exporting the StateInterfaces. If a empty vector is returned then + * the on_export_state_interfaces() method is called. If a vector with StateInterfaces is returned + * then the exporting of the StateInterfaces is only done with this function and the ownership is + * transferred to the resource manager. The set_command(...), get_command(...), ..., can then not + * be used. * * Note the ownership over the state interfaces is transferred to the caller. * * \return vector of state interfaces */ - virtual std::vector export_state_interfaces() = 0; + [[deprecated( + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is " + "handled " + "by the Framework.")]] virtual std::vector + export_state_interfaces() + { + // return empty vector by default. For backward compatibility we check if all vectors is empty + // and if so call on_export_state_interfaces() + return {}; + } + + /** + * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_state_interfaces_ map. + * + * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated + * version is removed. + * + * \return vector of descriptions to the unlisted StateInterfaces + */ + virtual std::vector + export_state_interface_descriptions() + { + // return empty vector by default. + return {}; + } + + /** + * Default implementation for exporting the StateInterfaces. The StateInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the actuator_interface. + * + * \return vector of shared pointers to the created and stored StateInterfaces + */ + virtual std::vector> on_export_state_interfaces() + { + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_state_interface_descriptions(); + + std::vector> state_interfaces; + state_interfaces.reserve( + unlisted_interface_descriptions.size() + joint_state_interfaces_.size()); + + // add InterfaceDescriptions and create the StateInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_state_interfaces_.insert(std::make_pair(name, description)); + auto state_interface = std::make_shared(description); + actuator_states_.insert(std::make_pair(name, state_interface)); + state_interfaces.push_back(state_interface); + } + + for (const auto & [name, descr] : joint_state_interfaces_) + { + auto state_interface = std::make_shared(descr); + actuator_states_.insert(std::make_pair(name, state_interface)); + state_interfaces.push_back(state_interface); + } + return state_interfaces; + } /// Exports all command interfaces for this hardware interface. /** - * The command interfaces have to be created and transferred according - * to the hardware info passed in for the configuration. + * Old way of exporting the CommandInterfaces. If a empty vector is returned then + * the on_export_command_interfaces() method is called. If a vector with CommandInterfaces is + * returned then the exporting of the CommandInterfaces is only done with this function and the + * ownership is transferred to the resource manager. The set_command(...), get_command(...), ..., + * can then not be used. * * Note the ownership over the state interfaces is transferred to the caller. * - * \return vector of command interfaces + * \return vector of state interfaces + */ + [[deprecated( + "Replaced by vector> on_export_command_interfaces() method. " + "Exporting is " + "handled " + "by the Framework.")]] virtual std::vector + export_command_interfaces() + { + // return empty vector by default. For backward compatibility we check if all vectors is empty + // and if so call on_export_command_interfaces() + return {}; + } + + /** + * Override this method to export custom CommandInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_command_interfaces_ map. + * + * Note method name is going to be changed to export_command_interfaces() as soon as the + * deprecated version is removed. + * + * \return vector of descriptions to the unlisted CommandInterfaces + */ + virtual std::vector + export_command_interface_descriptions() + { + // return empty vector by default. + return {}; + } + + /** + * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the actuator_interface. + * + * \return vector of shared pointers to the created and stored CommandInterfaces */ - virtual std::vector export_command_interfaces() = 0; + virtual std::vector> on_export_command_interfaces() + { + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_command_interface_descriptions(); + + std::vector> command_interfaces; + command_interfaces.reserve( + unlisted_interface_descriptions.size() + joint_command_interfaces_.size()); + + // add InterfaceDescriptions and create the CommandInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_command_interfaces_.insert(std::make_pair(name, description)); + auto command_interface = std::make_shared(description); + actuator_commands_.insert(std::make_pair(name, command_interface)); + command_interfaces.push_back(command_interface); + } + + for (const auto & [name, descr] : joint_command_interfaces_) + { + auto command_interface = std::make_shared(descr); + actuator_commands_.insert(std::make_pair(name, command_interface)); + command_interfaces.push_back(command_interface); + } + return command_interfaces; + } /// Prepare for a new command interface switch. /** * Prepare for any mode-switching required by the new command interface combination. @@ -228,6 +396,26 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + void set_state(const std::string & interface_name, const double & value) + { + actuator_states_.at(interface_name)->set_value(value); + } + + double get_state(const std::string & interface_name) const + { + return actuator_states_.at(interface_name)->get_value(); + } + + void set_command(const std::string & interface_name, const double & value) + { + actuator_commands_.at(interface_name)->set_value(value); + } + + double get_command(const std::string & interface_name) const + { + return actuator_commands_.at(interface_name)->get_value(); + } + /// Get the logger of the ActuatorInterface. /** * \return logger of the ActuatorInterface. @@ -248,11 +436,19 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod protected: HardwareInfo info_; + std::unordered_map joint_state_interfaces_; + std::unordered_map joint_command_interfaces_; + + std::unordered_map unlisted_state_interfaces_; + std::unordered_map unlisted_command_interfaces_; + rclcpp_lifecycle::State lifecycle_state_; private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; rclcpp::Logger actuator_logger_; + std::unordered_map> actuator_states_; + std::unordered_map> actuator_commands_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 38ca0cf89d..b9ca460800 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -32,5 +32,23 @@ namespace hardware_interface HARDWARE_INTERFACE_PUBLIC std::vector parse_control_resources_from_urdf(const std::string & urdf); +/** + * \param[in] component_info information about a component (gpio, joint, sensor) + * \return vector filled with information about hardware's StateInterfaces for the component + * which are exported + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_state_interface_descriptions_from_hardware_info( + const std::vector & component_info); + +/** + * \param[in] component_info information about a component (gpio, joint, sensor) + * \return vector filled with information about hardware's CommandInterfaces for the component + * which are exported + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_command_interface_descriptions_from_hardware_info( + const std::vector & component_info); + } // namespace hardware_interface #endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index bfe0ff1eb8..72b1d6cf92 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,20 +15,27 @@ #ifndef HARDWARE_INTERFACE__HANDLE_HPP_ #define HARDWARE_INTERFACE__HANDLE_HPP_ +#include +#include #include +#include #include +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/macros.hpp" +#include "hardware_interface/visibility_control.h" namespace hardware_interface { -using HANDLE_DATATYPE = std::variant; +typedef std::variant HANDLE_DATATYPE; /// A handle used to get and set a value on a given interface. 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) @@ -36,11 +43,25 @@ class Handle { } + explicit Handle(const InterfaceDescription & interface_description) + : prefix_name_(interface_description.prefix_name), + interface_name_(interface_description.interface_info.name) + { + // As soon as multiple datatypes are used in HANDLE_DATATYPE + // we need to initialize according the type passed in interface description + value_ = std::numeric_limits::quiet_NaN(); + value_ptr_ = std::get_if(&value_); + } + + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + explicit Handle(const std::string & interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + explicit Handle(const char * interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { @@ -103,10 +124,8 @@ class Handle class StateInterface : public Handle { public: - explicit StateInterface( - const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : Handle(prefix_name, interface_name, value_ptr) + explicit StateInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) { } @@ -120,10 +139,8 @@ class StateInterface : public Handle class CommandInterface : public Handle { public: - explicit CommandInterface( - const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : Handle(prefix_name, interface_name, value_ptr) + explicit CommandInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) { } /// CommandInterface copy constructor is actively deleted. diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 2bd2099e69..bf14f24041 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -40,9 +40,9 @@ struct InterfaceInfo std::string max; /// (Optional) Initial value of the interface. std::string initial_value; - /// (Optional) The datatype of the interface, e.g. "bool", "int". Used by GPIOs. + /// (Optional) The datatype of the interface, e.g. "bool", "int". std::string data_type; - /// (Optional) If the handle is an array, the size of the array. Used by GPIOs. + /// (Optional) If the handle is an array, the size of the array. int size; /// (Optional) enable or disable the limits for the command interfaces bool enable_limits; @@ -126,6 +126,32 @@ struct TransmissionInfo std::unordered_map parameters; }; +/** + * This structure stores information about an interface for a specific hardware which should be + * instantiated internally. + */ +struct InterfaceDescription +{ + InterfaceDescription(const std::string & prefix_name_in, const InterfaceInfo & interface_info_in) + : prefix_name(prefix_name_in), interface_info(interface_info_in) + { + } + + /** + * Name of the interface defined by the user. + */ + std::string prefix_name; + + /** + * Information about the Interface type (position, velocity,...) as well as limits and so on. + */ + InterfaceInfo interface_info; + + std::string get_name() const { return prefix_name + "/" + interface_info.name; } + + std::string get_interface_type() const { return interface_info.name; } +}; + /// This structure stores information about hardware defined in a robot's URDF. struct HardwareInfo { diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 065ecabed9..3f886f7ef4 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -141,7 +141,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] interfaces list of controller's state interfaces as StateInterfaces. */ void import_controller_exported_state_interfaces( - const std::string & controller_name, std::vector & interfaces); + const std::string & controller_name, std::vector> & interfaces); /// Get list of exported tate interface of a controller. /** @@ -194,7 +194,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] interfaces list of controller's reference interfaces as CommandInterfaces. */ void import_controller_reference_interfaces( - const std::string & controller_name, std::vector & interfaces); + const std::string & controller_name, + const std::vector> & interfaces); /// Get list of reference interface of a controller. /** diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 98197bd0e9..fbc2f3b50d 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -71,7 +71,7 @@ class Sensor final const rclcpp_lifecycle::State & error(); HARDWARE_INTERFACE_PUBLIC - std::vector export_state_interfaces(); + std::vector> export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC std::string get_name() const; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 0c7f43e7fc..f7bc061d20 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -15,9 +15,14 @@ #ifndef HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_ +#include +#include #include +#include +#include #include +#include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" @@ -115,21 +120,105 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. * \returns CallbackReturn::ERROR if any error happens or data are missing. */ - virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/) + virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { + info_ = hardware_info; + import_state_interface_descriptions(info_); return CallbackReturn::SUCCESS; }; + /** + * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo. + * Separate them into the possible types: Sensor and store them. + */ + virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) + { + auto sensor_state_interface_descriptions = + parse_state_interface_descriptions_from_hardware_info(hardware_info.sensors); + for (const auto & description : sensor_state_interface_descriptions) + { + sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description)); + } + } + /// Exports all state interfaces for this hardware interface. /** - * The state interfaces have to be created and transferred according - * to the hardware info passed in for the configuration. + * Old way of exporting the StateInterfaces. If a empty vector is returned then + * the on_export_state_interfaces() method is called. If a vector with StateInterfaces is returned + * then the exporting of the StateInterfaces is only done with this function and the ownership is + * transferred to the resource manager. The set_command(...), get_command(...), ..., can then not + * be used. * * Note the ownership over the state interfaces is transferred to the caller. * * \return vector of state interfaces */ - virtual std::vector export_state_interfaces() = 0; + [[deprecated( + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is handled " + "by the Framework.")]] virtual std::vector + export_state_interfaces() + { + // return empty vector by default. For backward compatibility we check if all vectors is empty + // and if so call on_export_state_interfaces() + return {}; + } + + /** + * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_state_interfaces_ map. + * + * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated + * version is removed. + * + * \return vector of descriptions to the unlisted StateInterfaces + */ + virtual std::vector + export_state_interface_descriptions() + { + // return empty vector by default. + return {}; + } + + /** + * Default implementation for exporting the StateInterfaces. The StateInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the sensor_interface. + * + * \return vector of shared pointers to the created and stored StateInterfaces + */ + virtual std::vector> on_export_state_interfaces() + { + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_state_interface_descriptions(); + + std::vector> state_interfaces; + state_interfaces.reserve( + unlisted_interface_descriptions.size() + sensor_state_interfaces_.size()); + + // add InterfaceDescriptions and create the StateInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_state_interfaces_.insert(std::make_pair(name, description)); + auto state_interface = std::make_shared(description); + sensor_states_.insert(std::make_pair(name, state_interface)); + state_interfaces.push_back(state_interface); + } + + for (const auto & [name, descr] : sensor_state_interfaces_) + { + // TODO(Manuel) maybe check for duplicates otherwise only the first appearance of "name" is + // inserted + auto state_interface = std::make_shared(descr); + sensor_states_.insert(std::make_pair(name, state_interface)); + state_interfaces.push_back(state_interface); + } + + return state_interfaces; + } /// Read the current state values from the actuator. /** @@ -167,6 +256,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + void set_state(const std::string & interface_name, const double & value) + { + sensor_states_.at(interface_name)->set_value(value); + } + + double get_state(const std::string & interface_name) const + { + return sensor_states_.at(interface_name)->get_value(); + } + /// Get the logger of the SensorInterface. /** * \return logger of the SensorInterface. @@ -187,11 +286,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI protected: HardwareInfo info_; + + std::unordered_map sensor_state_interfaces_; + std::unordered_map unlisted_state_interfaces_; + rclcpp_lifecycle::State lifecycle_state_; private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; rclcpp::Logger sensor_logger_; + std::unordered_map> sensor_states_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 1cc29a085d..886cfc584d 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -71,10 +71,10 @@ class System final const rclcpp_lifecycle::State & error(); HARDWARE_INTERFACE_PUBLIC - std::vector export_state_interfaces(); + std::vector> export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC - std::vector export_command_interfaces(); + std::vector> export_command_interfaces(); HARDWARE_INTERFACE_PUBLIC return_type prepare_command_mode_switch( diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 54f1af33a7..7830254e9c 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -15,12 +15,18 @@ #ifndef HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ +#include +#include #include +#include +#include #include +#include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" @@ -117,32 +123,233 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. * \returns CallbackReturn::ERROR if any error happens or data are missing. */ - virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/) + virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { + info_ = hardware_info; + import_state_interface_descriptions(info_); + import_command_interface_descriptions(info_); return CallbackReturn::SUCCESS; }; + /** + * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo. + * Separate them into the possible types: Joint, GPIO, Sensor and store them. + */ + void import_state_interface_descriptions(const HardwareInfo & hardware_info) + { + auto joint_state_interface_descriptions = + parse_state_interface_descriptions_from_hardware_info(hardware_info.joints); + for (const auto & description : joint_state_interface_descriptions) + { + joint_state_interfaces_.insert(std::make_pair(description.get_name(), description)); + } + auto sensor_state_interface_descriptions = + parse_state_interface_descriptions_from_hardware_info(hardware_info.sensors); + for (const auto & description : sensor_state_interface_descriptions) + { + sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description)); + } + auto gpio_state_interface_descriptions = + parse_state_interface_descriptions_from_hardware_info(hardware_info.gpios); + for (const auto & description : gpio_state_interface_descriptions) + { + gpio_state_interfaces_.insert(std::make_pair(description.get_name(), description)); + } + } + + /** + * Import the InterfaceDescription for the CommandInterfaces from the HardwareInfo. + * Separate them into the possible types: Joint and GPIO and store them. + */ + void import_command_interface_descriptions(const HardwareInfo & hardware_info) + { + auto joint_command_interface_descriptions = + parse_command_interface_descriptions_from_hardware_info(hardware_info.joints); + for (const auto & description : joint_command_interface_descriptions) + { + joint_command_interfaces_.insert(std::make_pair(description.get_name(), description)); + } + auto gpio_command_interface_descriptions = + parse_command_interface_descriptions_from_hardware_info(hardware_info.gpios); + for (const auto & description : gpio_command_interface_descriptions) + { + gpio_command_interfaces_.insert(std::make_pair(description.get_name(), description)); + } + } + /// Exports all state interfaces for this hardware interface. /** - * The state interfaces have to be created and transferred according - * to the hardware info passed in for the configuration. + * Old way of exporting the StateInterfaces. If a empty vector is returned then + * the on_export_state_interfaces() method is called. If a vector with StateInterfaces is returned + * then the exporting of the StateInterfaces is only done with this function and the ownership is + * transferred to the resource manager. The set_command(...), get_command(...), ..., can then not + * be used. * * Note the ownership over the state interfaces is transferred to the caller. * * \return vector of state interfaces */ - virtual std::vector export_state_interfaces() = 0; + [[deprecated( + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is handled " + "by the Framework.")]] virtual std::vector + export_state_interfaces() + { + // return empty vector by default. For backward compatibility we check if all vectors is empty + // and if so call on_export_state_interfaces() + return {}; + } + + /** + * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_state_interfaces_ map. + * + * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated + * version is removed. + * + * \return vector of descriptions to the unlisted StateInterfaces + */ + virtual std::vector + export_state_interface_descriptions() + { + // return empty vector by default. + return {}; + } + + /** + * Default implementation for exporting the StateInterfaces. The StateInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the system_interface. + * + * \return vector of shared pointers to the created and stored StateInterfaces + */ + std::vector> on_export_state_interfaces() + { + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_state_interface_descriptions(); + + std::vector> state_interfaces; + state_interfaces.reserve( + unlisted_interface_descriptions.size() + joint_state_interfaces_.size() + + sensor_state_interfaces_.size() + gpio_state_interfaces_.size()); + + // add InterfaceDescriptions and create the StateInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_state_interfaces_.insert(std::make_pair(name, description)); + auto state_interface = std::make_shared(description); + system_states_.insert(std::make_pair(name, state_interface)); + state_interfaces.push_back(state_interface); + } + + for (const auto & [name, descr] : joint_state_interfaces_) + { + auto state_interface = std::make_shared(descr); + system_states_.insert(std::make_pair(name, state_interface)); + state_interfaces.push_back(state_interface); + } + for (const auto & [name, descr] : sensor_state_interfaces_) + { + auto state_interface = std::make_shared(descr); + system_states_.insert(std::make_pair(name, state_interface)); + state_interfaces.push_back(state_interface); + } + for (const auto & [name, descr] : gpio_state_interfaces_) + { + auto state_interface = std::make_shared(descr); + system_states_.insert(std::make_pair(name, state_interface)); + state_interfaces.push_back(state_interface); + } + return state_interfaces; + } /// Exports all command interfaces for this hardware interface. /** - * The command interfaces have to be created and transferred according - * to the hardware info passed in for the configuration. + * Old way of exporting the CommandInterfaces. If a empty vector is returned then + * the on_export_command_interfaces() method is called. If a vector with CommandInterfaces is + * returned then the exporting of the CommandInterfaces is only done with this function and the + * ownership is transferred to the resource manager. The set_command(...), get_command(...), ..., + * can then not be used. * * Note the ownership over the state interfaces is transferred to the caller. * - * \return vector of command interfaces + * \return vector of state interfaces */ - virtual std::vector export_command_interfaces() = 0; + [[deprecated( + "Replaced by vector> on_export_command_interfaces() method. " + "Exporting is " + "handled " + "by the Framework.")]] virtual std::vector + export_command_interfaces() + { + // return empty vector by default. For backward compatibility we check if all vectors is empty + // and if so call on_export_command_interfaces() + return {}; + } + + /** + * Override this method to export custom CommandInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_command_interfaces_ map. + * + * Note method name is going to be changed to export_command_interfaces() as soon as the + * deprecated version is removed. + * + * \return vector of descriptions to the unlisted CommandInterfaces + */ + virtual std::vector + export_command_interface_descriptions() + { + // return empty vector by default. + return {}; + } + + /** + * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the system_interface. + * + * \return vector of shared pointers to the created and stored CommandInterfaces + */ + std::vector> on_export_command_interfaces() + { + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_command_interface_descriptions(); + + std::vector> command_interfaces; + command_interfaces.reserve( + unlisted_interface_descriptions.size() + joint_command_interfaces_.size() + + gpio_command_interfaces_.size()); + + // add InterfaceDescriptions and create the CommandInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_command_interfaces_.insert(std::make_pair(name, description)); + auto command_interface = std::make_shared(description); + system_commands_.insert(std::make_pair(name, command_interface)); + command_interfaces.push_back(command_interface); + } + + for (const auto & [name, descr] : joint_command_interfaces_) + { + auto command_interface = std::make_shared(descr); + system_commands_.insert(std::make_pair(name, command_interface)); + command_interfaces.push_back(command_interface); + } + + for (const auto & [name, descr] : gpio_command_interfaces_) + { + auto command_interface = std::make_shared(descr); + system_commands_.insert(std::make_pair(name, command_interface)); + command_interfaces.push_back(command_interface); + } + return command_interfaces; + } /// Prepare for a new command interface switch. /** @@ -230,6 +437,26 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + void set_state(const std::string & interface_name, const double & value) + { + system_states_.at(interface_name)->set_value(value); + } + + double get_state(const std::string & interface_name) const + { + return system_states_.at(interface_name)->get_value(); + } + + void set_command(const std::string & interface_name, const double & value) + { + system_commands_.at(interface_name)->set_value(value); + } + + double get_command(const std::string & interface_name) const + { + return system_commands_.at(interface_name)->get_value(); + } + /// Get the logger of the SystemInterface. /** * \return logger of the SystemInterface. @@ -250,11 +477,24 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI protected: HardwareInfo info_; + std::unordered_map joint_state_interfaces_; + std::unordered_map joint_command_interfaces_; + + std::unordered_map sensor_state_interfaces_; + + std::unordered_map gpio_state_interfaces_; + std::unordered_map gpio_command_interfaces_; + + std::unordered_map unlisted_state_interfaces_; + std::unordered_map unlisted_command_interfaces_; + rclcpp_lifecycle::State lifecycle_state_; private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; rclcpp::Logger system_logger_; + std::unordered_map> system_states_; + std::unordered_map> system_commands_; }; } // namespace hardware_interface diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 52cfc681a1..38512bd455 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -201,16 +201,61 @@ const rclcpp_lifecycle::State & Actuator::error() return impl_->get_state(); } -std::vector Actuator::export_state_interfaces() +std::vector> Actuator::export_state_interfaces() { // TODO(karsten1987): Might be worth to do some brief sanity check here - return impl_->export_state_interfaces(); + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interfaces = impl_->export_state_interfaces(); + // END: for backward compatibility + + // If no StateInterfaces has been exported, this could mean: + // a) there is nothing to export -> on_export_state_interfaces() does return nothing as well + // b) default implementation for export_state_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.empty()) + { + return impl_->on_export_state_interfaces(); + } + + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector> interface_ptrs; + interface_ptrs.reserve(interfaces.size()); + for (auto const & interface : interfaces) + { + interface_ptrs.emplace_back(std::make_shared(interface)); + } + return interface_ptrs; + // END: for backward compatibility } -std::vector Actuator::export_command_interfaces() +std::vector> Actuator::export_command_interfaces() { // TODO(karsten1987): Might be worth to do some brief sanity check here - return impl_->export_command_interfaces(); + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interfaces = impl_->export_command_interfaces(); + // END: for backward compatibility + + // If no CommandInterface has been exported, this could mean: + // a) there is nothing to export -> on_export_command_interfaces() does return nothing as well + // b) default implementation for export_command_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.empty()) + { + return impl_->on_export_command_interfaces(); + } + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector> interface_ptrs; + interface_ptrs.reserve(interfaces.size()); + for (auto & interface : interfaces) + { + interface_ptrs.emplace_back(std::make_shared(std::move(interface))); + } + return interface_ptrs; + // END: for backward compatibility } return_type Actuator::prepare_command_mode_switch( diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 42432dda8d..9dac3d9f66 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -905,4 +905,38 @@ std::vector parse_control_resources_from_urdf(const std::string & return hardware_info; } +std::vector parse_state_interface_descriptions_from_hardware_info( + const std::vector & component_info) +{ + std::vector component_state_interface_descriptions; + component_state_interface_descriptions.reserve(component_info.size()); + + for (const auto & component : component_info) + { + for (const auto & state_interface : component.state_interfaces) + { + component_state_interface_descriptions.emplace_back( + InterfaceDescription(component.name, state_interface)); + } + } + return component_state_interface_descriptions; +} + +std::vector parse_command_interface_descriptions_from_hardware_info( + const std::vector & component_info) +{ + std::vector component_command_interface_descriptions; + component_command_interface_descriptions.reserve(component_info.size()); + + for (const auto & component : component_info) + { + for (const auto & command_interface : component.command_interfaces) + { + component_command_interface_descriptions.emplace_back( + InterfaceDescription(component.name, command_interface)); + } + } + return component_command_interface_descriptions; +} + } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 53fcad94d2..e512f2c1dc 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -606,45 +606,72 @@ class ResourceStorage template void import_state_interfaces(HardwareT & hardware) { - try + std::vector interface_names; + std::vector> interfaces = hardware.export_state_interfaces(); + + interface_names.reserve(interfaces.size()); + for (auto const & interface : interfaces) { - auto interfaces = hardware.export_state_interfaces(); - std::vector interface_names; - interface_names.reserve(interfaces.size()); - for (auto & interface : interfaces) + try { - auto key = interface.get_name(); - state_interface_map_.emplace(std::make_pair(key, std::move(interface))); - interface_names.push_back(key); + interface_names.push_back(add_state_interface(interface)); + } + // We don't want to crash during runtime because a StateInterface could not be added + catch (const std::exception & e) + { + RCLCPP_WARN( + get_logger(), + "Exception occurred while importing state interfaces for the hardware '%s' : %s", + hardware.get_name().c_str(), e.what()); } - hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; - available_state_interfaces_.reserve( - available_state_interfaces_.capacity() + interface_names.size()); } - catch (const std::exception & e) + + hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; + available_state_interfaces_.reserve( + available_state_interfaces_.capacity() + interface_names.size()); + } + + void insert_command_interface(const std::shared_ptr command_interface) + { + const auto [it, success] = command_interface_map_.insert( + std::make_pair(command_interface->get_name(), command_interface)); + if (!success) { - RCLCPP_ERROR( - get_logger(), - "Exception occurred while importing state interfaces for the hardware '%s' : %s", - hardware.get_name().c_str(), e.what()); + std::string msg( + "ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" + + command_interface->get_name() + "]"); + throw std::runtime_error(msg); } - catch (...) + } + + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + void insert_command_interface(CommandInterface && command_interface) + { + std::string key = command_interface.get_name(); + const auto [it, success] = command_interface_map_.emplace( + std::make_pair(key, std::make_shared(std::move(command_interface)))); + if (!success) { - RCLCPP_ERROR( - get_logger(), - "Unknown exception occurred while importing state interfaces for the hardware '%s'", - hardware.get_name().c_str()); + std::string msg( + "ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" + + key + "]"); + throw std::runtime_error(msg); } } + // END: for backward compatibility template void import_command_interfaces(HardwareT & hardware) { try { - auto interfaces = hardware.export_command_interfaces(); + std::vector> interfaces = + hardware.export_command_interfaces(); + hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); + // TODO(Manuel) END: for backward compatibility } catch (const std::exception & ex) { @@ -662,6 +689,19 @@ class ResourceStorage } } + std::string add_state_interface(std::shared_ptr interface) + { + auto interface_name = interface->get_name(); + const auto [it, success] = state_interface_map_.emplace(interface_name, interface); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" + + interface->get_name() + "]"); + throw std::runtime_error(msg); + } + return interface_name; + } /// Adds exported state interfaces into internal storage. /** * Adds state interfaces to the internal storage. State interfaces exported from hardware or @@ -673,15 +713,23 @@ class ResourceStorage * \returns list of interface names that are added into internal storage. The output is used to * avoid additional iterations to cache interface names, e.g., for initializing info structures. */ - std::vector add_state_interfaces(std::vector & interfaces) + std::vector add_state_interfaces( + std::vector> & interfaces) { std::vector interface_names; interface_names.reserve(interfaces.size()); for (auto & interface : interfaces) { - auto key = interface.get_name(); - state_interface_map_.emplace(std::make_pair(key, std::move(interface))); - interface_names.push_back(key); + try + { + interface_names.push_back(add_state_interface(interface)); + } + // We don't want to crash during runtime because a StateInterface could not be added + catch (const std::exception & e) + { + RCLCPP_WARN( + get_logger(), "Exception occurred while importing state interfaces: %s", e.what()); + } } available_state_interfaces_.reserve( available_state_interfaces_.capacity() + interface_names.size()); @@ -721,7 +769,25 @@ class ResourceStorage for (auto & interface : interfaces) { auto key = interface.get_name(); - command_interface_map_.emplace(std::make_pair(key, std::move(interface))); + insert_command_interface(std::move(interface)); + claimed_command_interface_map_.emplace(std::make_pair(key, false)); + interface_names.push_back(key); + } + available_command_interfaces_.reserve( + available_command_interfaces_.capacity() + interface_names.size()); + + return interface_names; + } + + std::vector add_command_interfaces( + const std::vector> & interfaces) + { + std::vector interface_names; + interface_names.reserve(interfaces.size()); + for (const auto & interface : interfaces) + { + auto key = interface->get_name(); + insert_command_interface(interface); claimed_command_interface_map_.emplace(std::make_pair(key, false)); interface_names.push_back(key); } @@ -1014,9 +1080,9 @@ class ResourceStorage std::unordered_map> controllers_reference_interfaces_map_; /// Storage of all available state interfaces - std::map state_interface_map_; + std::map> state_interface_map_; /// Storage of all available command interfaces - std::map command_interface_map_; + std::map> command_interface_map_; /// Vectors with interfaces available to controllers (depending on hardware component state) std::vector available_state_interfaces_; @@ -1153,7 +1219,7 @@ LoanedStateInterface ResourceManager::claim_state_interface(const std::string & } std::lock_guard guard(resource_interfaces_lock_); - return LoanedStateInterface(resource_storage_->state_interface_map_.at(key)); + return LoanedStateInterface(*(resource_storage_->state_interface_map_.at(key))); } // CM API: Called in "callback/slow"-thread @@ -1187,7 +1253,7 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_exported_state_interfaces( - const std::string & controller_name, std::vector & interfaces) + const std::string & controller_name, std::vector> & interfaces) { std::lock_guard guard(resource_interfaces_lock_); auto interface_names = resource_storage_->add_state_interfaces(interfaces); @@ -1249,7 +1315,8 @@ void ResourceManager::remove_controller_exported_state_interfaces( // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_reference_interfaces( - const std::string & controller_name, std::vector & interfaces) + const std::string & controller_name, + const std::vector> & interfaces) { std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); auto interface_names = resource_storage_->add_command_interfaces(interfaces); @@ -1385,7 +1452,7 @@ LoanedCommandInterface ResourceManager::claim_command_interface(const std::strin resource_storage_->claimed_command_interface_map_[key] = true; std::lock_guard guard(resource_interfaces_lock_); return LoanedCommandInterface( - resource_storage_->command_interface_map_.at(key), + *(resource_storage_->command_interface_map_.at(key)), std::bind(&ResourceManager::release_command_interface, this, key)); } diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 9f1d4b73e1..39a7fa6c5d 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -200,9 +200,33 @@ const rclcpp_lifecycle::State & Sensor::error() return impl_->get_state(); } -std::vector Sensor::export_state_interfaces() +std::vector> Sensor::export_state_interfaces() { - return impl_->export_state_interfaces(); + // TODO(karsten1987): Might be worth to do some brief sanity check here + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interfaces = impl_->export_state_interfaces(); + // END: for backward compatibility + + // If no StateInterfaces has been exported, this could mean: + // a) there is nothing to export -> on_export_state_interfaces() does return nothing as well + // b) default implementation for export_state_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.empty()) + { + return impl_->on_export_state_interfaces(); + } + + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector> interface_ptrs; + interface_ptrs.reserve(interfaces.size()); + for (auto const & interface : interfaces) + { + interface_ptrs.emplace_back(std::make_shared(interface)); + } + return interface_ptrs; + // END: for backward compatibility } std::string Sensor::get_name() const { return impl_->get_name(); } diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 82473563b6..5e918b8855 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -199,14 +199,61 @@ const rclcpp_lifecycle::State & System::error() return impl_->get_state(); } -std::vector System::export_state_interfaces() +std::vector> System::export_state_interfaces() { - return impl_->export_state_interfaces(); + // TODO(karsten1987): Might be worth to do some brief sanity check here + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interfaces = impl_->export_state_interfaces(); + // END: for backward compatibility + + // If no StateInterfaces has been exported, this could mean: + // a) there is nothing to export -> on_export_state_interfaces() does return nothing as well + // b) default implementation for export_state_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.empty()) + { + return impl_->on_export_state_interfaces(); + } + + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector> interface_ptrs; + interface_ptrs.reserve(interfaces.size()); + for (auto const & interface : interfaces) + { + interface_ptrs.emplace_back(std::make_shared(interface)); + } + return interface_ptrs; + // END: for backward compatibility } -std::vector System::export_command_interfaces() +std::vector> System::export_command_interfaces() { - return impl_->export_command_interfaces(); + // TODO(karsten1987): Might be worth to do some brief sanity check here + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interfaces = impl_->export_command_interfaces(); + // END: for backward compatibility + + // If no CommandInterface has been exported, this could mean: + // a) there is nothing to export -> on_export_command_interfaces() does return nothing as well + // b) default implementation for export_command_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.empty()) + { + return impl_->on_export_command_interfaces(); + } + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector> interface_ptrs; + interface_ptrs.reserve(interfaces.size()); + for (auto & interface : interfaces) + { + interface_ptrs.emplace_back(std::make_shared(std::move(interface))); + } + return interface_ptrs; + // END: for backward compatibility } return_type System::prepare_command_mode_switch( diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 15d24fc7e7..854b35c8d3 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -515,7 +515,6 @@ class TestGenericSystem : public ::testing::Test 2.78 - @@ -546,7 +545,6 @@ class TestGenericSystem : public ::testing::Test 2.78 - @@ -555,7 +553,6 @@ class TestGenericSystem : public ::testing::Test 2.78 - diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 986c32d37b..e8fcc5d41d 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -18,6 +18,8 @@ #include #include #include +#include +#include #include #include "hardware_interface/actuator.hpp" @@ -32,6 +34,9 @@ #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/components_urdfs.hpp" +#include "ros2_control_test_assets/descriptions.hpp" +#include "test_components.hpp" // Values to send over command interface to trigger error in write and read methods @@ -48,6 +53,7 @@ namespace test_components { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +// BEGIN (Handle export change): for backward compatibility class DummyActuator : public hardware_interface::ActuatorInterface { CallbackReturn on_init(const hardware_interface::HardwareInfo & /*info*/) override @@ -154,7 +160,96 @@ class DummyActuator : public hardware_interface::ActuatorInterface unsigned int write_calls_ = 0; bool recoverable_error_happened_ = false; }; +// END +class DummyActuatorDefault : public hardware_interface::ActuatorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + // We hardcode the info + if ( + hardware_interface::ActuatorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + set_state("joint1/position", 0.0); + set_state("joint1/velocity", 0.0); + + if (recoverable_error_happened_) + { + set_command("joint1/velocity", 0.0); + } + + read_calls_ = 0; + write_calls_ = 0; + + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummyActuatorDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + + // no-op, state is getting propagated within write. + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++write_calls_; + if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + auto position_state = get_state("joint1/position"); + set_state("joint1/position", position_state + get_command("joint1/velocity")); + set_state("joint1/velocity", get_command("joint1/velocity")); + + return hardware_interface::return_type::OK; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override + { + set_state("joint1/velocity", 0.0); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + // Helper variables to initiate error on read + unsigned int read_calls_ = 0; + unsigned int write_calls_ = 0; + bool recoverable_error_happened_ = false; +}; + +// BEGIN (Handle export change): for backward compatibility class DummySensor : public hardware_interface::SensorInterface { CallbackReturn on_init(const hardware_interface::HardwareInfo & /*info*/) override @@ -218,7 +313,72 @@ class DummySensor : public hardware_interface::SensorInterface int read_calls_ = 0; bool recoverable_error_happened_ = false; }; +// END + +class DummySensorDefault : public hardware_interface::SensorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + if ( + hardware_interface::SensorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // We hardcode the info + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (const auto & [name, descr] : sensor_state_interfaces_) + { + set_state(name, 0.0); + } + read_calls_ = 0; + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummySensorDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + + // no-op, static value + set_state("joint1/voltage", voltage_level_hw_value_); + return hardware_interface::return_type::OK; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + double voltage_level_hw_value_ = 0x666; + + // Helper variables to initiate error on read + int read_calls_ = 0; + bool recoverable_error_happened_ = false; +}; +// BEGIN (Handle export change): for backward compatibility class DummySystem : public hardware_interface::SystemInterface { CallbackReturn on_init(const hardware_interface::HardwareInfo & /* info */) override @@ -352,21 +512,121 @@ class DummySystem : public hardware_interface::SystemInterface unsigned int write_calls_ = 0; bool recoverable_error_happened_ = false; }; +// END -class DummySystemPreparePerform : public hardware_interface::SystemInterface +class DummySystemDefault : public hardware_interface::SystemInterface { - // Override the pure virtual functions with default behavior - CallbackReturn on_init(const hardware_interface::HardwareInfo & /* info */) override + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override { + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } // We hardcode the info return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override { return {}; } + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (auto i = 0ul; i < 3; ++i) + { + set_state(position_states_[i], 0.0); + set_state(velocity_states_[i], 0.0); + } + // reset command only if error is initiated + if (recoverable_error_happened_) + { + for (auto i = 0ul; i < 3; ++i) + { + set_command(velocity_commands_[i], 0.0); + } + } - std::vector export_command_interfaces() override + read_calls_ = 0; + write_calls_ = 0; + + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummySystemDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + + // no-op, state is getting propagated within write. + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++write_calls_; + if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + + for (auto i = 0; i < 3; ++i) + { + auto current_pos = get_state(position_states_[i]); + set_state(position_states_[i], current_pos + get_command(velocity_commands_[i])); + set_state(velocity_states_[i], get_command(velocity_commands_[i])); + } + return hardware_interface::return_type::OK; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (const auto & velocity_state : velocity_states_) + { + set_state(velocity_state, 0.0); + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + std::vector position_states_ = { + "joint1/position", "joint2/position", "joint3/position"}; + std::vector velocity_states_ = { + "joint1/velocity", "joint2/velocity", "joint3/velocity"}; + std::vector velocity_commands_ = { + "joint1/velocity", "joint2/velocity", "joint3/velocity"}; + + // Helper variables to initiate error on read + unsigned int read_calls_ = 0; + unsigned int write_calls_ = 0; + bool recoverable_error_happened_ = false; +}; + +class DummySystemPreparePerform : public hardware_interface::SystemInterface +{ + // Override the pure virtual functions with default behavior + CallbackReturn on_init(const hardware_interface::HardwareInfo & /* info */) override { - return {}; + // We hardcode the info + return CallbackReturn::SUCCESS; } std::string get_name() const override { return "DummySystemPreparePerform"; } @@ -419,6 +679,7 @@ class DummySystemPreparePerform : public hardware_interface::SystemInterface } // namespace test_components +// BEGIN (Handle export change): for backward compatibility TEST(TestComponentInterfaces, dummy_actuator) { hardware_interface::Actuator actuator_hw(std::make_unique()); @@ -431,21 +692,21 @@ TEST(TestComponentInterfaces, dummy_actuator) auto state_interfaces = actuator_hw.export_state_interfaces(); ASSERT_EQ(2u, state_interfaces.size()); - EXPECT_EQ("joint1/position", state_interfaces[0].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0].get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()); - EXPECT_EQ("joint1/velocity", state_interfaces[1].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1].get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1].get_prefix_name()); + EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); auto command_interfaces = actuator_hw.export_command_interfaces(); ASSERT_EQ(1u, command_interfaces.size()); - EXPECT_EQ("joint1/velocity", command_interfaces[0].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0].get_interface_name()); - EXPECT_EQ("joint1", command_interfaces[0].get_prefix_name()); + EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); double velocity_value = 1.0; - command_interfaces[0].set_value(velocity_value); // velocity + command_interfaces[0]->set_value(velocity_value); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED @@ -453,8 +714,8 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[0].get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[1].get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -468,8 +729,8 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(step * velocity_value, state_interfaces[0].get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1].get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -483,8 +744,8 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0].get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1].get_value()); // velocity + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -498,8 +759,8 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[0].get_value()); // position value - EXPECT_EQ(0, state_interfaces[1].get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(0, state_interfaces[1]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -509,41 +770,209 @@ TEST(TestComponentInterfaces, dummy_actuator) EXPECT_EQ( hardware_interface::return_type::OK, actuator_hw.perform_command_mode_switch({""}, {""})); } +// END -TEST(TestComponentInterfaces, dummy_sensor) +TEST(TestComponentInterfaces, dummy_actuator_default) { - hardware_interface::Sensor sensor_hw(std::make_unique()); + hardware_interface::Actuator actuator_hw( + std::make_unique()); + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); + auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); - hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_sensor_components"); - auto state = sensor_hw.initialize(mock_hw_info, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - auto state_interfaces = sensor_hw.export_state_interfaces(); - ASSERT_EQ(1u, state_interfaces.size()); - EXPECT_EQ("joint1/voltage", state_interfaces[0].get_name()); - EXPECT_EQ("voltage", state_interfaces[0].get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[0].get_value())); + auto state_interfaces = actuator_hw.export_state_interfaces(); + ASSERT_EQ(2u, state_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } - // Not updated because is is UNCONFIGURED - sensor_hw.read(TIME, PERIOD); - EXPECT_TRUE(std::isnan(state_interfaces[0].get_value())); + auto command_interfaces = actuator_hw.export_command_interfaces(); + ASSERT_EQ(1u, command_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } + double velocity_value = 1.0; + auto ci_joint1_vel = + test_components::vector_contains(command_interfaces, "joint1/velocity").second; + command_interfaces[ci_joint1_vel]->set_value(velocity_value); // velocity + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - // Updated because is is INACTIVE - state = sensor_hw.configure(); + // Noting should change because it is UNCONFIGURED + auto si_joint1_pos = test_components::vector_contains(state_interfaces, "joint1/position").second; + auto si_joint1_vel = test_components::vector_contains(state_interfaces, "joint1/velocity").second; + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_value())); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + + state = actuator_hw.configure(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - EXPECT_EQ(0.0, state_interfaces[0].get_value()); - // It can read now - sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[0].get_value()); -} + // Read and Write are working because it is INACTIVE + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); -TEST(TestComponentInterfaces, dummy_system) -{ + EXPECT_EQ( + step * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + // Read and Write are working because it is ACTIVE + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + + EXPECT_EQ( + (10 + step) * velocity_value, + state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + + state = actuator_hw.shutdown(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // Noting should change because it is FINALIZED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + + EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + + EXPECT_EQ( + hardware_interface::return_type::OK, actuator_hw.prepare_command_mode_switch({""}, {""})); + EXPECT_EQ( + hardware_interface::return_type::OK, actuator_hw.perform_command_mode_switch({""}, {""})); +} + +// BEGIN (Handle export change): for backward compatibility +TEST(TestComponentInterfaces, dummy_sensor) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + hardware_interface::HardwareInfo mock_hw_info{}; + rclcpp::Logger logger = rclcpp::get_logger("test_sensor_components"); + auto state = sensor_hw.initialize(mock_hw_info, logger, nullptr); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = sensor_hw.export_state_interfaces(); + ASSERT_EQ(1u, state_interfaces.size()); + EXPECT_EQ("joint1/voltage", state_interfaces[0]->get_name()); + EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); + + // Not updated because is is UNCONFIGURED + sensor_hw.read(TIME, PERIOD); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); + + // Updated because is is INACTIVE + state = sensor_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); + EXPECT_EQ(0.0, state_interfaces[0]->get_value()); + + // It can read now + sensor_hw.read(TIME, PERIOD); + EXPECT_EQ(0x666, state_interfaces[0]->get_value()); +} +// END + +TEST(TestComponentInterfaces, dummy_sensor_default) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_voltage_sensor_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; + rclcpp::Logger logger = rclcpp::get_logger("test_sensor_component"); + auto state = sensor_hw.initialize(voltage_sensor_res, logger, nullptr); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = sensor_hw.export_state_interfaces(); + ASSERT_EQ(1u, state_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/voltage"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/voltage", state_interfaces[position]->get_name()); + EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); + } + + // Not updated because is is UNCONFIGURED + auto si_joint1_vol = test_components::vector_contains(state_interfaces, "joint1/voltage").second; + sensor_hw.read(TIME, PERIOD); + EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_vol]->get_value())); + + // Updated because is is INACTIVE + state = sensor_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); + EXPECT_EQ(0.0, state_interfaces[si_joint1_vol]->get_value()); + + // It can read now + sensor_hw.read(TIME, PERIOD); + EXPECT_EQ(0x666, state_interfaces[si_joint1_vol]->get_value()); +} + +// BEGIN (Handle export change): for backward compatibility +TEST(TestComponentInterfaces, dummy_system) +{ hardware_interface::System system_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; @@ -554,54 +983,246 @@ TEST(TestComponentInterfaces, dummy_system) auto state_interfaces = system_hw.export_state_interfaces(); ASSERT_EQ(6u, state_interfaces.size()); - EXPECT_EQ("joint1/position", state_interfaces[0].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0].get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()); - EXPECT_EQ("joint1/velocity", state_interfaces[1].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1].get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1].get_prefix_name()); - EXPECT_EQ("joint2/position", state_interfaces[2].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[2].get_interface_name()); - EXPECT_EQ("joint2", state_interfaces[2].get_prefix_name()); - EXPECT_EQ("joint2/velocity", state_interfaces[3].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[3].get_interface_name()); - EXPECT_EQ("joint2", state_interfaces[3].get_prefix_name()); - EXPECT_EQ("joint3/position", state_interfaces[4].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[4].get_interface_name()); - EXPECT_EQ("joint3", state_interfaces[4].get_prefix_name()); - EXPECT_EQ("joint3/velocity", state_interfaces[5].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[5].get_interface_name()); - EXPECT_EQ("joint3", state_interfaces[5].get_prefix_name()); + EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); + EXPECT_EQ("joint2/position", state_interfaces[2]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[2]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[2]->get_prefix_name()); + EXPECT_EQ("joint2/velocity", state_interfaces[3]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[3]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[3]->get_prefix_name()); + EXPECT_EQ("joint3/position", state_interfaces[4]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[4]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[4]->get_prefix_name()); + EXPECT_EQ("joint3/velocity", state_interfaces[5]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[5]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[5]->get_prefix_name()); + + auto command_interfaces = system_hw.export_command_interfaces(); + ASSERT_EQ(3u, command_interfaces.size()); + EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); + EXPECT_EQ("joint2/velocity", command_interfaces[1]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[1]->get_interface_name()); + EXPECT_EQ("joint2", command_interfaces[1]->get_prefix_name()); + EXPECT_EQ("joint3/velocity", command_interfaces[2]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[2]->get_interface_name()); + EXPECT_EQ("joint3", command_interfaces[2]->get_prefix_name()); + + double velocity_value = 1.0; + command_interfaces[0]->set_value(velocity_value); // velocity + command_interfaces[1]->set_value(velocity_value); // velocity + command_interfaces[2]->set_value(velocity_value); // velocity + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Noting should change because it is UNCONFIGURED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + + ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[2]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[3]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[4]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[5]->get_value())); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + } + + state = system_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); + + // Read and Write are working because it is INACTIVE + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + + EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[2]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[3]->get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[4]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[5]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + } + + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + // Read and Write are working because it is ACTIVE + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[2]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[3]->get_value()); // velocity + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[4]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[5]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + } + + state = system_hw.shutdown(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // Noting should change because it is FINALIZED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + + EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[2]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[3]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[4]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[5]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + } + + EXPECT_EQ(hardware_interface::return_type::OK, system_hw.prepare_command_mode_switch({}, {})); + EXPECT_EQ(hardware_interface::return_type::OK, system_hw.perform_command_mode_switch({}, {})); +} +// END + +TEST(TestComponentInterfaces, dummy_system_default) +{ + hardware_interface::System system_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_system_robot + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_system = control_resources[0]; + rclcpp::Logger logger = rclcpp::get_logger("test_system_component"); + auto state = system_hw.initialize(dummy_system, logger, nullptr); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = system_hw.export_state_interfaces(); + ASSERT_EQ(6u, state_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint2/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint2/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint3/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint3/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[position]->get_prefix_name()); + } auto command_interfaces = system_hw.export_command_interfaces(); ASSERT_EQ(3u, command_interfaces.size()); - EXPECT_EQ("joint1/velocity", command_interfaces[0].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0].get_interface_name()); - EXPECT_EQ("joint1", command_interfaces[0].get_prefix_name()); - EXPECT_EQ("joint2/velocity", command_interfaces[1].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[1].get_interface_name()); - EXPECT_EQ("joint2", command_interfaces[1].get_prefix_name()); - EXPECT_EQ("joint3/velocity", command_interfaces[2].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[2].get_interface_name()); - EXPECT_EQ("joint3", command_interfaces[2].get_prefix_name()); + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint2/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint3/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", command_interfaces[position]->get_prefix_name()); + } + auto ci_joint1_vel = + test_components::vector_contains(command_interfaces, "joint1/velocity").second; + auto ci_joint2_vel = + test_components::vector_contains(command_interfaces, "joint2/velocity").second; + auto ci_joint3_vel = + test_components::vector_contains(command_interfaces, "joint3/velocity").second; double velocity_value = 1.0; - command_interfaces[0].set_value(velocity_value); // velocity - command_interfaces[1].set_value(velocity_value); // velocity - command_interfaces[2].set_value(velocity_value); // velocity + command_interfaces[ci_joint1_vel]->set_value(velocity_value); // velocity + command_interfaces[ci_joint2_vel]->set_value(velocity_value); // velocity + command_interfaces[ci_joint3_vel]->set_value(velocity_value); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED + auto si_joint1_pos = test_components::vector_contains(state_interfaces, "joint1/position").second; + auto si_joint1_vel = test_components::vector_contains(state_interfaces, "joint1/velocity").second; + auto si_joint2_pos = test_components::vector_contains(state_interfaces, "joint2/position").second; + auto si_joint2_vel = test_components::vector_contains(state_interfaces, "joint2/velocity").second; + auto si_joint3_pos = test_components::vector_contains(state_interfaces, "joint3/position").second; + auto si_joint3_vel = test_components::vector_contains(state_interfaces, "joint3/velocity").second; for (auto step = 0u; step < 10; ++step) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[0].get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[1].get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[2].get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[3].get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[4].get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[5].get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[si_joint2_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint2_vel]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[si_joint3_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint3_vel]->get_value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -615,12 +1236,15 @@ TEST(TestComponentInterfaces, dummy_system) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ(step * velocity_value, state_interfaces[0].get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1].get_value()); // velocity - EXPECT_EQ(step * velocity_value, state_interfaces[2].get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[3].get_value()); // velocity - EXPECT_EQ(step * velocity_value, state_interfaces[4].get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[5].get_value()); // velocity + EXPECT_EQ( + step * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity + EXPECT_EQ( + step * velocity_value, state_interfaces[si_joint2_pos]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint2_vel]->get_value()); // velocity + EXPECT_EQ( + step * velocity_value, state_interfaces[si_joint3_pos]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint3_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -634,12 +1258,18 @@ TEST(TestComponentInterfaces, dummy_system) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0].get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1].get_value()); // velocity - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[2].get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[3].get_value()); // velocity - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[4].get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[5].get_value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, + state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, + state_interfaces[si_joint2_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint2_vel]->get_value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, + state_interfaces[si_joint3_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint3_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -653,12 +1283,12 @@ TEST(TestComponentInterfaces, dummy_system) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[0].get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[1].get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[2].get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[3].get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[4].get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[5].get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint2_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint2_vel]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint3_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint3_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -699,6 +1329,7 @@ TEST(TestComponentInterfaces, dummy_command_mode_system) system_hw.perform_command_mode_switch(two_keys, one_key)); } +// BEGIN (Handle export change): for backward compatibility TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) { hardware_interface::Actuator actuator_hw(std::make_unique()); @@ -732,8 +1363,80 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) // activate again and expect reset values state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0].get_value(), 0.0); - EXPECT_EQ(command_interfaces[0].get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); + + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is the second time therefore unrecoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = actuator_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} +// END + +TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) +{ + hardware_interface::Actuator actuator_hw( + std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); + auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = actuator_hw.export_state_interfaces(); + auto command_interfaces = actuator_hw.export_command_interfaces(); + state = actuator_hw.configure(); + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is first time therefore recoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + auto si_joint1_pos = test_components::vector_contains(state_interfaces, "joint1/position").second; + auto ci_joint1_vel = + test_components::vector_contains(command_interfaces, "joint1/velocity").second; + state = actuator_hw.configure(); + EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -759,6 +1462,7 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } +// BEGIN (Handle export change): for backward compatibility TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) { hardware_interface::Actuator actuator_hw(std::make_unique()); @@ -792,8 +1496,79 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) // activate again and expect reset values state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0].get_value(), 0.0); - EXPECT_EQ(command_interfaces[0].get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); + + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is the second time therefore unrecoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = actuator_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} +// END + +TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) +{ + hardware_interface::Actuator actuator_hw( + std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); + auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = actuator_hw.export_state_interfaces(); + auto command_interfaces = actuator_hw.export_command_interfaces(); + state = actuator_hw.configure(); + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is first time therefore recoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + auto si_joint1_pos = test_components::vector_contains(state_interfaces, "joint1/position").second; + auto ci_joint1_vel = + test_components::vector_contains(command_interfaces, "joint1/velocity").second; + state = actuator_hw.configure(); + EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -819,6 +1594,7 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } +// BEGIN (Handle export change): for backward compatibility TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) { hardware_interface::Sensor sensor_hw(std::make_unique()); @@ -855,7 +1631,7 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) // activate again and expect reset values state = sensor_hw.configure(); - EXPECT_EQ(state_interfaces[0].get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); state = sensor_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -883,7 +1659,69 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } +// END + +TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_voltage_sensor_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; + rclcpp::Logger logger = rclcpp::get_logger("test_sensor_component"); + auto state = sensor_hw.initialize(voltage_sensor_res, logger, nullptr); + + auto state_interfaces = sensor_hw.export_state_interfaces(); + // Updated because is is INACTIVE + state = sensor_hw.configure(); + state = sensor_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + + // Initiate recoverable error - call read 99 times OK and on 100-time will return error + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); + + state = sensor_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + auto si_joint1_vol = test_components::vector_contains(state_interfaces, "joint1/voltage").second; + state = sensor_hw.configure(); + EXPECT_EQ(state_interfaces[si_joint1_vol]->get_value(), 0.0); + + state = sensor_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + // Initiate unrecoverable error - call read 99 times OK and on 100-time will return error + for (auto i = 1ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); + + state = sensor_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = sensor_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} +// BEGIN (Handle export change): for backward compatibility TEST(TestComponentInterfaces, dummy_system_read_error_behavior) { hardware_interface::System system_hw(std::make_unique()); @@ -919,11 +1757,83 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index].get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + } + for (auto index = 0ul; index < 3; ++index) + { + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + } + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is the second time therefore unrecoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + + state = system_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = system_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} +// END + +TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) +{ + hardware_interface::System system_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_system_robot + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_system = control_resources[0]; + rclcpp::Logger logger = rclcpp::get_logger("test_system_component"); + auto state = system_hw.initialize(dummy_system, logger, nullptr); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = system_hw.export_state_interfaces(); + auto command_interfaces = system_hw.export_command_interfaces(); + state = system_hw.configure(); + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is first time therefore recoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + + state = system_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = system_hw.configure(); + for (auto index = 0ul; index < 6; ++index) + { + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index].get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -949,6 +1859,7 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } +// BEGIN (Handle export change): for backward compatibility TEST(TestComponentInterfaces, dummy_system_write_error_behavior) { hardware_interface::System system_hw(std::make_unique()); @@ -984,11 +1895,83 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index].get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + } + for (auto index = 0ul; index < 3; ++index) + { + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + } + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is the second time therefore unrecoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + + state = system_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = system_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} +// END + +TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) +{ + hardware_interface::System system_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_system_robot + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_system = control_resources[0]; + rclcpp::Logger logger = rclcpp::get_logger("test_system_component"); + auto state = system_hw.initialize(dummy_system, logger, nullptr); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = system_hw.export_state_interfaces(); + auto command_interfaces = system_hw.export_command_interfaces(); + state = system_hw.configure(); + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is first time therefore recoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + + state = system_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = system_hw.configure(); + for (auto index = 0ul; index < 6; ++index) + { + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index].get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp new file mode 100644 index 0000000000..04408e1d1e --- /dev/null +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -0,0 +1,377 @@ +// Copyright 2020 ros2_control development team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/actuator.hpp" +#include "hardware_interface/actuator_interface.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/sensor.hpp" +#include "hardware_interface/sensor_interface.hpp" +#include "hardware_interface/system.hpp" +#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 "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/components_urdfs.hpp" +#include "ros2_control_test_assets/descriptions.hpp" +#include "test_components.hpp" + +// Values to send over command interface to trigger error in write and read methods + +namespace +{ +const auto TIME = rclcpp::Time(0); +const auto PERIOD = rclcpp::Duration::from_seconds(0.01); +constexpr unsigned int TRIGGER_READ_WRITE_ERROR_CALLS = 10000; +} // namespace + +using namespace ::testing; // NOLINT + +namespace test_components +{ +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class DummyActuatorDefault : public hardware_interface::ActuatorInterface +{ + std::string get_name() const override { return "DummyActuatorDefault"; } + + std::vector export_state_interface_descriptions() + override + { + 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 interfaces; + } + + std::vector export_command_interface_descriptions() + override + { + 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 interfaces; + } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return hardware_interface::return_type::OK; + } +}; + +class DummySensorDefault : public hardware_interface::SensorInterface +{ + std::string get_name() const override { return "DummySensorDefault"; } + + std::vector export_state_interface_descriptions() + override + { + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "some_unlisted_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface(info_.sensors[0].name, info); + interfaces.push_back(unlisted_state_interface); + + return interfaces; + } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return hardware_interface::return_type::OK; + } +}; + +class DummySystemDefault : public hardware_interface::SystemInterface +{ + std::string get_name() const override { return "DummySystemDefault"; } + + std::vector export_state_interface_descriptions() + override + { + 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 interfaces; + } + + std::vector export_command_interface_descriptions() + override + { + 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 interfaces; + } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return hardware_interface::return_type::OK; + } +}; + +} // namespace test_components + +TEST(TestComponentInterfaces, dummy_actuator_default_custom_export) +{ + hardware_interface::Actuator actuator_hw( + std::make_unique()); + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); + auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = actuator_hw.export_state_interfaces(); + ASSERT_EQ(3u, state_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/some_unlisted_interface"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[position]->get_name()); + EXPECT_EQ("some_unlisted_interface", state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + + auto command_interfaces = actuator_hw.export_command_interfaces(); + ASSERT_EQ(2u, command_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/some_unlisted_interface"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/some_unlisted_interface", command_interfaces[position]->get_name()); + EXPECT_EQ("some_unlisted_interface", command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } +} + +TEST(TestComponentInterfaces, dummy_sensor_default_custom_export) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_voltage_sensor_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; + rclcpp::Logger logger = rclcpp::get_logger("test_sensor_component"); + auto state = sensor_hw.initialize(voltage_sensor_res, logger, nullptr); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = sensor_hw.export_state_interfaces(); + ASSERT_EQ(2u, state_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/voltage"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/voltage", state_interfaces[position]->get_name()); + EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/some_unlisted_interface"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[position]->get_name()); + EXPECT_EQ("some_unlisted_interface", state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } +} + +TEST(TestComponentInterfaces, dummy_system_default_custom_export) +{ + hardware_interface::System system_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_system_robot + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_system = control_resources[0]; + rclcpp::Logger logger = rclcpp::get_logger("test_system_component"); + auto state = system_hw.initialize(dummy_system, logger, nullptr); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = system_hw.export_state_interfaces(); + ASSERT_EQ(7u, state_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint2/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint2/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint3/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint3/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/some_unlisted_interface"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[position]->get_name()); + EXPECT_EQ("some_unlisted_interface", state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + + auto command_interfaces = system_hw.export_command_interfaces(); + ASSERT_EQ(4u, command_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint2/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint3/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/some_unlisted_interface"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/some_unlisted_interface", command_interfaces[position]->get_name()); + EXPECT_EQ("some_unlisted_interface", command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } +} diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index be891787f3..cdbff8ca21 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1404,3 +1404,113 @@ TEST_F(TestComponentParser, urdf_incomplete_throws_error) std::string(ros2_control_test_assets::urdf_tail); ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } + +TEST_F(TestComponentParser, parse_joint_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto joint_state_descriptions = + parse_state_interface_descriptions_from_hardware_info(control_hardware[0].joints); + EXPECT_EQ(joint_state_descriptions[0].prefix_name, "joint1"); + EXPECT_EQ(joint_state_descriptions[0].get_interface_type(), "position"); + EXPECT_EQ(joint_state_descriptions[0].get_name(), "joint1/position"); + + EXPECT_EQ(joint_state_descriptions[1].prefix_name, "joint2"); + EXPECT_EQ(joint_state_descriptions[1].get_interface_type(), "position"); + EXPECT_EQ(joint_state_descriptions[1].get_name(), "joint2/position"); +} + +TEST_F(TestComponentParser, parse_joint_command_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto joint_command_descriptions = + parse_command_interface_descriptions_from_hardware_info(control_hardware[0].joints); + EXPECT_EQ(joint_command_descriptions[0].prefix_name, "joint1"); + EXPECT_EQ(joint_command_descriptions[0].get_interface_type(), "position"); + EXPECT_EQ(joint_command_descriptions[0].get_name(), "joint1/position"); + EXPECT_EQ(joint_command_descriptions[0].interface_info.min, "-1"); + EXPECT_EQ(joint_command_descriptions[0].interface_info.max, "1"); + + EXPECT_EQ(joint_command_descriptions[1].prefix_name, "joint2"); + EXPECT_EQ(joint_command_descriptions[1].get_interface_type(), "position"); + EXPECT_EQ(joint_command_descriptions[1].get_name(), "joint2/position"); + EXPECT_EQ(joint_command_descriptions[1].interface_info.min, "-1"); + EXPECT_EQ(joint_command_descriptions[1].interface_info.max, "1"); +} + +TEST_F(TestComponentParser, parse_sensor_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_sensor_only + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto sensor_state_descriptions = + parse_state_interface_descriptions_from_hardware_info(control_hardware[0].sensors); + EXPECT_EQ(sensor_state_descriptions[0].prefix_name, "sensor1"); + EXPECT_EQ(sensor_state_descriptions[0].get_interface_type(), "roll"); + EXPECT_EQ(sensor_state_descriptions[0].get_name(), "sensor1/roll"); + EXPECT_EQ(sensor_state_descriptions[1].prefix_name, "sensor1"); + EXPECT_EQ(sensor_state_descriptions[1].get_interface_type(), "pitch"); + EXPECT_EQ(sensor_state_descriptions[1].get_name(), "sensor1/pitch"); + EXPECT_EQ(sensor_state_descriptions[2].prefix_name, "sensor1"); + EXPECT_EQ(sensor_state_descriptions[2].get_interface_type(), "yaw"); + EXPECT_EQ(sensor_state_descriptions[2].get_name(), "sensor1/yaw"); + + EXPECT_EQ(sensor_state_descriptions[3].prefix_name, "sensor2"); + EXPECT_EQ(sensor_state_descriptions[3].get_interface_type(), "image"); + EXPECT_EQ(sensor_state_descriptions[3].get_name(), "sensor2/image"); +} + +TEST_F(TestComponentParser, parse_gpio_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto gpio_state_descriptions = + parse_state_interface_descriptions_from_hardware_info(control_hardware[0].gpios); + EXPECT_EQ(gpio_state_descriptions[0].prefix_name, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].get_interface_type(), "analog_output1"); + EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); + EXPECT_EQ(gpio_state_descriptions[1].prefix_name, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[1].get_interface_type(), "analog_input1"); + EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_analog_IOs/analog_input1"); + EXPECT_EQ(gpio_state_descriptions[2].prefix_name, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[2].get_interface_type(), "analog_input2"); + EXPECT_EQ(gpio_state_descriptions[2].get_name(), "flange_analog_IOs/analog_input2"); + + EXPECT_EQ(gpio_state_descriptions[3].prefix_name, "flange_vacuum"); + EXPECT_EQ(gpio_state_descriptions[3].get_interface_type(), "vacuum"); + EXPECT_EQ(gpio_state_descriptions[3].get_name(), "flange_vacuum/vacuum"); +} + +TEST_F(TestComponentParser, parse_gpio_command_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto gpio_state_descriptions = + parse_command_interface_descriptions_from_hardware_info(control_hardware[0].gpios); + EXPECT_EQ(gpio_state_descriptions[0].prefix_name, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].get_interface_type(), "analog_output1"); + EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); + + EXPECT_EQ(gpio_state_descriptions[1].prefix_name, "flange_vacuum"); + EXPECT_EQ(gpio_state_descriptions[1].get_interface_type(), "vacuum"); + EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_vacuum/vacuum"); +} diff --git a/hardware_interface/test/test_components.hpp b/hardware_interface/test/test_components.hpp new file mode 100644 index 0000000000..def72b8398 --- /dev/null +++ b/hardware_interface/test/test_components.hpp @@ -0,0 +1,39 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_COMPONENTS_HPP_ +#define TEST_COMPONENTS_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" + +namespace test_components +{ + +template +std::pair vector_contains(const std::vector & vec, const T & element) +{ + auto it = std::find_if( + vec.begin(), vec.end(), [element](const auto & state_interface) + { return state_interface->get_name() == std::string(element); }); + + return std::make_pair(it != vec.end(), std::distance(vec.begin(), it)); +} + +} // namespace test_components +#endif // TEST_COMPONENTS_HPP_ diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index da8258c643..7d79c032f0 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -14,8 +14,11 @@ #include #include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" using hardware_interface::CommandInterface; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; using hardware_interface::StateInterface; namespace @@ -64,3 +67,31 @@ TEST(TestHandle, value_methods_work_on_non_nullptr) EXPECT_NO_THROW(handle.set_value(0.0)); EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); } + +TEST(TestHandle, interface_description_state_interface_name_getters_work) +{ + const std::string POSITION_INTERFACE = "position"; + const std::string JOINT_NAME_1 = "joint1"; + InterfaceInfo info; + info.name = POSITION_INTERFACE; + InterfaceDescription interface_descr(JOINT_NAME_1, info); + StateInterface handle{interface_descr}; + + EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); + EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); + EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); +} + +TEST(TestHandle, interface_description_command_interface_name_getters_work) +{ + const std::string POSITION_INTERFACE = "position"; + const std::string JOINT_NAME_1 = "joint1"; + InterfaceInfo info; + info.name = POSITION_INTERFACE; + InterfaceDescription interface_descr(JOINT_NAME_1, info); + CommandInterface handle{interface_descr}; + + EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); + EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); + EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); +} diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 51d81a90ab..f932c610fe 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1230,12 +1230,12 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[1], CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[2]}; - std::vector reference_interfaces; + std::vector> reference_interfaces; std::vector reference_interface_values = {1.0, 2.0, 3.0}; for (size_t i = 0; i < REFERENCE_INTERFACE_NAMES.size(); ++i) { - reference_interfaces.push_back(hardware_interface::CommandInterface( + reference_interfaces.push_back(std::make_shared( CONTROLLER_NAME, REFERENCE_INTERFACE_NAMES[i], &(reference_interface_values[i]))); } diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index b0076b859b..f78eb6e75c 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -561,6 +561,81 @@ const auto valid_urdf_ros2_control_system_robot_with_all_interfaces = + +)"; + +// Voltage Sensor only +const auto valid_urdf_ros2_control_voltage_sensor_only = + R"( + + + ros2_control_demo_hardware/SingleJointVoltageSensor + 2 + + + + + +)"; + +const auto valid_urdf_ros2_control_dummy_actuator_only = + R"( + + + ros2_control_demo_hardware/VelocityActuatorHardware + 1.13 + 3 + + + + -1 + 1 + + + + + + transmission_interface/RotationToLinerTansmission + + 325.949 + + 1337 + + +)"; + +const auto valid_urdf_ros2_control_dummy_system_robot = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithGPIOHardware + 2 + 2 + + + + -1 + 1 + + + + + + + -1 + 1 + + + + + + + -1 + 1 + + + + )"; From 7bb89bc50587b33b981730cbbbcc5c091c06a8c4 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 23 Aug 2024 13:22:11 +0200 Subject: [PATCH 02/12] add orderd vector of StateInterfaces and CommandInterfaces --- .../hardware_interface/actuator_interface.hpp | 13 ++++++++++++ .../hardware_interface/resource_manager.hpp | 4 ++-- .../hardware_interface/sensor_interface.hpp | 19 +++++++++++------ .../hardware_interface/system_interface.hpp | 21 +++++++++++++++++++ 4 files changed, 49 insertions(+), 8 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 9d2c8e032a..a945cca1ae 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -221,6 +221,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod unlisted_state_interfaces_.insert(std::make_pair(name, description)); auto state_interface = std::make_shared(description); actuator_states_.insert(std::make_pair(name, state_interface)); + unlisted_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } @@ -228,6 +229,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod { auto state_interface = std::make_shared(descr); actuator_states_.insert(std::make_pair(name, state_interface)); + joint_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } return state_interfaces; @@ -298,6 +300,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod unlisted_command_interfaces_.insert(std::make_pair(name, description)); auto command_interface = std::make_shared(description); actuator_commands_.insert(std::make_pair(name, command_interface)); + unlisted_commands_.push_back(command_interface); command_interfaces.push_back(command_interface); } @@ -305,6 +308,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod { auto command_interface = std::make_shared(descr); actuator_commands_.insert(std::make_pair(name, command_interface)); + joint_commands_.push_back(command_interface); command_interfaces.push_back(command_interface); } @@ -436,17 +440,26 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod protected: HardwareInfo info_; + // interface names to InterfaceDescription std::unordered_map joint_state_interfaces_; std::unordered_map joint_command_interfaces_; std::unordered_map unlisted_state_interfaces_; std::unordered_map unlisted_command_interfaces_; + // Exported Command- and StateInterfaces in order they are listed in the hardware description. + std::vector> joint_states_; + std::vector> joint_commands_; + + std::vector> unlisted_states_; + std::vector> unlisted_commands_; + rclcpp_lifecycle::State lifecycle_state_; private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; rclcpp::Logger actuator_logger_; + // interface names to Handle accessed through getters/setters std::unordered_map> actuator_states_; std::unordered_map> actuator_commands_; }; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 3f886f7ef4..8f9f142c25 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -141,7 +141,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] interfaces list of controller's state interfaces as StateInterfaces. */ void import_controller_exported_state_interfaces( - const std::string & controller_name, std::vector> & interfaces); + const std::string & controller_name, std::vector & interfaces); /// Get list of exported tate interface of a controller. /** @@ -195,7 +195,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ void import_controller_reference_interfaces( const std::string & controller_name, - const std::vector> & interfaces); + std::vector & interfaces); /// Get list of reference interface of a controller. /** diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index f7bc061d20..e325dc2bc7 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -204,7 +204,8 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI auto name = description.get_name(); unlisted_state_interfaces_.insert(std::make_pair(name, description)); auto state_interface = std::make_shared(description); - sensor_states_.insert(std::make_pair(name, state_interface)); + sensor_states_map_.insert(std::make_pair(name, state_interface)); + unlisted_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } @@ -213,7 +214,8 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // TODO(Manuel) maybe check for duplicates otherwise only the first appearance of "name" is // inserted auto state_interface = std::make_shared(descr); - sensor_states_.insert(std::make_pair(name, state_interface)); + sensor_states_map_.insert(std::make_pair(name, state_interface)); + sensor_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } @@ -258,12 +260,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI void set_state(const std::string & interface_name, const double & value) { - sensor_states_.at(interface_name)->set_value(value); + sensor_states_map_.at(interface_name)->set_value(value); } double get_state(const std::string & interface_name) const { - return sensor_states_.at(interface_name)->get_value(); + return sensor_states_map_.at(interface_name)->get_value(); } /// Get the logger of the SensorInterface. @@ -286,16 +288,21 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI protected: HardwareInfo info_; - + // interface names to InterfaceDescription std::unordered_map sensor_state_interfaces_; std::unordered_map unlisted_state_interfaces_; + // Exported Command- and StateInterfaces in order they are listed in the hardware description. + std::vector> sensor_states_; + std::vector> unlisted_states_; + rclcpp_lifecycle::State lifecycle_state_; private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; rclcpp::Logger sensor_logger_; - std::unordered_map> sensor_states_; + // interface names to Handle accessed through getters/setters + std::unordered_map> sensor_states_map_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 7830254e9c..2623ff0b52 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -242,6 +242,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI unlisted_state_interfaces_.insert(std::make_pair(name, description)); auto state_interface = std::make_shared(description); system_states_.insert(std::make_pair(name, state_interface)); + unlisted_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } @@ -249,18 +250,21 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { auto state_interface = std::make_shared(descr); system_states_.insert(std::make_pair(name, state_interface)); + joint_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } for (const auto & [name, descr] : sensor_state_interfaces_) { auto state_interface = std::make_shared(descr); system_states_.insert(std::make_pair(name, state_interface)); + sensor_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } for (const auto & [name, descr] : gpio_state_interfaces_) { auto state_interface = std::make_shared(descr); system_states_.insert(std::make_pair(name, state_interface)); + gpio_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } return state_interfaces; @@ -332,6 +336,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI unlisted_command_interfaces_.insert(std::make_pair(name, description)); auto command_interface = std::make_shared(description); system_commands_.insert(std::make_pair(name, command_interface)); + unlisted_commands_.push_back(command_interface); command_interfaces.push_back(command_interface); } @@ -339,6 +344,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { auto command_interface = std::make_shared(descr); system_commands_.insert(std::make_pair(name, command_interface)); + joint_commands_.push_back(command_interface); command_interfaces.push_back(command_interface); } @@ -346,6 +352,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { auto command_interface = std::make_shared(descr); system_commands_.insert(std::make_pair(name, command_interface)); + gpio_commands_.push_back(command_interface); command_interfaces.push_back(command_interface); } return command_interfaces; @@ -477,6 +484,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI protected: HardwareInfo info_; + // interface names to InterfaceDescription std::unordered_map joint_state_interfaces_; std::unordered_map joint_command_interfaces_; @@ -490,9 +498,22 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI rclcpp_lifecycle::State lifecycle_state_; + // Exported Command- and StateInterfaces in order they are listed in the hardware description. + std::vector> joint_states_; + std::vector> joint_commands_; + + std::vector> sensor_states_; + + std::vector> gpio_states_; + std::vector> gpio_commands_; + + std::vector> unlisted_states_; + std::vector> unlisted_commands_; + private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; rclcpp::Logger system_logger_; + // interface names to Handle accessed through getters/setters std::unordered_map> system_states_; std::unordered_map> system_commands_; }; From c073b3169161d47abad2a2e88daa3a41d4ec7fe9 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 23 Aug 2024 13:22:45 +0200 Subject: [PATCH 03/12] Make compileable after splittin --- hardware_interface/src/resource_manager.cpp | 20 +++++++++++++++---- .../test/test_resource_manager.cpp | 4 ++-- 2 files changed, 18 insertions(+), 6 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e512f2c1dc..539cee28e9 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1253,10 +1253,16 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_exported_state_interfaces( - const std::string & controller_name, std::vector> & interfaces) + const std::string & controller_name, std::vector & interfaces) { std::lock_guard guard(resource_interfaces_lock_); - auto interface_names = resource_storage_->add_state_interfaces(interfaces); + std::vector> interface_ptrs; + interface_ptrs.reserve(interfaces.size()); + for (auto & interface : interfaces) + { + interface_ptrs.push_back(std::make_shared(interface)); + } + auto interface_names = resource_storage_->add_state_interfaces(interface_ptrs); resource_storage_->controllers_exported_state_interfaces_map_[controller_name] = interface_names; } @@ -1316,10 +1322,16 @@ void ResourceManager::remove_controller_exported_state_interfaces( // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_reference_interfaces( const std::string & controller_name, - const std::vector> & interfaces) + std::vector & interfaces) { std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); - auto interface_names = resource_storage_->add_command_interfaces(interfaces); + std::vector> interface_ptrs; + interface_ptrs.reserve(interfaces.size()); + for (auto & interface : interfaces) + { + interface_ptrs.push_back(std::make_shared(std::move(interface))); + } + auto interface_names = resource_storage_->add_command_interfaces(interface_ptrs); resource_storage_->controllers_reference_interfaces_map_[controller_name] = interface_names; } diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index f932c610fe..51d81a90ab 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1230,12 +1230,12 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[1], CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[2]}; - std::vector> reference_interfaces; + std::vector reference_interfaces; std::vector reference_interface_values = {1.0, 2.0, 3.0}; for (size_t i = 0; i < REFERENCE_INTERFACE_NAMES.size(); ++i) { - reference_interfaces.push_back(std::make_shared( + reference_interfaces.push_back(hardware_interface::CommandInterface( CONTROLLER_NAME, REFERENCE_INTERFACE_NAMES[i], &(reference_interface_values[i]))); } From 132a5e37b12ce89dcb74eac2a0fbfade0bed256d Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 28 Aug 2024 10:44:57 +0200 Subject: [PATCH 04/12] review suggestions --- doc/Iron.rst | 92 ------------------- doc/migration.rst | 87 ++++++++++++++++++ doc/release_notes.rst | 9 ++ .../hardware_interface/actuator_interface.hpp | 10 +- .../include/hardware_interface/handle.hpp | 10 +- .../hardware_interface/sensor_interface.hpp | 5 +- .../hardware_interface/system_interface.hpp | 10 +- 7 files changed, 115 insertions(+), 108 deletions(-) delete mode 100644 doc/Iron.rst diff --git a/doc/Iron.rst b/doc/Iron.rst deleted file mode 100644 index 323d5ecffe..0000000000 --- a/doc/Iron.rst +++ /dev/null @@ -1,92 +0,0 @@ -Iron to Jazzy -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -Adaption of Command-/StateInterfaces -*************************************** -Changes from `(PR #1240) `__ - -* ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which gets parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp `__). -* The memory for storing the value of a ``Command-/StateInterfaces`` is no longer allocated in the hardware but instead in the ``Command-/StateInterfaces`` itself. -* To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function by creating some custom ``Command-/StateInterfaces``. - -Migration of Command-/StateInterfaces -------------------------------------- -To adapt to the new way of creating and exporting ``Command-/StateInterfaces`` follow those steps: - -1. Delete the ``std::vector export_command_interfaces() override`` and ``std::vector export_state_interfaces() override``. -2. Delete allocated memory for any ``Command-/StateInterfaces``, e.g.: - - * If you have a ``std::vector hw_commands_;`` for joints' ``CommandInterfaces`` delete the definition and any usage/appearance. - * Wherever you iterated over a state/command or accessed commands like this: - -.. code-block:: c++ - - // states - for (uint i = 0; i < hw_states_.size(); i++) - { - hw_states_[i] = 0; - auto some_state = hw_states_[i]; - } - - // commands - for (uint i = 0; i < hw_commands_.size(); i++) - { - hw_commands_[i] = 0; - auto some_command = hw_commands_[i]; - } - - // specific state/command - hw_commands_[x] = hw_states_[y]; - -replace it with - -.. code-block:: c++ - - // states replace with this - for (const auto & [name, descr] : joint_state_interfaces_) - { - set_state(name, 0.0); - auto some_state = get_state(name); - } - - //commands replace with this - for (const auto & [name, descr] : joint_commands_interfaces_) - { - set_command(name, 0.0); - auto some_command = get_command(name); - } - - // replace specific state/command, for this you need to store the names which are std::strings - // somewhere or know them. However be careful since the names are fully qualified names which - // means that the prefix is included for the name: E.g.: prefix/joint_1/velocity - set_command(name_of_command_interface_x, get_state(name_of_state_interface_y)); - -Migration of unlisted Command-/StateInterfaces not defined in ``ros2_control`` XML-tag --------------------------------------------------------------------------------------- -If you want some unlisted ``Command-/StateInterfaces`` not included in the ``ros2_control`` XML-tag you can follow those steps: - -1. Override the ``virtual std::vector export_command_interfaces_2()`` or ``virtual std::vector export_state_interfaces_2()`` -2. Create the InterfaceDescription for each of the interfaces you want to create in the override ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function, add it to a vector and return the vector: - - .. code-block:: c++ - - std::vector my_unlisted_interfaces; - - InterfaceInfo unlisted_interface; - unlisted_interface.name = "some_unlisted_interface"; - unlisted_interface.min = "-5.0"; - unlisted_interface.data_type = "double"; - my_unlisted_interfaces.push_back(InterfaceDescription(info_.name, unlisted_interface)); - - return my_unlisted_interfaces; - -3. The unlisted interface will then be stored in either the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_`` map depending in which function they are created. -4. You can access it like any other interface with the ``get_state(name)``, ``set_state(name, value)``, ``get_command(name)`` or ``set_command(name, value)``. E.g. ``get_state("some_unlisted_interface")``. - -Custom exportation of Command-/StateInterfaces ----------------------------------------------- -In case the default implementation (``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` ) for exporting the ``Command-/StateInterfaces`` is not enough you can override them. You should however consider the following things: - -* If you want to have unlisted interfaces available you need to call the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. -* Don't forget to store the created ``Command-/StateInterfaces`` internally as you only return ``shared_ptrs`` and the resource manager will not provide access to the created ``Command-/StateInterfaces`` for the hardware. So you must take care of storing them yourself. -* Names must be unique! diff --git a/doc/migration.rst b/doc/migration.rst index 255db9b8ae..173cd5e095 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -75,3 +75,90 @@ hardware_interface ****************** * ``test_components`` was moved to its own package. Update the dependencies if you are using them. (`#1325 `_) * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. + +Adaption of Command-/StateInterfaces +*************************************** + +* ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which get parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp `__). The memory is now allocated in the handle itself. + +Migration of Command-/StateInterfaces +------------------------------------- +To adapt to the new way of creating and exporting ``Command-/StateInterfaces`` follow those steps: + +1. Delete the ``std::vector export_command_interfaces() override`` and ``std::vector export_state_interfaces() override``. +2. Delete allocated memory for any ``Command-/StateInterfaces``, e.g.: + + * If you have a ``std::vector hw_commands_;`` for joints' ``CommandInterfaces`` delete the definition and any usage/appearance. + * Wherever you iterated over a state/command or accessed commands like this: + +.. code-block:: c++ + + // states + for (uint i = 0; i < hw_states_.size(); i++) + { + hw_states_[i] = 0; + auto some_state = hw_states_[i]; + } + + // commands + for (uint i = 0; i < hw_commands_.size(); i++) + { + hw_commands_[i] = 0; + auto some_command = hw_commands_[i]; + } + + // specific state/command + hw_commands_[x] = hw_states_[y]; + +replace it with + +.. code-block:: c++ + + // states replace with this + for (const auto & [name, descr] : joint_state_interfaces_) + { + set_state(name, 0.0); + auto some_state = get_state(name); + } + + //commands replace with this + for (const auto & [name, descr] : joint_commands_interfaces_) + { + set_command(name, 0.0); + auto some_command = get_command(name); + } + + // replace specific state/command, for this you need to store the names which are strings + // somewhere or know them. However be careful since the names are fully qualified names which + // means that the prefix is included for the name: E.g.: prefix/joint_1/velocity + set_command(name_of_command_interface_x, get_state(name_of_state_interface_y)); + +Migration of unlisted Command-/StateInterfaces not defined in ``ros2_control`` XML-tag +-------------------------------------------------------------------------------------- +If you want some unlisted ``Command-/StateInterfaces`` not included in the ``ros2_control`` XML-tag you can follow those steps: + +1. Override the ``virtual std::vector export_command_interfaces_2()`` or ``virtual std::vector export_state_interfaces_2()`` +2. Create the InterfaceDescription for each of the interfaces you want to create in the override ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function, add it to a vector and return the vector: + + .. code-block:: c++ + + std::vector my_unlisted_interfaces; + + InterfaceInfo unlisted_interface; + unlisted_interface.name = "some_unlisted_interface"; + unlisted_interface.min = "-5.0"; + unlisted_interface.data_type = "double"; + my_unlisted_interfaces.push_back(InterfaceDescription(info_.name, unlisted_interface)); + + return my_unlisted_interfaces; + +3. The unlisted interface will then be stored in either the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_`` map depending in which function they are created. +4. You can access it like any other interface with the ``get_state(name)``, ``set_state(name, value)``, ``get_command(name)`` or ``set_command(name, value)``. E.g. ``get_state("some_unlisted_interface")``. + +Custom export of Command-/StateInterfaces +---------------------------------------------- +In case the default implementation (``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` ) for exporting the ``Command-/StateInterfaces`` is not enough you can override them. You should however consider the following things: + +* If you want to have unlisted interfaces available you need to call the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. +* Don't forget to store the created ``Command-/StateInterfaces`` internally as you only return ``shared_ptrs`` and the resource manager will not provide access to the created ``Command-/StateInterfaces`` for the hardware. So you must take care of storing them yourself. +* Names must be unique! diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 50583b3bf4..46fe382d41 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -109,6 +109,15 @@ joint_limits ************ * Add header to import limits from standard URDF definition (`#1298 `_) +Adaption of Command-/StateInterfaces +*************************************** +Changes from `(PR #1688) `_ for an overview of related changes and discussion refer to `(PR #1240) `_. + +* ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which gets parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp `__). +* The memory for storing the value of a ``Command-/StateInterfaces`` is no longer allocated in the hardware but instead in the ``Command-/StateInterfaces`` itself. +* To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function by creating some custom ``Command-/StateInterfaces``. + + ros2controlcli ************** * Spawner colours were added to ``list_controllers`` depending upon active or inactive (`#1409 `_) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 723cfd460e..79237834d2 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -175,8 +175,9 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod "by the Framework.")]] virtual std::vector export_state_interfaces() { - // return empty vector by default. For backward compatibility we check if all vectors is empty - // and if so call on_export_state_interfaces() + // return empty vector by default. For backward compatibility we try calling + // export_state_interfaces() and only when empty vector is returned call + // on_export_state_interfaces() return {}; } @@ -254,8 +255,9 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod "by the Framework.")]] virtual std::vector export_command_interfaces() { - // return empty vector by default. For backward compatibility we check if all vectors is empty - // and if so call on_export_command_interfaces() + // return empty vector by default. For backward compatibility we try calling + // export_command_interfaces() and only when empty vector is returned call + // on_export_command_interfaces() return {}; } diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 1604349d2f..97c61bcb24 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -17,17 +17,15 @@ #include #include -#include #include #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/macros.hpp" -#include "hardware_interface/visibility_control.h" namespace hardware_interface { -typedef std::variant HANDLE_DATATYPE; +using HANDLE_DATATYPE = std::variant; /// A handle used to get and set a value on a given interface. class Handle @@ -46,9 +44,9 @@ class Handle } explicit Handle(const InterfaceDescription & interface_description) - : prefix_name_(interface_description.prefix_name), - interface_name_(interface_description.interface_info.name), - handle_name_(prefix_name_ + "/" + interface_name_) + : prefix_name_(interface_description.get_prefix_name()), + interface_name_(interface_description.get_interface_name()), + handle_name_(interface_description.get_name()) { // As soon as multiple datatypes are used in HANDLE_DATATYPE // we need to initialize according the type passed in interface description diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index cb7dd6db6f..93f1df0d41 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -159,8 +159,9 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI "by the Framework.")]] virtual std::vector export_state_interfaces() { - // return empty vector by default. For backward compatibility we check if all vectors is empty - // and if so call on_export_state_interfaces() + // return empty vector by default. For backward compatibility we try calling + // export_state_interfaces() and only when empty vector is returned call + // on_export_state_interfaces() return {}; } diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 1c7c960cd2..812c562731 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -195,8 +195,9 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI "by the Framework.")]] virtual std::vector export_state_interfaces() { - // return empty vector by default. For backward compatibility we check if all vectors is empty - // and if so call on_export_state_interfaces() + // return empty vector by default. For backward compatibility we try calling + // export_state_interfaces() and only when empty vector is returned call + // on_export_state_interfaces() return {}; } @@ -289,8 +290,9 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI "by the Framework.")]] virtual std::vector export_command_interfaces() { - // return empty vector by default. For backward compatibility we check if all vectors is empty - // and if so call on_export_command_interfaces() + // return empty vector by default. For backward compatibility we try calling + // export_command_interfaces() and only when empty vector is returned call + // on_export_command_interfaces() return {}; } From 0a61ac52351216614b432bf5e9f8eb856aa617ec Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 28 Aug 2024 11:24:46 +0200 Subject: [PATCH 05/12] make compileable --- .../hardware_interface/actuator_interface.hpp | 4 ++-- .../hardware_interface/sensor_interface.hpp | 2 +- .../hardware_interface/system_interface.hpp | 10 +++++----- .../test/test_component_interfaces.cpp | 20 +++++++++---------- 4 files changed, 18 insertions(+), 18 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 79237834d2..a6b29a4e82 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -135,7 +135,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) { auto joint_state_interface_descriptions = - parse_state_interface_descriptions_from_hardware_info(hardware_info.joints); + parse_state_interface_descriptions(hardware_info.joints); for (const auto & description : joint_state_interface_descriptions) { joint_state_interfaces_.insert(std::make_pair(description.get_name(), description)); @@ -149,7 +149,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod virtual void import_command_interface_descriptions(const HardwareInfo & hardware_info) { auto joint_command_interface_descriptions = - parse_command_interface_descriptions_from_hardware_info(hardware_info.joints); + parse_command_interface_descriptions(hardware_info.joints); for (const auto & description : joint_command_interface_descriptions) { joint_command_interfaces_.insert(std::make_pair(description.get_name(), description)); diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 93f1df0d41..5850eaeef9 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -134,7 +134,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) { auto sensor_state_interface_descriptions = - parse_state_interface_descriptions_from_hardware_info(hardware_info.sensors); + parse_state_interface_descriptions(hardware_info.sensors); for (const auto & description : sensor_state_interface_descriptions) { sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description)); diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 812c562731..2b835c0093 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -138,19 +138,19 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI void import_state_interface_descriptions(const HardwareInfo & hardware_info) { auto joint_state_interface_descriptions = - parse_state_interface_descriptions_from_hardware_info(hardware_info.joints); + parse_state_interface_descriptions(hardware_info.joints); for (const auto & description : joint_state_interface_descriptions) { joint_state_interfaces_.insert(std::make_pair(description.get_name(), description)); } auto sensor_state_interface_descriptions = - parse_state_interface_descriptions_from_hardware_info(hardware_info.sensors); + parse_state_interface_descriptions(hardware_info.sensors); for (const auto & description : sensor_state_interface_descriptions) { sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description)); } auto gpio_state_interface_descriptions = - parse_state_interface_descriptions_from_hardware_info(hardware_info.gpios); + parse_state_interface_descriptions(hardware_info.gpios); for (const auto & description : gpio_state_interface_descriptions) { gpio_state_interfaces_.insert(std::make_pair(description.get_name(), description)); @@ -164,13 +164,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI void import_command_interface_descriptions(const HardwareInfo & hardware_info) { auto joint_command_interface_descriptions = - parse_command_interface_descriptions_from_hardware_info(hardware_info.joints); + parse_command_interface_descriptions(hardware_info.joints); for (const auto & description : joint_command_interface_descriptions) { joint_command_interfaces_.insert(std::make_pair(description.get_name(), description)); } auto gpio_command_interface_descriptions = - parse_command_interface_descriptions_from_hardware_info(hardware_info.gpios); + parse_command_interface_descriptions(hardware_info.gpios); for (const auto & description : gpio_command_interface_descriptions) { gpio_command_interfaces_.insert(std::make_pair(description.get_name(), description)); diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 8c34a90c2f..d68beb6c6d 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -1380,7 +1380,7 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - state = actuator_hw.get_state(); + state = actuator_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); @@ -1426,7 +1426,7 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - state = actuator_hw.get_state(); + state = actuator_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1513,7 +1513,7 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - state = actuator_hw.get_state(); + state = actuator_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); @@ -1558,7 +1558,7 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - state = actuator_hw.get_state(); + state = actuator_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1691,7 +1691,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); - state = sensor_hw.get_state(); + state = sensor_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1711,7 +1711,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); - state = sensor_hw.get_state(); + state = sensor_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); @@ -1777,7 +1777,7 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - state = system_hw.get_state(); + state = system_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); @@ -1821,7 +1821,7 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - state = system_hw.get_state(); + state = system_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1915,7 +1915,7 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - state = system_hw.get_state(); + state = system_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); @@ -1959,7 +1959,7 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - state = system_hw.get_state(); + state = system_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); From 569658ebf84515695bdee2690751edd94eee6998 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 12 Sep 2024 08:23:20 +0100 Subject: [PATCH 06/12] Introduce type aliases for shared_ptr CommandInterface and StateInterface --- .../include/hardware_interface/actuator.hpp | 4 +-- .../hardware_interface/actuator_interface.hpp | 24 +++++++-------- .../include/hardware_interface/handle.hpp | 5 ++++ .../include/hardware_interface/sensor.hpp | 2 +- .../hardware_interface/sensor_interface.hpp | 12 ++++---- .../include/hardware_interface/system.hpp | 4 +-- .../hardware_interface/system_interface.hpp | 30 +++++++++---------- hardware_interface/src/actuator.cpp | 8 ++--- hardware_interface/src/resource_manager.cpp | 30 +++++++------------ hardware_interface/src/sensor.cpp | 4 +-- hardware_interface/src/system.cpp | 8 ++--- 11 files changed, 63 insertions(+), 68 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 71e68731fd..6834b9a74b 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -71,10 +71,10 @@ class Actuator final const rclcpp_lifecycle::State & error(); HARDWARE_INTERFACE_PUBLIC - std::vector> export_state_interfaces(); + std::vector export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC - std::vector> export_command_interfaces(); + std::vector export_command_interfaces(); HARDWARE_INTERFACE_PUBLIC return_type prepare_command_mode_switch( diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index a6b29a4e82..aae4a48406 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -169,7 +169,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * \return vector of state interfaces */ [[deprecated( - "Replaced by vector> on_export_state_interfaces() method. " + "Replaced by vector on_export_state_interfaces() method. " "Exporting is " "handled " "by the Framework.")]] virtual std::vector @@ -204,13 +204,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * * \return vector of shared pointers to the created and stored StateInterfaces */ - virtual std::vector> on_export_state_interfaces() + virtual std::vector on_export_state_interfaces() { // import the unlisted interfaces std::vector unlisted_interface_descriptions = export_state_interface_descriptions(); - std::vector> state_interfaces; + std::vector state_interfaces; state_interfaces.reserve( unlisted_interface_descriptions.size() + joint_state_interfaces_.size()); @@ -249,7 +249,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * \return vector of state interfaces */ [[deprecated( - "Replaced by vector> on_export_command_interfaces() method. " + "Replaced by vector on_export_command_interfaces() method. " "Exporting is " "handled " "by the Framework.")]] virtual std::vector @@ -284,13 +284,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * * \return vector of shared pointers to the created and stored CommandInterfaces */ - virtual std::vector> on_export_command_interfaces() + virtual std::vector on_export_command_interfaces() { // import the unlisted interfaces std::vector unlisted_interface_descriptions = export_command_interface_descriptions(); - std::vector> command_interfaces; + std::vector command_interfaces; command_interfaces.reserve( unlisted_interface_descriptions.size() + joint_command_interfaces_.size()); @@ -453,11 +453,11 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod std::unordered_map unlisted_command_interfaces_; // Exported Command- and StateInterfaces in order they are listed in the hardware description. - std::vector> joint_states_; - std::vector> joint_commands_; + std::vector joint_states_; + std::vector joint_commands_; - std::vector> unlisted_states_; - std::vector> unlisted_commands_; + std::vector unlisted_states_; + std::vector unlisted_commands_; rclcpp_lifecycle::State lifecycle_state_; @@ -465,8 +465,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; rclcpp::Logger actuator_logger_; // interface names to Handle accessed through getters/setters - std::unordered_map> actuator_states_; - std::unordered_map> actuator_commands_; + std::unordered_map actuator_states_; + std::unordered_map actuator_commands_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 97c61bcb24..dddcfef6b7 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -16,6 +16,7 @@ #define HARDWARE_INTERFACE__HANDLE_HPP_ #include +#include #include #include @@ -136,6 +137,8 @@ class StateInterface : public Handle StateInterface(StateInterface && other) = default; using Handle::Handle; + + using SharedPtr = std::shared_ptr; }; class CommandInterface : public Handle @@ -156,6 +159,8 @@ class CommandInterface : public Handle CommandInterface(CommandInterface && other) = default; using Handle::Handle; + + using SharedPtr = std::shared_ptr; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index c9a8425afc..edcd30cf21 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -71,7 +71,7 @@ class Sensor final const rclcpp_lifecycle::State & error(); HARDWARE_INTERFACE_PUBLIC - std::vector> export_state_interfaces(); + std::vector export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC std::string get_name() const; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 5850eaeef9..ece5a6e7a8 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -154,7 +154,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return vector of state interfaces */ [[deprecated( - "Replaced by vector> on_export_state_interfaces() method. " + "Replaced by vector on_export_state_interfaces() method. " "Exporting is handled " "by the Framework.")]] virtual std::vector export_state_interfaces() @@ -188,13 +188,13 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of shared pointers to the created and stored StateInterfaces */ - virtual std::vector> on_export_state_interfaces() + virtual std::vector on_export_state_interfaces() { // import the unlisted interfaces std::vector unlisted_interface_descriptions = export_state_interface_descriptions(); - std::vector> state_interfaces; + std::vector state_interfaces; state_interfaces.reserve( unlisted_interface_descriptions.size() + sensor_state_interfaces_.size()); @@ -297,8 +297,8 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::unordered_map unlisted_state_interfaces_; // Exported Command- and StateInterfaces in order they are listed in the hardware description. - std::vector> sensor_states_; - std::vector> unlisted_states_; + std::vector sensor_states_; + std::vector unlisted_states_; rclcpp_lifecycle::State lifecycle_state_; @@ -306,7 +306,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; rclcpp::Logger sensor_logger_; // interface names to Handle accessed through getters/setters - std::unordered_map> sensor_states_map_; + std::unordered_map sensor_states_map_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 1b129ea92d..432ccc1fc8 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -71,10 +71,10 @@ class System final const rclcpp_lifecycle::State & error(); HARDWARE_INTERFACE_PUBLIC - std::vector> export_state_interfaces(); + std::vector export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC - std::vector> export_command_interfaces(); + std::vector export_command_interfaces(); HARDWARE_INTERFACE_PUBLIC return_type prepare_command_mode_switch( diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 2b835c0093..658fffecd3 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -190,7 +190,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return vector of state interfaces */ [[deprecated( - "Replaced by vector> on_export_state_interfaces() method. " + "Replaced by vector on_export_state_interfaces() method. " "Exporting is handled " "by the Framework.")]] virtual std::vector export_state_interfaces() @@ -224,13 +224,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of shared pointers to the created and stored StateInterfaces */ - std::vector> on_export_state_interfaces() + std::vector on_export_state_interfaces() { // import the unlisted interfaces std::vector unlisted_interface_descriptions = export_state_interface_descriptions(); - std::vector> state_interfaces; + std::vector state_interfaces; state_interfaces.reserve( unlisted_interface_descriptions.size() + joint_state_interfaces_.size() + sensor_state_interfaces_.size() + gpio_state_interfaces_.size()); @@ -284,7 +284,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return vector of state interfaces */ [[deprecated( - "Replaced by vector> on_export_command_interfaces() method. " + "Replaced by vector on_export_command_interfaces() method. " "Exporting is " "handled " "by the Framework.")]] virtual std::vector @@ -319,13 +319,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of shared pointers to the created and stored CommandInterfaces */ - std::vector> on_export_command_interfaces() + std::vector on_export_command_interfaces() { // import the unlisted interfaces std::vector unlisted_interface_descriptions = export_command_interface_descriptions(); - std::vector> command_interfaces; + std::vector command_interfaces; command_interfaces.reserve( unlisted_interface_descriptions.size() + joint_command_interfaces_.size() + gpio_command_interfaces_.size()); @@ -504,23 +504,23 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI rclcpp_lifecycle::State lifecycle_state_; // Exported Command- and StateInterfaces in order they are listed in the hardware description. - std::vector> joint_states_; - std::vector> joint_commands_; + std::vector joint_states_; + std::vector joint_commands_; - std::vector> sensor_states_; + std::vector sensor_states_; - std::vector> gpio_states_; - std::vector> gpio_commands_; + std::vector gpio_states_; + std::vector gpio_commands_; - std::vector> unlisted_states_; - std::vector> unlisted_commands_; + std::vector unlisted_states_; + std::vector unlisted_commands_; private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; rclcpp::Logger system_logger_; // interface names to Handle accessed through getters/setters - std::unordered_map> system_states_; - std::unordered_map> system_commands_; + std::unordered_map system_states_; + std::unordered_map system_commands_; }; } // namespace hardware_interface diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 6eb6b52e93..a2d116a93c 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -204,7 +204,7 @@ const rclcpp_lifecycle::State & Actuator::error() return impl_->get_lifecycle_state(); } -std::vector> Actuator::export_state_interfaces() +std::vector Actuator::export_state_interfaces() { // TODO(karsten1987): Might be worth to do some brief sanity check here // BEGIN (Handle export change): for backward compatibility, can be removed if @@ -223,7 +223,7 @@ std::vector> Actuator::export_state_interfaces() // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed - std::vector> interface_ptrs; + std::vector interface_ptrs; interface_ptrs.reserve(interfaces.size()); for (auto const & interface : interfaces) { @@ -233,7 +233,7 @@ std::vector> Actuator::export_state_interfaces() // END: for backward compatibility } -std::vector> Actuator::export_command_interfaces() +std::vector Actuator::export_command_interfaces() { // TODO(karsten1987): Might be worth to do some brief sanity check here // BEGIN (Handle export change): for backward compatibility, can be removed if @@ -251,7 +251,7 @@ std::vector> Actuator::export_command_interfac } // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed - std::vector> interface_ptrs; + std::vector interface_ptrs; interface_ptrs.reserve(interfaces.size()); for (auto & interface : interfaces) { diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index b0893e9631..75f61b6649 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -609,7 +609,7 @@ class ResourceStorage void import_state_interfaces(HardwareT & hardware) { std::vector interface_names; - std::vector> interfaces = hardware.export_state_interfaces(); + std::vector interfaces = hardware.export_state_interfaces(); interface_names.reserve(interfaces.size()); for (auto const & interface : interfaces) @@ -633,24 +633,16 @@ class ResourceStorage available_state_interfaces_.capacity() + interface_names.size()); } - void insert_command_interface(const std::shared_ptr command_interface) + void insert_command_interface(const CommandInterface::SharedPtr command_interface) { const auto [it, success] = command_interface_map_.insert( std::make_pair(command_interface->get_name(), command_interface)); if (!success) { -<<<<<<< hardware_interfaces_getter_setter std::string msg( "ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" + command_interface->get_name() + "]"); throw std::runtime_error(msg); -======= - RCLCPP_ERROR( - get_logger(), - "Exception of type : %s occurred while importing state interfaces for the hardware '%s' : " - "%s", - typeid(e).name(), hardware.get_name().c_str(), e.what()); ->>>>>>> master } } @@ -676,8 +668,7 @@ class ResourceStorage { try { - std::vector> interfaces = - hardware.export_command_interfaces(); + std::vector interfaces = hardware.export_command_interfaces(); hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); @@ -700,7 +691,7 @@ class ResourceStorage } } - std::string add_state_interface(std::shared_ptr interface) + std::string add_state_interface(StateInterface::SharedPtr interface) { auto interface_name = interface->get_name(); const auto [it, success] = state_interface_map_.emplace(interface_name, interface); @@ -724,8 +715,7 @@ class ResourceStorage * \returns list of interface names that are added into internal storage. The output is used to * avoid additional iterations to cache interface names, e.g., for initializing info structures. */ - std::vector add_state_interfaces( - std::vector> & interfaces) + std::vector add_state_interfaces(std::vector & interfaces) { std::vector interface_names; interface_names.reserve(interfaces.size()); @@ -791,7 +781,7 @@ class ResourceStorage } std::vector add_command_interfaces( - const std::vector> & interfaces) + const std::vector & interfaces) { std::vector interface_names; interface_names.reserve(interfaces.size()); @@ -1091,9 +1081,9 @@ class ResourceStorage std::unordered_map> controllers_reference_interfaces_map_; /// Storage of all available state interfaces - std::map> state_interface_map_; + std::map state_interface_map_; /// Storage of all available command interfaces - std::map> command_interface_map_; + std::map command_interface_map_; /// Vectors with interfaces available to controllers (depending on hardware component state) std::vector available_state_interfaces_; @@ -1267,7 +1257,7 @@ void ResourceManager::import_controller_exported_state_interfaces( const std::string & controller_name, std::vector & interfaces) { std::lock_guard guard(resource_interfaces_lock_); - std::vector> interface_ptrs; + std::vector interface_ptrs; interface_ptrs.reserve(interfaces.size()); for (auto & interface : interfaces) { @@ -1336,7 +1326,7 @@ void ResourceManager::import_controller_reference_interfaces( std::vector & interfaces) { std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); - std::vector> interface_ptrs; + std::vector interface_ptrs; interface_ptrs.reserve(interfaces.size()); for (auto & interface : interfaces) { diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 891e83aa97..56001161a4 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -203,7 +203,7 @@ const rclcpp_lifecycle::State & Sensor::error() return impl_->get_lifecycle_state(); } -std::vector> Sensor::export_state_interfaces() +std::vector Sensor::export_state_interfaces() { // TODO(karsten1987): Might be worth to do some brief sanity check here // BEGIN (Handle export change): for backward compatibility, can be removed if @@ -222,7 +222,7 @@ std::vector> Sensor::export_state_interfaces() // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed - std::vector> interface_ptrs; + std::vector interface_ptrs; interface_ptrs.reserve(interfaces.size()); for (auto const & interface : interfaces) { diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index e500ab37b5..2df798e122 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -202,7 +202,7 @@ const rclcpp_lifecycle::State & System::error() return impl_->get_lifecycle_state(); } -std::vector> System::export_state_interfaces() +std::vector System::export_state_interfaces() { // TODO(karsten1987): Might be worth to do some brief sanity check here // BEGIN (Handle export change): for backward compatibility, can be removed if @@ -221,7 +221,7 @@ std::vector> System::export_state_interfaces() // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed - std::vector> interface_ptrs; + std::vector interface_ptrs; interface_ptrs.reserve(interfaces.size()); for (auto const & interface : interfaces) { @@ -231,7 +231,7 @@ std::vector> System::export_state_interfaces() // END: for backward compatibility } -std::vector> System::export_command_interfaces() +std::vector System::export_command_interfaces() { // TODO(karsten1987): Might be worth to do some brief sanity check here // BEGIN (Handle export change): for backward compatibility, can be removed if @@ -249,7 +249,7 @@ std::vector> System::export_command_interfaces } // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed - std::vector> interface_ptrs; + std::vector interface_ptrs; interface_ptrs.reserve(interfaces.size()); for (auto & interface : interfaces) { From b1e490b14ea20d40e3a904a18c7263a67fde1391 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 12 Sep 2024 08:23:59 +0100 Subject: [PATCH 07/12] Remove old TODOs --- hardware_interface/src/actuator.cpp | 2 -- hardware_interface/src/sensor.cpp | 1 - hardware_interface/src/system.cpp | 2 -- 3 files changed, 5 deletions(-) diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index a2d116a93c..aae16a79ee 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -206,7 +206,6 @@ const rclcpp_lifecycle::State & Actuator::error() std::vector Actuator::export_state_interfaces() { - // TODO(karsten1987): Might be worth to do some brief sanity check here // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed std::vector interfaces = impl_->export_state_interfaces(); @@ -235,7 +234,6 @@ std::vector Actuator::export_state_interfaces() std::vector Actuator::export_command_interfaces() { - // TODO(karsten1987): Might be worth to do some brief sanity check here // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed std::vector interfaces = impl_->export_command_interfaces(); diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 56001161a4..f3dc7efba5 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -205,7 +205,6 @@ const rclcpp_lifecycle::State & Sensor::error() std::vector Sensor::export_state_interfaces() { - // TODO(karsten1987): Might be worth to do some brief sanity check here // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed std::vector interfaces = impl_->export_state_interfaces(); diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 2df798e122..9f1a35d736 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -204,7 +204,6 @@ const rclcpp_lifecycle::State & System::error() std::vector System::export_state_interfaces() { - // TODO(karsten1987): Might be worth to do some brief sanity check here // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed std::vector interfaces = impl_->export_state_interfaces(); @@ -233,7 +232,6 @@ std::vector System::export_state_interfaces() std::vector System::export_command_interfaces() { - // TODO(karsten1987): Might be worth to do some brief sanity check here // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed std::vector interfaces = impl_->export_command_interfaces(); From 995db635d10002c7f8ff419e21710c35f632b164 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 16 Sep 2024 13:46:39 +0100 Subject: [PATCH 08/12] export_unlisted_state_interface_descriptions() --- hardware_interface/doc/writing_new_hardware_component.rst | 8 ++++---- .../include/hardware_interface/actuator_interface.hpp | 4 ++-- .../include/hardware_interface/sensor_interface.hpp | 4 ++-- .../include/hardware_interface/system_interface.hpp | 4 ++-- .../test/test_component_interfaces_custom_export.cpp | 6 +++--- 5 files changed, 13 insertions(+), 13 deletions(-) diff --git a/hardware_interface/doc/writing_new_hardware_component.rst b/hardware_interface/doc/writing_new_hardware_component.rst index ad3728c21a..fdd9503ebc 100644 --- a/hardware_interface/doc/writing_new_hardware_component.rst +++ b/hardware_interface/doc/writing_new_hardware_component.rst @@ -55,14 +55,14 @@ The following is a step-by-step guide to create source files, basic tests, and c #. Implement ``on_cleanup`` method, which does the opposite of ``on_configure``. #. ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which gets parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp `__). - * To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_command_interface_descriptions()`` or ``export_state_interface_descriptions()`` function by creating some custom ``Command-/StateInterfaces``. + * To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_command_interface_descriptions()`` or ``export_unlisted_state_interface_descriptions()`` function by creating some custom ``Command-/StateInterfaces``. * For the ``Sensor``-type hardware interface there is no ``export_command_interfaces`` method. * As a reminder, the full interface names have structure ``/``. #. (optional) If you want some unlisted ``Command-/StateInterfaces`` not included in the ``ros2_control`` XML-tag you can follow those steps: - #. Override the ``virtual std::vector export_command_interface_descriptions()`` or ``virtual std::vector export_state_interface_descriptions()`` - #. Create the InterfaceDescription for each of the interfaces you want to create in the override ``export_command_interface_descriptions()`` or ``export_state_interface_descriptions()`` function, add it to a vector and return the vector: + #. Override the ``virtual std::vector export_command_interface_descriptions()`` or ``virtual std::vector export_unlisted_state_interface_descriptions()`` + #. Create the InterfaceDescription for each of the interfaces you want to create in the override ``export_command_interface_descriptions()`` or ``export_unlisted_state_interface_descriptions()`` function, add it to a vector and return the vector: .. code-block:: c++ @@ -81,7 +81,7 @@ The following is a step-by-step guide to create source files, basic tests, and c #. (optional) In case the default implementation (``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` ) for exporting the ``Command-/StateInterfaces`` is not enough you can override them. You should however consider the following things: - * If you want to have unlisted interfaces available you need to call the ``export_command_interface_descriptions()`` or ``export_state_interface_descriptions()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. + * If you want to have unlisted interfaces available you need to call the ``export_command_interface_descriptions()`` or ``export_unlisted_state_interface_descriptions()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. * Don't forget to store the created ``Command-/StateInterfaces`` internally as you only return shared_ptrs and the resource manager will not provide access to the created ``Command-/StateInterfaces`` for the hardware. So you must take care of storing them yourself. * Names must be unique! diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index aae4a48406..c070a5a0c7 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -191,7 +191,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * \return vector of descriptions to the unlisted StateInterfaces */ virtual std::vector - export_state_interface_descriptions() + export_unlisted_state_interface_descriptions() { // return empty vector by default. return {}; @@ -208,7 +208,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod { // import the unlisted interfaces std::vector unlisted_interface_descriptions = - export_state_interface_descriptions(); + export_unlisted_state_interface_descriptions(); std::vector state_interfaces; state_interfaces.reserve( diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index ece5a6e7a8..9195694de4 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -175,7 +175,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return vector of descriptions to the unlisted StateInterfaces */ virtual std::vector - export_state_interface_descriptions() + export_unlisted_state_interface_descriptions() { // return empty vector by default. return {}; @@ -192,7 +192,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { // import the unlisted interfaces std::vector unlisted_interface_descriptions = - export_state_interface_descriptions(); + export_unlisted_state_interface_descriptions(); std::vector state_interfaces; state_interfaces.reserve( diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 658fffecd3..900c0de6ca 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -211,7 +211,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return vector of descriptions to the unlisted StateInterfaces */ virtual std::vector - export_state_interface_descriptions() + export_unlisted_state_interface_descriptions() { // return empty vector by default. return {}; @@ -228,7 +228,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { // import the unlisted interfaces std::vector unlisted_interface_descriptions = - export_state_interface_descriptions(); + export_unlisted_state_interface_descriptions(); std::vector state_interfaces; state_interfaces.reserve( diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index 04408e1d1e..72873b180b 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -57,7 +57,7 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface { std::string get_name() const override { return "DummyActuatorDefault"; } - std::vector export_state_interface_descriptions() + std::vector export_unlisted_state_interface_descriptions() override { std::vector interfaces; @@ -98,7 +98,7 @@ class DummySensorDefault : public hardware_interface::SensorInterface { std::string get_name() const override { return "DummySensorDefault"; } - std::vector export_state_interface_descriptions() + std::vector export_unlisted_state_interface_descriptions() override { std::vector interfaces; @@ -121,7 +121,7 @@ class DummySystemDefault : public hardware_interface::SystemInterface { std::string get_name() const override { return "DummySystemDefault"; } - std::vector export_state_interface_descriptions() + std::vector export_unlisted_state_interface_descriptions() override { std::vector interfaces; From 6a5430f8b89cbbe80f8f0471bd14591704288be1 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 16 Sep 2024 13:51:13 +0100 Subject: [PATCH 09/12] export_unlisted_command_interface_descriptions() --- hardware_interface/doc/writing_new_hardware_component.rst | 8 ++++---- .../include/hardware_interface/actuator_interface.hpp | 4 ++-- .../include/hardware_interface/system_interface.hpp | 4 ++-- .../test/test_component_interfaces_custom_export.cpp | 4 ++-- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/hardware_interface/doc/writing_new_hardware_component.rst b/hardware_interface/doc/writing_new_hardware_component.rst index fdd9503ebc..5452af6cda 100644 --- a/hardware_interface/doc/writing_new_hardware_component.rst +++ b/hardware_interface/doc/writing_new_hardware_component.rst @@ -55,14 +55,14 @@ The following is a step-by-step guide to create source files, basic tests, and c #. Implement ``on_cleanup`` method, which does the opposite of ``on_configure``. #. ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which gets parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp `__). - * To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_command_interface_descriptions()`` or ``export_unlisted_state_interface_descriptions()`` function by creating some custom ``Command-/StateInterfaces``. + * To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_unlisted_command_interface_descriptions()`` or ``export_unlisted_state_interface_descriptions()`` function by creating some custom ``Command-/StateInterfaces``. * For the ``Sensor``-type hardware interface there is no ``export_command_interfaces`` method. * As a reminder, the full interface names have structure ``/``. #. (optional) If you want some unlisted ``Command-/StateInterfaces`` not included in the ``ros2_control`` XML-tag you can follow those steps: - #. Override the ``virtual std::vector export_command_interface_descriptions()`` or ``virtual std::vector export_unlisted_state_interface_descriptions()`` - #. Create the InterfaceDescription for each of the interfaces you want to create in the override ``export_command_interface_descriptions()`` or ``export_unlisted_state_interface_descriptions()`` function, add it to a vector and return the vector: + #. Override the ``virtual std::vector export_unlisted_command_interface_descriptions()`` or ``virtual std::vector export_unlisted_state_interface_descriptions()`` + #. Create the InterfaceDescription for each of the interfaces you want to create in the override ``export_unlisted_command_interface_descriptions()`` or ``export_unlisted_state_interface_descriptions()`` function, add it to a vector and return the vector: .. code-block:: c++ @@ -81,7 +81,7 @@ The following is a step-by-step guide to create source files, basic tests, and c #. (optional) In case the default implementation (``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` ) for exporting the ``Command-/StateInterfaces`` is not enough you can override them. You should however consider the following things: - * If you want to have unlisted interfaces available you need to call the ``export_command_interface_descriptions()`` or ``export_unlisted_state_interface_descriptions()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. + * If you want to have unlisted interfaces available you need to call the ``export_unlisted_command_interface_descriptions()`` or ``export_unlisted_state_interface_descriptions()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. * Don't forget to store the created ``Command-/StateInterfaces`` internally as you only return shared_ptrs and the resource manager will not provide access to the created ``Command-/StateInterfaces`` for the hardware. So you must take care of storing them yourself. * Names must be unique! diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index c070a5a0c7..4c514fafe3 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -271,7 +271,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * \return vector of descriptions to the unlisted CommandInterfaces */ virtual std::vector - export_command_interface_descriptions() + export_unlisted_command_interface_descriptions() { // return empty vector by default. return {}; @@ -288,7 +288,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod { // import the unlisted interfaces std::vector unlisted_interface_descriptions = - export_command_interface_descriptions(); + export_unlisted_command_interface_descriptions(); std::vector command_interfaces; command_interfaces.reserve( diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 900c0de6ca..89cc23a3ff 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -306,7 +306,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return vector of descriptions to the unlisted CommandInterfaces */ virtual std::vector - export_command_interface_descriptions() + export_unlisted_command_interface_descriptions() { // return empty vector by default. return {}; @@ -323,7 +323,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { // import the unlisted interfaces std::vector unlisted_interface_descriptions = - export_command_interface_descriptions(); + export_unlisted_command_interface_descriptions(); std::vector command_interfaces; command_interfaces.reserve( diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index 72873b180b..c51e3a05ba 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -69,7 +69,7 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface return interfaces; } - std::vector export_command_interface_descriptions() + std::vector export_unlisted_command_interface_descriptions() override { std::vector interfaces; @@ -133,7 +133,7 @@ class DummySystemDefault : public hardware_interface::SystemInterface return interfaces; } - std::vector export_command_interface_descriptions() + std::vector export_unlisted_command_interface_descriptions() override { std::vector interfaces; From bec9c061308934eecd3b1aa8628a26978c2befc5 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 16 Sep 2024 13:57:45 +0100 Subject: [PATCH 10/12] Fix _2() variants in docs --- doc/migration.rst | 8 ++++---- doc/release_notes.rst | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/doc/migration.rst b/doc/migration.rst index 173cd5e095..7dbb952890 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -137,8 +137,8 @@ Migration of unlisted Command-/StateInterfaces not defined in ``ros2_control`` X -------------------------------------------------------------------------------------- If you want some unlisted ``Command-/StateInterfaces`` not included in the ``ros2_control`` XML-tag you can follow those steps: -1. Override the ``virtual std::vector export_command_interfaces_2()`` or ``virtual std::vector export_state_interfaces_2()`` -2. Create the InterfaceDescription for each of the interfaces you want to create in the override ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function, add it to a vector and return the vector: +1. Override the ``virtual std::vector export_unlisted_command_interfaces()`` or ``virtual std::vector export_unlisted_state_interfaces()`` +2. Create the InterfaceDescription for each of the interfaces you want to create in the override ``export_unlisted_command_interfaces()`` or ``export_unlisted_state_interfaces()`` function, add it to a vector and return the vector: .. code-block:: c++ @@ -159,6 +159,6 @@ Custom export of Command-/StateInterfaces ---------------------------------------------- In case the default implementation (``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` ) for exporting the ``Command-/StateInterfaces`` is not enough you can override them. You should however consider the following things: -* If you want to have unlisted interfaces available you need to call the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. -* Don't forget to store the created ``Command-/StateInterfaces`` internally as you only return ``shared_ptrs`` and the resource manager will not provide access to the created ``Command-/StateInterfaces`` for the hardware. So you must take care of storing them yourself. +* If you want to have unlisted interfaces available you need to call the ``export_unlisted_command_interfaces()`` or ``export_unlisted_state_interfaces()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. +* Don't forget to store the created ``Command-/StateInterfaces`` internally as you only return ``std::shared_ptr``s and the resource manager will not provide access to the created ``Command-/StateInterface``s for the hardware. So you must take care of storing them yourself. * Names must be unique! diff --git a/doc/release_notes.rst b/doc/release_notes.rst index aad06ce61f..8320a81705 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -117,7 +117,7 @@ Changes from `(PR #1688) `__). * The memory for storing the value of a ``Command-/StateInterfaces`` is no longer allocated in the hardware but instead in the ``Command-/StateInterfaces`` itself. -* To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function by creating some custom ``Command-/StateInterfaces``. +* To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_unlisted_command_interfaces()`` or ``export_unlisted_state_interfaces()`` function by creating some custom ``Command-/StateInterfaces``. ros2controlcli From a369f969bd8ee2c35532845d9ed699bf294d2d47 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 16 Sep 2024 14:08:30 +0100 Subject: [PATCH 11/12] shorten comments --- .../include/hardware_interface/sensor_interface.hpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 9195694de4..3a24e74295 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -198,8 +198,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI state_interfaces.reserve( unlisted_interface_descriptions.size() + sensor_state_interfaces_.size()); - // add InterfaceDescriptions and create the StateInterfaces from the descriptions and add to - // maps. + // add InterfaceDescriptions and create StateInterfaces from the descriptions and add to maps. for (const auto & description : unlisted_interface_descriptions) { auto name = description.get_name(); @@ -212,8 +211,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI for (const auto & [name, descr] : sensor_state_interfaces_) { - // TODO(Manuel) maybe check for duplicates otherwise only the first appearance of "name" is - // inserted + // TODO(Manuel) check for duplicates otherwise only the first appearance of "name" is inserted auto state_interface = std::make_shared(descr); sensor_states_map_.insert(std::make_pair(name, state_interface)); sensor_states_.push_back(state_interface); From 4c63d28731c8600ed32faa5db1f26adc9fa93260 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 16 Sep 2024 14:09:45 +0100 Subject: [PATCH 12/12] Remove comments about renaming function --- .../include/hardware_interface/actuator_interface.hpp | 6 ------ .../include/hardware_interface/sensor_interface.hpp | 3 --- .../include/hardware_interface/system_interface.hpp | 6 ------ 3 files changed, 15 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 4c514fafe3..123576d686 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -185,9 +185,6 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * Override this method to export custom StateInterfaces which are not defined in the URDF file. * Those interfaces will be added to the unlisted_state_interfaces_ map. * - * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated - * version is removed. - * * \return vector of descriptions to the unlisted StateInterfaces */ virtual std::vector @@ -265,9 +262,6 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * Override this method to export custom CommandInterfaces which are not defined in the URDF file. * Those interfaces will be added to the unlisted_command_interfaces_ map. * - * Note method name is going to be changed to export_command_interfaces() as soon as the - * deprecated version is removed. - * * \return vector of descriptions to the unlisted CommandInterfaces */ virtual std::vector diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 3a24e74295..5d859cc4f1 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -169,9 +169,6 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * Override this method to export custom StateInterfaces which are not defined in the URDF file. * Those interfaces will be added to the unlisted_state_interfaces_ map. * - * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated - * version is removed. - * * \return vector of descriptions to the unlisted StateInterfaces */ virtual std::vector diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 89cc23a3ff..4814f6f559 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -205,9 +205,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * Override this method to export custom StateInterfaces which are not defined in the URDF file. * Those interfaces will be added to the unlisted_state_interfaces_ map. * - * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated - * version is removed. - * * \return vector of descriptions to the unlisted StateInterfaces */ virtual std::vector @@ -300,9 +297,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * Override this method to export custom CommandInterfaces which are not defined in the URDF file. * Those interfaces will be added to the unlisted_command_interfaces_ map. * - * Note method name is going to be changed to export_command_interfaces() as soon as the - * deprecated version is removed. - * * \return vector of descriptions to the unlisted CommandInterfaces */ virtual std::vector