[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_state_interfaces() final;
+
  CONTROLLER_INTERFACE_PUBLIC
  std::vector<hardware_interface::CommandInterface> export_reference_interfaces() final;
 
@@ -65,8 +68,19 @@ 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() = 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,6 +128,9 @@ 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<double> state_interfaces_values_;
+
  /// Storage of values for reference interfaces
  std::vector<double> reference_interfaces_;
 

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/src/chainable_controller_interface.cpp

@@ -44,6 +44,42 @@ return_type ChainableControllerInterface::update(
  return ret;
 }
 
+std::vector<hardware_interface::StateInterface>
+ChainableControllerInterface::export_state_interfaces()
+{
+ auto state_interfaces = on_export_state_interfaces();
+ // check if the "state_interfaces_values_" variable is resized to number of interfaces
+ if (state_interfaces_values_.size() != state_interfaces.size())
+ {
+ RCLCPP_FATAL(
+ get_node()->get_logger(),
+ "The internal storage for exported state values 'state_interfaces_values_' variable has size "
+ "'%zu', but it is expected to have the size '%zu' equal to the number of exported state "
+ "interfaces. No state interface will be exported. Please correct and recompile "
+ "the controller with name '%s' and try again.",
+ state_interfaces_values_.size(), state_interfaces.size(), get_node()->get_name());
+ state_interfaces.clear();
+ }
+
+ // 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;
+ }
+ }
+
+ return state_interfaces;
+}
+
 std::vector<hardware_interface::CommandInterface>
 ChainableControllerInterface::export_reference_interfaces()
 {

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 {};

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_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 exported_state_interfaces = controller.export_state_interfaces();
+
+ ASSERT_THAT(exported_state_interfaces, SizeIs(1));
+ EXPECT_EQ(exported_state_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME);
+ EXPECT_EQ(exported_state_interfaces[0].get_interface_name(), "test_state");
+
+ EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_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.state_interfaces_values_.clear();
+ auto exported_state_interfaces = controller.export_state_interfaces();
+ ASSERT_THAT(exported_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 exported_state_interfaces = controller.export_state_interfaces();
+ ASSERT_THAT(exported_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 exported_state_interfaces = controller.export_state_interfaces();
+ ASSERT_THAT(exported_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(exported_state_interfaces[0].get_value(), EXPORTED_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(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE);
 
  controller.configure();
  EXPECT_TRUE(controller.set_chained_mode(false));
@@ -147,27 +181,31 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic)
  controller_interface::return_type::OK);
  ASSERT_NO_THROW(controller.get_node());
 
+ EXPECT_FALSE(controller.set_chained_mode(false));
  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.state_interfaces_values_[0], EXPORTED_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.state_interfaces_values_[0], EXPORTED_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.state_interfaces_values_[0], EXPORTED_STATE_INTERFACE_VALUE + 1);
 
  controller.reference_interfaces_[0] = 0.0;
 
@@ -181,6 +219,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.state_interfaces_values_[0], EXPORTED_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 +229,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.state_interfaces_values_[0], EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1);
 }

controller_interface/test/test_chainable_controller_interface.hpp

@@ -29,24 +29,28 @@ constexpr double INTERFACE_VALUE = 1989.0;
 constexpr double INTERFACE_VALUE_SUBSCRIBER_ERROR = 12345.0;
 constexpr double INTERFACE_VALUE_UPDATE_ERROR = 67890.0;
 constexpr double INTERFACE_VALUE_INITIAL_REF = 1984.0;
+constexpr double EXPORTED_STATE_INTERFACE_VALUE = 21833.0;
+constexpr double EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE = 82802.0;
 
 class TestableChainableControllerInterface
 : public controller_interface::ChainableControllerInterface
 {
 public:
- FRIEND_TEST(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size);
+ FRIEND_TEST(ChainableControllerInterfaceTest, interfaces_storage_not_correct_size);
  FRIEND_TEST(ChainableControllerInterfaceTest, test_update_logic);
 
  TestableChainableControllerInterface()
  {
  reference_interfaces_.reserve(1);
  reference_interfaces_.push_back(INTERFACE_VALUE);
+ state_interfaces_values_.reserve(1);
+ state_interfaces_values_.push_back(EXPORTED_STATE_INTERFACE_VALUE);
  }
 
  controller_interface::CallbackReturn on_init() override
  {
  // set default value
- name_prefix_of_reference_interfaces_ = get_node()->get_name();
+ name_prefix_of_interfaces_ = get_node()->get_name();
 
  return controller_interface::CallbackReturn::SUCCESS;
  }
@@ -63,13 +67,24 @@ class TestableChainableControllerInterface
  controller_interface::interface_configuration_type::NONE};
  }
 
+ // Implementation of ChainableController virtual methods
+ std::vector<hardware_interface::StateInterface> on_export_state_interfaces() override
+ {
+ std::vector<hardware_interface::StateInterface> state_interfaces;
+
+ state_interfaces.push_back(hardware_interface::StateInterface(
+ name_prefix_of_interfaces_, "test_state", &state_interfaces_values_[0]));
+
+ return state_interfaces;
+ }
+
  // Implementation of ChainableController virtual methods
  std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override
  {
  std::vector<hardware_interface::CommandInterface> command_interfaces;
 
  command_interfaces.push_back(hardware_interface::CommandInterface(
- name_prefix_of_reference_interfaces_, "test_itf", &reference_interfaces_[0]));
+ name_prefix_of_interfaces_, "test_itf", &reference_interfaces_[0]));
 
  return command_interfaces;
  }
@@ -78,6 +93,7 @@ class TestableChainableControllerInterface
  {
  if (reference_interfaces_[0] == 0.0)
  {
+ state_interfaces_values_[0] = EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE;
  return true;
  }
  else
@@ -107,21 +123,22 @@ class TestableChainableControllerInterface
  }
 
  reference_interfaces_[0] -= 1;
+ state_interfaces_values_[0] += 1;
 
  return controller_interface::return_type::OK;
  }
 
  void set_name_prefix_of_reference_interfaces(const std::string & prefix)
  {
- name_prefix_of_reference_interfaces_ = prefix;
+ name_prefix_of_interfaces_ = prefix;
  }
 
  void set_new_reference_interface_value(const double ref_itf_value)
  {
  reference_interface_value_ = ref_itf_value;
  }
 
- std::string name_prefix_of_reference_interfaces_;
+ std::string name_prefix_of_interfaces_;
  double reference_interface_value_ = INTERFACE_VALUE_INITIAL_REF;
 };