[Feature] Export chainable state interfaces from chainable controllers

controller_interface/include/controller_interface/chainable_controller_interface.hpp

@@ -55,6 +55,9 @@ class ChainableControllerInterface : public ControllerInterfaceBase
   CONTROLLER_INTERFACE_PUBLIC
   bool is_chainable() const final;
 
+  CONTROLLER_INTERFACE_PUBLIC
+  std::vector<hardware_interface::StateInterface> export_internal_state_interfaces() final;
+
   CONTROLLER_INTERFACE_PUBLIC
   std::vector<hardware_interface::CommandInterface> export_reference_interfaces() final;
 
@@ -64,9 +67,23 @@ class ChainableControllerInterface : public ControllerInterfaceBase
   CONTROLLER_INTERFACE_PUBLIC
   bool is_in_chained_mode() const final;
 
+  CONTROLLER_INTERFACE_PUBLIC
+  bool toggle_references_from_subscribers(bool enable) 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_internal_state_interfaces` method from
+   * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface.
+   *
+   * \returns list of StateInterfaces that other controller can use as their outputs.
+   */
+  virtual std::vector<hardware_interface::StateInterface> on_export_internal_state_interfaces() = 0;
+
+  /// 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
@@ -114,12 +131,19 @@ class ChainableControllerInterface : public ControllerInterfaceBase
   virtual return_type update_and_write_commands(
     const rclcpp::Time & time, const rclcpp::Duration & period) = 0;
 
+  /// Storage of values for internal_state interfaces
+  std::vector<double> internal_state_interfaces_data_;
+
   /// Storage of values for reference interfaces
   std::vector<double> reference_interfaces_;
 
 private:
   /// A flag marking if a chainable controller is currently preceded by another controller.
   bool in_chained_mode_ = false;
+
+  /// A flag marking whether to use references from subscribers or from the interfaces as input
+  /// commands
+  bool use_references_from_subscribers_ = false;
 };
 
 }  // namespace controller_interface

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_internal_state_interfaces() final;
+
   /**
    * Controller has no reference interfaces.
    *
@@ -65,6 +73,14 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase
    */
   CONTROLLER_INTERFACE_PUBLIC
   bool is_in_chained_mode() const final;
+
+  /**
+   * A non-chainable controller cannot use references from subscribers
+   *
+   * @return false
+   */
+  CONTROLLER_INTERFACE_PUBLIC
+  bool toggle_references_from_subscribers(bool enable) final;
 };
 
 using ControllerInterfaceSharedPtr = std::shared_ptr<ControllerInterface>;

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -223,11 +223,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_internal_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/internal state interfaces by the other
+   * controllers
    *
    * \returns true if mode is switched successfully and false if not.
    */
@@ -245,6 +254,17 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   CONTROLLER_INTERFACE_PUBLIC
   virtual bool is_in_chained_mode() const = 0;
 
+  /// A method to enable the usage of references from subscribers
+  /**
+   * Enable or disable the retrieval of references for a chainable controller. This methods helps
+   * to toggle between the usage of reference from the subscribers or the external interfaces to
+   * avoid potential concurrency in input commands
+   *
+   * @return true if the toggle is successful and false if not
+   */
+  CONTROLLER_INTERFACE_PUBLIC
+  virtual bool toggle_references_from_subscribers(bool enable) = 0;
+
 protected:
   std::vector<hardware_interface::LoanedCommandInterface> command_interfaces_;
   std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;

controller_interface/src/chainable_controller_interface.cpp

@@ -30,7 +30,7 @@ return_type ChainableControllerInterface::update(
 {
   return_type ret = return_type::ERROR;
 
-  if (!is_in_chained_mode())
+  if (use_references_from_subscribers_)
   {
     ret = update_reference_from_subscribers(time, period);
     if (ret != return_type::OK)
@@ -44,6 +44,45 @@ return_type ChainableControllerInterface::update(
   return ret;
 }
 
+std::vector<hardware_interface::StateInterface>
+ChainableControllerInterface::export_internal_state_interfaces()
+{
+  auto internal_state_interfaces = on_export_internal_state_interfaces();
+  // check if the "internal_state_interfaces_data_" variable is resized to number of interfaces
+  if (internal_state_interfaces_data_.size() != internal_state_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 internal state values 'internal_state_interfaces_data_' variable "
+      "has size '%zu', but it is expected to have the size '%zu' equal to the number of internal "
+      "state interfaces. No state interface will be exported. Please correct and recompile "
+      "the controller with name '%s' and try again.",
+      internal_state_interfaces_data_.size(), internal_state_interfaces.size(),
+      get_node()->get_name());
+    internal_state_interfaces.clear();
+  }
+
+  // check if the names of the controller state interfaces begin with the controller's name
+  for (const auto & interface : internal_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 internal state interfaces. No internal 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());
+      internal_state_interfaces.clear();
+      break;
+    }
+  }
+
+  return internal_state_interfaces;
+}
+
 std::vector<hardware_interface::CommandInterface>
 ChainableControllerInterface::export_reference_interfaces()
 {
@@ -111,6 +150,26 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode)
 
 bool ChainableControllerInterface::is_in_chained_mode() const { return in_chained_mode_; }
 
+bool ChainableControllerInterface::toggle_references_from_subscribers(bool enable)
+{
+  bool result = false;
+  if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+  {
+    use_references_from_subscribers_ = enable;
+    result = true;
+  }
+  else
+  {
+    RCLCPP_ERROR(
+      get_node()->get_logger(),
+      "Can not toggle the controller's references between subscribers and interfaces in '%s' "
+      "state. "
+      " Current state is '%s'.",
+      hardware_interface::lifecycle_state_names::ACTIVE, get_state().label().c_str());
+  }
+  return result;
+}
+
 bool ChainableControllerInterface::on_set_chained_mode(bool /*chained_mode*/) { return true; }
 
 }  // namespace controller_interface

controller_interface/src/controller_interface.cpp

@@ -28,6 +28,12 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {}
 
 bool ControllerInterface::is_chainable() const { return false; }
 
+std::vector<hardware_interface::StateInterface>
+ControllerInterface::export_internal_state_interfaces()
+{
+  return {};
+}
+
 std::vector<hardware_interface::CommandInterface> ControllerInterface::export_reference_interfaces()
 {
   return {};
@@ -37,4 +43,6 @@ bool ControllerInterface::set_chained_mode(bool /*chained_mode*/) { return false
 
 bool ControllerInterface::is_in_chained_mode() const { return false; }
 
+bool ControllerInterface::toggle_references_from_subscribers(bool /*enable*/) { return false; }
+
 }  // namespace controller_interface

controller_interface/test/test_chainable_controller_interface.cpp

@@ -16,6 +16,9 @@
 
 #include <gmock/gmock.h>
 
+using ::testing::IsEmpty;
+using ::testing::SizeIs;
+
 TEST_F(ChainableControllerInterfaceTest, default_returns)
 {
   TestableChainableControllerInterface controller;
@@ -31,6 +34,26 @@ TEST_F(ChainableControllerInterfaceTest, default_returns)
   EXPECT_FALSE(controller.is_in_chained_mode());
 }
 
+TEST_F(ChainableControllerInterfaceTest, export_internal_state_interfaces)
+{
+  TestableChainableControllerInterface controller;
+
+  // initialize, create node
+  const auto node_options = controller.define_custom_node_options();
+  ASSERT_EQ(
+    controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options),
+    controller_interface::return_type::OK);
+  ASSERT_NO_THROW(controller.get_node());
+
+  auto internal_state_interfaces = controller.export_internal_state_interfaces();
+
+  ASSERT_THAT(internal_state_interfaces, SizeIs(1));
+  EXPECT_EQ(internal_state_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME);
+  EXPECT_EQ(internal_state_interfaces[0].get_interface_name(), "test_state");
+
+  EXPECT_EQ(internal_state_interfaces[0].get_value(), INTERNAL_STATE_INTERFACE_VALUE);
+}
+
 TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
 {
   TestableChainableControllerInterface controller;
@@ -44,14 +67,14 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
 
   auto reference_interfaces = controller.export_reference_interfaces();
 
-  ASSERT_EQ(reference_interfaces.size(), 1u);
+  ASSERT_THAT(reference_interfaces, SizeIs(1));
   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);
 }
 
-TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size)
+TEST_F(ChainableControllerInterfaceTest, interfaces_storage_not_correct_size)
 {
   TestableChainableControllerInterface controller;
 
@@ -65,10 +88,14 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correc
   // expect empty return because storage is not resized
   controller.reference_interfaces_.clear();
   auto reference_interfaces = controller.export_reference_interfaces();
-  ASSERT_TRUE(reference_interfaces.empty());
+  ASSERT_THAT(reference_interfaces, IsEmpty());
+  // expect empty return because storage is not resized
+  controller.internal_state_interfaces_data_.clear();
+  auto internal_state_interfaces = controller.export_internal_state_interfaces();
+  ASSERT_THAT(internal_state_interfaces, IsEmpty());
 }
 
-TEST_F(ChainableControllerInterfaceTest, reference_interfaces_prefix_is_not_node_name)
+TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name)
 {
   TestableChainableControllerInterface controller;
 
@@ -83,7 +110,10 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_prefix_is_not_node
 
   // expect empty return because interface prefix is not equal to the node name
   auto reference_interfaces = controller.export_reference_interfaces();
-  ASSERT_TRUE(reference_interfaces.empty());
+  ASSERT_THAT(reference_interfaces, IsEmpty());
+  // expect empty return because interface prefix is not equal to the node name
+  auto internal_state_interfaces = controller.export_internal_state_interfaces();
+  ASSERT_THAT(internal_state_interfaces, IsEmpty());
 }
 
 TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)
@@ -98,12 +128,15 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)
   ASSERT_NO_THROW(controller.get_node());
 
   auto reference_interfaces = controller.export_reference_interfaces();
-  ASSERT_EQ(reference_interfaces.size(), 1u);
+  ASSERT_THAT(reference_interfaces, SizeIs(1));
+  auto internal_state_interfaces = controller.export_internal_state_interfaces();
+  ASSERT_THAT(internal_state_interfaces, SizeIs(1));
 
   EXPECT_FALSE(controller.is_in_chained_mode());
 
   // Fail setting chained mode
   EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE);
+  EXPECT_EQ(internal_state_interfaces[0].get_value(), INTERNAL_STATE_INTERFACE_VALUE);
 
   EXPECT_FALSE(controller.set_chained_mode(true));
   EXPECT_FALSE(controller.is_in_chained_mode());
@@ -116,6 +149,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)
 
   EXPECT_TRUE(controller.set_chained_mode(true));
   EXPECT_TRUE(controller.is_in_chained_mode());
+  EXPECT_EQ(internal_state_interfaces[0].get_value(), INTERNAL_STATE_INTERFACE_VALUE_IN_CHAINMODE);
 
   controller.configure();
   EXPECT_TRUE(controller.set_chained_mode(false));
@@ -147,31 +181,36 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic)
     controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
 
+  EXPECT_TRUE(controller.toggle_references_from_subscribers(true));
   EXPECT_FALSE(controller.is_in_chained_mode());
 
   // call update and update it from subscriber because not in chained mode
   ASSERT_EQ(
     controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
   ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1);
+  ASSERT_EQ(controller.internal_state_interfaces_data_[0], INTERNAL_STATE_INTERFACE_VALUE + 1);
 
   // Provoke error in update from subscribers - return ERROR and update_and_write_commands not exec.
   controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR);
   ASSERT_EQ(
     controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::ERROR);
   ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1);
+  ASSERT_EQ(controller.internal_state_interfaces_data_[0], INTERNAL_STATE_INTERFACE_VALUE + 1);
 
   // Provoke error from update - return ERROR, but reference interface is updated and not reduced
   controller.set_new_reference_interface_value(INTERFACE_VALUE_UPDATE_ERROR);
   ASSERT_EQ(
     controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::ERROR);
   ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR);
+  ASSERT_EQ(controller.internal_state_interfaces_data_[0], INTERNAL_STATE_INTERFACE_VALUE + 1);
 
   controller.reference_interfaces_[0] = 0.0;
 
   EXPECT_TRUE(controller.set_chained_mode(true));
+  EXPECT_TRUE(controller.toggle_references_from_subscribers(false));
   EXPECT_TRUE(controller.is_in_chained_mode());
 
   // Provoke error in update from subscribers - return OK because update of subscribers is not used
@@ -181,6 +220,8 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic)
     controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
   ASSERT_EQ(controller.reference_interfaces_[0], -1.0);
+  ASSERT_EQ(
+    controller.internal_state_interfaces_data_[0], INTERNAL_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1);
 
   // Provoke error from update - return ERROR, but reference interface is updated directly
   controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR);
@@ -189,4 +230,6 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic)
     controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::ERROR);
   ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR);
+  ASSERT_EQ(
+    controller.internal_state_interfaces_data_[0], INTERNAL_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1);
 }