[PID Controller] Export state interfaces for easier chaining with oth…

…er controllers (#1214)

pid_controller/include/pid_controller/pid_controller.hpp

@@ -117,6 +117,8 @@ class PidController : public controller_interface::ChainableControllerInterface
   // override methods from ChainableControllerInterface
   std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
 
+  std::vector<hardware_interface::StateInterface> on_export_state_interfaces() override;
+
   bool on_set_chained_mode(bool chained_mode) override;
 
   // internal methods

pid_controller/src/pid_controller.cpp

@@ -187,6 +187,43 @@ controller_interface::CallbackReturn PidController::on_configure(
     auto measured_state_callback =
       [&](const std::shared_ptr<ControllerMeasuredStateMsg> state_msg) -> void
     {
+      if (state_msg->dof_names.size() != reference_and_state_dof_names_.size())
+      {
+        RCLCPP_ERROR(
+          get_node()->get_logger(),
+          "Size of input data names (%zu) is not matching the expected size (%zu).",
+          state_msg->dof_names.size(), reference_and_state_dof_names_.size());
+        return;
+      }
+      if (state_msg->values.size() != reference_and_state_dof_names_.size())
+      {
+        RCLCPP_ERROR(
+          get_node()->get_logger(),
+          "Size of input data values (%zu) is not matching the expected size (%zu).",
+          state_msg->values.size(), reference_and_state_dof_names_.size());
+        return;
+      }
+
+      if (!state_msg->values_dot.empty())
+      {
+        if (params_.reference_and_state_interfaces.size() != 2)
+        {
+          RCLCPP_ERROR(
+            get_node()->get_logger(),
+            "The reference_and_state_interfaces parameter has to have two interfaces [the "
+            "interface and the derivative of the interface], in order to use the values_dot "
+            "field.");
+          return;
+        }
+        if (state_msg->values_dot.size() != reference_and_state_dof_names_.size())
+        {
+          RCLCPP_ERROR(
+            get_node()->get_logger(),
+            "Size of input data values_dot (%zu) is not matching the expected size (%zu).",
+            state_msg->values_dot.size(), reference_and_state_dof_names_.size());
+          return;
+        }
+      }
       // TODO(destogl): Sort the input values based on joint and interface names
       measured_state_.writeFromNonRT(state_msg);
     };
@@ -363,6 +400,27 @@ std::vector<hardware_interface::CommandInterface> PidController::on_export_refer
   return reference_interfaces;
 }
 
+std::vector<hardware_interface::StateInterface> PidController::on_export_state_interfaces()
+{
+  std::vector<hardware_interface::StateInterface> state_interfaces;
+  state_interfaces.reserve(state_interfaces_values_.size());
+
+  state_interfaces_values_.resize(
+    reference_and_state_dof_names_.size() * params_.reference_and_state_interfaces.size(),
+    std::numeric_limits<double>::quiet_NaN());
+  size_t index = 0;
+  for (const auto & interface : params_.reference_and_state_interfaces)
+  {
+    for (const auto & dof_name : reference_and_state_dof_names_)
+    {
+      state_interfaces.push_back(hardware_interface::StateInterface(
+        get_node()->get_name(), dof_name + "/" + interface, &state_interfaces_values_[index]));
+      ++index;
+    }
+  }
+  return state_interfaces;
+}
+
 bool PidController::on_set_chained_mode(bool chained_mode)
 {
   // Always accept switch to/from chained mode
@@ -438,6 +496,12 @@ controller_interface::return_type PidController::update_and_write_commands(
     }
   }
 
+  // Fill the information of the exported state interfaces
+  for (size_t i = 0; i < measured_state_values_.size(); ++i)
+  {
+    state_interfaces_values_[i] = measured_state_values_[i];
+  }
+
   for (size_t i = 0; i < dof_; ++i)
   {
     double tmp_command = std::numeric_limits<double>::quiet_NaN();

pid_controller/src/pid_controller.yaml

@@ -34,6 +34,7 @@ pid_controller:
     read_only: true,
     validation: {
       not_empty<>: null,
+      size_gt<>: 0,
       size_lt<>: 3,
     }
   }

pid_controller/test/test_pid_controller.hpp

@@ -71,6 +71,7 @@ class TestablePidController : public pid_controller::PidController
     const rclcpp_lifecycle::State & previous_state) override
   {
     auto ref_itfs = on_export_reference_interfaces();
+    auto state_itfs = on_export_state_interfaces();
     return pid_controller::PidController::on_activate(previous_state);
   }
 

pid_controller/test/test_pid_controller_preceding.cpp

@@ -90,6 +90,24 @@ TEST_F(PidControllerTest, check_exported_interfaces)
       ++ri_index;
     }
   }
+
+  // check exported state itfs
+  auto exported_state_itfs = controller_->export_state_interfaces();
+  ASSERT_EQ(exported_state_itfs.size(), dof_state_values_.size());
+  size_t esi_index = 0;
+  for (const auto & interface : state_interfaces_)
+  {
+    for (const auto & dof_name : reference_and_state_dof_names_)
+    {
+      const std::string state_itf_name =
+        std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface;
+      EXPECT_EQ(exported_state_itfs[esi_index].get_name(), state_itf_name);
+      EXPECT_EQ(
+        exported_state_itfs[esi_index].get_prefix_name(), controller_->get_node()->get_name());
+      EXPECT_EQ(exported_state_itfs[esi_index].get_interface_name(), dof_name + "/" + interface);
+      ++esi_index;
+    }
+  }
 }
 
 int main(int argc, char ** argv)