Merge branch 'master' into fix/on_error/controller_deactivation

.pre-commit-config.yaml

@@ -63,7 +63,7 @@ repos:
 
   # CPP hooks
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v18.1.8
+    rev: v19.1.0
     hooks:
       - id: clang-format
         args: ['-fallback-style=none', '-i']
@@ -109,7 +109,7 @@ repos:
 
   # Docs - RestructuredText hooks
   - repo: https://github.com/PyCQA/doc8
-    rev: v1.1.1
+    rev: v1.1.2
     hooks:
       - id: doc8
         args: ['--max-line-length=100', '--ignore=D001']
@@ -133,7 +133,7 @@ repos:
         exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
 
   - repo: https://github.com/python-jsonschema/check-jsonschema
-    rev: 0.29.2
+    rev: 0.29.3
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

controller_interface/CHANGELOG.rst

@@ -2,6 +2,13 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.18.0 (2024-10-07)
+-------------------
+* Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 <https://github.com/ros-controls/ros2_control/issues/1689>`_)
+* [ControllerInterface] Fix to properly propagate the controller NodeOptions (`#1762 <https://github.com/ros-controls/ros2_control/issues/1762>`_)
+* [Controller Interface] Make assign and release interfaces virtual (`#1743 <https://github.com/ros-controls/ros2_control/issues/1743>`_)
+* Contributors: Manuel Muth, Sai Kishor Kothakota
+
 4.17.0 (2024-09-11)
 -------------------
 * Rename `get_state` and `set_state` Functions to `get/set_lifecylce_state` (variant support) (`#1683 <https://github.com/ros-controls/ros2_control/issues/1683>`_)

controller_interface/include/controller_interface/chainable_controller_interface.hpp

@@ -15,7 +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"
@@ -57,10 +59,10 @@ class ChainableControllerInterface : public ControllerInterfaceBase
   bool is_chainable() const final;
 
   CONTROLLER_INTERFACE_PUBLIC
-  std::vector<hardware_interface::StateInterface> export_state_interfaces() final;
+  std::vector<hardware_interface::StateInterface::ConstSharedPtr> export_state_interfaces() final;
 
   CONTROLLER_INTERFACE_PUBLIC
-  std::vector<hardware_interface::CommandInterface> export_reference_interfaces() final;
+  std::vector<hardware_interface::CommandInterface::SharedPtr> export_reference_interfaces() final;
 
   CONTROLLER_INTERFACE_PUBLIC
   bool set_chained_mode(bool chained_mode) final;
@@ -131,11 +133,21 @@ class ChainableControllerInterface : public ControllerInterfaceBase
 
   /// Storage of values for state interfaces
   std::vector<std::string> exported_state_interface_names_;
+  std::vector<hardware_interface::StateInterface::SharedPtr> ordered_exported_state_interfaces_;
+  std::unordered_map<std::string, hardware_interface::StateInterface::SharedPtr>
+    exported_state_interfaces_;
+  // BEGIN (Handle export change): for backward compatibility
   std::vector<double> state_interfaces_values_;
+  // END
 
   /// Storage of values for reference interfaces
   std::vector<std::string> exported_reference_interface_names_;
+  // BEGIN (Handle export change): for backward compatibility
   std::vector<double> reference_interfaces_;
+  // END
+  std::vector<hardware_interface::CommandInterface::SharedPtr> ordered_reference_interfaces_;
+  std::unordered_map<std::string, hardware_interface::CommandInterface::SharedPtr>
+    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

@@ -47,15 +47,15 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase
    * \returns empty list.
    */
   CONTROLLER_INTERFACE_PUBLIC
-  std::vector<hardware_interface::StateInterface> export_state_interfaces() final;
+  std::vector<hardware_interface::StateInterface::ConstSharedPtr> export_state_interfaces() final;
 
   /**
    * Controller has no reference interfaces.
    *
    * \returns empty list.
    */
   CONTROLLER_INTERFACE_PUBLIC
