diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index 236b067929..ce66fd06d4 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -117,6 +117,8 @@ class PidController : public controller_interface::ChainableControllerInterface // override methods from ChainableControllerInterface std::vector on_export_reference_interfaces() override; + std::vector on_export_state_interfaces() override; + bool on_set_chained_mode(bool chained_mode) override; // internal methods diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index c8e6cc0fe0..032dc1d666 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -187,6 +187,43 @@ controller_interface::CallbackReturn PidController::on_configure( auto measured_state_callback = [&](const std::shared_ptr 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 PidController::on_export_refer return reference_interfaces; } +std::vector PidController::on_export_state_interfaces() +{ + std::vector 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::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::quiet_NaN(); diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index f645738862..651cc1e7de 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/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, } } diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 158b5d9147..4471f35a12 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/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); } diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp index 498ca633da..9e6a7ef04c 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/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)