Add default implementation to the on_export_state_interfaces and on_e…

…xport_reference_interfaces
ros-controls · Jun 14, 2024 · fa2340c · fa2340c
1 parent 62263ea
commit fa2340c
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 2 deletions.
diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/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"
@@ -77,7 +78,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase
    *
    * \returns list of StateInterfaces that other controller can use as their inputs.
    */
-  virtual std::vector<hardware_interface::StateInterface> on_export_state_interfaces() = 0;
+  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.
@@ -88,7 +89,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase
    *
    * \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.
   /**
@@ -129,9 +130,11 @@ class ChainableControllerInterface : public ControllerInterfaceBase
     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:

diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp
@@ -149,4 +149,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