-  std::vector<hardware_interface::CommandInterface> export_reference_interfaces() final;
+  std::vector<hardware_interface::CommandInterface::SharedPtr> export_reference_interfaces() final;
 
   /**
    * Controller is not chainable, therefore no chained mode can be set.

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -185,14 +185,15 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   CONTROLLER_INTERFACE_PUBLIC
   virtual rclcpp::NodeOptions define_custom_node_options() const
   {
+    rclcpp::NodeOptions node_options;
 // \note The versions conditioning is added here to support the source-compatibility with Humble
 #if RCLCPP_VERSION_MAJOR >= 21
-    return rclcpp::NodeOptions().enable_logger_service(true);
+    node_options.enable_logger_service(true);
 #else
-    return rclcpp::NodeOptions()
-      .allow_undeclared_parameters(true)
-      .automatically_declare_parameters_from_overrides(true);
+    node_options.allow_undeclared_parameters(true);
+    node_options.automatically_declare_parameters_from_overrides(true);
 #endif
+    return node_options;
   }
 
   /// Declare and initialize a parameter with a type.
@@ -234,7 +235,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<hardware_interface::CommandInterface::SharedPtr>
+  export_reference_interfaces() = 0;
 
   /**
    * Export interfaces for a chainable controller that can be used as state interface by other
@@ -243,7 +245,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
    * \returns list of state interfaces for preceding controllers.
    */
   CONTROLLER_INTERFACE_PUBLIC
