Features integration joint limits

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 <memory>
+#include <string>
+#include <unordered_map>
 #include <vector>
 
 #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<hardware_interface::CommandInterface> export_reference_interfaces() final;
+  std::vector<std::shared_ptr<hardware_interface::CommandInterface>> 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<double> reference_interfaces_;
+  // END
+  std::unordered_map<std::string, std::shared_ptr<hardware_interface::CommandInterface>>
+    reference_interfaces_ptrs_;
 
 private:
   /// A flag marking if a chainable controller is currently preceded by another controller.

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<hardware_interface::CommandInterface> export_reference_interfaces() final;
+  std::vector<std::shared_ptr<hardware_interface::CommandInterface>> export_reference_interfaces()
+    final;
 
   /**
    * Controller is not chainable, therefore no chained mode can be set.

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<hardware_interface::CommandInterface> export_reference_interfaces() = 0;
+  virtual std::vector<std::shared_ptr<hardware_interface::CommandInterface>>
+  export_reference_interfaces() = 0;
 
   /**
    * Set chained mode of a chainable controller. This method triggers internal processes to switch

controller_interface/src/chainable_controller_interface.cpp

@@ -44,44 +44,63 @@ return_type ChainableControllerInterface::update(
   return ret;
 }
 
-std::vector<hardware_interface::CommandInterface>
+std::vector<std::shared_ptr<hardware_interface::CommandInterface>>
 ChainableControllerInterface::export_reference_interfaces()
 {
   auto reference_interfaces = on_export_reference_interfaces();
+  std::vector<std::shared_ptr<hardware_interface::CommandInterface>> 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<hardware_interface::CommandInterface> interface_ptr =
+      std::make_shared<hardware_interface::CommandInterface>(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)

controller_interface/src/controller_interface.cpp

@@ -28,7 +28,8 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {}
 
 bool ControllerInterface::is_chainable() const { return false; }
 
-std::vector<hardware_interface::CommandInterface> ControllerInterface::export_reference_interfaces()
+std::vector<std::shared_ptr<hardware_interface::CommandInterface>>
+ControllerInterface::export_reference_interfaces()
 {
   return {};
 }

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());

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<std::shared_ptr<hardware_interface::CommandInterface>> 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);

doc/migration/Iron.rst

@@ -0,0 +1,140 @@
+Iron to Jazzy
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+component parser
+*****************
+Changes from `(PR #1256) <https://github.com/ros-controls/ros2_control/pull/1256>`__
+
+* All ``joints`` defined in the ``<ros2_control>``-tag have to be present in the URDF received :ref:`by the controller manager <doc/ros2_control/controller_manager/doc/userdoc:subscribers>`, otherwise a ``std::runtime_error`` is thrown. This is to ensure that the URDF and the ``<ros2_control>``-tag are consistent. E.g., for configuration ports use ``gpio`` tags instead.
+* The syntax for mimic joints is changed to the `official URDF specification <https://wiki.ros.org/urdf/XML/joint>`__. The parameters within the ``ros2_control`` tag are not supported any more. Instead of
+
+  .. code-block:: xml
+
+    <ros2_control name="GazeboSystem" type="system">
+      <joint name="right_finger_joint">
+        <command_interface name="position"/>
+        <state_interface name="position">
+          <param name="initial_value">0.15</param>
+        </state_interface>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
+      <joint name="left_finger_joint">
+        <param name="mimic">right_finger_joint</param>
+        <param name="multiplier">1</param>
+        <command_interface name="position"/>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
+    </ros2_control>
+
+  define your mimic joints directly in the joint definitions:
+
+  .. code-block:: xml
+
+    <joint name="right_finger_joint" type="prismatic">
+      <axis xyz="0 1 0"/>
+      <origin xyz="0.0 -0.48 1" rpy="0.0 0.0 0.0"/>
+      <parent link="base"/>
+      <child link="finger_right"/>
+      <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
+    </joint>
+    <joint name="left_finger_joint" type="prismatic">
+      <mimic joint="right_finger_joint" multiplier="1" offset="0"/>
+      <axis xyz="0 1 0"/>
+      <origin xyz="0.0 0.48 1" rpy="0.0 0.0 3.1415926535"/>
+      <parent link="base"/>
+      <child link="finger_left"/>
+      <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
+    </joint>
+
+Adaption of Command-/StateInterfaces
+***************************************
+Changes from `(PR #1240) <https://github.com/ros-controls/ros2_control/pull/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 <https://github.com/ros-controls/ros2_control/tree/{REPOS_FILE_BRANCH}/hardware_interface/include/hardware_interface/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<std::string, InterfaceDescription>``, 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<hardware_interface::CommandInterface> export_command_interfaces() override`` and ``std::vector<hardware_interface::StateInterface> export_state_interfaces() override``.
+2. Delete allocated memory for any ``Command-/StateInterfaces``, e.g.:
+
+  * If you have a ``std::vector<double> 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<hardware_interface::InterfaceDescription> export_command_interfaces_2()`` or ``virtual std::vector<hardware_interface::InterfaceDescription> 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<hardware_interface::InterfaceDescription> 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!

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)