Merge branch 'master' into fix/spawner-interrupt

.github/workflows/docker.yml

@@ -42,7 +42,7 @@ jobs:
             type=semver,pattern={{major}}.{{minor}}
 
       - name: Build and push Docker image
-        uses: docker/build-push-action@v5
+        uses: docker/build-push-action@v6
         with:
           context: .
           push: true
@@ -82,7 +82,7 @@ jobs:
             type=semver,pattern={{major}}.{{minor}}
 
       - name: Build and push Docker image
-        uses: docker/build-push-action@v5
+        uses: docker/build-push-action@v6
         with:
           context: .
           push: true

.github/workflows/rosdoc2.yml

@@ -0,0 +1,14 @@
+name: rosdoc2
+
+on:
+  workflow_dispatch:
+  pull_request:
+    paths:
+      - ros2_control/doc/**
+      - ros2_control/rosdoc2.yaml
+      - ros2_control/package.xml
+
+
+jobs:
+  check:
+    uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rosdoc2.yml@master

.pre-commit-config.yaml

@@ -26,6 +26,7 @@ repos:
       - id: check-symlinks
       - id: check-xml
       - id: check-yaml
+        args: ["--allow-multiple-documents"]
       - id: debug-statements
       - id: end-of-file-fixer
       - id: mixed-line-ending
@@ -36,7 +37,7 @@ repos:
 
   # Python hooks
   - repo: https://github.com/asottile/pyupgrade
-    rev: v3.15.2
+    rev: v3.16.0
     hooks:
     -   id: pyupgrade
         args: [--py36-plus]
@@ -55,14 +56,14 @@ repos:
         args: ["--line-length=99"]
 
   - repo: https://github.com/pycqa/flake8
-    rev: 7.0.0
+    rev: 7.1.0
     hooks:
     - id: flake8
       args: ["--extend-ignore=E501"]
 
   # CPP hooks
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v18.1.5
+    rev: v18.1.8
     hooks:
       - id: clang-format
         args: ['-fallback-style=none', '-i']
@@ -132,7 +133,7 @@ repos:
         exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
 
   - repo: https://github.com/python-jsonschema/check-jsonschema
-    rev: 0.28.4
+    rev: 0.28.6
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

controller_interface/CHANGELOG.rst

@@ -2,6 +2,14 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.13.0 (2024-07-08)
+-------------------
+* [ControllerChaining] Export state interfaces from chainable controllers (`#1021 <https://github.com/ros-controls/ros2_control/issues/1021>`_)
+* Contributors: Sai Kishor Kothakota
+
+4.12.0 (2024-07-01)
+-------------------
+
 4.11.0 (2024-05-14)
 -------------------
 * Fix dependencies for source build (`#1533 <https://github.com/ros-controls/ros2_control/issues/1533>`_)

controller_interface/include/controller_interface/chainable_controller_interface.hpp

@@ -15,6 +15,7 @@
 #ifndef CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_
 #define CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_
 
+#include <string>
 #include <vector>
 
 #include "controller_interface/controller_interface_base.hpp"
@@ -55,6 +56,9 @@ class ChainableControllerInterface : public ControllerInterfaceBase
   CONTROLLER_INTERFACE_PUBLIC
   bool is_chainable() const final;
 
+  CONTROLLER_INTERFACE_PUBLIC
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() final;
+
   CONTROLLER_INTERFACE_PUBLIC
   std::vector<hardware_interface::CommandInterface> export_reference_interfaces() final;
 
@@ -65,16 +69,27 @@ class ChainableControllerInterface : public ControllerInterfaceBase
   bool is_in_chained_mode() const final;
 
 protected:
-  /// Virtual method that each chainable controller should implement to export its chainable
-  /// interfaces.
+  /// Virtual method that each chainable controller should implement to export its read-only
+  /// chainable interfaces.
+  /**
+   * Each chainable controller implements this methods where all its state(read only) interfaces are
+   * exported. The method has the same meaning as `export_state_interfaces` method from
+   * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface.
+   *
+   * \returns list of StateInterfaces that other controller can use as their inputs.
+   */
+  virtual std::vector<hardware_interface::StateInterface> on_export_state_interfaces();
+
+  /// Virtual method that each chainable controller should implement to export its read/write
+  /// chainable interfaces.
   /**
    * Each chainable controller implements this methods where all input (command) interfaces are
    * exported. The method has the same meaning as `export_command_interface` method from
    * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface.
    *
    * \returns list of CommandInterfaces that other controller can use as their outputs.
    */