-  virtual std::vector<hardware_interface::StateInterface> export_state_interfaces() = 0;
+  virtual std::vector<hardware_interface::StateInterface::ConstSharedPtr>
+  export_state_interfaces() = 0;
 
   /**
    * Set chained mode of a chainable controller. This method triggers internal processes to switch

controller_interface/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>controller_interface</name>
-  <version>4.17.0</version>
+  <version>4.18.0</version>
   <description>Description of controller_interface</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

controller_interface/src/chainable_controller_interface.cpp

@@ -44,53 +44,135 @@ return_type ChainableControllerInterface::update(
   return ret;
 }
 
-std::vector<hardware_interface::StateInterface>
+std::vector<hardware_interface::StateInterface::ConstSharedPtr>
 ChainableControllerInterface::export_state_interfaces()
 {
   auto state_interfaces = on_export_state_interfaces();
+  std::vector<hardware_interface::StateInterface::ConstSharedPtr> state_interfaces_ptrs_vec;
+  state_interfaces_ptrs_vec.reserve(state_interfaces.size());
+  ordered_exported_state_interfaces_.reserve(state_interfaces.size());
+  exported_state_interface_names_.reserve(state_interfaces.size());
 
   // check if the names of the controller state interfaces begin with the controller's name
   for (const auto & interface : state_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 state interfaces. No state 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());
-      state_interfaces.clear();
-      break;
+      std::string error_msg =
+        "The prefix of the interface '" + interface.get_prefix_name() +
+        "' does not equal the controller's name '" + get_node()->get_name() +
+        "'. This is mandatory for state interfaces. No state interface will be exported. Please "
+        "correct and recompile the controller with name '" +
+        get_node()->get_name() + "' and try again.";
+      throw std::runtime_error(error_msg);
+    }
+    auto state_interface = std::make_shared<hardware_interface::StateInterface>(interface);
+    const auto interface_name = state_interface->get_name();
+    auto [it, succ] = exported_state_interfaces_.insert({interface_name, state_interface});
+    // either we have name duplicate which we want to avoid under all circumstances since interfaces
+    // need to be uniquely identify able or something else really went wrong. In any case abort and
+    // inform cm by throwing exception
+    if (!succ)
+    {
+      std::string error_msg =
+        "Could not insert StateInterface<" + interface_name +
+        "> into exported_state_interfaces_ map. Check if you export duplicates. The "
+        "map returned iterator with interface_name<" +
+        it->second->get_name() +
+        ">. If its a duplicate adjust exportation of InterfacesDescription so that all the "
+        "interface names are unique.";
+      exported_state_interfaces_.clear();
+      exported_state_interface_names_.clear();
+      state_interfaces_ptrs_vec.clear();
+      throw std::runtime_error(error_msg);
     }
+    ordered_exported_state_interfaces_.push_back(state_interface);
+    exported_state_interface_names_.push_back(interface_name);
+    state_interfaces_ptrs_vec.push_back(
+      std::const_pointer_cast<const hardware_interface::StateInterface>(state_interface));
   }
 
-  return state_interfaces;
+  return state_interfaces_ptrs_vec;
 }
 
-std::vector<hardware_interface::CommandInterface>
+std::vector<hardware_interface::CommandInterface::SharedPtr>
 ChainableControllerInterface::export_reference_interfaces()
 {
   auto reference_interfaces = on_export_reference_interfaces();
+  std::vector<hardware_interface::CommandInterface::SharedPtr> reference_interfaces_ptrs_vec;
+  reference_interfaces_ptrs_vec.reserve(reference_interfaces.size());
+  exported_reference_interface_names_.reserve(reference_interfaces.size());
+  ordered_reference_interfaces_.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())
+  {
+    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);
     }
+
+    hardware_interface::CommandInterface::SharedPtr reference_interface =
+      std::make_shared<hardware_interface::CommandInterface>(std::move(interface));
+    const auto inteface_name = reference_interface->get_name();
+    // check the exported interface name is unique
+    auto [it, succ] = reference_interfaces_ptrs_.insert({inteface_name, reference_interface});
+    // either we have name duplicate which we want to avoid under all circumstances since interfaces
+    // need to be uniquely identify able or something else really went wrong. In any case abort and
+    // inform cm by throwing exception
+    if (!succ)
+    {
+      std::string error_msg =
+        "Could not insert Reference interface<" + inteface_name +
+        "> into reference_interfaces_ map. Check if you export duplicates. The "
+        "map returned iterator with interface_name<" +
+        it->second->get_name() +
+        ">. If its a duplicate adjust exportation of InterfacesDescription so that all the "
+        "interface names are unique.";
+      reference_interfaces_.clear();
+      exported_reference_interface_names_.clear();
+      reference_interfaces_ptrs_vec.clear();
+      throw std::runtime_error(error_msg);
+    }
+    ordered_reference_interfaces_.push_back(reference_interface);
+    exported_reference_interface_names_.push_back(inteface_name);
+    reference_interfaces_ptrs_vec.push_back(reference_interface);
   }
 
-  return reference_interfaces;
+  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_ptrs_vec;
 }
 
 bool ChainableControllerInterface::set_chained_mode(bool chained_mode)
@@ -130,8 +212,8 @@ ChainableControllerInterface::on_export_state_interfaces()
   std::vector<hardware_interface::StateInterface> state_interfaces;
   for (size_t i = 0; i < exported_state_interface_names_.size(); ++i)
   {
-    state_interfaces.emplace_back(hardware_interface::StateInterface(
-      get_node()->get_name(), exported_state_interface_names_[i], &state_interfaces_values_[i]));
+    state_interfaces.emplace_back(
+      get_node()->get_name(), exported_state_interface_names_[i], &state_interfaces_values_[i]);
   }
   return state_interfaces;
 }

controller_interface/src/controller_interface.cpp

@@ -22,12 +22,14 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {}
 
 bool ControllerInterface::is_chainable() const { return false; }
 
-std::vector<hardware_interface::StateInterface> ControllerInterface::export_state_interfaces()
+std::vector<hardware_interface::StateInterface::ConstSharedPtr>
+ControllerInterface::export_state_interfaces()
 {
   return {};
 }
 
-std::vector<hardware_interface::CommandInterface> ControllerInterface::export_reference_interfaces()
+std::vector<hardware_interface::CommandInterface::SharedPtr>
+ControllerInterface::export_reference_interfaces()
 {
   return {};
 }