diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 2bdccefdc5..a40a7a4088 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -15,6 +15,9 @@ #ifndef CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ #define CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ +#include +#include +#include #include #include "controller_interface/controller_interface_base.hpp" @@ -56,7 +59,8 @@ class ChainableControllerInterface : public ControllerInterfaceBase bool is_chainable() const final; CONTROLLER_INTERFACE_PUBLIC - std::vector export_reference_interfaces() final; + std::vector> export_reference_interfaces() + final; CONTROLLER_INTERFACE_PUBLIC bool set_chained_mode(bool chained_mode) final; @@ -115,7 +119,11 @@ class ChainableControllerInterface : public ControllerInterfaceBase const rclcpp::Time & time, const rclcpp::Duration & period) = 0; /// Storage of values for reference interfaces + // BEGIN (Handle export change): for backward compatibility std::vector reference_interfaces_; + // END + std::unordered_map> + reference_interfaces_ptrs_; private: /// A flag marking if a chainable controller is currently preceded by another controller. diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index a3d006755f..9614ab46a5 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -48,7 +48,8 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase * \returns empty list. */ CONTROLLER_INTERFACE_PUBLIC - std::vector export_reference_interfaces() final; + std::vector> export_reference_interfaces() + final; /** * Controller is not chainable, therefore no chained mode can be set. diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 1c09f71e98..888d499688 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -222,7 +222,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * \returns list of command interfaces for preceding controllers. */ CONTROLLER_INTERFACE_PUBLIC - virtual std::vector export_reference_interfaces() = 0; + virtual std::vector> + export_reference_interfaces() = 0; /** * Set chained mode of a chainable controller. This method triggers internal processes to switch diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 2f7c67741e..7497f6d5a6 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -44,44 +44,63 @@ return_type ChainableControllerInterface::update( return ret; } -std::vector +std::vector> ChainableControllerInterface::export_reference_interfaces() { auto reference_interfaces = on_export_reference_interfaces(); + std::vector> reference_interfaces_ptrs_vec; + reference_interfaces_ptrs_vec.reserve(reference_interfaces.size()); + // BEGIN (Handle export change): for backward compatibility // check if the "reference_interfaces_" variable is resized to number of interfaces if (reference_interfaces_.size() != reference_interfaces.size()) { // TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the // framework - RCLCPP_FATAL( - get_node()->get_logger(), - "The internal storage for reference values 'reference_interfaces_' variable has size '%zu', " - "but it is expected to have the size '%zu' equal to the number of exported reference " - "interfaces. No reference interface will be exported. Please correct and recompile " - "the controller with name '%s' and try again.", - reference_interfaces_.size(), reference_interfaces.size(), get_node()->get_name()); - reference_interfaces.clear(); + std::string error_msg = + "The internal storage for reference values 'reference_interfaces_' variable has size '" + + std::to_string(reference_interfaces_.size()) + "', but it is expected to have the size '" + + std::to_string(reference_interfaces.size()) + + "' equal to the number of exported reference interfaces. Please correct and recompile the " + "controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); } + // END // check if the names of the reference interfaces begin with the controller's name - for (const auto & interface : reference_interfaces) + const auto ref_interface_size = reference_interfaces.size(); + for (auto & interface : reference_interfaces) { if (interface.get_prefix_name() != get_node()->get_name()) { - RCLCPP_FATAL( - get_node()->get_logger(), - "The name of the interface '%s' does not begin with the controller's name. This is " - "mandatory " - " for reference interfaces. No reference interface will be exported. Please correct and " - "recompile the controller with name '%s' and try again.", - interface.get_name().c_str(), get_node()->get_name()); - reference_interfaces.clear(); - break; + std::string error_msg = "The name of the interface " + interface.get_name() + + " does not begin with the controller's name. This is mandatory for " + "reference interfaces. Please " + "correct and recompile the controller with name " + + get_node()->get_name() + " and try again."; + throw std::runtime_error(error_msg); } + + std::shared_ptr interface_ptr = + std::make_shared(std::move(interface)); + reference_interfaces_ptrs_vec.push_back(interface_ptr); + reference_interfaces_ptrs_.insert(std::make_pair(interface_ptr->get_name(), interface_ptr)); + } + + if (reference_interfaces_ptrs_.size() != ref_interface_size) + { + std::string error_msg = + "The internal storage for reference ptrs 'reference_interfaces_ptrs_' variable has size '" + + std::to_string(reference_interfaces_ptrs_.size()) + + "', but it is expected to have the size '" + std::to_string(ref_interface_size) + + "' equal to the number of exported reference interfaces. Please correct and recompile the " + "controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); } - return reference_interfaces; + return reference_interfaces_ptrs_vec; } bool ChainableControllerInterface::set_chained_mode(bool chained_mode) diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 392a48ffa4..7e7f563bd4 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -28,7 +28,8 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {} bool ControllerInterface::is_chainable() const { return false; } -std::vector ControllerInterface::export_reference_interfaces() +std::vector> +ControllerInterface::export_reference_interfaces() { return {}; } diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 47487e019c..ef951f376c 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -45,10 +45,10 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), 1u); - EXPECT_EQ(reference_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); - EXPECT_EQ(reference_interfaces[0].get_interface_name(), "test_itf"); + EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(reference_interfaces[0]->get_interface_name(), "test_itf"); - EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size) @@ -103,7 +103,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_FALSE(controller.is_in_chained_mode()); // Fail setting chained mode - EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); EXPECT_FALSE(controller.set_chained_mode(true)); EXPECT_FALSE(controller.is_in_chained_mode()); @@ -112,7 +112,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_FALSE(controller.is_in_chained_mode()); // Success setting chained mode - reference_interfaces[0].set_value(0.0); + reference_interfaces[0]->set_value(0.0); EXPECT_TRUE(controller.set_chained_mode(true)); EXPECT_TRUE(controller.is_in_chained_mode()); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 3b84fc07e4..9809c48cb2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -740,13 +740,27 @@ controller_interface::return_type ControllerManager::configure_controller( get_logger(), "Controller '%s' is chainable. Interfaces are being exported to resource manager.", controller_name.c_str()); - auto interfaces = controller->export_reference_interfaces(); - if (interfaces.empty()) + + std::vector> interfaces; + try { - // TODO(destogl): Add test for this! - RCLCPP_ERROR( - get_logger(), "Controller '%s' is chainable, but does not export any reference interfaces.", - controller_name.c_str()); + interfaces = controller->export_reference_interfaces(); + if (interfaces.empty()) + { + // TODO(destogl): Add test for this! + RCLCPP_ERROR( + get_logger(), + "Controller '%s' is chainable, but does not export any reference interfaces. Did you " + "override the on_export_method() correctly?", + controller_name.c_str()); + return controller_interface::return_type::ERROR; + } + } + catch (const std::runtime_error & e) + { + RCLCPP_FATAL( + get_logger(), "Creation of the reference interfaces failed with following error: %s", + e.what()); return controller_interface::return_type::ERROR; } resource_manager_->import_controller_reference_interfaces(controller_name, interfaces); diff --git a/doc/migration/Iron.rst b/doc/migration/Iron.rst new file mode 100644 index 0000000000..3a463c736a --- /dev/null +++ b/doc/migration/Iron.rst @@ -0,0 +1,140 @@ +Iron to Jazzy +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +component parser +***************** +Changes from `(PR #1256) `__ + +* All ``joints`` defined in the ````-tag have to be present in the URDF received :ref:`by the controller manager `, otherwise a ``std::runtime_error`` is thrown. This is to ensure that the URDF and the ````-tag are consistent. E.g., for configuration ports use ``gpio`` tags instead. +* The syntax for mimic joints is changed to the `official URDF specification `__. The parameters within the ``ros2_control`` tag are not supported any more. Instead of + + .. code-block:: xml + + + + + + 0.15 + + + + + + right_finger_joint + 1 + + + + + + + + define your mimic joints directly in the joint definitions: + + .. code-block:: xml + + + + + + + + + + + + + + + + + +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 c557a4970f..8c22947b96 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..21c55cb311 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_interfaces_2()`` or ``export_state_interfaces_2()`` 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_interfaces_2()`` or ``virtual std::vector export_state_interfaces_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; + + #. 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_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! - 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 b23b913d75..2ff7fc0dbd 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -65,10 +65,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 1ae05aa5f7..06cd0be869 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -15,10 +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" @@ -97,31 +101,191 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod 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_interfaces_2() + { + // 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_interfaces_2(); + + 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_interfaces_2() + { + // 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_interfaces_2(); + 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. @@ -208,9 +372,39 @@ 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(); + } + 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: + 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 d5d999cca8..bb68e72b12 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -33,5 +33,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 1aea017754..72b1d6cf92 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,44 +15,67 @@ #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 { + +typedef std::variant HANDLE_DATATYPE; + /// A handle used to get and set a value on a given interface. -class ReadOnlyHandle +class Handle { public: - ReadOnlyHandle( + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + + Handle( const std::string & prefix_name, const std::string & interface_name, double * value_ptr = nullptr) : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) { } - explicit ReadOnlyHandle(const std::string & interface_name) + 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) { } - explicit ReadOnlyHandle(const char * interface_name) + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + + explicit Handle(const char * interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } - ReadOnlyHandle(const ReadOnlyHandle & other) = default; + Handle(const Handle & other) = default; - ReadOnlyHandle(ReadOnlyHandle && other) = default; + Handle(Handle && other) = default; - ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default; + Handle & operator=(const Handle & other) = default; - ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default; + Handle & operator=(Handle && other) = default; - virtual ~ReadOnlyHandle() = default; + virtual ~Handle() = default; /// Returns true if handle references a value. inline operator bool() const { return value_ptr_ != nullptr; } @@ -72,60 +95,54 @@ class ReadOnlyHandle double get_value() const { + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) return value_ if old functionality is removed THROW_ON_NULLPTR(value_ptr_); return *value_ptr_; + // END + } + + void set_value(double value) + { + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) set value_ directly if old functionality is removed + THROW_ON_NULLPTR(this->value_ptr_); + *this->value_ptr_ = value; + // END } protected: std::string prefix_name_; std::string interface_name_; + HANDLE_DATATYPE value_; + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed double * value_ptr_; + // END }; -class ReadWriteHandle : public ReadOnlyHandle +class StateInterface : public Handle { public: - ReadWriteHandle( - const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : ReadOnlyHandle(prefix_name, interface_name, value_ptr) - { - } - - explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {} - - explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(interface_name) {} - - ReadWriteHandle(const ReadWriteHandle & other) = default; - - ReadWriteHandle(ReadWriteHandle && other) = default; - - ReadWriteHandle & operator=(const ReadWriteHandle & other) = default; - - ReadWriteHandle & operator=(ReadWriteHandle && other) = default; - - virtual ~ReadWriteHandle() = default; - - void set_value(double value) + explicit StateInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) { - THROW_ON_NULLPTR(this->value_ptr_); - *this->value_ptr_ = value; } -}; -class StateInterface : public ReadOnlyHandle -{ -public: StateInterface(const StateInterface & other) = default; StateInterface(StateInterface && other) = default; - using ReadOnlyHandle::ReadOnlyHandle; + using Handle::Handle; }; -class CommandInterface : public ReadWriteHandle +class CommandInterface : public Handle { public: + explicit CommandInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) + { + } /// CommandInterface copy constructor is actively deleted. /** * Command interfaces are having a unique ownership and thus @@ -136,7 +153,7 @@ class CommandInterface : public ReadWriteHandle CommandInterface(CommandInterface && other) = default; - using ReadWriteHandle::ReadWriteHandle; + using Handle::Handle; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 2bd2099e69..6abc8b56d7 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -40,12 +40,16 @@ 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; + /// (Optional) Key-value pairs of command/stateInterface parameters. This is + /// useful for drivers that operate on protocols like modbus, where each + /// interface needs own address(register), datatype, ... + std::unordered_map parameters; }; /// @brief This structure stores information about a joint that is mimicking another joint @@ -64,7 +68,6 @@ enum class MimicAttribute TRUE, FALSE }; - /** * This structure stores information about components defined for a specific hardware * in robot's URDF. @@ -126,6 +129,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 31def4598b..95c5d31283 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -145,7 +145,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 4c267bef77..bc96b913a9 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -66,7 +66,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 5a3601afa8..6057267ead 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -15,10 +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" @@ -97,19 +101,101 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI 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_interfaces_2() + { + // 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_interfaces_2(); + + 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. /** @@ -147,9 +233,26 @@ 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(); + } + protected: HardwareInfo info_; + + std::unordered_map sensor_state_interfaces_; + std::unordered_map unlisted_state_interfaces_; + rclcpp_lifecycle::State lifecycle_state_; + +private: + 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 fb28929948..ac5b4649a4 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -66,10 +66,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 6b2c38b915..864f6f44b2 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -15,13 +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" @@ -98,30 +103,228 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI 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_interfaces_2() + { + // 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_interfaces_2(); + + 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_interfaces_2() + { + // 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_interfaces_2(); + + 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. /** @@ -209,9 +412,44 @@ 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(); + } + 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: + 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 b80f76ebf5..d5fbb520d5 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -186,16 +186,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 ef585c971b..c0250d418a 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -314,6 +314,13 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml( interface.data_type = "double"; interface.size = 1; + // Parse parameters + const auto * params_it = interfaces_it->FirstChildElement(kParamTag); + if (params_it) + { + interface.parameters = parse_parameters_from_xml(params_it); + } + return interface; } @@ -797,11 +804,15 @@ std::vector parse_control_resources_from_urdf(const std::string & tinyxml2::XMLDocument doc; if (!doc.Parse(urdf.c_str()) && doc.Error()) { - throw std::runtime_error("invalid URDF passed in to robot parser"); + throw std::runtime_error( + "invalid URDF passed in to robot parser. Could not parse it because of following error:" + + std::string(doc.ErrorStr())); } if (doc.Error()) { - throw std::runtime_error("invalid URDF passed in to robot parser"); + throw std::runtime_error( + "invalid URDF passed in to robot parser. Following error occurred:" + + std::string(doc.ErrorStr())); } // Find robot tag @@ -949,4 +960,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/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 162c3aa60d..055ccc4eb8 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -521,6 +521,11 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur joint_states_[ACCELERATION_INTERFACE_INDEX][j] = (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); } + else + { + joint_states_[VELOCITY_INTERFACE_INDEX][j] = 0.0; + joint_states_[ACCELERATION_INTERFACE_INDEX][j] = 0.0; + } break; } case POSITION_INTERFACE_INDEX: diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index d77f915eee..39d204f5b7 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -34,6 +34,10 @@ #include "hardware_interface/system.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limiter_struct.hpp" +#include "joint_limits/joint_saturation_limiter.hpp" +#include "joint_limits/joint_soft_limiter.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "pluginlib/class_loader.hpp" #include "rcutils/logging_macros.h" @@ -600,59 +604,191 @@ 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) + const auto [it, success] = + state_interface_map_.insert(std::make_pair(interface->get_name(), interface)); + if (!success) { - auto key = interface.get_name(); - state_interface_map_.emplace(std::make_pair(key, std::move(interface))); - interface_names.push_back(key); + std::string msg( + "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" + + interface->get_name() + "]"); + throw std::runtime_error(msg); } - hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; - available_state_interfaces_.reserve( - available_state_interfaces_.capacity() + interface_names.size()); + interface_names.push_back(interface->get_name()); } - 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) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", - "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) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", - "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 + std::vector> interfaces = + hardware.export_command_interfaces(); + + hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); + } + + void import_joint_limiters(const std::vector & hardware_infos) + { + for (const auto & hw_info : hardware_infos) { - auto interfaces = hardware.export_command_interfaces(); - hardware_info_map_[hardware.get_name()].command_interfaces = - add_command_interfaces(interfaces); + limiters_data_[hw_info.name] = {}; + for (const auto & [joint_name, limits] : hw_info.limits) + { + RCUTILS_LOG_INFO_NAMED( + "resource_manager", "%s with limits:%s", joint_name.c_str(), limits.to_string().c_str()); + + std::unique_ptr< + joint_limits::JointLimiterInterface> + limits_interface; + const joint_limits::JointLimits hard_limit{limits}; + joint_limits::JointInterfacesCommandLimiterData data; + data.joint_name = joint_name; + limiters_data_[hw_info.name].push_back(data); + // Check if soft limits exist. If yes then extract them and use soft limiter + if (hw_info.soft_limits.find(joint_name) != hw_info.soft_limits.end()) + { + std::vector soft_limits = { + hw_info.soft_limits.at(joint_name)}; + limits_interface = std::make_unique(); + limits_interface->init({joint_name}, {hard_limit}, soft_limits, nullptr, nullptr); + } + // no soft limits given so we use the joint saturation limiter (joint_range_limiter.hpp) + else + { + limits_interface = std::make_unique< + joint_limits::JointSaturationLimiter>(); + limits_interface->init({joint_name}, {hard_limit}, {}, nullptr, nullptr); + } + joint_limiters_interface_[hw_info.name].push_back(std::move(limits_interface)); + } } - catch (const std::exception & ex) + } + + template + void update_joint_limiters_state( + const std::string & joint_name, const std::map & interface_map, + joint_limits::JointControlInterfacesData & state) + { + state.joint_name = joint_name; + // update the actual state of the limiters + if ( + interface_map.find(joint_name + "/" + hardware_interface::HW_IF_POSITION) != + interface_map.end()) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", - "Exception occurred while importing command interfaces for the hardware '%s' : %s", - hardware.get_name().c_str(), ex.what()); + state.position = + interface_map.at(joint_name + "/" + hardware_interface::HW_IF_POSITION)->get_value(); } - catch (...) + if ( + interface_map.find(joint_name + "/" + hardware_interface::HW_IF_VELOCITY) != + interface_map.end()) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", - "Unknown exception occurred while importing command interfaces for the hardware '%s'", - hardware.get_name().c_str()); + state.velocity = + interface_map.at(joint_name + "/" + hardware_interface::HW_IF_VELOCITY)->get_value(); + } + if ( + interface_map.find(joint_name + "/" + hardware_interface::HW_IF_EFFORT) != + interface_map.end()) + { + state.effort = + interface_map.at(joint_name + "/" + hardware_interface::HW_IF_EFFORT)->get_value(); + } + if ( + interface_map.find(joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) != + interface_map.end()) + { + state.acceleration = + interface_map.at(joint_name + "/" + hardware_interface::HW_IF_ACCELERATION)->get_value(); + } + } + + template + void update_joint_limiters_commands( + const joint_limits::JointControlInterfacesData & state, + std::map & interface_map) + { + if ( + interface_map.find(state.joint_name + "/" + hardware_interface::HW_IF_POSITION) != + interface_map.end() && + state.position.has_value()) + { + interface_map.at(state.joint_name + "/" + hardware_interface::HW_IF_POSITION) + ->set_value(state.position.value()); + } + if ( + interface_map.find(state.joint_name + "/" + hardware_interface::HW_IF_VELOCITY) != + interface_map.end() && + state.velocity.has_value()) + { + interface_map.at(state.joint_name + "/" + hardware_interface::HW_IF_VELOCITY) + ->set_value(state.velocity.value()); + } + if ( + interface_map.find(state.joint_name + "/" + hardware_interface::HW_IF_EFFORT) != + interface_map.end() && + state.effort.has_value()) + { + interface_map.at(state.joint_name + "/" + hardware_interface::HW_IF_EFFORT) + ->set_value(state.effort.value()); + } + if ( + interface_map.find(state.joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) != + interface_map.end() && + state.acceleration.has_value()) + { + interface_map.at(state.joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) + ->set_value(state.acceleration.value()); + } + } + + void update_joint_limiters_data() + { + for (auto & [hw_info_name, joint_limiter_data] : limiters_data_) + { + for (auto & data : joint_limiter_data) + { + update_joint_limiters_state(data.joint_name, state_interface_map_, data.actual); + update_joint_limiters_state(data.joint_name, command_interface_map_, data.command); + data.limited = data.command; + } } } @@ -674,7 +810,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); } @@ -942,9 +1096,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_; @@ -956,6 +1110,14 @@ class ResourceStorage /// List of async components by type std::unordered_map async_component_threads_; + std::unordered_map> + limiters_data_; + + std::unordered_map< + std::string, std::vector>>> + joint_limiters_interface_; + // Update rate of the controller manager, and the clock interface of its node // Used by async components. unsigned int cm_update_rate_; @@ -998,6 +1160,7 @@ void ResourceManager::load_urdf( const std::string actuator_type = "actuator"; const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + resource_storage_->import_joint_limiters(hardware_info); if (load_and_initialize_components) { for (const auto & individual_hardware_info : hardware_info) @@ -1043,7 +1206,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 @@ -1077,7 +1240,8 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con // 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); @@ -1214,7 +1378,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)); } @@ -1664,6 +1828,22 @@ HardwareReadWriteStatus ResourceManager::write( read_write_status.ok = true; read_write_status.failed_hardware_names.clear(); + // Joint Limiters operations + { + resource_storage_->update_joint_limiters_data(); + for (auto & [hw_name, limiters] : resource_storage_->joint_limiters_interface_) + { + for (size_t i = 0; i < limiters.size(); i++) + { + joint_limits::JointInterfacesCommandLimiterData & data = + resource_storage_->limiters_data_[hw_name][i]; + limiters[i]->enforce(data.actual, data.limited, period); + resource_storage_->update_joint_limiters_commands( + data.limited, resource_storage_->command_interface_map_); + } + } + } + auto write_components = [&](auto & components) { for (auto & component : components) diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 2da627f892..dafe8cb959 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -184,9 +184,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 8e950faa89..4b04462101 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -184,14 +184,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 c7777b3e21..fe0a239d9f 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 6dc1c394b0..ef856f58f5 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -33,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 @@ -49,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 @@ -155,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 @@ -219,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 @@ -353,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"; } @@ -420,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 +691,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 +713,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 +728,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 +743,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 +758,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 +769,207 @@ 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]; + auto state = actuator_hw.initialize(dummy_actuator); - hardware_interface::HardwareInfo mock_hw_info{}; - auto state = sensor_hw.initialize(mock_hw_info); 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) -{ - hardware_interface::System system_hw(std::make_unique()); + 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{}; + auto state = sensor_hw.initialize(mock_hw_info); + 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]; + auto state = sensor_hw.initialize(voltage_sensor_res); + 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{}; auto state = system_hw.initialize(mock_hw_info); @@ -552,41 +978,41 @@ 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()); + 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 + 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 @@ -594,12 +1020,12 @@ TEST(TestComponentInterfaces, dummy_system) { 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[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)); } @@ -613,12 +1039,12 @@ 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[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)); } @@ -632,12 +1058,12 @@ 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[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)); } @@ -651,12 +1077,212 @@ 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[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]; + auto state = system_hw.initialize(dummy_system); + 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()); + { + 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[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[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)); + } + + 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[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)); + } + + 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[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)); + } + + 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[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)); } @@ -696,6 +1322,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()); @@ -728,8 +1355,8 @@ 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()); @@ -754,7 +1381,79 @@ TEST(TestComponentInterfaces, dummy_actuator_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_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]; + auto state = actuator_hw.initialize(dummy_actuator); + + 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()); + 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()); +} + +// BEGIN (Handle export change): for backward compatibility TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) { hardware_interface::Actuator actuator_hw(std::make_unique()); @@ -787,8 +1486,78 @@ 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]; + auto state = actuator_hw.initialize(dummy_actuator); + 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()); @@ -814,6 +1583,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()); @@ -849,7 +1619,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()); @@ -877,7 +1647,68 @@ 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]; + auto state = sensor_hw.initialize(voltage_sensor_res); + + 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()); @@ -912,11 +1743,82 @@ 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]; + auto state = system_hw.initialize(dummy_system); + 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()); @@ -942,6 +1844,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()); @@ -976,11 +1879,82 @@ 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]; + auto state = system_hw.initialize(dummy_system); + 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..7a5011892b --- /dev/null +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -0,0 +1,369 @@ +// 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_interfaces_2() 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_interfaces_2() 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_interfaces_2() 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_interfaces_2() 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_interfaces_2() 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]; + auto state = actuator_hw.initialize(dummy_actuator); + + 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]; + auto state = sensor_hw.initialize(voltage_sensor_res); + 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]; + auto state = system_hw.initialize(dummy_system); + 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 2e2cae9807..fff2e45323 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -297,6 +297,108 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens } } +TEST_F( + TestComponentParser, + successfully_parse_valid_urdf_system_multi_interface_custom_interface_parameters) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets:: + valid_urdf_ros2_control_system_multi_interface_and_custom_interface_parameters + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + const auto hardware_info = control_hardware.front(); + + EXPECT_EQ(hardware_info.name, "RRBotSystemMultiInterface"); + EXPECT_EQ(hardware_info.type, "system"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, + "ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware"); + ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); + EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2"); + EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); + + ASSERT_THAT(hardware_info.joints, SizeIs(2)); + + EXPECT_EQ(hardware_info.joints[0].name, "joint1"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + EXPECT_EQ(hardware_info.joints[0].parameters.size(), 3); + EXPECT_EQ(hardware_info.joints[0].parameters.at("modbus_server_ip"), "1.1.1.1"); + EXPECT_EQ(hardware_info.joints[0].parameters.at("modbus_server_port"), "1234"); + EXPECT_EQ(hardware_info.joints[0].parameters.at("use_persistent_connection"), "true"); + ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(3)); + ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(3)); + // CommandInterfaces of joints + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].initial_value, "1.2"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-1"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "1"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].parameters.size(), 5); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].parameters.at("register"), "1"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].parameters.at("register_size"), "2"); + + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].initial_value, "3.4"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].min, "-1.5"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].max, "1.5"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].parameters.size(), 5); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].parameters.at("register"), "2"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].parameters.at("register_size"), "4"); + + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].min, "-0.5"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].max, "0.5"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].initial_value, ""); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].parameters.size(), 2); + + // StateInterfaces of joints + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].parameters.at("register"), "3"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].parameters.at("register_size"), "2"); + + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].parameters.at("register"), "4"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].parameters.at("register_size"), "4"); + + EXPECT_EQ(hardware_info.joints[0].state_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[2].parameters.size(), 0); + + // Second Joint + EXPECT_EQ(hardware_info.joints[1].name, "joint2"); + EXPECT_EQ(hardware_info.joints[1].type, "joint"); + EXPECT_EQ(hardware_info.joints[1].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[1].parameters.at("modbus_server_ip"), "192.168.178.123"); + EXPECT_EQ(hardware_info.joints[1].parameters.at("modbus_server_port"), "4321"); + ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1)); + ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(3)); + // CommandInterfaces + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].initial_value, ""); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].min, "-1"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].max, "1"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].parameters.size(), 4); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].parameters.at("register"), "20"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].parameters.at("data_type"), "int32_t"); + // StateInterfaces of joints + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].parameters.at("register"), "21"); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].parameters.at("data_type"), "int32_t"); + + EXPECT_EQ(hardware_info.joints[1].state_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[1].parameters.size(), 0); + + EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].parameters.size(), 2); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].parameters.at("register"), "21"); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].parameters.at("data_type"), "int32_t"); +} + TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_external_sensor) { std::string urdf_to_test = @@ -1433,3 +1535,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 c8b077229e..5a7eeb53a0 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1186,12 +1186,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/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index 3e296cd584..f88e308b21 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -7,12 +7,16 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") endif() set(THIS_PACKAGE_INCLUDE_DEPENDS + pluginlib + realtime_tools rclcpp rclcpp_lifecycle + trajectory_msgs urdf ) find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) find_package(ament_cmake_gen_version_h REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -26,10 +30,56 @@ target_include_directories(joint_limits INTERFACE ) ament_target_dependencies(joint_limits INTERFACE ${THIS_PACKAGE_INCLUDE_DEPENDS}) +add_library(joint_limiter_interface SHARED + src/joint_limiter_interface.cpp +) +target_compile_features(joint_limiter_interface PUBLIC cxx_std_17) +target_include_directories(joint_limiter_interface PUBLIC + $ + $ +) +ament_target_dependencies(joint_limiter_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(joint_limiter_interface PRIVATE "JOINT_LIMITS_BUILDING_DLL") + +add_library(joint_limits_helpers SHARED + src/joint_limits_helpers.cpp +) +target_compile_features(joint_limits_helpers PUBLIC cxx_std_17) +target_include_directories(joint_limits_helpers PUBLIC + $ + $ +) +ament_target_dependencies(joint_limits_helpers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + + +add_library(joint_saturation_limiter SHARED + src/joint_saturation_limiter.cpp + src/joint_range_limiter.cpp + src/joint_soft_limiter.cpp +) +target_compile_features(joint_saturation_limiter PUBLIC cxx_std_17) +target_include_directories(joint_saturation_limiter PUBLIC + $ + $ +) +target_link_libraries(joint_saturation_limiter PUBLIC joint_limits_helpers) + +ament_target_dependencies(joint_saturation_limiter PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(joint_saturation_limiter PRIVATE "JOINT_LIMITS_BUILDING_DLL") + +pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + find_package(generate_parameter_library REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) + find_package(pluginlib REQUIRED) + find_package(rclcpp REQUIRED) ament_add_gtest_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) target_link_libraries(joint_limits_rosparam_test joint_limits) @@ -43,16 +93,50 @@ if(BUILD_TESTING) DESTINATION lib/joint_limits ) install( - FILES test/joint_limits_rosparam.yaml + FILES test/joint_limits_rosparam.yaml test/joint_saturation_limiter_param.yaml DESTINATION share/joint_limits/test ) + + add_rostest_with_parameters_gmock(test_joint_saturation_limiter test/test_joint_saturation_limiter.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/joint_saturation_limiter_param.yaml + ) + target_include_directories(test_joint_saturation_limiter PRIVATE include) + target_link_libraries(test_joint_saturation_limiter joint_limiter_interface) + ament_target_dependencies( + test_joint_saturation_limiter + pluginlib + rclcpp + ) + + ament_add_gtest(test_joint_range_limiter test/test_joint_range_limiter.cpp) + target_include_directories(test_joint_range_limiter PRIVATE include) + target_link_libraries(test_joint_range_limiter joint_limiter_interface) + ament_target_dependencies( + test_joint_range_limiter + pluginlib + rclcpp + ) + + ament_add_gtest(test_joint_soft_limiter test/test_joint_soft_limiter.cpp) + target_include_directories(test_joint_soft_limiter PRIVATE include) + target_link_libraries(test_joint_soft_limiter joint_limiter_interface) + ament_target_dependencies( + test_joint_soft_limiter + pluginlib + rclcpp + ) + endif() install( DIRECTORY include/ DESTINATION include/joint_limits ) -install(TARGETS joint_limits +install(TARGETS + joint_limits + joint_limiter_interface + joint_saturation_limiter + joint_limits_helpers EXPORT export_joint_limits ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp new file mode 100644 index 0000000000..d6b61abfd2 --- /dev/null +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -0,0 +1,273 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. + +/// \author Denis Stogl + +#ifndef JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ +#define JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ + +#include +#include + +#include "joint_limits.hpp" +#include "joint_limits/joint_limits.hpp" +#include "joint_limits/joint_limits_rosparam.hpp" +#include "joint_limits/visibility_control.h" +#include "rclcpp/node.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace joint_limits +{ + +template +class JointLimiterInterface +{ +public: + JOINT_LIMITS_PUBLIC JointLimiterInterface() = default; + + JOINT_LIMITS_PUBLIC virtual ~JointLimiterInterface() = default; + + /// Initialization of every JointLimiter. + /** + * Initialization of JointLimiter for defined joints with their names. + * Robot description topic provides a topic name where URDF of the robot can be found. + * This is needed to use joint limits from URDF (not implemented yet!). + * Override this method only if initialization and reading joint limits should be adapted. + * Otherwise, initialize your custom limiter in `on_limit` method. + * + * \param[in] joint_names names of joints where limits should be applied. + * \param[in] param_itf node parameters interface object to access parameters. + * \param[in] logging_itf node logging interface to log if error happens. + * \param[in] robot_description_topic string of a topic where robot description is accessible. + */ + JOINT_LIMITS_PUBLIC virtual bool init( + const std::vector & joint_names, + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, + const std::string & robot_description_topic = "/robot_description") + { + number_of_joints_ = joint_names.size(); + joint_names_ = joint_names; + joint_limits_.resize(number_of_joints_); + node_param_itf_ = param_itf; + node_logging_itf_ = logging_itf; + + bool result = true; + + // TODO(destogl): get limits from URDF + + // Initialize and get joint limits from parameter server + if (has_parameter_interface()) + { + for (size_t i = 0; i < number_of_joints_; ++i) + { + if (!declare_parameters(joint_names[i], node_param_itf_, node_logging_itf_)) + { + RCLCPP_ERROR( + node_logging_itf_->get_logger(), + "JointLimiter: Joint '%s': parameter declaration has failed", joint_names[i].c_str()); + result = false; + break; + } + if (!get_joint_limits(joint_names[i], node_param_itf_, node_logging_itf_, joint_limits_[i])) + { + RCLCPP_ERROR( + node_logging_itf_->get_logger(), + "JointLimiter: Joint '%s': getting parameters has failed", joint_names[i].c_str()); + result = false; + break; + } + RCLCPP_INFO( + node_logging_itf_->get_logger(), "Limits for joint %zu (%s) are:\n%s", i, + joint_names[i].c_str(), joint_limits_[i].to_string().c_str()); + } + updated_limits_.writeFromNonRT(joint_limits_); + + auto on_parameter_event_callback = [this](const std::vector & parameters) + { + rcl_interfaces::msg::SetParametersResult set_parameters_result; + set_parameters_result.successful = true; + + std::vector updated_joint_limits = joint_limits_; + bool changed = false; + + for (size_t i = 0; i < number_of_joints_; ++i) + { + changed |= joint_limits::check_for_limits_update( + joint_names_[i], parameters, node_logging_itf_, updated_joint_limits[i]); + } + + if (changed) + { + updated_limits_.writeFromNonRT(updated_joint_limits); + RCLCPP_INFO(node_logging_itf_->get_logger(), "Limits are dynamically updated!"); + } + + return set_parameters_result; + }; + + parameter_callback_ = + node_param_itf_->add_on_set_parameters_callback(on_parameter_event_callback); + } + + if (result) + { + result = on_init(); + } + + (void)robot_description_topic; // avoid linters output + + return result; + } + + /** + * Wrapper init method that accepts the joint names and their limits directly + */ + JOINT_LIMITS_PUBLIC virtual bool init( + const std::vector & joint_names, + const std::vector & joint_limits, + const std::vector & soft_joint_limits, + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf) + { + number_of_joints_ = joint_names.size(); + joint_names_ = joint_names; + joint_limits_ = joint_limits; + soft_joint_limits_ = soft_joint_limits; + node_param_itf_ = param_itf; + node_logging_itf_ = logging_itf; + updated_limits_.writeFromNonRT(joint_limits_); + + if ((number_of_joints_ != joint_limits_.size()) && has_logging_interface()) + { + RCLCPP_ERROR( + node_logging_itf_->get_logger(), + "JointLimiter: Number of joint names and limits do not match: %zu != %zu", + number_of_joints_, joint_limits_.size()); + } + return (number_of_joints_ == joint_limits_.size()) && on_init(); + } + + /** + * Wrapper init method that accepts pointer to the Node. + * For details see other init method. + */ + JOINT_LIMITS_PUBLIC virtual bool init( + const std::vector & joint_names, const rclcpp::Node::SharedPtr & node, + const std::string & robot_description_topic = "/robot_description") + { + return init( + joint_names, node->get_node_parameters_interface(), node->get_node_logging_interface(), + robot_description_topic); + } + + /** + * Wrapper init method that accepts pointer to the LifecycleNode. + * For details see other init method. + */ + JOINT_LIMITS_PUBLIC virtual bool init( + const std::vector & joint_names, + const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node, + const std::string & robot_description_topic = "/robot_description") + { + return init( + joint_names, lifecycle_node->get_node_parameters_interface(), + lifecycle_node->get_node_logging_interface(), robot_description_topic); + } + + JOINT_LIMITS_PUBLIC virtual bool configure(const JointLimitsStateDataType & current_joint_states) + { + return on_configure(current_joint_states); + } + + /** \brief Enforce joint limits to desired joint state for multiple physical quantities. + * + * Generic enforce method that calls implementation-specific `on_enforce` method. + * + * \param[in] current_joint_states current joint states a robot is in. + * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. + * \param[in] dt time delta to calculate missing integrals and derivation in joint limits. + * \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC virtual bool enforce( + JointLimitsStateDataType & current_joint_states, + JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) + { + joint_limits_ = *(updated_limits_.readFromRT()); + return on_enforce(current_joint_states, desired_joint_states, dt); + } + +protected: + /** \brief Method is realized by an implementation. + * + * Implementation-specific initialization of limiter's internal states and libraries. + * \returns true if initialization was successful, otherwise false. + */ + JOINT_LIMITS_PUBLIC virtual bool on_init() = 0; + + /** \brief Method is realized by an implementation. + * + * Implementation-specific configuration of limiter's internal states and libraries. + * \returns true if initialization was successful, otherwise false. + */ + JOINT_LIMITS_PUBLIC virtual bool on_configure( + const JointLimitsStateDataType & current_joint_states) = 0; + + /** \brief Method is realized by an implementation. + * + * Filter-specific implementation of the joint limits enforce algorithm for multiple dependent + * physical quantities. + * + * \param[in] current_joint_states current joint states a robot is in. + * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. + * \param[in] dt time delta to calculate missing integrals and derivation in joint limits. + * \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC virtual bool on_enforce( + JointLimitsStateDataType & current_joint_states, + JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) = 0; + + /** \brief Checks if the logging interface is set. + * \returns true if the logging interface is available, otherwise false. + * + * \note this way of interfacing would be useful for instances where the logging interface is not + * available, for example in the ResourceManager or ResourceStorage classes. + */ + bool has_logging_interface() const { return node_logging_itf_ != nullptr; } + + /** \brief Checks if the parameter interface is set. + * \returns true if the parameter interface is available, otherwise false. + * + * * \note this way of interfacing would be useful for instances where the logging interface is + * not available, for example in the ResourceManager or ResourceStorage classes. + */ + bool has_parameter_interface() const { return node_param_itf_ != nullptr; } + + size_t number_of_joints_; + std::vector joint_names_; + std::vector joint_limits_; + std::vector soft_joint_limits_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_param_itf_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_itf_; + +private: + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_; + realtime_tools::RealtimeBuffer> updated_limits_; +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_LIMITER_INTERFACE_HPP_ diff --git a/joint_limits/include/joint_limits/joint_limiter_struct.hpp b/joint_limits/include/joint_limits/joint_limiter_struct.hpp new file mode 100644 index 0000000000..c4d7440603 --- /dev/null +++ b/joint_limits/include/joint_limits/joint_limiter_struct.hpp @@ -0,0 +1,66 @@ +// Copyright 2024 PAL Robotics S.L. +// +// 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. + +/// \author Sai Kishor Kothakota + +#ifndef JOINT_LIMITS__JOINT_LIMITER_STRUCT_HPP_ +#define JOINT_LIMITS__JOINT_LIMITER_STRUCT_HPP_ + +#include +#include +#include + +namespace joint_limits +{ + +struct JointControlInterfacesData +{ + std::string joint_name; + std::optional position = std::nullopt; + std::optional velocity = std::nullopt; + std::optional effort = std::nullopt; + std::optional acceleration = std::nullopt; + std::optional jerk = std::nullopt; + + bool has_data() const + { + return has_position() || has_velocity() || has_effort() || has_acceleration() || has_jerk(); + } + + bool has_position() const { return position.has_value(); } + + bool has_velocity() const { return velocity.has_value(); } + + bool has_effort() const { return effort.has_value(); } + + bool has_acceleration() const { return acceleration.has_value(); } + + bool has_jerk() const { return jerk.has_value(); } +}; + +struct JointInterfacesCommandLimiterData +{ + std::string joint_name; + JointControlInterfacesData actual; + JointControlInterfacesData command; + JointControlInterfacesData prev_command; + JointControlInterfacesData limited; + + bool has_actual_data() const { return actual.has_data(); } + + bool has_command_data() const { return command.has_data(); } +}; + +} // namespace joint_limits +#endif // JOINT_LIMITS__JOINT_LIMITER_STRUCT_HPP_ diff --git a/joint_limits/include/joint_limits/joint_limits.hpp b/joint_limits/include/joint_limits/joint_limits.hpp index 809bfd777b..8ad3009b45 100644 --- a/joint_limits/include/joint_limits/joint_limits.hpp +++ b/joint_limits/include/joint_limits/joint_limits.hpp @@ -67,7 +67,7 @@ struct JointLimits bool has_effort_limits; bool angle_wraparound; - std::string to_string() + std::string to_string() const { std::stringstream ss_output; diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp new file mode 100644 index 0000000000..e9ede97d9b --- /dev/null +++ b/joint_limits/include/joint_limits/joint_limits_helpers.hpp @@ -0,0 +1,88 @@ +// Copyright 2024 PAL Robotics S.L. +// +// 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. + +/// \author Adrià Roig Moreno + +#ifndef JOINT_LIMITS__JOINT_LIMITS_HELPERS_HPP_ +#define JOINT_LIMITS__JOINT_LIMITS_HELPERS_HPP_ + +#include +#include +#include +#include "joint_limits/joint_limits.hpp" + +namespace joint_limits +{ + +/** + * @brief Checks if a value is limited by the given limits. + * @param value The value to check. + * @param min The minimum limit. + * @param max The maximum limit. + * @return True if the value is limited, false otherwise. + */ +bool is_limited(double value, double min, double max); + +/** + * @brief Computes the position limits based on the velocity and acceleration limits. + * @param limits The joint limits. + * @param act_vel The actual velocity of the joint. + * @param prev_command_pos The previous commanded position of the joint. + * @param dt The time step. + * @return The position limits. + */ +std::pair compute_position_limits( + const joint_limits::JointLimits & limits, const std::optional & act_vel, + const std::optional & prev_command_pos, double dt); + +/** + * @brief Computes the velocity limits based on the position and acceleration limits. + * @param joint_name The name of the joint. + * @param limits The joint limits. + * @param act_pos The actual position of the joint. + * @param prev_command_vel The previous commanded velocity of the joint. + * @param dt The time step. + * @return The velocity limits. + */ +std::pair compute_velocity_limits( + const std::string & joint_name, const joint_limits::JointLimits & limits, + const double & desired_vel, const std::optional & act_pos, + const std::optional & prev_command_vel, double dt); + +/** + * @brief Computes the effort limits based on the position and velocity limits. + * @param limits The joint limits. + * @param act_pos The actual position of the joint. + * @param act_vel The actual velocity of the joint. + * @param dt The time step. + * @return The effort limits. + */ +std::pair compute_effort_limits( + const joint_limits::JointLimits & limits, const std::optional & act_pos, + const std::optional & act_vel, double /*dt*/); + +/** + * @brief Computes the acceleration limits based on the change in velocity and acceleration and + * deceleration limits. + * @param limits The joint limits. + * @param desired_acceleration The desired acceleration. + * @param actual_velocity The actual velocity of the joint. + * @return The acceleration limits. + */ +std::pair compute_acceleration_limits( + const JointLimits & limits, double desired_acceleration, std::optional actual_velocity); + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_LIMITS_HELPERS_HPP_ diff --git a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp index 2f32d49271..b23607f53d 100644 --- a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp +++ b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp @@ -19,6 +19,7 @@ #include #include +#include #include "joint_limits/joint_limits.hpp" #include "rclcpp/rclcpp.hpp" @@ -96,8 +97,6 @@ inline bool declare_parameters( auto_declare( param_itf, param_base_name + ".max_position", std::numeric_limits::quiet_NaN()); auto_declare(param_itf, param_base_name + ".has_velocity_limits", false); - auto_declare( - param_itf, param_base_name + ".min_velocity", std::numeric_limits::quiet_NaN()); auto_declare( param_itf, param_base_name + ".max_velocity", std::numeric_limits::quiet_NaN()); auto_declare(param_itf, param_base_name + ".has_acceleration_limits", false); @@ -237,7 +236,6 @@ inline bool get_joint_limits( !param_itf->has_parameter(param_base_name + ".min_position") && !param_itf->has_parameter(param_base_name + ".max_position") && !param_itf->has_parameter(param_base_name + ".has_velocity_limits") && - !param_itf->has_parameter(param_base_name + ".min_velocity") && !param_itf->has_parameter(param_base_name + ".max_velocity") && !param_itf->has_parameter(param_base_name + ".has_acceleration_limits") && !param_itf->has_parameter(param_base_name + ".max_acceleration") && @@ -247,12 +245,7 @@ inline bool get_joint_limits( !param_itf->has_parameter(param_base_name + ".max_jerk") && !param_itf->has_parameter(param_base_name + ".has_effort_limits") && !param_itf->has_parameter(param_base_name + ".max_effort") && - !param_itf->has_parameter(param_base_name + ".angle_wraparound") && - !param_itf->has_parameter(param_base_name + ".has_soft_limits") && - !param_itf->has_parameter(param_base_name + ".k_position") && - !param_itf->has_parameter(param_base_name + ".k_velocity") && - !param_itf->has_parameter(param_base_name + ".soft_lower_limit") && - !param_itf->has_parameter(param_base_name + ".soft_upper_limit")) + !param_itf->has_parameter(param_base_name + ".angle_wraparound")) { RCLCPP_ERROR( logging_itf->get_logger(), @@ -421,6 +414,218 @@ inline bool get_joint_limits( lifecycle_node->get_node_logging_interface(), limits); } +/** + * Check if any of updated parameters are related to JointLimits. + * + * This method is intended to be used in the parameters update callback. + * It is recommended that it's result is temporarily stored and synchronized with the JointLimits + * structure in the main loop. + * + * \param[in] joint_name Name of the joint whose limits should be checked. + * \param[in] parameters List of the parameters that should be checked if they belong to this + * structure and that are used for updating the internal values. + * \param[in] logging_itf node logging interface to provide log errors. + * \param[out] updated_limits structure with updated JointLimits. It is recommended that the + * currently used limits are stored into this structure before calling this method. + */ +inline bool check_for_limits_update( + const std::string & joint_name, const std::vector & parameters, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, + JointLimits & updated_limits) +{ + const std::string param_base_name = "joint_limits." + joint_name; + bool changed = false; + + // update first numerical values to make later checks for "has" limits members + for (auto & parameter : parameters) + { + const std::string param_name = parameter.get_name(); + try + { + if (param_name == param_base_name + ".min_position") + { + changed = updated_limits.min_position != parameter.get_value(); + updated_limits.min_position = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_position") + { + changed = updated_limits.max_position != parameter.get_value(); + updated_limits.max_position = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_velocity") + { + changed = updated_limits.max_velocity != parameter.get_value(); + updated_limits.max_velocity = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_acceleration") + { + changed = updated_limits.max_acceleration != parameter.get_value(); + updated_limits.max_acceleration = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_deceleration") + { + changed = updated_limits.max_deceleration != parameter.get_value(); + updated_limits.max_deceleration = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_jerk") + { + changed = updated_limits.max_jerk != parameter.get_value(); + updated_limits.max_jerk = parameter.get_value(); + } + else if (param_name == param_base_name + ".max_effort") + { + changed = updated_limits.max_effort != parameter.get_value(); + updated_limits.max_effort = parameter.get_value(); + } + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { + RCLCPP_WARN(logging_itf->get_logger(), "Please use the right type: %s", e.what()); + } + } + + for (auto & parameter : parameters) + { + const std::string param_name = parameter.get_name(); + try + { + if (param_name == param_base_name + ".has_position_limits") + { + updated_limits.has_position_limits = parameter.get_value(); + if (updated_limits.has_position_limits) + { + if (std::isnan(updated_limits.min_position) || std::isnan(updated_limits.max_position)) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: Position limits can not be used, i.e., " + "'has_position_limits' flag can not be set, if 'min_position' " + "and 'max_position' are not set or not have valid double values."); + updated_limits.has_position_limits = false; + } + else if (updated_limits.min_position >= updated_limits.max_position) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: Position limits can not be used, i.e., " + "'has_position_limits' flag can not be set, if not " + "'min_position' < 'max_position'"); + updated_limits.has_position_limits = false; + } + else + { + changed = true; + } + } + } + else if (param_name == param_base_name + ".has_velocity_limits") + { + updated_limits.has_velocity_limits = parameter.get_value(); + if (updated_limits.has_velocity_limits && std::isnan(updated_limits.max_velocity)) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_velocity_limits' flag can not be set if 'min_velocity' " + "and 'max_velocity' are not set or not have valid double values."); + updated_limits.has_velocity_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".has_acceleration_limits") + { + updated_limits.has_acceleration_limits = parameter.get_value(); + if (updated_limits.has_acceleration_limits && std::isnan(updated_limits.max_acceleration)) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_acceleration_limits' flag can not be set if " + "'max_acceleration' is not set or not have valid double values."); + updated_limits.has_acceleration_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".has_deceleration_limits") + { + updated_limits.has_deceleration_limits = parameter.get_value(); + if (updated_limits.has_deceleration_limits && std::isnan(updated_limits.max_deceleration)) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_deceleration_limits' flag can not be set if " + "'max_deceleration' is not set or not have valid double values."); + updated_limits.has_deceleration_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".has_jerk_limits") + { + updated_limits.has_jerk_limits = parameter.get_value(); + if (updated_limits.has_jerk_limits && std::isnan(updated_limits.max_jerk)) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_jerk_limits' flag can not be set if 'max_jerk' is not set " + "or not have valid double values."); + updated_limits.has_jerk_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".has_effort_limits") + { + updated_limits.has_effort_limits = parameter.get_value(); + if (updated_limits.has_effort_limits && std::isnan(updated_limits.max_effort)) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'has_effort_limits' flag can not be set if 'max_effort' is not " + "set or not have valid double values."); + updated_limits.has_effort_limits = false; + } + else + { + changed = true; + } + } + else if (param_name == param_base_name + ".angle_wraparound") + { + updated_limits.angle_wraparound = parameter.get_value(); + if (updated_limits.angle_wraparound && updated_limits.has_position_limits) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "PARAMETER NOT UPDATED: 'angle_wraparound' flag can not be set if " + "'has_position_limits' flag is set."); + updated_limits.angle_wraparound = false; + } + else + { + changed = true; + } + } + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { + RCLCPP_WARN( + logging_itf->get_logger(), "PARAMETER NOT UPDATED: Please use the right type: %s", + e.what()); + } + } + + return changed; +} + /// Populate a SoftJointLimits instance from the ROS parameter server. /** * It is assumed that the parameter structure is the following: @@ -550,6 +755,85 @@ inline bool get_joint_limits( lifecycle_node->get_node_logging_interface(), soft_limits); } +/** + * Check if any of updated parameters are related to SoftJointLimits. + * + * This method is intended to be used in the parameters update callback. + * It is recommended that it's result is temporarily stored and synchronized with the + * SoftJointLimits structure in the main loop. + * + * \param[in] joint_name Name of the joint whose limits should be checked. + * \param[in] parameters List of the parameters that should be checked if they belong to this + * structure and that are used for updating the internal values. + * \param[in] logging_itf node logging interface to provide log errors. + * \param[out] updated_limits structure with updated SoftJointLimits. It is recommended that the + * currently used limits are stored into this structure before calling this method. + */ +inline bool check_for_limits_update( + const std::string & joint_name, const std::vector & parameters, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, + SoftJointLimits & updated_limits) +{ + const std::string param_base_name = "joint_limits." + joint_name; + bool changed = false; + + for (auto & parameter : parameters) + { + const std::string param_name = parameter.get_name(); + try + { + if (param_name == param_base_name + ".has_soft_limits") + { + if (!parameter.get_value()) + { + RCLCPP_WARN( + logging_itf->get_logger(), + "Parameter 'has_soft_limits' is not set, therefore the limits will not be updated!"); + return false; + } + } + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { + RCLCPP_INFO(logging_itf->get_logger(), "Please use the right type: %s", e.what()); + } + } + + for (auto & parameter : parameters) + { + const std::string param_name = parameter.get_name(); + try + { + if (param_name == param_base_name + ".k_position") + { + changed = updated_limits.k_position != parameter.get_value(); + updated_limits.k_position = parameter.get_value(); + } + else if (param_name == param_base_name + ".k_velocity") + { + changed = updated_limits.k_velocity != parameter.get_value(); + updated_limits.k_velocity = parameter.get_value(); + } + else if (param_name == param_base_name + ".soft_lower_limit") + { + changed = updated_limits.min_position != parameter.get_value(); + updated_limits.min_position = parameter.get_value(); + } + else if (param_name == param_base_name + ".soft_upper_limit") + { + changed = updated_limits.max_position != parameter.get_value(); + updated_limits.max_position = parameter.get_value(); + } + } + catch (const rclcpp::exceptions::InvalidParameterTypeException & e) + { + RCLCPP_INFO(logging_itf->get_logger(), "Please use the right type: %s", e.what()); + } + } + + return changed; +} + } // namespace joint_limits #endif // JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_ diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp new file mode 100644 index 0000000000..798b1632ad --- /dev/null +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -0,0 +1,96 @@ +// Copyright (c) 2023, PickNik Inc. +// +// 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. + +/// \author Dr. Denis Stogl + +#ifndef JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_ +#define JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_ + +#include +#include +#include +#include + +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" + +namespace joint_limits +{ +/** + * Joint Saturation Limiter limits joints' position, velocity and acceleration by clamping values + * to its minimal and maximal allowed values. Since the position, velocity and accelerations are + * variables in physical relation, it might be that some values are limited lower then specified + * limit. For example, if a joint is close to its position limit, velocity and acceleration will be + * reduced accordingly. + */ +template +class JointSaturationLimiter : public JointLimiterInterface +{ +public: + /** \brief Constructor */ + JOINT_LIMITS_PUBLIC JointSaturationLimiter(); + + /** \brief Destructor */ + JOINT_LIMITS_PUBLIC ~JointSaturationLimiter(); + + JOINT_LIMITS_PUBLIC bool on_init() override { return true; } + + JOINT_LIMITS_PUBLIC bool on_configure( + const JointLimitsStateDataType & current_joint_states) override + { + prev_command_ = current_joint_states; + return true; + } + + /** \brief Enforce joint limits to desired position, velocity and acceleration using clamping. + * Class implements this method accepting following data types: + * - trajectory_msgs::msg::JointTrajectoryPoint: limiting position, velocity and acceleration; + * + * Implementation of saturation approach for joints with position, velocity or acceleration limits + * and values. First, position limits are checked to adjust desired velocity accordingly, then + * velocity and finally acceleration. + * The method support partial existence of limits, e.g., missing position limits for continuous + * joins. + * + * \param[in] current_joint_states current joint states a robot is in. + * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. + * \param[in] dt time delta to calculate missing integrals and derivation in joint limits. + * \returns true if limits are enforced, otherwise false. + */ + JOINT_LIMITS_PUBLIC bool on_enforce( + JointLimitsStateDataType & current_joint_states, + JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) override; + +protected: + rclcpp::Clock::SharedPtr clock_; + JointLimitsStateDataType prev_command_; +}; + +template +JointSaturationLimiter::JointSaturationLimiter() +: JointLimiterInterface() +{ + clock_ = std::make_shared(rclcpp::Clock(RCL_ROS_TIME)); +} + +template +JointSaturationLimiter::~JointSaturationLimiter() +{ +} + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_ diff --git a/joint_limits/include/joint_limits/joint_soft_limiter.hpp b/joint_limits/include/joint_limits/joint_soft_limiter.hpp new file mode 100644 index 0000000000..28612127b9 --- /dev/null +++ b/joint_limits/include/joint_limits/joint_soft_limiter.hpp @@ -0,0 +1,70 @@ +// Copyright 2024 PAL Robotics S.L. +// +// 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. + +/// \author Adrià Roig Moreno + +#ifndef JOINT_LIMITS__JOINT_SOFT_LIMITER_HPP_ +#define JOINT_LIMITS__JOINT_SOFT_LIMITER_HPP_ + +#include +#include "joint_limits/joint_limiter_struct.hpp" +#include "joint_limits/joint_limits_helpers.hpp" +#include "joint_limits/joint_saturation_limiter.hpp" + +constexpr double VALUE_CONSIDERED_ZERO = 1e-10; + +namespace joint_limits +{ + +class JointSoftLimiter : public JointSaturationLimiter +{ +public: + bool on_init() override + { + const bool result = (number_of_joints_ == 1); + if (!result && has_logging_interface()) + { + RCLCPP_ERROR( + node_logging_itf_->get_logger(), + "JointInterfacesSaturationLimiter: Expects the number of joints to be 1, but given : " + "%zu", + number_of_joints_); + } + prev_command_ = JointControlInterfacesData(); + return result; + } + + bool on_enforce( + JointControlInterfacesData & actual, JointControlInterfacesData & desired, + const rclcpp::Duration & dt) override; + + bool has_soft_position_limits(const joint_limits::SoftJointLimits & soft_joint_limits) + { + return std::isfinite(soft_joint_limits.min_position) && + std::isfinite(soft_joint_limits.max_position) && + (soft_joint_limits.max_position - soft_joint_limits.min_position) > + VALUE_CONSIDERED_ZERO; + } + + bool has_soft_limits(const joint_limits::SoftJointLimits & soft_joint_limits) + { + return has_soft_position_limits(soft_joint_limits) && + std::isfinite(soft_joint_limits.k_position) && + std::abs(soft_joint_limits.k_position) > VALUE_CONSIDERED_ZERO; + } +}; + +} // namespace joint_limits + +#endif // JOINT_LIMITS__JOINT_SOFT_LIMITER_HPP_ diff --git a/joint_limits/include/joint_limits/visibility_control.h b/joint_limits/include/joint_limits/visibility_control.h new file mode 100644 index 0000000000..9c957a3cf9 --- /dev/null +++ b/joint_limits/include/joint_limits/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 JOINT_LIMITS__VISIBILITY_CONTROL_H_ +#define JOINT_LIMITS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define JOINT_LIMITS_EXPORT __attribute__((dllexport)) +#define JOINT_LIMITS_IMPORT __attribute__((dllimport)) +#else +#define JOINT_LIMITS_EXPORT __declspec(dllexport) +#define JOINT_LIMITS_IMPORT __declspec(dllimport) +#endif +#ifdef JOINT_LIMITS_BUILDING_DLL +#define JOINT_LIMITS_PUBLIC JOINT_LIMITS_EXPORT +#else +#define JOINT_LIMITS_PUBLIC JOINT_LIMITS_IMPORT +#endif +#define JOINT_LIMITS_PUBLIC_TYPE JOINT_LIMITS_PUBLIC +#define JOINT_LIMITS_LOCAL +#else +#define JOINT_LIMITS_EXPORT __attribute__((visibility("default"))) +#define JOINT_LIMITS_IMPORT +#if __GNUC__ >= 4 +#define JOINT_LIMITS_PUBLIC __attribute__((visibility("default"))) +#define JOINT_LIMITS_LOCAL __attribute__((visibility("hidden"))) +#else +#define JOINT_LIMITS_PUBLIC +#define JOINT_LIMITS_LOCAL +#endif +#define JOINT_LIMITS_PUBLIC_TYPE +#endif + +#endif // JOINT_LIMITS__VISIBILITY_CONTROL_H_ diff --git a/joint_limits/joint_limiters.xml b/joint_limits/joint_limiters.xml new file mode 100644 index 0000000000..f2a4b8e20c --- /dev/null +++ b/joint_limits/joint_limiters.xml @@ -0,0 +1,25 @@ + + + + + Simple joint limiter using clamping approach. Warning: clamping can cause discontinuities. + + + + + Simple joint range limiter using clamping approach with the parsed limits. + + + + + Simple joint range limiter performing clamping between the parsed soft limits and the parsed joint limits. + + + + diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 29b32e9819..921ec2b44f 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,10 +1,10 @@ joint_limits 4.11.0 - Interfaces for handling of joint limits for controllers or hardware. + Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces. Bence Magyar - Denis Štogl + Denis Štogl Apache License 2.0 @@ -15,13 +15,19 @@ ament_cmake ament_cmake_gen_version_h + backward_ros + pluginlib + realtime_tools rclcpp rclcpp_lifecycle + trajectory_msgs urdf + ament_cmake_gmock + ament_cmake_gtest + generate_parameter_library launch_ros launch_testing_ament_cmake - ament_cmake_gtest ament_cmake diff --git a/joint_limits/src/joint_limiter_interface.cpp b/joint_limits/src/joint_limiter_interface.cpp new file mode 100644 index 0000000000..843337259d --- /dev/null +++ b/joint_limits/src/joint_limiter_interface.cpp @@ -0,0 +1,21 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. + +/// \author Dr. Denis Stogl + +#include "joint_limits/joint_limiter_interface.hpp" + +namespace joint_limits +{ +} // namespace joint_limits diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp new file mode 100644 index 0000000000..f98752336f --- /dev/null +++ b/joint_limits/src/joint_limits_helpers.cpp @@ -0,0 +1,161 @@ +// Copyright 2024 PAL Robotics S.L. +// +// 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. + +/// \author Adrià Roig Moreno + +#include "joint_limits/joint_limits_helpers.hpp" +#include +#include +#include "rclcpp/logging.hpp" + +namespace joint_limits +{ +namespace internal +{ +/** + * @brief Check if the limits are in the correct order and swap them if they are not. + */ +void check_and_swap_limits(std::pair & limits) +{ + if (limits.first > limits.second) + { + std::swap(limits.first, limits.second); + } +} +} // namespace internal + +bool is_limited(double value, double min, double max) { return value < min || value > max; } + +std::pair compute_position_limits( + const joint_limits::JointLimits & limits, const std::optional & act_vel, + const std::optional & prev_command_pos, double dt) +{ + std::pair pos_limits({limits.min_position, limits.max_position}); + if (limits.has_velocity_limits) + { + const double act_vel_abs = act_vel.has_value() ? std::fabs(act_vel.value()) : 0.0; + const double delta_vel = limits.has_acceleration_limits + ? act_vel_abs + (limits.max_acceleration * dt) + : limits.max_velocity; + const double max_vel = std::min(limits.max_velocity, delta_vel); + const double delta_pos = max_vel * dt; + pos_limits.first = std::max(prev_command_pos.value() - delta_pos, pos_limits.first); + pos_limits.second = std::min(prev_command_pos.value() + delta_pos, pos_limits.second); + } + internal::check_and_swap_limits(pos_limits); + return pos_limits; +} + +std::pair compute_velocity_limits( + const std::string & joint_name, const joint_limits::JointLimits & limits, + const double & desired_vel, const std::optional & act_pos, + const std::optional & prev_command_vel, double dt) +{ + const double max_vel = + limits.has_velocity_limits ? limits.max_velocity : std::numeric_limits::infinity(); + std::pair vel_limits({-max_vel, max_vel}); + if (limits.has_position_limits && act_pos.has_value()) + { + const double max_vel_with_pos_limits = (limits.max_position - act_pos.value()) / dt; + const double min_vel_with_pos_limits = (limits.min_position - act_pos.value()) / dt; + vel_limits.first = std::max(min_vel_with_pos_limits, vel_limits.first); + vel_limits.second = std::min(max_vel_with_pos_limits, vel_limits.second); + if ( + (act_pos.value() > limits.max_position && desired_vel >= 0.0) || + (act_pos.value() < limits.min_position && desired_vel <= 0.0)) + { + RCLCPP_WARN_EXPRESSION( + rclcpp::get_logger("joint_limiter_interface"), + prev_command_vel.has_value() && prev_command_vel.value() != 0.0, + "Joint position %.5f is out of bounds[%.5f, %.5f] for the joint and we want to move " + "further into bounds with vel %.5f: '%s'. Joint velocity limits will be " + "restrictred to zero.", + act_pos.value(), limits.min_position, limits.max_position, desired_vel, joint_name.c_str()); + vel_limits = {0.0, 0.0}; + } + } + if (limits.has_acceleration_limits && prev_command_vel.has_value()) + { + const double delta_vel = limits.max_acceleration * dt; + vel_limits.first = std::max(prev_command_vel.value() - delta_vel, vel_limits.first); + vel_limits.second = std::min(prev_command_vel.value() + delta_vel, vel_limits.second); + } + RCLCPP_DEBUG( + rclcpp::get_logger("joint_limiter_interface"), + "Joint velocity limits for joint '%s' are [%f, %f]", joint_name.c_str(), vel_limits.first, + vel_limits.second); + internal::check_and_swap_limits(vel_limits); + RCLCPP_DEBUG( + rclcpp::get_logger("joint_limiter_interface"), + "After swapping Joint velocity limits for joint '%s' are [%f, %f]", joint_name.c_str(), + vel_limits.first, vel_limits.second); + return vel_limits; +} + +std::pair compute_effort_limits( + const joint_limits::JointLimits & limits, const std::optional & act_pos, + const std::optional & act_vel, double /*dt*/) +{ + const double max_effort = + limits.has_effort_limits ? limits.max_effort : std::numeric_limits::infinity(); + std::pair eff_limits({-max_effort, max_effort}); + if (limits.has_position_limits && act_pos.has_value() && act_vel.has_value()) + { + if ((act_pos.value() <= limits.min_position) && (act_vel.value() <= 0.0)) + { + eff_limits.first = 0.0; + } + else if ((act_pos.value() >= limits.max_position) && (act_vel.value() >= 0.0)) + { + eff_limits.second = 0.0; + } + } + if (limits.has_velocity_limits && act_vel.has_value()) + { + if (act_vel.value() < -limits.max_velocity) + { + eff_limits.first = 0.0; + } + else if (act_vel.value() > limits.max_velocity) + { + eff_limits.second = 0.0; + } + } + internal::check_and_swap_limits(eff_limits); + return eff_limits; +} + +std::pair compute_acceleration_limits( + const joint_limits::JointLimits & limits, double desired_acceleration, + std::optional actual_velocity) +{ + std::pair acc_or_dec_limits( + -std::numeric_limits::infinity(), std::numeric_limits::infinity()); + if ( + limits.has_deceleration_limits && + ((desired_acceleration < 0 && actual_velocity && actual_velocity.value() > 0) || + (desired_acceleration > 0 && actual_velocity && actual_velocity.value() < 0))) + { + acc_or_dec_limits.first = -limits.max_deceleration; + acc_or_dec_limits.second = limits.max_deceleration; + } + else if (limits.has_acceleration_limits) + { + acc_or_dec_limits.first = -limits.max_acceleration; + acc_or_dec_limits.second = limits.max_acceleration; + } + return acc_or_dec_limits; +} + +} // namespace joint_limits diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp new file mode 100644 index 0000000000..3adc30d2e0 --- /dev/null +++ b/joint_limits/src/joint_range_limiter.cpp @@ -0,0 +1,172 @@ +// Copyright 2024 PAL Robotics S.L. +// +// 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. + +/// \author Sai Kishor Kothakota + +#include "joint_limits/joint_saturation_limiter.hpp" + +#include +#include "joint_limits/joint_limiter_struct.hpp" +#include "joint_limits/joint_limits_helpers.hpp" +#include "rclcpp/duration.hpp" +#include "rcutils/logging_macros.h" + +namespace joint_limits +{ + +template <> +bool JointSaturationLimiter::on_init() +{ + const bool result = (number_of_joints_ == 1); + if (!result && has_logging_interface()) + { + RCLCPP_ERROR( + node_logging_itf_->get_logger(), + "JointInterfacesSaturationLimiter: Expects the number of joints to be 1, but given : " + "%zu", + number_of_joints_); + } + prev_command_ = JointControlInterfacesData(); + return result; +} + +template <> +bool JointSaturationLimiter::on_enforce( + JointControlInterfacesData & actual, JointControlInterfacesData & desired, + const rclcpp::Duration & dt) +{ + bool limits_enforced = false; + + const auto dt_seconds = dt.seconds(); + // negative or null is not allowed + if (dt_seconds <= 0.0) + { + return false; + } + + const auto joint_limits = joint_limits_[0]; + const std::string joint_name = joint_names_[0]; + // The following conditional filling is needed for cases of having certain information missing + if (!prev_command_.has_data()) + { + if (actual.has_position()) + { + prev_command_.position = actual.position; + } + else if (desired.has_position()) + { + prev_command_.position = desired.position; + } + if (actual.has_velocity()) + { + prev_command_.velocity = actual.velocity; + } + else if (desired.has_velocity()) + { + prev_command_.velocity = desired.velocity; + } + if (actual.has_effort()) + { + prev_command_.effort = actual.effort; + } + else if (desired.has_effort()) + { + prev_command_.effort = desired.effort; + } + if (actual.has_acceleration()) + { + prev_command_.acceleration = actual.acceleration; + } + else if (desired.has_acceleration()) + { + prev_command_.acceleration = desired.acceleration; + } + if (actual.has_jerk()) + { + prev_command_.jerk = actual.jerk; + } + else if (desired.has_jerk()) + { + prev_command_.jerk = desired.jerk; + } + if (actual.has_data()) + { + prev_command_.joint_name = actual.joint_name; + } + else if (desired.has_data()) + { + prev_command_.joint_name = desired.joint_name; + } + } + + if (desired.has_position()) + { + const auto limits = + compute_position_limits(joint_limits, actual.velocity, prev_command_.position, dt_seconds); + limits_enforced = is_limited(desired.position.value(), limits.first, limits.second); + desired.position = std::clamp(desired.position.value(), limits.first, limits.second); + } + + if (desired.has_velocity()) + { + const auto limits = compute_velocity_limits( + joint_name, joint_limits, desired.velocity.value(), actual.position, prev_command_.velocity, + dt_seconds); + limits_enforced = + is_limited(desired.velocity.value(), limits.first, limits.second) || limits_enforced; + desired.velocity = std::clamp(desired.velocity.value(), limits.first, limits.second); + } + + if (desired.has_effort()) + { + const auto limits = + compute_effort_limits(joint_limits, actual.position, actual.velocity, dt_seconds); + limits_enforced = + is_limited(desired.effort.value(), limits.first, limits.second) || limits_enforced; + desired.effort = std::clamp(desired.effort.value(), limits.first, limits.second); + } + + if (desired.has_acceleration()) + { + const auto limits = + compute_acceleration_limits(joint_limits, desired.acceleration.value(), actual.velocity); + limits_enforced = + is_limited(desired.acceleration.value(), limits.first, limits.second) || limits_enforced; + desired.acceleration = std::clamp(desired.acceleration.value(), limits.first, limits.second); + } + + if (desired.has_jerk()) + { + limits_enforced = + is_limited(desired.jerk.value(), -joint_limits.max_jerk, joint_limits.max_jerk) || + limits_enforced; + desired.jerk = std::clamp(desired.jerk.value(), -joint_limits.max_jerk, joint_limits.max_jerk); + } + + prev_command_ = desired; + + return limits_enforced; +} + +} // namespace joint_limits + +#include "pluginlib/class_list_macros.hpp" + +// typedefs are needed here to avoid issues with macro expansion. ref: +// https://stackoverflow.com/a/8942986 +typedef joint_limits::JointSaturationLimiter + JointInterfacesSaturationLimiter; +typedef joint_limits::JointLimiterInterface + JointInterfacesLimiterInterfaceBase; +PLUGINLIB_EXPORT_CLASS(JointInterfacesSaturationLimiter, JointInterfacesLimiterInterfaceBase) diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp new file mode 100644 index 0000000000..9754c94a20 --- /dev/null +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -0,0 +1,443 @@ +// Copyright (c) 2023, PickNik Inc. +// +// 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. + +/// \authors Nathan Brooks, Dr. Denis Stogl, Guillaume Walck + +#include "joint_limits/joint_saturation_limiter.hpp" +#include "joint_limits/joint_limits_helpers.hpp" + +#include + +#include "rclcpp/duration.hpp" +#include "rcutils/logging_macros.h" + +constexpr size_t ROS_LOG_THROTTLE_PERIOD = 1 * 1000; // Milliseconds to throttle logs inside loops +constexpr double VALUE_CONSIDERED_ZERO = 1e-10; + +namespace joint_limits +{ +template <> +bool JointSaturationLimiter::on_enforce( + trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt) +{ + bool limits_enforced = false; + + const auto dt_seconds = dt.seconds(); + // negative or null is not allowed + if (dt_seconds <= 0.0) + { + return false; + } + + // TODO(gwalck) compute if the max are not implicitly violated with the given dt + // e.g. for max vel 2.0 and max acc 5.0, with dt >0.4 + // velocity max is implicitly already violated due to max_acc * dt > 2.0 + + // check for required inputs combination + bool has_desired_position = (desired_joint_states.positions.size() == number_of_joints_); + bool has_desired_velocity = (desired_joint_states.velocities.size() == number_of_joints_); + bool has_desired_acceleration = (desired_joint_states.accelerations.size() == number_of_joints_); + bool has_current_position = (current_joint_states.positions.size() == number_of_joints_); + bool has_current_velocity = (current_joint_states.velocities.size() == number_of_joints_); + + // pos state and vel or pos cmd is required, vel state is optional + if (!(has_current_position && (has_desired_position || has_desired_velocity))) + { + return false; + } + + if (!has_current_velocity) + { + // First update() after activating does not have velocity available, assume 0 + current_joint_states.velocities.resize(number_of_joints_, 0.0); + } + + // TODO(destogl): please check if we get too much malloc from this initialization, + // if so then we should use members instead local variables and initialize them in other method + std::vector desired_pos(number_of_joints_); + std::vector desired_vel(number_of_joints_); + std::vector desired_acc(number_of_joints_); + std::vector expected_vel(number_of_joints_); + std::vector expected_pos(number_of_joints_); + + // limits triggered + std::vector limited_jnts_pos, limited_jnts_vel, limited_jnts_acc, limited_jnts_dec; + + bool braking_near_position_limit_triggered = false; + + for (size_t index = 0; index < number_of_joints_; ++index) + { + if (has_desired_position) + { + desired_pos[index] = desired_joint_states.positions[index]; + } + if (has_desired_velocity) + { + desired_vel[index] = desired_joint_states.velocities[index]; + } + if (has_desired_acceleration) + { + desired_acc[index] = desired_joint_states.accelerations[index]; + } + + if (has_desired_position) + { + // limit position + if (joint_limits_[index].has_position_limits) + { + // clamp input pos_cmd + auto pos = std::clamp( + desired_pos[index], joint_limits_[index].min_position, joint_limits_[index].max_position); + if (pos != desired_pos[index]) + { + desired_pos[index] = pos; + limited_jnts_pos.emplace_back(joint_names_[index]); + limits_enforced = true; + } + } + // priority to pos_cmd derivative over cmd_vel when not defined. If done always then we might + // get jumps in the velocity based on the system's dynamics. Position limit clamping is done + // below once again. + const double position_difference = desired_pos[index] - current_joint_states.positions[index]; + if ( + std::fabs(position_difference) > VALUE_CONSIDERED_ZERO && + std::fabs(desired_vel[index]) <= VALUE_CONSIDERED_ZERO) + { + desired_vel[index] = position_difference / dt_seconds; + } + } + + // limit velocity + if (joint_limits_[index].has_velocity_limits) + { + // if desired velocity is not defined calculate it from positions + if (std::fabs(desired_vel[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_vel[index])) + { + desired_vel[index] = + (desired_pos[index] - current_joint_states.positions[index]) / dt_seconds; + } + // clamp input vel_cmd + if (std::fabs(desired_vel[index]) > joint_limits_[index].max_velocity) + { + desired_vel[index] = std::copysign(joint_limits_[index].max_velocity, desired_vel[index]); + limited_jnts_vel.emplace_back(joint_names_[index]); + limits_enforced = true; + + // recompute pos_cmd if needed + if (has_desired_position) + { + desired_pos[index] = + current_joint_states.positions[index] + desired_vel[index] * dt_seconds; + } + + desired_acc[index] = + (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; + } + } + + // limit acceleration + if ( + joint_limits_[index].has_acceleration_limits || joint_limits_[index].has_deceleration_limits) + { + // if(has_current_velocity) + if (1) // for now use a zero velocity if not provided + { + // limiting acc or dec function + auto apply_acc_or_dec_limit = [&]( + const double max_acc_or_dec, std::vector & acc, + std::vector & limited_jnts) -> bool + { + if (std::fabs(acc[index]) > max_acc_or_dec) + { + acc[index] = std::copysign(max_acc_or_dec, acc[index]); + limited_jnts.emplace_back(joint_names_[index]); + limits_enforced = true; + return true; + } + else + { + return false; + } + }; + + // if desired acceleration if not provided compute it from desired_vel and vel_state + if ( + std::fabs(desired_acc[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_acc[index])) + { + desired_acc[index] = + (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds; + } + + // check if decelerating - if velocity is changing toward 0 + bool deceleration_limit_applied = false; + bool limit_applied = false; + if ( + (desired_acc[index] < 0 && current_joint_states.velocities[index] > 0) || + (desired_acc[index] > 0 && current_joint_states.velocities[index] < 0)) + { + // limit deceleration + if (joint_limits_[index].has_deceleration_limits) + { + limit_applied = apply_acc_or_dec_limit( + joint_limits_[index].max_deceleration, desired_acc, limited_jnts_dec); + deceleration_limit_applied = true; + } + } + + // limit acceleration (fallback to acceleration if no deceleration limits) + if (joint_limits_[index].has_acceleration_limits && !deceleration_limit_applied) + { + limit_applied = apply_acc_or_dec_limit( + joint_limits_[index].max_acceleration, desired_acc, limited_jnts_acc); + } + + if (limit_applied) + { + // vel_cmd from integration of desired_acc, needed even if no vel output + desired_vel[index] = + current_joint_states.velocities[index] + desired_acc[index] * dt_seconds; + if (has_desired_position) + { + // pos_cmd from from double integration of desired_acc + desired_pos[index] = current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * desired_acc[index] * dt_seconds * dt_seconds; + } + } + } + // else we cannot compute acc, so not limiting it + } + + // plan ahead for position limits + if (joint_limits_[index].has_position_limits) + { + if (has_desired_velocity && !has_desired_position) + { + // Check immediate next step when using vel_cmd only, other cases already handled + // integrate pos + expected_pos[index] = + current_joint_states.positions[index] + desired_vel[index] * dt_seconds; + // if expected_pos over limit + auto pos = std::clamp( + expected_pos[index], joint_limits_[index].min_position, + joint_limits_[index].max_position); + if (pos != expected_pos[index]) + { + // TODO(gwalck) compute vel_cmd that would permit to slow down in time at full + // deceleration in any case limit pos to max + expected_pos[index] = pos; + // and recompute vel_cmd that would lead to pos_max (not ideal as velocity would not be + // zero) + desired_vel[index] = + (expected_pos[index] - current_joint_states.positions[index]) / dt_seconds; + limited_jnts_pos.emplace_back(joint_names_[index]); + limits_enforced = true; + } + } + + // Check that stopping distance is within joint limits + // Slow down all joints at maximum decel if any don't have stopping distance within joint + // limits + + // delta_x = (v2*v2 - v1*v1) / (2*a) + // stopping_distance = (- v1*v1) / (2*max_acceleration) + // Here we assume we will not trigger velocity limits while maximally decelerating. + // This is a valid assumption if we are not currently at a velocity limit since we are just + // coming to a rest. + double stopping_deccel = std::fabs(desired_vel[index] / dt_seconds); + if (joint_limits_[index].has_deceleration_limits) + { + stopping_deccel = joint_limits_[index].max_deceleration; + } + else if (joint_limits_[index].has_acceleration_limits) + { + stopping_deccel = joint_limits_[index].max_acceleration; + } + + double stopping_distance = + std::fabs((-desired_vel[index] * desired_vel[index]) / (2 * stopping_deccel)); + // compute stopping duration at stopping_deccel + double stopping_duration = std::fabs((desired_vel[index]) / (stopping_deccel)); + + // Check that joint limits are beyond stopping_distance and desired_velocity is towards + // that limit + if ( + (desired_vel[index] < 0 && + (current_joint_states.positions[index] - joint_limits_[index].min_position < + stopping_distance)) || + (desired_vel[index] > 0 && + (joint_limits_[index].max_position - current_joint_states.positions[index] < + stopping_distance))) + { + limited_jnts_pos.emplace_back(joint_names_[index]); + braking_near_position_limit_triggered = true; + limits_enforced = true; + } + else + { + // compute the travel_distance at new desired velocity, with best case duration + // stopping_duration + double motion_after_stopping_duration = desired_vel[index] * stopping_duration; + // re-check what happens if we don't slow down + if ( + (desired_vel[index] < 0 && + (current_joint_states.positions[index] - joint_limits_[index].min_position < + motion_after_stopping_duration)) || + (desired_vel[index] > 0 && + (joint_limits_[index].max_position - current_joint_states.positions[index] < + motion_after_stopping_duration))) + { + limited_jnts_pos.emplace_back(joint_names_[index]); + braking_near_position_limit_triggered = true; + limits_enforced = true; + } + // else no need to slow down. in worse case we won't hit the limit at current velocity + } + } + } + + // update variables according to triggers + if (braking_near_position_limit_triggered) + { + // this limit applies to all joints even if a single one is triggered + for (size_t index = 0; index < number_of_joints_; ++index) + { + // Compute accel to stop + // Here we aren't explicitly maximally decelerating, but for joints near their limits this + // should still result in max decel being used + desired_acc[index] = -current_joint_states.velocities[index] / dt_seconds; + if (joint_limits_[index].has_deceleration_limits) + { + desired_acc[index] = std::copysign( + std::min(std::fabs(desired_acc[index]), joint_limits_[index].max_deceleration), + desired_acc[index]); + } + else if (joint_limits_[index].has_acceleration_limits) + { + desired_acc[index] = std::copysign( + std::min(std::fabs(desired_acc[index]), joint_limits_[index].max_acceleration), + desired_acc[index]); + } + + // Recompute velocity and position + if (has_desired_velocity) + { + desired_vel[index] = + current_joint_states.velocities[index] + desired_acc[index] * dt_seconds; + } + if (has_desired_position) + { + desired_pos[index] = current_joint_states.positions[index] + + current_joint_states.velocities[index] * dt_seconds + + 0.5 * desired_acc[index] * dt_seconds * dt_seconds; + } + } + std::ostringstream ostr; + for (auto jnt : limited_jnts_pos) + { + ostr << jnt << " "; + } + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() + << "] would exceed position limits" + " if continuing at current state, limiting all joints"); + } + + // display limitations + + // if position limiting + if (limited_jnts_pos.size() > 0) + { + std::ostringstream ostr; + for (auto jnt : limited_jnts_pos) + { + ostr << jnt << " "; + } + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed position limits, limiting"); + } + + if (limited_jnts_vel.size() > 0) + { + std::ostringstream ostr; + for (auto jnt : limited_jnts_vel) + { + ostr << jnt << " "; + } + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed velocity limits, limiting"); + } + + if (limited_jnts_acc.size() > 0) + { + std::ostringstream ostr; + for (auto jnt : limited_jnts_acc) + { + ostr << jnt << " "; + } + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed acceleration limits, limiting"); + } + + if (limited_jnts_dec.size() > 0) + { + std::ostringstream ostr; + for (auto jnt : limited_jnts_dec) + { + ostr << jnt << " "; + } + ostr << "\b \b"; // erase last character + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << ostr.str().c_str() << "] would exceed deceleration limits, limiting"); + } + + if (has_desired_position) + { + desired_joint_states.positions = desired_pos; + } + if (has_desired_velocity) + { + desired_joint_states.velocities = desired_vel; + } + if (has_desired_acceleration) + { + desired_joint_states.accelerations = desired_acc; + } + + return limits_enforced; +} + +} // namespace joint_limits + +#include "pluginlib/class_list_macros.hpp" + +// typedefs are needed here to avoid issues with macro expansion. ref: +// https://stackoverflow.com/a/8942986 +typedef std::map int_map; +typedef joint_limits::JointSaturationLimiter + JointTrajectoryPointSaturationLimiter; +typedef joint_limits::JointLimiterInterface + JointTrajectoryPointLimiterInterfaceBase; +PLUGINLIB_EXPORT_CLASS( + JointTrajectoryPointSaturationLimiter, JointTrajectoryPointLimiterInterfaceBase) diff --git a/joint_limits/src/joint_soft_limiter.cpp b/joint_limits/src/joint_soft_limiter.cpp new file mode 100644 index 0000000000..fdac151557 --- /dev/null +++ b/joint_limits/src/joint_soft_limiter.cpp @@ -0,0 +1,267 @@ +// Copyright 2024 PAL Robotics S.L. +// +// 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. + +/// \author Adrià Roig Moreno +#include "joint_limits/joint_soft_limiter.hpp" + +namespace joint_limits +{ + +bool JointSoftLimiter::on_enforce( + JointControlInterfacesData & actual, JointControlInterfacesData & desired, + const rclcpp::Duration & dt) +{ + bool limits_enforced = false; + + const auto dt_seconds = dt.seconds(); + // negative or null is not allowed + if (dt_seconds <= 0.0) + { + return false; + } + + const auto hard_limits = joint_limits_[0]; + joint_limits::SoftJointLimits soft_joint_limits; + if (!soft_joint_limits_.empty()) + { + soft_joint_limits = soft_joint_limits_[0]; + } + + const std::string joint_name = joint_names_[0]; + + if (!prev_command_.has_data()) + { + if (actual.has_position()) + { + prev_command_.position = actual.position; + } + else if (desired.has_position()) + { + prev_command_.position = desired.position; + } + if (actual.has_velocity()) + { + prev_command_.velocity = actual.velocity; + } + else if (desired.has_velocity()) + { + prev_command_.velocity = desired.velocity; + } + if (actual.has_effort()) + { + prev_command_.effort = actual.effort; + } + else if (desired.has_effort()) + { + prev_command_.effort = desired.effort; + } + if (actual.has_acceleration()) + { + prev_command_.acceleration = actual.acceleration; + } + else if (desired.has_acceleration()) + { + prev_command_.acceleration = desired.acceleration; + } + if (actual.has_jerk()) + { + prev_command_.jerk = actual.jerk; + } + else if (desired.has_jerk()) + { + prev_command_.jerk = desired.jerk; + } + if (actual.has_data()) + { + prev_command_.joint_name = actual.joint_name; + } + else if (desired.has_data()) + { + prev_command_.joint_name = desired.joint_name; + } + } + + double soft_min_vel = -std::numeric_limits::infinity(); + double soft_max_vel = std::numeric_limits::infinity(); + double position = std::numeric_limits::infinity(); + + if (prev_command_.has_position() && std::isfinite(prev_command_.position.value())) + { + position = prev_command_.position.value(); + } + else if (actual.has_position()) + { + position = actual.position.value(); + } + + if (hard_limits.has_velocity_limits) + { + soft_min_vel = -hard_limits.max_velocity; + soft_max_vel = hard_limits.max_velocity; + + if ( + hard_limits.has_position_limits && has_soft_limits(soft_joint_limits) && + std::isfinite(position)) + { + soft_min_vel = std::clamp( + -soft_joint_limits.k_position * (position - soft_joint_limits.min_position), + -hard_limits.max_velocity, hard_limits.max_velocity); + + soft_max_vel = std::clamp( + -soft_joint_limits.k_position * (position - soft_joint_limits.max_position), + -hard_limits.max_velocity, hard_limits.max_velocity); + + if ((position < hard_limits.min_position) || (position > hard_limits.max_position)) + { + soft_min_vel = 0.0; + soft_max_vel = 0.0; + } + else if ( + (position < soft_joint_limits.min_position) || (position > soft_joint_limits.max_position)) + { + constexpr double soft_limit_reach_velocity = 1.0 * (M_PI / 180.0); + soft_min_vel = std::copysign(soft_limit_reach_velocity, soft_min_vel); + soft_max_vel = std::copysign(soft_limit_reach_velocity, soft_max_vel); + } + } + } + + if (desired.has_position()) + { + const auto position_limits = + compute_position_limits(hard_limits, actual.velocity, prev_command_.position, dt_seconds); + + double pos_low = -std::numeric_limits::infinity(); + double pos_high = std::numeric_limits::infinity(); + + if (has_soft_position_limits(soft_joint_limits)) + { + pos_low = soft_joint_limits.min_position; + pos_high = soft_joint_limits.max_position; + } + + if (hard_limits.has_velocity_limits) + { + pos_low = std::clamp(position + soft_min_vel * dt_seconds, pos_low, pos_high); + pos_high = std::clamp(position + soft_max_vel * dt_seconds, pos_low, pos_high); + } + pos_low = std::max(pos_low, position_limits.first); + pos_high = std::min(pos_high, position_limits.second); + + limits_enforced = is_limited(desired.position.value(), pos_low, pos_high); + desired.position = std::clamp(desired.position.value(), pos_low, pos_high); + } + + if (desired.has_velocity()) + { + const auto velocity_limits = compute_velocity_limits( + joint_name, hard_limits, desired.velocity.value(), actual.position, prev_command_.velocity, + dt_seconds); + + if (hard_limits.has_acceleration_limits && actual.has_velocity()) + { + soft_min_vel = + std::max(actual.velocity.value() - hard_limits.max_acceleration * dt_seconds, soft_min_vel); + soft_max_vel = + std::min(actual.velocity.value() + hard_limits.max_acceleration * dt_seconds, soft_max_vel); + } + + soft_min_vel = std::max(soft_min_vel, velocity_limits.first); + soft_max_vel = std::min(soft_max_vel, velocity_limits.second); + + limits_enforced = + is_limited(desired.velocity.value(), soft_min_vel, soft_max_vel) || limits_enforced; + desired.velocity = std::clamp(desired.velocity.value(), soft_min_vel, soft_max_vel); + } + + if (desired.has_effort()) + { + const auto effort_limits = + compute_effort_limits(hard_limits, actual.position, actual.velocity, dt_seconds); + + double soft_min_eff = effort_limits.first; + double soft_max_eff = effort_limits.second; + + if ( + hard_limits.has_effort_limits && std::isfinite(soft_joint_limits.k_velocity) && + actual.has_velocity()) + { + soft_min_eff = std::clamp( + -soft_joint_limits.k_velocity * (actual.velocity.value() - soft_min_vel), + -hard_limits.max_effort, hard_limits.max_effort); + + soft_max_eff = std::clamp( + -soft_joint_limits.k_velocity * (actual.velocity.value() - soft_max_vel), + -hard_limits.max_effort, hard_limits.max_effort); + + soft_min_eff = std::max(soft_min_eff, effort_limits.first); + soft_max_eff = std::min(soft_max_eff, effort_limits.second); + } + + limits_enforced = + is_limited(desired.effort.value(), soft_min_eff, soft_max_eff) || limits_enforced; + desired.effort = std::clamp(desired.effort.value(), soft_min_eff, soft_max_eff); + } + + if (desired.has_acceleration()) + { + const auto limits = + compute_acceleration_limits(hard_limits, desired.acceleration.value(), actual.velocity); + limits_enforced = + is_limited(desired.acceleration.value(), limits.first, limits.second) || limits_enforced; + desired.acceleration = std::clamp(desired.acceleration.value(), limits.first, limits.second); + } + + if (desired.has_jerk()) + { + limits_enforced = + is_limited(desired.jerk.value(), -hard_limits.max_jerk, hard_limits.max_jerk) || + limits_enforced; + desired.jerk = std::clamp(desired.jerk.value(), -hard_limits.max_jerk, hard_limits.max_jerk); + } + + if (desired.has_position() && !std::isfinite(desired.position.value()) && actual.has_position()) + { + desired.position = actual.position; + limits_enforced = true; + } + if (desired.has_velocity() && !std::isfinite(desired.velocity.value())) + { + desired.velocity = 0.0; + limits_enforced = true; + } + if (desired.has_acceleration() && !std::isfinite(desired.acceleration.value())) + { + desired.acceleration = 0.0; + limits_enforced = true; + } + if (desired.has_jerk() && !std::isfinite(desired.jerk.value())) + { + desired.jerk = 0.0; + limits_enforced = true; + } + + prev_command_ = desired; + + return limits_enforced; +} + +} // namespace joint_limits + +#include "pluginlib/class_list_macros.hpp" + +typedef joint_limits::JointSoftLimiter JointInterfacesSoftLimiter; +typedef joint_limits::JointLimiterInterface + JointInterfacesLimiterInterfaceBase; +PLUGINLIB_EXPORT_CLASS(JointInterfacesSoftLimiter, JointInterfacesLimiterInterfaceBase) diff --git a/joint_limits/test/joint_saturation_limiter_param.yaml b/joint_limits/test/joint_saturation_limiter_param.yaml new file mode 100644 index 0000000000..f025421519 --- /dev/null +++ b/joint_limits/test/joint_saturation_limiter_param.yaml @@ -0,0 +1,57 @@ +joint_saturation_limiter: + ros__parameters: + joint_limits: + # Get full specification from parameter server + foo_joint: + has_position_limits: true + min_position: -5.0 + max_position: 5.0 + has_velocity_limits: true + max_velocity: 2.0 + has_acceleration_limits: true + max_acceleration: 5.0 + has_deceleration_limits: true + max_deceleration: 7.5 + has_jerk_limits: true + max_jerk: 100.0 + has_effort_limits: true + max_effort: 20.0 + angle_wraparound: true # should be ignored, has position limits + has_soft_limits: true + k_position: 10.0 + k_velocity: 20.0 + soft_lower_limit: 0.1 + soft_upper_limit: 0.9 + + foo_joint_no_effort: + has_position_limits: true + min_position: -5.0 + max_position: 5.0 + has_velocity_limits: true + max_velocity: 2.0 + has_acceleration_limits: true + max_acceleration: 5.0 + has_deceleration_limits: true + max_deceleration: 7.5 + has_jerk_limits: true + max_jerk: 100.0 + has_effort_limits: false + max_effort: 20.0 + angle_wraparound: true # should be ignored, has position limits + has_soft_limits: true + k_position: 10.0 + k_velocity: 20.0 + soft_lower_limit: 0.1 + soft_upper_limit: 0.9 + +joint_saturation_limiter_nodeclimit: + ros__parameters: + joint_limits: + foo_joint: + has_position_limits: true + min_position: -5.0 + max_position: 5.0 + has_velocity_limits: true + max_velocity: 2.0 + has_acceleration_limits: true + max_acceleration: 5.0 diff --git a/joint_limits/test/test_joint_limiter.hpp b/joint_limits/test/test_joint_limiter.hpp new file mode 100644 index 0000000000..33b01a52a4 --- /dev/null +++ b/joint_limits/test/test_joint_limiter.hpp @@ -0,0 +1,155 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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_JOINT_LIMITER_HPP_ +#define TEST_JOINT_LIMITER_HPP_ + +#include + +#include +#include +#include +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limiter_struct.hpp" +#include "joint_limits/joint_limits.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/node.hpp" + +const double COMMON_THRESHOLD = 1.0e-6; + +using JointLimiter = joint_limits::JointLimiterInterface; + +class JointLimiterTest : public ::testing::Test +{ +public: + void SetUp() override + { + node_name_ = ::testing::UnitTest::GetInstance()->current_test_info()->name(); + } + + void SetupNode(const std::string node_name = "") + { + if (!node_name.empty()) + { + node_name_ = node_name; + } + node_ = std::make_shared(node_name_); + } + + bool Load() + { + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_)); + return joint_limiter_ != nullptr; + } + + bool Init(const std::string & joint_name = "foo_joint") + { + joint_names_ = {joint_name}; + num_joints_ = joint_names_.size(); + last_commanded_state_.joint_name = joint_name; + last_commanded_state_.position = 0.0; + last_commanded_state_.velocity = 0.0; + last_commanded_state_.acceleration = 0.0; + last_commanded_state_.effort = 0.0; + desired_state_ = last_commanded_state_; + actual_state_ = last_commanded_state_; + return joint_limiter_->init(joint_names_, node_); + } + + bool Init(const joint_limits::JointLimits & limits, const std::string & joint_name = "foo_joint") + { + joint_names_ = {joint_name}; + num_joints_ = joint_names_.size(); + last_commanded_state_.joint_name = joint_name; + last_commanded_state_.position = 0.0; + last_commanded_state_.velocity = 0.0; + last_commanded_state_.acceleration = 0.0; + last_commanded_state_.effort = 0.0; + desired_state_ = last_commanded_state_; + actual_state_ = last_commanded_state_; + return joint_limiter_->init( + joint_names_, {limits}, std::vector(), nullptr, + node_->get_node_logging_interface()); + } + + bool Configure() { return joint_limiter_->configure(last_commanded_state_); } + + void Integrate(double dt) + { + actual_state_.position = actual_state_.position.value() + desired_state_.velocity.value() * dt + + 0.5 * desired_state_.acceleration.value() * dt * dt; + actual_state_.velocity = + actual_state_.velocity.value() + desired_state_.acceleration.value() * dt; + } + + explicit JointLimiterTest(const std::string & joint_limiter_type) + : joint_limiter_type_(joint_limiter_type), + joint_limiter_loader_( + "joint_limits", + "joint_limits::JointLimiterInterface") + { + } + + virtual ~JointLimiterTest() {} + + void TearDown() override { node_.reset(); } + +protected: + std::string node_name_; + rclcpp::Node::SharedPtr node_; + std::vector joint_names_; + size_t num_joints_; + std::unique_ptr joint_limiter_; + std::string joint_limiter_type_; + pluginlib::ClassLoader joint_limiter_loader_; + + joint_limits::JointControlInterfacesData last_commanded_state_; + joint_limits::JointControlInterfacesData desired_state_; + joint_limits::JointControlInterfacesData actual_state_; +}; + +class JointSaturationLimiterTest : public JointLimiterTest +{ +public: + JointSaturationLimiterTest() : JointLimiterTest("joint_limits/JointInterfacesSaturationLimiter") + { + } +}; + +class JointSoftLimiterTest : public JointLimiterTest +{ +public: + JointSoftLimiterTest() : JointLimiterTest("joint_limits/JointInterfacesSoftLimiter") {} + + bool Init( + const joint_limits::JointLimits & limits, const joint_limits::SoftJointLimits & soft_limits, + const std::string & joint_name = "foo_joint") + { + joint_names_ = {joint_name}; + num_joints_ = joint_names_.size(); + last_commanded_state_.joint_name = joint_name; + last_commanded_state_.position = 0.0; + last_commanded_state_.velocity = 0.0; + last_commanded_state_.acceleration = 0.0; + last_commanded_state_.effort = 0.0; + desired_state_ = last_commanded_state_; + actual_state_ = last_commanded_state_; + return joint_limiter_->init( + joint_names_, {limits}, {soft_limits}, nullptr, node_->get_node_logging_interface()); + } +}; + +#endif // TEST_JOINT_LIMITER_HPP_ diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp new file mode 100644 index 0000000000..4571c8ced5 --- /dev/null +++ b/joint_limits/test/test_joint_range_limiter.cpp @@ -0,0 +1,657 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. + +/// \author Sai Kishor Kothakota + +#include +#include "test_joint_limiter.hpp" + +TEST_F(JointSaturationLimiterTest, when_loading_limiter_plugin_expect_loaded) +{ + // Test JointSaturationLimiter loading + ASSERT_NO_THROW( + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_))); + ASSERT_NE(joint_limiter_, nullptr); +} + +// NOTE: We accept also if there is no limit defined for a joint. +TEST_F(JointSaturationLimiterTest, when_joint_not_found_expect_init_fail) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + // initialize the limiter + std::vector joint_names = {"bar_joint"}; + ASSERT_TRUE(joint_limiter_->init(joint_names, node_)); +} + +TEST_F(JointSaturationLimiterTest, when_invalid_dt_expect_enforce_fail) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + ASSERT_TRUE(Init()); + ASSERT_TRUE(Configure()); + rclcpp::Duration period(0, 0); // 0 second + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); +} + +TEST_F(JointSaturationLimiterTest, check_desired_position_only_cases) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_position_limits = true; + limits.min_position = -M_PI; + limits.max_position = M_PI; + ASSERT_TRUE(Init(limits)); + // no size check occurs (yet) so expect true + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + + rclcpp::Duration period(1, 0); // 1 second + desired_state_.position = 4.0; + EXPECT_TRUE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // For hard limits, if there is no actual state but the desired state is outside the limit, then + // saturate it to the limits + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), limits.max_position, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + desired_state_.position = -5.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), limits.min_position, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // If the desired state is within the limits, then no saturation is needed + desired_state_.position = 0.0; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 0.0, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // Now add the velocity limits + limits.max_velocity = 1.0; + limits.has_velocity_limits = true; + ASSERT_TRUE(Init(limits)); + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + desired_state_.position = 0.0; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 0.0, COMMON_THRESHOLD); + EXPECT_TRUE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // Let's set the desired position greater than reachable with max velocity limit + desired_state_.position = 2.0; + // As per max velocity limit, it can only reach 1.0 in 1 second + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); + + // As per max velocity limit, it can only reach 0.0 in 1 second + desired_state_.position = -3.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 0.0, COMMON_THRESHOLD); + + // Now let's check the case where the actual position is at 2.0 and the desired position is -M_PI + // with max velocity limit of 1.0 + ASSERT_TRUE(Init(limits)); + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + actual_state_.position = 2.0; + desired_state_.position = -M_PI; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); + EXPECT_TRUE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // Now test when there are no position limits, then the desired position is not saturated + limits = joint_limits::JointLimits(); + ASSERT_TRUE(Init(limits)); + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + actual_state_.position = 2.0; + desired_state_.position = 5.0 * M_PI; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 5.0 * M_PI, COMMON_THRESHOLD); + EXPECT_TRUE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); +} + +TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_position_limits = true; + limits.min_position = -5.0; + limits.max_position = 5.0; + limits.has_velocity_limits = true; + limits.max_velocity = 1.0; + ASSERT_TRUE(Init(limits)); + // no size check occurs (yet) so expect true + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + + rclcpp::Duration period(1, 0); // 1 second + desired_state_.velocity = 2.0; + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_TRUE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // Now as the position limits are already configure, set the actual position nearby the limits, + // then the max velocity needs to adapt wrt to the position limits + // It is saturated as the position reported is close to the position limits + auto test_limit_enforcing = [&]( + const std::optional & actual_position, + double desired_velocity, double expected_velocity, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + const double act_pos = actual_position.has_value() ? actual_position.value() + : std::numeric_limits::quiet_NaN(); + SCOPED_TRACE( + "Testing for actual position: " + std::to_string(act_pos) + + ", desired velocity: " + std::to_string(desired_velocity) + ", expected velocity: " + + std::to_string(expected_velocity) + ", is clamped: " + std::to_string(is_clamped) + + " for the joint limits : " + limits.to_string()); + if (actual_position.has_value()) + { + actual_state_.position = actual_position.value(); + } + desired_state_.velocity = desired_velocity; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_TRUE(desired_state_.has_velocity()); + EXPECT_NEAR(desired_state_.velocity.value(), expected_velocity, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + }; + + // Test cases when there is no actual position + // For hard limits, if there is no actual state but the desired state is outside the limit, then + // saturate it to the limits + test_limit_enforcing(std::nullopt, 2.0, 1.0, true); + test_limit_enforcing(std::nullopt, 1.1, 1.0, true); + test_limit_enforcing(std::nullopt, -5.0, -1.0, true); + test_limit_enforcing(std::nullopt, -std::numeric_limits::infinity(), -1.0, true); + test_limit_enforcing(std::nullopt, std::numeric_limits::infinity(), 1.0, true); + test_limit_enforcing(std::nullopt, -3.212, -1.0, true); + test_limit_enforcing(std::nullopt, -0.8, -0.8, false); + test_limit_enforcing(std::nullopt, 0.8, 0.8, false); + test_limit_enforcing(std::nullopt, 0.12, 0.12, false); + test_limit_enforcing(std::nullopt, 0.0, 0.0, false); + + // The cases where the actual position value exist + test_limit_enforcing(4.5, 5.0, 0.5, true); + test_limit_enforcing(4.8, 5.0, 0.2, true); + test_limit_enforcing(4.5, 0.3, 0.3, false); + test_limit_enforcing(4.5, 0.5, 0.5, false); + test_limit_enforcing(5.0, 0.9, 0.0, true); + // When the position is out of the limits, then the velocity is saturated to zero + test_limit_enforcing(6.0, 2.0, 0.0, true); + test_limit_enforcing(6.0, -2.0, 0.0, true); + test_limit_enforcing(4.0, 0.5, 0.5, false); + test_limit_enforcing(-4.8, -6.0, -0.2, true); + test_limit_enforcing(4.3, 5.0, 0.7, true); + test_limit_enforcing(-4.5, -5.0, -0.5, true); + test_limit_enforcing(-4.5, -0.2, -0.2, false); + test_limit_enforcing(-3.0, -5.0, -1.0, true); + test_limit_enforcing(-3.0, -1.0, -1.0, false); + test_limit_enforcing(-5.0, -3.0, 0.0, true); + test_limit_enforcing(-5.0, -1.0, 0.0, true); + // When the position is out of the limits, then the velocity is saturated to zero + test_limit_enforcing(-6.0, -1.0, 0.0, true); + test_limit_enforcing(-6.0, -2.0, 0.0, true); + test_limit_enforcing(-6.0, 1.0, 0.0, true); + + // Now remove the position limits and only test with acceleration limits + limits.has_position_limits = false; + limits.has_acceleration_limits = true; + limits.max_acceleration = 0.5; + // When launching init, the prev_command_ within the limiter will be reset + ASSERT_TRUE(Init(limits)); + // Now the velocity limits are now saturated by the acceleration limits so in succeeding call it + // will reach the desired if it is within the max velocity limits. Here, the order of the tests is + // important. + for (auto act_pos : + {std::optional(std::nullopt), std::optional(10.0), + std::optional(-10.0)}) + { + test_limit_enforcing(act_pos, 0.0, 0.0, false); // Helps to reset th prev_command internally + test_limit_enforcing(act_pos, 1.0, 0.5, true); + test_limit_enforcing(act_pos, 1.0, 1.0, false); + test_limit_enforcing(act_pos, -0.2, 0.5, true); + test_limit_enforcing(act_pos, -0.2, 0.0, true); + test_limit_enforcing(act_pos, -0.2, -0.2, false); + test_limit_enforcing(act_pos, -0.3, -0.3, false); + test_limit_enforcing(act_pos, -0.9, -0.8, true); + test_limit_enforcing(act_pos, -0.9, -0.9, false); + test_limit_enforcing(act_pos, -2.0, -1.0, true); + test_limit_enforcing(act_pos, 2.0, -0.5, true); + test_limit_enforcing(act_pos, 2.0, 0.0, true); + test_limit_enforcing(act_pos, 2.0, 0.5, true); + test_limit_enforcing(act_pos, 2.0, 1.0, true); + test_limit_enforcing(act_pos, 0.0, 0.5, true); + test_limit_enforcing(act_pos, 0.0, 0.0, false); + } + + // Now re-enable the position limits and test with acceleration limits + limits.has_position_limits = true; + limits.has_acceleration_limits = true; + limits.max_acceleration = 0.5; + // When launching init, the prev_command_ within the limiter will be reset + ASSERT_TRUE(Init(limits)); + // Now the velocity limits are now saturated by the acceleration limits so in succeeding call it + // will reach the desired if it is within the max velocity limits. Here, the order of the tests is + // important. + test_limit_enforcing(4.5, 0.0, 0.0, false); // Helps to reset th prev_command internally + test_limit_enforcing(4.5, 1.0, 0.5, true); + test_limit_enforcing(4.8, 1.0, 0.2, true); + test_limit_enforcing(4.8, -1.0, -0.3, true); + test_limit_enforcing(4.8, -1.0, -0.8, true); + test_limit_enforcing(4.8, -1.0, -1.0, false); + // here the velocity is saturated by the acceleration limits, if not then the velocity will be + // directly -0.2 + test_limit_enforcing(-4.8, -2.0, -0.5, true); + test_limit_enforcing(-4.8, -1.0, -0.2, true); + test_limit_enforcing(-4.3, -1.0, -0.7, true); + test_limit_enforcing(-4.3, 0.0, -0.2, true); + test_limit_enforcing(-4.3, 0.0, 0.0, false); + test_limit_enforcing(-6.0, 1.0, 0.0, true); + test_limit_enforcing(-6.0, -1.0, 0.0, true); + test_limit_enforcing(6.0, 1.0, 0.0, true); + test_limit_enforcing(6.0, -1.0, 0.0, true); +} + +TEST_F(JointSaturationLimiterTest, check_desired_effort_only_cases) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_position_limits = true; + limits.min_position = -5.0; + limits.max_position = 5.0; + limits.has_velocity_limits = true; + limits.max_velocity = 1.0; + limits.has_effort_limits = true; + limits.max_effort = 200.0; + ASSERT_TRUE(Init(limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + + rclcpp::Duration period(1, 0); // 1 second + desired_state_.effort = 20.0; + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_TRUE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // Now as the position limits are already configure, set the actual position nearby the limits, + // then the max velocity needs to adapt wrt to the position limits + // It is saturated as the position reported is close to the position limits + auto test_limit_enforcing = [&]( + const std::optional & actual_position, + const std::optional & actual_velocity, + double desired_effort, double expected_effort, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + const double act_pos = actual_position.has_value() ? actual_position.value() + : std::numeric_limits::quiet_NaN(); + const double act_vel = actual_velocity.has_value() ? actual_velocity.value() + : std::numeric_limits::quiet_NaN(); + SCOPED_TRACE( + "Testing for actual position: " + std::to_string(act_pos) + ", actual velocity: " + + std::to_string(act_vel) + ", desired effort: " + std::to_string(desired_effort) + + ", expected effort: " + std::to_string(expected_effort) + ", is clamped: " + + std::to_string(is_clamped) + " for the joint limits : " + limits.to_string()); + if (actual_position.has_value()) + { + actual_state_.position = actual_position.value(); + } + if (actual_velocity.has_value()) + { + actual_state_.velocity = actual_velocity.value(); + } + desired_state_.effort = desired_effort; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_TRUE(desired_state_.has_effort()); + EXPECT_NEAR(desired_state_.effort.value(), expected_effort, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_jerk()); + }; + + for (auto act_pos : + {std::optional(std::nullopt), std::optional(4.0), + std::optional(-4.0)}) + { + for (auto act_vel : + {std::optional(std::nullopt), std::optional(0.4), + std::optional(-0.2)}) + { + test_limit_enforcing(act_pos, act_vel, 20.0, 20.0, false); + test_limit_enforcing(act_pos, act_vel, 200.0, 200.0, false); + test_limit_enforcing(act_pos, act_vel, 201.0, 200.0, true); + test_limit_enforcing(act_pos, act_vel, 0.0, 0.0, false); + test_limit_enforcing(act_pos, act_vel, -20.0, -20.0, false); + test_limit_enforcing(act_pos, act_vel, -200.0, -200.0, false); + test_limit_enforcing(act_pos, act_vel, -201.0, -200.0, true); + } + } + + // The convention is that the positive velocity/position will result in positive effort + // Now the cases where the actual position or the actual velocity is out of the limits + test_limit_enforcing(5.0, 0.0, 20.0, 0.0, true); + test_limit_enforcing(5.0, 0.0, 400.0, 0.0, true); + test_limit_enforcing(6.0, 0.0, 400.0, 0.0, true); + test_limit_enforcing(5.0, 0.0, -20.0, -20.0, false); + test_limit_enforcing(5.0, 0.0, -400.0, -200.0, true); + test_limit_enforcing(6.0, 0.0, -400.0, -200.0, true); + + // At the limit, when trying to move away from the limit, it should allow + test_limit_enforcing(5.0, -0.2, 400.0, 200.0, true); + test_limit_enforcing(5.0, -0.2, -400.0, -200.0, true); + test_limit_enforcing(5.0, -0.2, 30.0, 30.0, false); + test_limit_enforcing(5.0, -0.2, -30.0, -30.0, false); + // For the positive velocity with limit, the effort is saturated + test_limit_enforcing(5.0, 0.2, 400.0, 0.0, true); + test_limit_enforcing(5.0, 0.2, 30.0, 0.0, true); + test_limit_enforcing(5.0, 0.2, -400.0, -200.0, true); + test_limit_enforcing(5.0, 0.2, -30.0, -30.0, false); + + test_limit_enforcing(-5.0, 0.2, 20.0, 20.0, false); + test_limit_enforcing(-5.0, 0.2, 400.0, 200.0, true); + test_limit_enforcing(-5.0, 0.2, -20.0, -20.0, false); + test_limit_enforcing(-5.0, 0.2, -400.0, -200.0, true); + // For the negative velocity with limit, the effort is saturated + test_limit_enforcing(-5.0, -0.2, -400.0, 0.0, true); + test_limit_enforcing(-5.0, -0.2, -30.0, 0.0, true); + test_limit_enforcing(-5.0, -0.2, 400.0, 200.0, true); + test_limit_enforcing(-5.0, -0.2, 30.0, 30.0, false); +} + +TEST_F(JointSaturationLimiterTest, check_desired_acceleration_only_cases) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_acceleration_limits = true; + limits.max_acceleration = 0.5; + ASSERT_TRUE(Init(limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + rclcpp::Duration period(1, 0); // 1 second + auto test_limit_enforcing = [&]( + const std::optional & actual_velocity, double desired_accel, + double expected_accel, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + const double act_vel = actual_velocity.has_value() ? actual_velocity.value() + : std::numeric_limits::quiet_NaN(); + SCOPED_TRACE( + "Testing for actual velocity: " + std::to_string(act_vel) + ", desired acceleration: " + + std::to_string(desired_accel) + ", expected acceleration: " + std::to_string(expected_accel) + + ", is clamped: " + std::to_string(is_clamped) + + " for the joint limits : " + limits.to_string()); + if (actual_velocity.has_value()) + { + actual_state_.velocity = actual_velocity.value(); + } + desired_state_.acceleration = desired_accel; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_TRUE(desired_state_.has_acceleration()); + EXPECT_NEAR(desired_state_.acceleration.value(), expected_accel, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_jerk()); + }; + + // Tests without applying deceleration limits + for (auto act_vel : + {std::optional(std::nullopt), std::optional(0.4), + std::optional(-0.2)}) + { + test_limit_enforcing(act_vel, 0.0, 0.0, false); + test_limit_enforcing(act_vel, 0.5, 0.5, false); + test_limit_enforcing(act_vel, 0.6, 0.5, true); + test_limit_enforcing(act_vel, 1.5, 0.5, true); + test_limit_enforcing(act_vel, -0.5, -0.5, false); + test_limit_enforcing(act_vel, -0.6, -0.5, true); + test_limit_enforcing(act_vel, -1.5, -0.5, true); + } + + // Now let's test with applying deceleration limits + limits.has_deceleration_limits = true; + limits.max_deceleration = 0.25; + // When launching init, the prev_command_ within the limiter will be reset + ASSERT_TRUE(Init(limits)); + + // If you don't have the actual velocity, the deceleration limits are not applied + test_limit_enforcing(std::nullopt, 0.0, 0.0, false); + test_limit_enforcing(std::nullopt, 0.5, 0.5, false); + test_limit_enforcing(std::nullopt, 0.6, 0.5, true); + test_limit_enforcing(std::nullopt, 1.5, 0.5, true); + test_limit_enforcing(std::nullopt, -0.5, -0.5, false); + test_limit_enforcing(std::nullopt, -0.6, -0.5, true); + test_limit_enforcing(std::nullopt, -1.5, -0.5, true); + test_limit_enforcing(std::nullopt, 0.0, 0.0, false); + + // Testing both positive and negative velocities and accelerations together without a proper + // deceleration + test_limit_enforcing(0.4, 0.2, 0.2, false); + test_limit_enforcing(0.4, 0.8, 0.5, true); + test_limit_enforcing(-0.4, -0.2, -0.2, false); + test_limit_enforcing(-0.4, -0.6, -0.5, true); + + // The deceleration limits are basically applied when the acceleration is positive and the + // velocity is negative and when the acceleration is negative and the velocity is positive + test_limit_enforcing(0.4, -0.1, -0.1, false); + test_limit_enforcing(0.4, -0.25, -0.25, false); + test_limit_enforcing(0.4, -0.6, -0.25, true); + test_limit_enforcing(0.4, -4.0, -0.25, true); + test_limit_enforcing(-0.4, 0.1, 0.1, false); + test_limit_enforcing(-0.4, 0.25, 0.25, false); + test_limit_enforcing(-0.4, 0.6, 0.25, true); + test_limit_enforcing(-0.4, 3.0, 0.25, true); +} + +TEST_F(JointSaturationLimiterTest, check_desired_jerk_only_cases) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_jerk_limits = true; + limits.max_jerk = 0.5; + ASSERT_TRUE(Init(limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + rclcpp::Duration period(1, 0); // 1 second + auto test_limit_enforcing = [&](double desired_jerk, double expected_jerk, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + SCOPED_TRACE( + "Testing for desired jerk : " + std::to_string(desired_jerk) + ", expected jerk: " + + std::to_string(expected_jerk) + ", is clamped: " + std::to_string(is_clamped) + + " for the joint limits : " + limits.to_string()); + desired_state_.jerk = desired_jerk; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_TRUE(desired_state_.has_jerk()); + EXPECT_NEAR(desired_state_.jerk.value(), expected_jerk, COMMON_THRESHOLD); + }; + + // Check with jerk limits + test_limit_enforcing(0.0, 0.0, false); + test_limit_enforcing(0.5, 0.5, false); + test_limit_enforcing(0.6, 0.5, true); + test_limit_enforcing(1.5, 0.5, true); + test_limit_enforcing(-0.5, -0.5, false); + test_limit_enforcing(-0.6, -0.5, true); + test_limit_enforcing(-1.5, -0.5, true); +} + +TEST_F(JointSaturationLimiterTest, check_all_desired_references_limiting) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_position_limits = true; + limits.min_position = -5.0; + limits.max_position = 5.0; + limits.has_velocity_limits = true; + limits.max_velocity = 1.0; + limits.has_acceleration_limits = true; + limits.max_acceleration = 0.5; + limits.has_deceleration_limits = true; + limits.max_deceleration = 0.25; + limits.has_jerk_limits = true; + limits.max_jerk = 2.0; + ASSERT_TRUE(Init(limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + rclcpp::Duration period(1, 0); // 1 second + auto test_limit_enforcing = + [&]( + const std::optional & actual_position, const std::optional & actual_velocity, + double desired_position, double desired_velocity, double desired_acceleration, + double desired_jerk, double expected_position, double expected_velocity, + double expected_acceleration, double expected_jerk, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + const double act_pos = actual_position.has_value() ? actual_position.value() + : std::numeric_limits::quiet_NaN(); + const double act_vel = actual_velocity.has_value() ? actual_velocity.value() + : std::numeric_limits::quiet_NaN(); + SCOPED_TRACE( + "Testing for actual position: " + std::to_string(act_pos) + ", actual velocity: " + + std::to_string(act_vel) + ", desired position: " + std::to_string(desired_position) + + ", desired velocity: " + std::to_string(desired_velocity) + ", desired acceleration: " + + std::to_string(desired_acceleration) + ", desired jerk: " + std::to_string(desired_jerk) + + ", expected position: " + std::to_string(expected_position) + + ", expected velocity: " + std::to_string(expected_velocity) + ", expected acceleration: " + + std::to_string(expected_acceleration) + ", expected jerk: " + std::to_string(expected_jerk) + + ", is clamped: " + std::to_string(is_clamped) + + " for the joint limits : " + limits.to_string()); + if (actual_position.has_value()) + { + actual_state_.position = actual_position.value(); + } + if (actual_velocity.has_value()) + { + actual_state_.velocity = actual_velocity.value(); + } + desired_state_.position = desired_position; + desired_state_.velocity = desired_velocity; + desired_state_.acceleration = desired_acceleration; + desired_state_.jerk = desired_jerk; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), expected_position, COMMON_THRESHOLD); + EXPECT_NEAR(desired_state_.velocity.value(), expected_velocity, COMMON_THRESHOLD); + EXPECT_NEAR(desired_state_.acceleration.value(), expected_acceleration, COMMON_THRESHOLD); + EXPECT_NEAR(desired_state_.jerk.value(), expected_jerk, COMMON_THRESHOLD); + }; + + // Test cases when there is no actual position and velocity + // Desired position and velocity affected due to the acceleration limits + test_limit_enforcing(std::nullopt, std::nullopt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 1.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 1.5, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 2.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 2.0, 1.0, 0.5, 2.5, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 1.0, 0.5, 0.5, 3.0, 1.0, 0.5, 0.5, false); + + // Now enforce the limits with actual position and velocity + ASSERT_TRUE(Init(limits)); + // Desired position and velocity affected due to the acceleration limits + test_limit_enforcing(0.5, 0.0, 6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.0, 0.0, -6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.0, 0.5, 0.5, 0.5, true); + test_limit_enforcing(2.0, 0.5, 6.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(3.0, 0.5, 6.0, 2.0, 1.0, 0.5, 4.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(4.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(4.8, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 0.5, 0.5, 0.5, true); + test_limit_enforcing(5.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 0.0, 0.5, 0.5, true); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/joint_limits/test/test_joint_saturation_limiter.cpp b/joint_limits/test/test_joint_saturation_limiter.cpp new file mode 100644 index 0000000000..72cecfcbf8 --- /dev/null +++ b/joint_limits/test/test_joint_saturation_limiter.cpp @@ -0,0 +1,527 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. + +/// \author Dr. Denis Stogl, Guillaume Walck + +#include "test_joint_saturation_limiter.hpp" + +TEST_F(JointSaturationLimiterTest, when_loading_limiter_plugin_expect_loaded) +{ + // Test JointSaturationLimiter loading + ASSERT_NO_THROW( + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_))); + ASSERT_NE(joint_limiter_, nullptr); +} + +// NOTE: We accept also if there is no limit defined for a joint. +TEST_F(JointSaturationLimiterTest, when_joint_not_found_expect_init_fail) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + // initialize the limiter + std::vector joint_names = {"bar_joint"}; + ASSERT_TRUE(joint_limiter_->init(joint_names, node_)); + } +} + +TEST_F(JointSaturationLimiterTest, when_invalid_dt_expect_enforce_fail) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + rclcpp::Duration period(0, 0); // 0 second + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + } +} + +TEST_F(JointSaturationLimiterTest, when_neigher_poscmd_nor_velcmd_expect_enforce_fail) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + // no size check occurs (yet) so expect true + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + rclcpp::Duration period(1, 0); // 1 second + // test no desired interface + desired_joint_states_.positions.clear(); + desired_joint_states_.velocities.clear(); + // currently not handled desired_joint_states_.accelerations.clear(); + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + } +} + +TEST_F(JointSaturationLimiterTest, when_no_posstate_expect_enforce_false) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + joint_limiter_->configure(last_commanded_state_); + + rclcpp::Duration period(1, 0); // 1 second + // test no position interface + current_joint_states_.positions.clear(); + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // also fail if out of limits + desired_joint_states_.positions[0] = 20.0; + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + } +} + +TEST_F(JointSaturationLimiterTest, when_no_velstate_expect_limiting) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + joint_limiter_->configure(last_commanded_state_); + + rclcpp::Duration period(1, 0); // 1 second + // test no vel interface + current_joint_states_.velocities.clear(); + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + // also fail if out of limits + desired_joint_states_.positions[0] = 20.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + } +} + +TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + // pos, vel, acc, dec = 1.0, 2.0, 5.0, 7.5 + + // within limits + desired_joint_states_.positions[0] = 1.0; + desired_joint_states_.velocities[0] = 1.0; // valid pos derivatite as well + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if no limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 1.0, // pos unchanged + 1.0, // vel unchanged + 1.0 // acc = vel / 1.0 + ); + } +} + +TEST_F(JointSaturationLimiterTest, when_within_limits_expect_no_limits_applied_with_acc) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + // pos, vel, acc, dec = 1.0, 2.0, 5.0, 7.5 + + // within limits + desired_joint_states_.positions[0] = 1.0; + desired_joint_states_.velocities[0] = 1.5; // valid pos derivative as well + desired_joint_states_.accelerations[0] = 2.9; // valid pos derivative as well + ASSERT_FALSE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if no limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 1.0, // pos unchanged + 1.5, // vel unchanged + 2.9 // acc = vel / 1.0 + ); + } +} + +TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_vel_exceeded_expect_limits_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + + // pos/vel cmd ifs + current_joint_states_.positions[0] = -2.0; + desired_joint_states_.positions[0] = 1.0; + // desired velocity exceeds although correctly computed from pos derivative + desired_joint_states_.velocities[0] = 3.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 0.0, // pos = pos + max_vel * dt + 2.0, // vel limited to max_vel + 2.0 / 1.0 // acc set to vel change/DT + ); + + // check opposite velocity direction (sign copy) + current_joint_states_.positions[0] = 2.0; + desired_joint_states_.positions[0] = -1.0; + // desired velocity exceeds although correctly computed from pos derivative + desired_joint_states_.velocities[0] = -3.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 0.0, // pos = pos - max_vel * dt + -2.0, // vel limited to -max_vel + -2.0 / 1.0 // acc set to vel change/DT + ); + } +} + +TEST_F(JointSaturationLimiterTest, when_posonly_leads_to_vel_exceeded_expect_pos_acc_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + + // set current velocity close to limits to not be blocked by max acc to reach max + current_joint_states_.velocities[0] = 1.9; + // desired pos leads to vel exceeded (4.0 / sec instead of 2.0) + desired_joint_states_.positions[0] = 4.0; + // no vel cmd (will lead to internal computation of velocity) + desired_joint_states_.velocities.clear(); + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos and acc limits applied + ASSERT_EQ(desired_joint_states_.positions[0], 2.0); // pos limited to max_vel * DT + // no vel cmd ifs + ASSERT_EQ( + desired_joint_states_.accelerations[0], (2.0 - 1.9) / 1.0); // acc set to vel change/DT + + // check opposite position and direction + current_joint_states_.positions[0] = 0.0; + current_joint_states_.velocities[0] = -1.9; + desired_joint_states_.positions[0] = -4.0; + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos and acc limits applied + ASSERT_EQ(desired_joint_states_.positions[0], -2.0); // pos limited to -max_vel * DT + // no vel cmd ifs + ASSERT_EQ( + desired_joint_states_.accelerations[0], (-2.0 + 1.9) / 1.0); // acc set to vel change/DT + } +} + +TEST_F(JointSaturationLimiterTest, when_velonly_leads_to_vel_exceeded_expect_vel_acc_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + + // vel cmd ifs + current_joint_states_.positions[0] = -2.0; + // set current velocity close to limits to not be blocked by max acc to reach max + current_joint_states_.velocities[0] = 1.9; + // no pos cmd + desired_joint_states_.positions.clear(); + // desired velocity exceeds + desired_joint_states_.velocities[0] = 3.0; + + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + ASSERT_EQ(desired_joint_states_.velocities[0], 2.0); // vel limited to max_vel + // no vel cmd ifs + ASSERT_EQ( + desired_joint_states_.accelerations[0], (2.0 - 1.9) / 1.0); // acc set to vel change/DT + + // check opposite velocity direction + current_joint_states_.positions[0] = 2.0; + // set current velocity close to limits to not be blocked by max acc to reach max + current_joint_states_.velocities[0] = -1.9; + // desired velocity exceeds + desired_joint_states_.velocities[0] = -3.0; + + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + ASSERT_EQ(desired_joint_states_.velocities[0], -2.0); // vel limited to -max_vel + // no vel cmd ifs + ASSERT_EQ( + desired_joint_states_.accelerations[0], (-2.0 + 1.9) / 1.0); // acc set to vel change/DT + } +} + +TEST_F(JointSaturationLimiterTest, when_posonly_exceeded_expect_pos_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(1.0, 0.0); // 1 second + + // desired pos exceeds + current_joint_states_.positions[0] = 5.0; + desired_joint_states_.positions[0] = 20.0; + // no velocity interface + desired_joint_states_.velocities.clear(); + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos limits applied + ASSERT_EQ( + desired_joint_states_.positions[0], 5.0); // pos limited clamped (was already at limit) + // no vel cmd ifs + ASSERT_EQ(desired_joint_states_.accelerations[0], 0.0); // acc set to vel change/DT + } +} + +TEST_F(JointSaturationLimiterTest, when_position_close_to_pos_limit_expect_deceleration_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + // using 0.05 because 1.0 sec invalidates the "small dt integration" + rclcpp::Duration period(0, 50000000); // 0.05 second + + // close to pos limit should reduce velocity and stop + current_joint_states_.positions[0] = 4.851; + current_joint_states_.velocities[0] = 1.5; + desired_joint_states_.positions[0] = 4.926; + desired_joint_states_.velocities[0] = 1.5; + + // this setup requires 0.15 distance to stop, and 0.2 seconds (so 4 cycles at 0.05) + std::vector expected_ret = {true, true, true, false}; + for (auto i = 0u; i < 4; ++i) + { + auto previous_vel_request = desired_joint_states_.velocities[0]; + // expect limits applied until the end stop + ASSERT_EQ( + joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period), + expected_ret[i]); + + ASSERT_LE( + desired_joint_states_.velocities[0], + previous_vel_request); // vel adapted to reach end-stop should be decreasing + // NOTE: after the first cycle, vel is reduced and does not trigger stopping position limit + // hence no max deceleration anymore... + ASSERT_LT( + desired_joint_states_.positions[0], + 5.0 + COMMON_THRESHOLD); // should decelerate at each cycle and stay below limits + ASSERT_LE(desired_joint_states_.accelerations[0], 0.0); // should decelerate + + Integrate(period.seconds()); + + ASSERT_LT(current_joint_states_.positions[0], 5.0); // below joint limit + } + } +} + +TEST_F(JointSaturationLimiterTest, when_posvel_leads_to_acc_exceeded_expect_limits_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + // desired acceleration exceeds + current_joint_states_.positions[0] = 0.1; + current_joint_states_.velocities[0] = 0.1; + desired_joint_states_.positions[0] = 0.125; // valid pos cmd for the desired velocity + desired_joint_states_.velocities[0] = 0.5; // leads to acc > max acc + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 0.11125, // pos = double integration from max acc with current state + 0.35, // vel limited to vel + max acc * dt + 5.0 // acc max acc + ); + } +} + +TEST_F(JointSaturationLimiterTest, when_posonly_leads_to_acc_exceeded_expect_limits_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + // desired acceleration exceeds + current_joint_states_.positions[0] = 0.1; + current_joint_states_.velocities[0] = 0.1; + desired_joint_states_.positions[0] = + 0.125; // pos cmd leads to vel 0.5 that leads to acc > max acc + desired_joint_states_.velocities.clear(); + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos and acc limits applied + ASSERT_NEAR( + desired_joint_states_.positions[0], 0.111250, + COMMON_THRESHOLD); // pos = double integration from max acc with current state + // no vel cmd ifs + ASSERT_EQ(desired_joint_states_.accelerations[0], 5.0); // acc max acc + } +} + +TEST_F(JointSaturationLimiterTest, when_velonly_leads_to_acc_exceeded_expect_limits_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + // desired acceleration exceeds + current_joint_states_.positions[0] = 0.1; + current_joint_states_.velocities[0] = 0.1; + desired_joint_states_.positions.clear(); // = 0.125; + desired_joint_states_.velocities[0] = 0.5; // leads to acc > max acc + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if pos and acc limits applied + // no pos cmd ifs + ASSERT_EQ(desired_joint_states_.velocities[0], 0.35); // vel limited to vel + max acc * dt + ASSERT_EQ(desired_joint_states_.accelerations[0], 5.0); // acc max acc + } +} + +TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_expect_dec_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + // desired deceleration exceeds + current_joint_states_.positions[0] = 0.3; + current_joint_states_.velocities[0] = 0.5; + desired_joint_states_.positions[0] = 0.305; // valid pos cmd for the desired velocity + desired_joint_states_.velocities[0] = 0.1; // leads to acc < -max dec + + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 0.315625, // pos = double integration from max dec with current state + 0.125, // vel limited by vel - max dec * dt + -7.5 // acc limited by -max dec + ); + } +} + +TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_with_no_maxdec_expect_acc_enforced) +{ + SetupNode("joint_saturation_limiter_nodeclimit"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + // desired deceleration exceeds + current_joint_states_.positions[0] = 1.0; + current_joint_states_.velocities[0] = 1.0; + desired_joint_states_.positions[0] = 1.025; // valid pos cmd for the desired decreased velocity + desired_joint_states_.velocities[0] = 0.5; // leads to acc > -max acc + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + // check if vel and acc limits applied + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 1.04375, // pos = double integration from max acc with current state + 0.75, // vel limited by vel-max acc * dt + -5.0 // acc limited to -max acc + ); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/joint_limits/test/test_joint_saturation_limiter.hpp b/joint_limits/test/test_joint_saturation_limiter.hpp new file mode 100644 index 0000000000..b94fc6ca2a --- /dev/null +++ b/joint_limits/test/test_joint_saturation_limiter.hpp @@ -0,0 +1,109 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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_JOINT_SATURATION_LIMITER_HPP_ +#define TEST_JOINT_SATURATION_LIMITER_HPP_ + +#include + +#include +#include +#include +#include "joint_limits/joint_limiter_interface.hpp" +#include "joint_limits/joint_limits.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/node.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +const double COMMON_THRESHOLD = 0.0011; + +#define CHECK_STATE_SINGLE_JOINT(tested_traj_point, idx, expected_pos, expected_vel, expected_acc) \ + EXPECT_NEAR(tested_traj_point.positions[idx], expected_pos, COMMON_THRESHOLD); \ + EXPECT_NEAR(tested_traj_point.velocities[idx], expected_vel, COMMON_THRESHOLD); \ + EXPECT_NEAR(tested_traj_point.accelerations[idx], expected_acc, COMMON_THRESHOLD); + +using JointLimiter = + joint_limits::JointLimiterInterface; + +class JointSaturationLimiterTest : public ::testing::Test +{ +public: + void SetUp() override + { + node_name_ = ::testing::UnitTest::GetInstance()->current_test_info()->name(); + } + + void SetupNode(const std::string node_name = "") + { + if (!node_name.empty()) + { + node_name_ = node_name; + } + node_ = std::make_shared(node_name_); + } + + void Load() + { + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_)); + } + + void Init(const std::string & joint_name = "foo_joint") + { + joint_names_ = {joint_name}; + joint_limiter_->init(joint_names_, node_); + num_joints_ = joint_names_.size(); + last_commanded_state_.positions.resize(num_joints_, 0.0); + last_commanded_state_.velocities.resize(num_joints_, 0.0); + last_commanded_state_.accelerations.resize(num_joints_, 0.0); + last_commanded_state_.effort.resize(num_joints_, 0.0); + desired_joint_states_ = last_commanded_state_; + current_joint_states_ = last_commanded_state_; + } + + void Configure() { joint_limiter_->configure(last_commanded_state_); } + + void Integrate(double dt) + { + current_joint_states_.positions[0] += desired_joint_states_.velocities[0] * dt + + 0.5 * desired_joint_states_.accelerations[0] * dt * dt; + current_joint_states_.velocities[0] += desired_joint_states_.accelerations[0] * dt; + } + + JointSaturationLimiterTest() + : joint_limiter_type_("joint_limits/JointTrajectoryPointSaturationLimiter"), + joint_limiter_loader_( + "joint_limits", + "joint_limits::JointLimiterInterface") + { + } + + void TearDown() override { node_.reset(); } + +protected: + std::string node_name_; + rclcpp::Node::SharedPtr node_; + std::vector joint_names_; + size_t num_joints_; + std::unique_ptr joint_limiter_; + std::string joint_limiter_type_; + pluginlib::ClassLoader joint_limiter_loader_; + + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; + trajectory_msgs::msg::JointTrajectoryPoint desired_joint_states_; + trajectory_msgs::msg::JointTrajectoryPoint current_joint_states_; +}; + +#endif // TEST_JOINT_SATURATION_LIMITER_HPP_ diff --git a/joint_limits/test/test_joint_soft_limiter.cpp b/joint_limits/test/test_joint_soft_limiter.cpp new file mode 100644 index 0000000000..d0d814dff9 --- /dev/null +++ b/joint_limits/test/test_joint_soft_limiter.cpp @@ -0,0 +1,1012 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. + +/// \author Adrià Roig Moreno + +#include +#include +#include "test_joint_limiter.hpp" + +TEST_F(JointSoftLimiterTest, when_loading_limiter_plugin_expect_loaded) +{ + // Test JointSoftLimiter loading + ASSERT_NO_THROW( + joint_limiter_ = std::unique_ptr( + joint_limiter_loader_.createUnmanagedInstance(joint_limiter_type_))); + ASSERT_NE(joint_limiter_, nullptr); +} + +// NOTE: We accept also if there is no limit defined for a joint. +TEST_F(JointSoftLimiterTest, when_joint_not_found_expect_init_fail) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + // initialize the limiter + std::vector joint_names = {"bar_joint"}; + ASSERT_TRUE(joint_limiter_->init(joint_names, node_)); +} + +TEST_F(JointSoftLimiterTest, when_invalid_dt_expect_enforce_fail) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + ASSERT_TRUE(JointLimiterTest::Init()); + ASSERT_TRUE(Configure()); + rclcpp::Duration period(0, 0); // 0 second + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); +} + +TEST_F(JointSoftLimiterTest, check_desired_position_only_cases) +{ + SetupNode("soft_joint_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_position_limits = true; + limits.min_position = -M_PI; + limits.max_position = M_PI; + joint_limits::SoftJointLimits soft_limits; + ASSERT_TRUE(Init(limits, soft_limits)); + // no size check occurs (yet) so expect true + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + + rclcpp::Duration period(1, 0); // 1 second + desired_state_.position = 4.0; + EXPECT_TRUE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // For hard limits, if there is no actual state but the desired state is outside the limit, then + // saturate it to the limits + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), limits.max_position, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + desired_state_.position = -5.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), limits.min_position, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // If the desired state is within the limits, then no saturation is needed + desired_state_.position = 0.0; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 0.0, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // Check defining only max_position soft_limit (shouldn't consider it) + soft_limits.max_position = 3.0; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 4.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), limits.max_position, COMMON_THRESHOLD); + // Check defining max_position soft_limit equal to min_position soft_limit (shouldn't consider it) + soft_limits.min_position = 3.0; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 4.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), limits.max_position, COMMON_THRESHOLD); + // Check defining min_postion soft_limit greater than max_position soft_limit (shouldn't consider + // it) + soft_limits.min_position = 5.0; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 4.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), limits.max_position, COMMON_THRESHOLD); + // Check defining correct limits (lower than hard_limits). It should clamp to soft limits. + soft_limits.min_position = -1.0; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 4.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), soft_limits.max_position, COMMON_THRESHOLD); + // If desired is inside the limits shouldn't clamp. + desired_state_.position = 0.0; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 0.0, COMMON_THRESHOLD); + // Clamp to min_position soft_limits + desired_state_.position = -2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), soft_limits.min_position, COMMON_THRESHOLD); + + // Now add the velocity limits + soft_limits = joint_limits::SoftJointLimits(); + limits.max_velocity = 1.0; + limits.has_velocity_limits = true; + ASSERT_TRUE(Init(limits, soft_limits)); + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + desired_state_.position = 0.0; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 0.0, COMMON_THRESHOLD); + EXPECT_TRUE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // Let's set the desired position greater than reachable with max velocity limit + desired_state_.position = 2.0; + // As per max velocity limit, it can only reach 1.0 in 1 second + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); + // As per max velocity limit, it can only reach 1.0 in 1 second + + // If soft position limits are less restrictive consider velocity_limits + soft_limits.max_position = 1.5; + soft_limits.min_position = -1.5; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); + // If soft position limits are more restrictive consider soft position limits + soft_limits.max_position = 0.75; + soft_limits.min_position = -0.75; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), soft_limits.max_position, COMMON_THRESHOLD); + desired_state_.position = -3.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + // As per max velocity limit, it can only reach -0.25 in 1 second + EXPECT_NEAR(desired_state_.position.value(), -0.25, COMMON_THRESHOLD); + + // Now let's check the case where the actual position is at 2.0 and the desired position is -M_PI + // with max velocity limit of 1.0 + soft_limits = joint_limits::SoftJointLimits(); + ASSERT_TRUE(Init(limits, soft_limits)); + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + actual_state_.position = 2.0; + desired_state_.position = -M_PI; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); + EXPECT_TRUE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + soft_limits.min_position = 1.5; + soft_limits.max_position = 3.0; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_ = {}; + actual_state_ = {}; + actual_state_.position = 2.0; + desired_state_.position = -M_PI; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), soft_limits.min_position, COMMON_THRESHOLD); + + // Check using k_position + soft_limits = joint_limits::SoftJointLimits(); + soft_limits.k_position = 1.0; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 0.0; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 0.0, COMMON_THRESHOLD); + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); + desired_state_.position = 2.0; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 2.0, COMMON_THRESHOLD); + + soft_limits.k_position = 1.0; + soft_limits.max_position = 1.5; + soft_limits.min_position = -1.5; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 0.0; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 0.0, COMMON_THRESHOLD); + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.5, COMMON_THRESHOLD); + + soft_limits.k_position = 2.0; + soft_limits.max_position = 1.5; + soft_limits.min_position = -1.5; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.5, COMMON_THRESHOLD); + + soft_limits.k_position = 0.5; + soft_limits.max_position = 2.0; + soft_limits.min_position = -2.0; + ASSERT_TRUE(Init(limits, soft_limits)); + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD); + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.5, COMMON_THRESHOLD); + desired_state_.position = 2.0; + ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 1.75, COMMON_THRESHOLD); + + // Now test when there are no position limits and soft limits, then the desired position is not + // saturated + limits = joint_limits::JointLimits(); + soft_limits = joint_limits::SoftJointLimits(); + ASSERT_TRUE(Init(limits, soft_limits)); + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + actual_state_.position = 2.0; + desired_state_.position = 5.0 * M_PI; + ASSERT_FALSE(joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), 5.0 * M_PI, COMMON_THRESHOLD); + EXPECT_TRUE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); +} + +TEST_F(JointSoftLimiterTest, check_desired_velocity_only_cases) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_position_limits = true; + limits.min_position = -5.0; + limits.max_position = 5.0; + limits.has_velocity_limits = true; + limits.max_velocity = 1.0; + joint_limits::SoftJointLimits soft_limits; + ASSERT_TRUE(Init(limits, soft_limits)); + // no size check occurs (yet) so expect true + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + + rclcpp::Duration period(1, 0); // 1 second + desired_state_.velocity = 2.0; + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_TRUE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // Now as the position limits are already configure, set the actual position nearby the limits, + // then the max velocity needs to adapt wrt to the position limits + // It is saturated as the position reported is close to the position limits + auto test_limit_enforcing = [&]( + const std::optional & actual_position, + double desired_velocity, double expected_velocity, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + const double act_pos = actual_position.has_value() ? actual_position.value() + : std::numeric_limits::quiet_NaN(); + SCOPED_TRACE( + "Testing for actual position: " + std::to_string(act_pos) + + ", desired velocity: " + std::to_string(desired_velocity) + ", expected velocity: " + + std::to_string(expected_velocity) + ", is clamped: " + std::to_string(is_clamped) + + " for the joint limits : " + limits.to_string()); + if (actual_position.has_value()) + { + actual_state_.position = actual_position.value(); + } + desired_state_.velocity = desired_velocity; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_TRUE(desired_state_.has_velocity()); + EXPECT_NEAR(desired_state_.velocity.value(), expected_velocity, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + }; + + auto test_generic_cases = [&](const joint_limits::SoftJointLimits & soft_joint_limits) + { + ASSERT_TRUE(Init(limits, soft_joint_limits)); + + // Test cases when there is no actual position + // For hard limits, if there is no actual state but the desired state is outside the limit, then + // saturate it to the limits + test_limit_enforcing(std::nullopt, 2.0, 1.0, true); + test_limit_enforcing(std::nullopt, 1.1, 1.0, true); + test_limit_enforcing(std::nullopt, -5.0, -1.0, true); + test_limit_enforcing(std::nullopt, -std::numeric_limits::infinity(), -1.0, true); + test_limit_enforcing(std::nullopt, std::numeric_limits::infinity(), 1.0, true); + test_limit_enforcing(std::nullopt, -3.212, -1.0, true); + test_limit_enforcing(std::nullopt, -0.8, -0.8, false); + test_limit_enforcing(std::nullopt, 0.8, 0.8, false); + test_limit_enforcing(std::nullopt, 0.12, 0.12, false); + test_limit_enforcing(std::nullopt, 0.0, 0.0, false); + + // The cases where the actual position value exist + test_limit_enforcing(4.5, 5.0, 0.5, true); + test_limit_enforcing(4.8, 5.0, 0.2, true); + test_limit_enforcing(4.5, 0.3, 0.3, false); + test_limit_enforcing(4.5, 0.5, 0.5, false); + test_limit_enforcing(5.0, 0.9, 0.0, true); + // When the position is out of the limits, then the velocity is saturated to zero + test_limit_enforcing(6.0, 2.0, 0.0, true); + test_limit_enforcing(6.0, -2.0, 0.0, true); + test_limit_enforcing(4.0, 0.5, 0.5, false); + test_limit_enforcing(-4.8, -6.0, -0.2, true); + test_limit_enforcing(4.3, 5.0, 0.7, true); + test_limit_enforcing(-4.5, -5.0, -0.5, true); + test_limit_enforcing(-4.5, -0.2, -0.2, false); + test_limit_enforcing(-3.0, -5.0, -1.0, true); + test_limit_enforcing(-3.0, -1.0, -1.0, false); + test_limit_enforcing(-5.0, -3.0, 0.0, true); + test_limit_enforcing(-5.0, -1.0, 0.0, true); + // When the position is out of the limits, then the velocity is saturated to zero + test_limit_enforcing(-6.0, -1.0, 0.0, true); + test_limit_enforcing(-6.0, -2.0, 0.0, true); + test_limit_enforcing(-6.0, 1.0, 0.0, true); + }; + + test_generic_cases(soft_limits); + soft_limits.k_position = 1.0; + test_generic_cases(soft_limits); + soft_limits.max_position = 10.0; + test_generic_cases(soft_limits); + soft_limits.min_position = -std::numeric_limits::infinity(); + test_generic_cases(soft_limits); + soft_limits.min_position = -10.0; + test_generic_cases(soft_limits); + + soft_limits.k_position = 1.0; + soft_limits.max_position = 4.0; + soft_limits.min_position = -2.0; + ASSERT_TRUE(Init(limits, soft_limits)); + test_limit_enforcing(std::nullopt, 2.0, 1.0, true); + test_limit_enforcing(std::nullopt, 1.1, 1.0, true); + test_limit_enforcing(std::nullopt, -5.0, -1.0, true); + test_limit_enforcing(std::nullopt, -std::numeric_limits::infinity(), -1.0, true); + test_limit_enforcing(std::nullopt, std::numeric_limits::infinity(), 1.0, true); + test_limit_enforcing(std::nullopt, -3.212, -1.0, true); + test_limit_enforcing(std::nullopt, -0.8, -0.8, false); + test_limit_enforcing(std::nullopt, 0.8, 0.8, false); + test_limit_enforcing(std::nullopt, 0.12, 0.12, false); + test_limit_enforcing(std::nullopt, 0.0, 0.0, false); + + // The cases where the actual position value exist + test_limit_enforcing(3.0, 5.0, 1.0, true); + test_limit_enforcing(3.8, 5.0, 0.2, true); + test_limit_enforcing(3.5, 0.3, 0.3, false); + test_limit_enforcing(3.5, 0.5, 0.5, false); + test_limit_enforcing(3.8, 0.5, 0.2, true); + + test_limit_enforcing(4.0, 0.5, 0.0, true); + test_limit_enforcing(-2.0, -0.5, 0.0, true); + test_limit_enforcing(4.0, -0.5, -0.5, false); + test_limit_enforcing(-2.0, 0.5, 0.5, false); + test_limit_enforcing(4.0, -3.0, -1.0, true); + test_limit_enforcing(-2.0, 3.0, 1.0, true); + + test_limit_enforcing(4.8, 0.6, -M_PI / 180., true); + test_limit_enforcing(4.3, 5.0, -M_PI / 180., true); + test_limit_enforcing(-4.5, -0.2, M_PI / 180., true); + test_limit_enforcing(-3.0, -5.0, M_PI / 180., true); + + test_limit_enforcing(4.8, -0.6, -M_PI / 180., true); + test_limit_enforcing(4.3, -5.0, -M_PI / 180., true); + test_limit_enforcing(-4.5, 0.2, M_PI / 180., true); + test_limit_enforcing(-3.0, 5.0, M_PI / 180., true); + + test_limit_enforcing(6.0, 2.0, 0.0, true); + test_limit_enforcing(6.0, -2.0, 0.0, true); + test_limit_enforcing(-6.0, -2.0, 0.0, true); + test_limit_enforcing(-6.0, 2.0, 0.0, true); + + test_limit_enforcing(-5.0, -3.0, M_PI / 180., true); + test_limit_enforcing(-5.0, -1.0, M_PI / 180., true); + test_limit_enforcing(5.0, 3.0, -M_PI / 180., true); + test_limit_enforcing(5.0, 1.0, -M_PI / 180., true); + test_limit_enforcing(-5.0, 3.0, M_PI / 180., true); + test_limit_enforcing(-5.0, 1.0, M_PI / 180., true); + test_limit_enforcing(5.0, -3.0, -M_PI / 180., true); + test_limit_enforcing(5.0, -1.0, -M_PI / 180., true); + + // Now remove the position limits and only test with acceleration limits + soft_limits = joint_limits::SoftJointLimits(); + limits.has_position_limits = false; + limits.has_acceleration_limits = true; + limits.max_acceleration = 0.5; + // When launching init, the prev_command_ within the limiter will be reset + ASSERT_TRUE(Init(limits, soft_limits)); + // Now the velocity limits are now saturated by the acceleration limits so in succeeding call it + // will reach the desired if it is within the max velocity limits. Here, the order of the tests is + // important. + for (auto act_pos : + {std::optional(std::nullopt), std::optional(10.0), + std::optional(-10.0)}) + { + test_limit_enforcing(act_pos, 0.0, 0.0, false); // Helps to reset th prev_command internally + test_limit_enforcing(act_pos, 1.0, 0.5, true); + test_limit_enforcing(act_pos, 1.0, 1.0, false); + test_limit_enforcing(act_pos, -0.2, 0.5, true); + test_limit_enforcing(act_pos, -0.2, 0.0, true); + test_limit_enforcing(act_pos, -0.2, -0.2, false); + test_limit_enforcing(act_pos, -0.3, -0.3, false); + test_limit_enforcing(act_pos, -0.9, -0.8, true); + test_limit_enforcing(act_pos, -0.9, -0.9, false); + test_limit_enforcing(act_pos, -2.0, -1.0, true); + test_limit_enforcing(act_pos, 2.0, -0.5, true); + test_limit_enforcing(act_pos, 2.0, 0.0, true); + test_limit_enforcing(act_pos, 2.0, 0.5, true); + test_limit_enforcing(act_pos, 2.0, 1.0, true); + test_limit_enforcing(act_pos, 0.0, 0.5, true); + test_limit_enforcing(act_pos, 0.0, 0.0, false); + } + + // Now re-enable the position limits and test with acceleration limits + limits.has_position_limits = true; + limits.has_acceleration_limits = true; + limits.max_acceleration = 0.5; + // When launching init, the prev_command_ within the limiter will be reset + ASSERT_TRUE(Init(limits, soft_limits)); + // Now the velocity limits are now saturated by the acceleration limits so in succeeding call it + // will reach the desired if it is within the max velocity limits. Here, the order of the tests is + // important. + test_limit_enforcing(4.5, 0.0, 0.0, false); // Helps to reset th prev_command internally + test_limit_enforcing(4.5, 1.0, 0.5, true); + test_limit_enforcing(4.8, 1.0, 0.2, true); + test_limit_enforcing(4.8, -1.0, -0.3, true); + test_limit_enforcing(4.8, -1.0, -0.8, true); + test_limit_enforcing(4.8, -1.0, -1.0, false); + // here the velocity is saturated by the acceleration limits, if not then the velocity will be + // directly -0.2 + test_limit_enforcing(-4.8, -2.0, -0.5, true); + test_limit_enforcing(-4.8, -1.0, -0.2, true); + test_limit_enforcing(-4.3, -1.0, -0.7, true); + test_limit_enforcing(-4.3, 0.0, -0.2, true); + test_limit_enforcing(-4.3, 0.0, 0.0, false); + test_limit_enforcing(-6.0, 1.0, 0.0, true); + test_limit_enforcing(-6.0, -1.0, 0.0, true); + test_limit_enforcing(6.0, 1.0, 0.0, true); + test_limit_enforcing(6.0, -1.0, 0.0, true); +} + +TEST_F(JointSoftLimiterTest, check_desired_effort_only_cases) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_position_limits = true; + limits.min_position = -5.0; + limits.max_position = 5.0; + limits.has_velocity_limits = true; + limits.max_velocity = 1.0; + limits.has_effort_limits = true; + limits.max_effort = 200.0; + joint_limits::SoftJointLimits soft_limits; + ASSERT_TRUE(Init(limits, soft_limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + + rclcpp::Duration period(1, 0); // 1 second + desired_state_.effort = 20.0; + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_TRUE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_jerk()); + + // Now as the position limits are already configure, set the actual position nearby the limits, + // then the max velocity needs to adapt wrt to the position limits + // It is saturated as the position reported is close to the position limits + auto test_limit_enforcing = [&]( + const std::optional & actual_position, + const std::optional & actual_velocity, + double desired_effort, double expected_effort, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + const double act_pos = actual_position.has_value() ? actual_position.value() + : std::numeric_limits::quiet_NaN(); + const double act_vel = actual_velocity.has_value() ? actual_velocity.value() + : std::numeric_limits::quiet_NaN(); + SCOPED_TRACE( + "Testing for actual position: " + std::to_string(act_pos) + ", actual velocity: " + + std::to_string(act_vel) + ", desired effort: " + std::to_string(desired_effort) + + ", expected effort: " + std::to_string(expected_effort) + ", is clamped: " + + std::to_string(is_clamped) + " for the joint limits : " + limits.to_string()); + if (actual_position.has_value()) + { + actual_state_.position = actual_position.value(); + } + if (actual_velocity.has_value()) + { + actual_state_.velocity = actual_velocity.value(); + } + desired_state_.effort = desired_effort; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_TRUE(desired_state_.has_effort()); + EXPECT_NEAR(desired_state_.effort.value(), expected_effort, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_FALSE(desired_state_.has_jerk()); + }; + + for (auto act_pos : + {std::optional(std::nullopt), std::optional(4.0), + std::optional(-4.0)}) + { + for (auto act_vel : + {std::optional(std::nullopt), std::optional(0.4), + std::optional(-0.2)}) + { + test_limit_enforcing(act_pos, act_vel, 20.0, 20.0, false); + test_limit_enforcing(act_pos, act_vel, 200.0, 200.0, false); + test_limit_enforcing(act_pos, act_vel, 201.0, 200.0, true); + test_limit_enforcing(act_pos, act_vel, 0.0, 0.0, false); + test_limit_enforcing(act_pos, act_vel, -20.0, -20.0, false); + test_limit_enforcing(act_pos, act_vel, -200.0, -200.0, false); + test_limit_enforcing(act_pos, act_vel, -201.0, -200.0, true); + } + } + + // The convention is that the positive velocity/position will result in positive effort + // Now the cases where the actual position or the actual velocity is out of the limits + test_limit_enforcing(5.0, 0.0, 20.0, 0.0, true); + test_limit_enforcing(5.0, 0.0, 400.0, 0.0, true); + test_limit_enforcing(6.0, 0.0, 400.0, 0.0, true); + test_limit_enforcing(5.0, 0.0, -20.0, -20.0, false); + test_limit_enforcing(5.0, 0.0, -400.0, -200.0, true); + test_limit_enforcing(6.0, 0.0, -400.0, -200.0, true); + + // At the limit, when trying to move away from the limit, it should allow + test_limit_enforcing(5.0, -0.2, 400.0, 200.0, true); + test_limit_enforcing(5.0, -0.2, -400.0, -200.0, true); + test_limit_enforcing(5.0, -0.2, 30.0, 30.0, false); + test_limit_enforcing(5.0, -0.2, -30.0, -30.0, false); + // For the positive velocity with limit, the effort is saturated + test_limit_enforcing(5.0, 0.2, 400.0, 0.0, true); + test_limit_enforcing(5.0, 0.2, 30.0, 0.0, true); + test_limit_enforcing(5.0, 0.2, -400.0, -200.0, true); + test_limit_enforcing(5.0, 0.2, -30.0, -30.0, false); + + test_limit_enforcing(-5.0, 0.2, 20.0, 20.0, false); + test_limit_enforcing(-5.0, 0.2, 400.0, 200.0, true); + test_limit_enforcing(-5.0, 0.2, -20.0, -20.0, false); + test_limit_enforcing(-5.0, 0.2, -400.0, -200.0, true); + // For the negative velocity with limit, the effort is saturated + test_limit_enforcing(-5.0, -0.2, -400.0, 0.0, true); + test_limit_enforcing(-5.0, -0.2, -30.0, 0.0, true); + test_limit_enforcing(-5.0, -0.2, 400.0, 200.0, true); + test_limit_enforcing(-5.0, -0.2, 30.0, 30.0, false); + + // Check with high values of k_velocity (hard limits should be considered) + soft_limits.k_velocity = 5000.0; + ASSERT_TRUE(Init(limits, soft_limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + for (auto act_pos : + {std::optional(std::nullopt), std::optional(4.0), + std::optional(-4.0)}) + { + for (auto act_vel : + {std::optional(std::nullopt), std::optional(0.5), + std::optional(-0.2)}) + { + test_limit_enforcing(act_pos, act_vel, 20.0, 20.0, false); + test_limit_enforcing(act_pos, act_vel, 200.0, 200.0, false); + test_limit_enforcing(act_pos, act_vel, 201.0, 200.0, true); + test_limit_enforcing(act_pos, act_vel, 0.0, 0.0, false); + test_limit_enforcing(act_pos, act_vel, -20.0, -20.0, false); + test_limit_enforcing(act_pos, act_vel, -200.0, -200.0, false); + test_limit_enforcing(act_pos, act_vel, -201.0, -200.0, true); + } + } + + // Check with low values of k_velocity + soft_limits.k_velocity = 300.0; + ASSERT_TRUE(Init(limits, soft_limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + for (auto act_pos : + {std::optional(std::nullopt), std::optional(4.0), + std::optional(-4.0)}) + { + test_limit_enforcing(act_pos, 0.5, 20.0, 20.0, false); + test_limit_enforcing(act_pos, 0.5, 200.0, 150.0, true); + test_limit_enforcing(act_pos, 0.5, 201.0, 150.0, true); + test_limit_enforcing(act_pos, 0.5, 0.0, 0.0, false); + test_limit_enforcing(act_pos, 0.5, -20.0, -20.0, false); + test_limit_enforcing(act_pos, 0.5, -200.0, -200.0, false); + test_limit_enforcing(act_pos, 0.5, -201.0, -200.0, true); + test_limit_enforcing(act_pos, -0.5, 20.0, 20.0, false); + test_limit_enforcing(act_pos, -0.5, 200.0, 200.0, false); + test_limit_enforcing(act_pos, -0.5, 201.0, 200.0, true); + test_limit_enforcing(act_pos, -0.5, 0.0, 0.0, false); + test_limit_enforcing(act_pos, -0.5, -20.0, -20.0, false); + test_limit_enforcing(act_pos, -0.5, -200.0, -150.0, true); + test_limit_enforcing(act_pos, -0.5, -201.0, -150.0, true); + } + + // Check with low values of k_velocity and low soft_limits + soft_limits.k_velocity = 300.0; + soft_limits.k_position = 1.0; + soft_limits.min_position = -4.0; + soft_limits.max_position = 4.0; + ASSERT_TRUE(Init(limits, soft_limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + test_limit_enforcing(0.0, 0.5, 20.0, 20.0, false); + test_limit_enforcing(0.0, 0.5, 200.0, 150.0, true); + test_limit_enforcing(0.0, 0.5, 201.0, 150.0, true); + test_limit_enforcing(0.0, 0.5, 0.0, 0.0, false); + test_limit_enforcing(0.0, 0.5, -20.0, -20.0, false); + test_limit_enforcing(0.0, 0.5, -200.0, -200.0, false); + test_limit_enforcing(0.0, 0.5, -201.0, -200.0, true); + + test_limit_enforcing(0.0, -0.5, 20.0, 20.0, false); + test_limit_enforcing(0.0, -0.5, 200.0, 200.0, false); + test_limit_enforcing(0.0, -0.5, 201.0, 200.0, true); + test_limit_enforcing(0.0, -0.5, 0.0, 0.0, false); + test_limit_enforcing(0.0, -0.5, -20.0, -20.0, false); + test_limit_enforcing(0.0, -0.5, -200.0, -150.0, true); + test_limit_enforcing(0.0, -0.5, -201.0, -150.0, true); + + // Close to the soft limit with a velocity moving away from the limit + test_limit_enforcing(-3.5, 0.5, 20.0, 20.0, false); + test_limit_enforcing(-3.5, 0.5, 200.0, 150.0, true); + test_limit_enforcing(-3.5, 0.5, 201.0, 150.0, true); + test_limit_enforcing(-3.5, 0.5, 0.0, 0.0, false); + test_limit_enforcing(-3.5, 0.5, -20.0, -20.0, false); + test_limit_enforcing(-3.5, 0.5, -200.0, -200.0, false); + test_limit_enforcing(-3.5, 0.5, -201.0, -200.0, true); + + test_limit_enforcing(3.5, -0.5, 20.0, 20.0, false); + test_limit_enforcing(3.5, -0.5, 200.0, 200.0, false); + test_limit_enforcing(3.5, -0.5, 201.0, 200.0, true); + test_limit_enforcing(3.5, -0.5, 0.0, 0.0, false); + test_limit_enforcing(3.5, -0.5, -20.0, -20.0, false); + test_limit_enforcing(3.5, -0.5, -200.0, -150.0, true); + test_limit_enforcing(3.5, -0.5, -201.0, -150.0, true); + + // Close to the soft limit with a velocity moving close to the limit + test_limit_enforcing(3.5, 0.4, 20.0, 20.0, false); + test_limit_enforcing(3.5, 0.4, 200.0, 30.0, true); + test_limit_enforcing(3.5, 0.5, 200.0, 0.0, true); + test_limit_enforcing(3.5, 0.4, 201.0, 30.0, true); + test_limit_enforcing(3.5, 0.4, 0.0, 0.0, false); + test_limit_enforcing(3.5, 0.4, -20.0, -20.0, false); + test_limit_enforcing(3.5, 0.4, -200.0, -200.0, false); + test_limit_enforcing(3.5, 0.4, -201.0, -200.0, true); + + test_limit_enforcing(-3.5, -0.4, 20.0, 20.0, false); + test_limit_enforcing(-3.5, -0.4, 200.0, 200.0, false); + test_limit_enforcing(-3.5, -0.4, 201.0, 200.0, true); + test_limit_enforcing(-3.5, -0.4, 0.0, 0.0, false); + test_limit_enforcing(-3.5, -0.4, -20.0, -20.0, false); + test_limit_enforcing(-3.5, -0.4, -200.0, -30.0, true); + test_limit_enforcing(-3.5, -0.5, -200.0, 0.0, true); + test_limit_enforcing(-3.5, -0.4, -201.0, -30.0, true); + + // Check with high values of k_velocity and high soft limits (higher than hard limits) + soft_limits.k_velocity = 500.0; + soft_limits.k_position = 1.0; + soft_limits.min_position = -10.0; + soft_limits.max_position = 10.0; + ASSERT_TRUE(Init(limits, soft_limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + for (auto act_pos : + {std::optional(std::nullopt), std::optional(4.0), + std::optional(-4.0)}) + { + for (auto act_vel : + {std::optional(std::nullopt), std::optional(0.5), + std::optional(-0.2)}) + { + test_limit_enforcing(act_pos, act_vel, 20.0, 20.0, false); + test_limit_enforcing(act_pos, act_vel, 200.0, 200.0, false); + test_limit_enforcing(act_pos, act_vel, 201.0, 200.0, true); + test_limit_enforcing(act_pos, act_vel, 0.0, 0.0, false); + test_limit_enforcing(act_pos, act_vel, -20.0, -20.0, false); + test_limit_enforcing(act_pos, act_vel, -200.0, -200.0, false); + test_limit_enforcing(act_pos, act_vel, -201.0, -200.0, true); + } + } + + // Check with low values of k_velocity and high soft limits (higher than hard limits) + soft_limits.k_velocity = 300.0; + soft_limits.k_position = 1.0; + soft_limits.min_position = -10.0; + soft_limits.max_position = 10.0; + ASSERT_TRUE(Init(limits, soft_limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + for (auto act_pos : + {std::optional(std::nullopt), std::optional(4.0), + std::optional(-4.0)}) + { + test_limit_enforcing(act_pos, 0.5, 20.0, 20.0, false); + test_limit_enforcing(act_pos, 0.5, 200.0, 150.0, true); + test_limit_enforcing(act_pos, 0.5, 201.0, 150.0, true); + test_limit_enforcing(act_pos, 0.5, 0.0, 0.0, false); + test_limit_enforcing(act_pos, 0.5, -20.0, -20.0, false); + test_limit_enforcing(act_pos, 0.5, -200.0, -200.0, false); + test_limit_enforcing(act_pos, 0.5, -201.0, -200.0, true); + + test_limit_enforcing(act_pos, -0.5, 20.0, 20.0, false); + test_limit_enforcing(act_pos, -0.5, 200.0, 200.0, false); + test_limit_enforcing(act_pos, -0.5, 201.0, 200.0, true); + test_limit_enforcing(act_pos, -0.5, 0.0, 0.0, false); + test_limit_enforcing(act_pos, -0.5, -20.0, -20.0, false); + test_limit_enforcing(act_pos, -0.5, -200.0, -150.0, true); + test_limit_enforcing(act_pos, -0.5, -201.0, -150.0, true); + } +} + +TEST_F(JointSoftLimiterTest, check_desired_acceleration_only_cases) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_acceleration_limits = true; + limits.max_acceleration = 0.5; + joint_limits::SoftJointLimits soft_limits; + ASSERT_TRUE(Init(limits, soft_limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + rclcpp::Duration period(1, 0); // 1 second + auto test_limit_enforcing = [&]( + const std::optional & actual_velocity, double desired_accel, + double expected_accel, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + const double act_vel = actual_velocity.has_value() ? actual_velocity.value() + : std::numeric_limits::quiet_NaN(); + SCOPED_TRACE( + "Testing for actual velocity: " + std::to_string(act_vel) + ", desired acceleration: " + + std::to_string(desired_accel) + ", expected acceleration: " + std::to_string(expected_accel) + + ", is clamped: " + std::to_string(is_clamped) + + " for the joint limits : " + limits.to_string()); + if (actual_velocity.has_value()) + { + actual_state_.velocity = actual_velocity.value(); + } + desired_state_.acceleration = desired_accel; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_TRUE(desired_state_.has_acceleration()); + EXPECT_NEAR(desired_state_.acceleration.value(), expected_accel, COMMON_THRESHOLD); + EXPECT_FALSE(desired_state_.has_jerk()); + }; + + // Tests without applying deceleration limits + for (auto act_vel : + {std::optional(std::nullopt), std::optional(0.4), + std::optional(-0.2)}) + { + test_limit_enforcing(act_vel, 0.0, 0.0, false); + test_limit_enforcing(act_vel, 0.5, 0.5, false); + test_limit_enforcing(act_vel, 0.6, 0.5, true); + test_limit_enforcing(act_vel, 1.5, 0.5, true); + test_limit_enforcing(act_vel, -0.5, -0.5, false); + test_limit_enforcing(act_vel, -0.6, -0.5, true); + test_limit_enforcing(act_vel, -1.5, -0.5, true); + } + + // Now let's test with applying deceleration limits + limits.has_deceleration_limits = true; + limits.max_deceleration = 0.25; + // When launching init, the prev_command_ within the limiter will be reset + ASSERT_TRUE(Init(limits, soft_limits)); + + // If you don't have the actual velocity, the deceleration limits are not applied + test_limit_enforcing(std::nullopt, 0.0, 0.0, false); + test_limit_enforcing(std::nullopt, 0.5, 0.5, false); + test_limit_enforcing(std::nullopt, 0.6, 0.5, true); + test_limit_enforcing(std::nullopt, 1.5, 0.5, true); + test_limit_enforcing(std::nullopt, -0.5, -0.5, false); + test_limit_enforcing(std::nullopt, -0.6, -0.5, true); + test_limit_enforcing(std::nullopt, -1.5, -0.5, true); + test_limit_enforcing(std::nullopt, 0.0, 0.0, false); + + // Testing both positive and negative velocities and accelerations together without a proper + // deceleration + test_limit_enforcing(0.4, 0.2, 0.2, false); + test_limit_enforcing(0.4, 0.8, 0.5, true); + test_limit_enforcing(-0.4, -0.2, -0.2, false); + test_limit_enforcing(-0.4, -0.6, -0.5, true); + + // The deceleration limits are basically applied when the acceleration is positive and the + // velocity is negative and when the acceleration is negative and the velocity is positive + test_limit_enforcing(0.4, -0.1, -0.1, false); + test_limit_enforcing(0.4, -0.25, -0.25, false); + test_limit_enforcing(0.4, -0.6, -0.25, true); + test_limit_enforcing(0.4, -4.0, -0.25, true); + test_limit_enforcing(-0.4, 0.1, 0.1, false); + test_limit_enforcing(-0.4, 0.25, 0.25, false); + test_limit_enforcing(-0.4, 0.6, 0.25, true); + test_limit_enforcing(-0.4, 3.0, 0.25, true); +} + +TEST_F(JointSoftLimiterTest, check_desired_jerk_only_cases) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_jerk_limits = true; + limits.max_jerk = 0.5; + joint_limits::SoftJointLimits soft_limits; + ASSERT_TRUE(Init(limits, soft_limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + rclcpp::Duration period(1, 0); // 1 second + auto test_limit_enforcing = [&](double desired_jerk, double expected_jerk, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + SCOPED_TRACE( + "Testing for desired jerk : " + std::to_string(desired_jerk) + ", expected jerk: " + + std::to_string(expected_jerk) + ", is clamped: " + std::to_string(is_clamped) + + " for the joint limits : " + limits.to_string()); + desired_state_.jerk = desired_jerk; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_FALSE(desired_state_.has_position()); + EXPECT_FALSE(desired_state_.has_velocity()); + EXPECT_FALSE(desired_state_.has_effort()); + EXPECT_FALSE(desired_state_.has_acceleration()); + EXPECT_TRUE(desired_state_.has_jerk()); + EXPECT_NEAR(desired_state_.jerk.value(), expected_jerk, COMMON_THRESHOLD); + }; + + // Check with jerk limits + test_limit_enforcing(0.0, 0.0, false); + test_limit_enforcing(0.5, 0.5, false); + test_limit_enforcing(0.6, 0.5, true); + test_limit_enforcing(1.5, 0.5, true); + test_limit_enforcing(-0.5, -0.5, false); + test_limit_enforcing(-0.6, -0.5, true); + test_limit_enforcing(-1.5, -0.5, true); +} + +TEST_F(JointSoftLimiterTest, check_all_desired_references_limiting) +{ + SetupNode("joint_saturation_limiter"); + ASSERT_TRUE(Load()); + + joint_limits::JointLimits limits; + limits.has_position_limits = true; + limits.min_position = -5.0; + limits.max_position = 5.0; + limits.has_velocity_limits = true; + limits.max_velocity = 1.0; + limits.has_acceleration_limits = true; + limits.max_acceleration = 0.5; + limits.has_deceleration_limits = true; + limits.max_deceleration = 0.25; + limits.has_jerk_limits = true; + limits.max_jerk = 2.0; + joint_limits::SoftJointLimits soft_limits; + ASSERT_TRUE(Init(limits, soft_limits)); + ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_)); + + rclcpp::Duration period(1, 0); // 1 second + auto test_limit_enforcing = + [&]( + const std::optional & actual_position, const std::optional & actual_velocity, + double desired_position, double desired_velocity, double desired_acceleration, + double desired_jerk, double expected_position, double expected_velocity, + double expected_acceleration, double expected_jerk, bool is_clamped) + { + // Reset the desired and actual states + desired_state_ = {}; + actual_state_ = {}; + const double act_pos = actual_position.has_value() ? actual_position.value() + : std::numeric_limits::quiet_NaN(); + const double act_vel = actual_velocity.has_value() ? actual_velocity.value() + : std::numeric_limits::quiet_NaN(); + SCOPED_TRACE( + "Testing for actual position: " + std::to_string(act_pos) + ", actual velocity: " + + std::to_string(act_vel) + ", desired position: " + std::to_string(desired_position) + + ", desired velocity: " + std::to_string(desired_velocity) + ", desired acceleration: " + + std::to_string(desired_acceleration) + ", desired jerk: " + std::to_string(desired_jerk) + + ", expected position: " + std::to_string(expected_position) + + ", expected velocity: " + std::to_string(expected_velocity) + ", expected acceleration: " + + std::to_string(expected_acceleration) + ", expected jerk: " + std::to_string(expected_jerk) + + ", is clamped: " + std::to_string(is_clamped) + + " for the joint limits : " + limits.to_string()); + if (actual_position.has_value()) + { + actual_state_.position = actual_position.value(); + } + if (actual_velocity.has_value()) + { + actual_state_.velocity = actual_velocity.value(); + } + desired_state_.position = desired_position; + desired_state_.velocity = desired_velocity; + desired_state_.acceleration = desired_acceleration; + desired_state_.jerk = desired_jerk; + ASSERT_EQ(is_clamped, joint_limiter_->enforce(actual_state_, desired_state_, period)); + EXPECT_NEAR(desired_state_.position.value(), expected_position, COMMON_THRESHOLD); + EXPECT_NEAR(desired_state_.velocity.value(), expected_velocity, COMMON_THRESHOLD); + EXPECT_NEAR(desired_state_.acceleration.value(), expected_acceleration, COMMON_THRESHOLD); + EXPECT_NEAR(desired_state_.jerk.value(), expected_jerk, COMMON_THRESHOLD); + }; + + // Test cases when there is no actual position and velocity + // Desired position and velocity affected due to the acceleration limits + test_limit_enforcing(std::nullopt, std::nullopt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 1.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 1.5, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 2.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 2.0, 1.0, 0.5, 2.5, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 1.0, 0.5, 0.5, 3.0, 1.0, 0.5, 0.5, false); + + // Now enforce the limits with actual position and velocity + ASSERT_TRUE(Init(limits, soft_limits)); + // Desired position and velocity affected due to the acceleration limits + test_limit_enforcing(0.5, 0.0, 6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true); + test_limit_enforcing(1.0, 0.0, -6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true); + test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.0, 0.5, 0.5, 0.5, true); + test_limit_enforcing(2.0, 0.5, 6.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(3.0, 0.5, 6.0, 2.0, 1.0, 0.5, 4.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(4.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true); + test_limit_enforcing(4.8, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 0.5, 0.5, 0.5, true); + test_limit_enforcing(5.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 0.0, 0.5, 0.5, true); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} 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 eba16c1e71..1954239a2d 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 @@ -45,7 +45,7 @@ const auto valid_urdf_ros2_control_system_one_interface = )"; -// 2. Industrial Robots with multiple interfaces (cannot be written at the same time) +// 2.1 Industrial Robots with multiple interfaces (cannot be written at the same time) // Note, joint parameters can be any string const auto valid_urdf_ros2_control_system_multi_interface = R"( @@ -86,6 +86,70 @@ const auto valid_urdf_ros2_control_system_multi_interface = )"; +// 2.2 Industrial Robots with multiple interfaces (cannot be written at the same time) +// Additionally some of the Command-/StateInterfaces have parameters defined per interface +const auto valid_urdf_ros2_control_system_multi_interface_and_custom_interface_parameters = + R"( + + + ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware + 2 + 2 + + + + -1 + 1 + 1.2 + 1 + 2 + + + -1.5 + 1.5 + 3.4 + 2 + 4 + + + -0.5 + 0.5 + + + 3 + 2 + + + 4 + 4 + + + 1.1.1.1 + 1234 + true + + + + -1 + 1 + 20 + int32_t + + + 21 + int32_t + + + + 21 + int32_t + + 192.168.178.123 + 4321 + + +)"; + // 3. Industrial Robots with integrated sensor const auto valid_urdf_ros2_control_system_robot_with_sensor = R"( @@ -406,6 +470,81 @@ const auto valid_urdf_ros2_control_system_robot_with_size_and_data_type = )"; +// 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 + + + + + +)"; + // 12. Industrial Robots with integrated GPIO with few disabled limits in joints const auto valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface_limits = R"( diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index 31ce814865..ea0f3c3e2f 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py @@ -22,7 +22,7 @@ from ros2controlcli.api import add_controller_mgr_parsers -def print_controller_state(c, args): +def print_controller_state(c, args, col_width_name, col_width_state, col_width_type): state_color = "" if c.state == "active": state_color = bcolors.OKGREEN @@ -31,7 +31,7 @@ def print_controller_state(c, args): elif c.state == "unconfigured": state_color = bcolors.WARNING - print(f"{c.name:20s}[{c.type:20s}] {state_color}{c.state:10s}{bcolors.ENDC}") + print(f"{state_color}{c.name:<{col_width_name}}{bcolors.ENDC} {c.type:<{col_width_type}} {state_color}{c.state:<{col_width_state}}{bcolors.ENDC}") if args.claimed_interfaces or args.verbose: print("\tclaimed interfaces:") for claimed_interface in c.claimed_interfaces: @@ -96,7 +96,13 @@ def add_arguments(self, parser, cli_name): def main(self, *, args): with NodeStrategy(args) as node: response = list_controllers(node, args.controller_manager) + + # Structure data as table for nicer output + col_width_name = max(len(ctrl.name) for ctrl in response.controller) + col_width_type = max(len(ctrl.type) for ctrl in response.controller) + col_width_state = max(len(ctrl.state) for ctrl in response.controller) + for c in response.controller: - print_controller_state(c, args) + print_controller_state(c, args, col_width_name, col_width_state, col_width_type) return 0 diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py index 8a5884f2cb..f159c223f5 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py @@ -15,6 +15,8 @@ from controller_manager import list_hardware_components from controller_manager.spawner import bcolors +from lifecycle_msgs.msg import State + from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy from ros2cli.verb import VerbExtension @@ -39,17 +41,26 @@ def main(self, *, args): hardware_components = list_hardware_components(node, args.controller_manager) for idx, component in enumerate(hardware_components.component): + # Set activity color for nicer visualization + activity_color = bcolors.FAIL + if component.state.id == State.PRIMARY_STATE_UNCONFIGURED: + activity_color = bcolors.WARNING + if component.state.id == State.PRIMARY_STATE_INACTIVE: + activity_color = bcolors.OKCYAN + if component.state.id == State.PRIMARY_STATE_ACTIVE: + activity_color = bcolors.OKGREEN + print( - f"Hardware Component {idx+1}\n\tname: {component.name}\n\ttype: {component.type}" + f"Hardware Component {idx+1}\n\tname: {activity_color}{component.name}{bcolors.ENDC}\n\ttype: {component.type}" ) if hasattr(component, "plugin_name"): - plugin_name = component.plugin_name + plugin_name = f"{component.plugin_name}" else: plugin_name = f"{bcolors.WARNING}plugin name missing!{bcolors.ENDC}" print( f"\tplugin name: {plugin_name}\n" - f"\tstate: id={component.state.id} label={component.state.label}\n" + f"\tstate: id={component.state.id} label={activity_color}{component.state.label}{bcolors.ENDC}\n" f"\tcommand interfaces" ) for cmd_interface in component.command_interfaces: diff --git a/transmission_interface/include/transmission_interface/handle.hpp b/transmission_interface/include/transmission_interface/handle.hpp index bc1c0a78d8..7696db03d9 100644 --- a/transmission_interface/include/transmission_interface/handle.hpp +++ b/transmission_interface/include/transmission_interface/handle.hpp @@ -22,17 +22,17 @@ namespace transmission_interface { /** A handle used to get and set a value on a given actuator interface. */ -class ActuatorHandle : public hardware_interface::ReadWriteHandle +class ActuatorHandle : public hardware_interface::Handle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::Handle::Handle; }; /** A handle used to get and set a value on a given joint interface. */ -class JointHandle : public hardware_interface::ReadWriteHandle +class JointHandle : public hardware_interface::Handle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::Handle::Handle; }; } // namespace transmission_interface