-  virtual std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() = 0;
+  virtual std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces();
 
   /// Virtual method that each chainable controller should implement to switch chained mode.
   /**
@@ -114,7 +129,12 @@ class ChainableControllerInterface : public ControllerInterfaceBase
   virtual return_type update_and_write_commands(
     const rclcpp::Time & time, const rclcpp::Duration & period) = 0;
 
+  /// Storage of values for state interfaces
+  std::vector<std::string> exported_state_interface_names_;
+  std::vector<double> state_interfaces_values_;
+
   /// Storage of values for reference interfaces
+  std::vector<std::string> exported_reference_interface_names_;
   std::vector<double> reference_interfaces_;
 
 private:

controller_interface/include/controller_interface/controller_interface.hpp

@@ -42,6 +42,14 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase
   CONTROLLER_INTERFACE_PUBLIC
   bool is_chainable() const final;
 
+  /**
+   * A non-chainable controller doesn't export any state interfaces.
+   *
+   * \returns empty list.
+   */
+  CONTROLLER_INTERFACE_PUBLIC
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() final;
+
   /**
    * Controller has no reference interfaces.
    *

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -224,11 +224,20 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   CONTROLLER_INTERFACE_PUBLIC
   virtual std::vector<hardware_interface::CommandInterface> export_reference_interfaces() = 0;
 
+  /**
+   * Export interfaces for a chainable controller that can be used as state interface by other
+   * controllers.
+   *
+   * \returns list of state interfaces for preceding controllers.
+   */
+  CONTROLLER_INTERFACE_PUBLIC
+  virtual std::vector<hardware_interface::StateInterface> export_state_interfaces() = 0;
+
   /**
    * Set chained mode of a chainable controller. This method triggers internal processes to switch
    * a chainable controller to "chained" mode and vice-versa. Setting controller to "chained" mode
-   * usually involves disabling of subscribers and other external interfaces to avoid potential
-   * concurrency in input commands.
+   * usually involves the usage of the controller's reference interfaces by the other
+   * controllers
    *
    * \returns true if mode is switched successfully and false if not.
    */

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.11.0</version>
+  <version>4.13.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,26 +44,35 @@ return_type ChainableControllerInterface::update(
   return ret;
 }
 
-std::vector<hardware_interface::CommandInterface>
-ChainableControllerInterface::export_reference_interfaces()
+std::vector<hardware_interface::StateInterface>
+ChainableControllerInterface::export_state_interfaces()
 {
-  auto reference_interfaces = on_export_reference_interfaces();
+  auto state_interfaces = on_export_state_interfaces();
 
-  // check if the "reference_interfaces_" variable is resized to number of interfaces
-  if (reference_interfaces_.size() != reference_interfaces.size())
+  // check if the names of the controller state interfaces begin with the controller's name
+  for (const auto & interface : state_interfaces)
   {
-    // 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();
+    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;
+    }
   }
 
+  return state_interfaces;
+}
+
+std::vector<hardware_interface::CommandInterface>
+ChainableControllerInterface::export_reference_interfaces()
+{
+  auto reference_interfaces = on_export_reference_interfaces();
+
   // check if the names of the reference interfaces begin with the controller's name
   for (const auto & interface : reference_interfaces)
   {
@@ -113,4 +122,30 @@ bool ChainableControllerInterface::is_in_chained_mode() const { return in_chaine
 
 bool ChainableControllerInterface::on_set_chained_mode(bool /*chained_mode*/) { return true; }
 
+std::vector<hardware_interface::StateInterface>
+ChainableControllerInterface::on_export_state_interfaces()
+{
+  state_interfaces_values_.resize(exported_state_interface_names_.size(), 0.0);
+  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]));
+  }
+  return state_interfaces;
+}
+
+std::vector<hardware_interface::CommandInterface>
+ChainableControllerInterface::on_export_reference_interfaces()
+{
+  reference_interfaces_.resize(exported_reference_interface_names_.size(), 0.0);
+  std::vector<hardware_interface::CommandInterface> reference_interfaces;
+  for (size_t i = 0; i < exported_reference_interface_names_.size(); ++i)
+  {
+    reference_interfaces.emplace_back(hardware_interface::CommandInterface(
+      get_node()->get_name(), exported_reference_interface_names_[i], &reference_interfaces_[i]));
+  }
+  return reference_interfaces;
+}
+
 }  // namespace controller_interface

controller_interface/src/controller_interface.cpp

@@ -28,6 +28,11 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {}
 
 bool ControllerInterface::is_chainable() const { return false; }
 
+std::vector<hardware_interface::StateInterface> ControllerInterface::export_state_interfaces()
+{
+  return {};
+}
+
 std::vector<hardware_interface::CommandInterface> ControllerInterface::export_reference_interfaces()
 {
   return {};