From 9371bca7ad821b197a05c457b3ea12ebdc52f947 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 Jul 2024 18:51:39 +0200 Subject: [PATCH 1/6] Add filter checks for the measured state callback --- pid_controller/src/pid_controller.cpp | 37 +++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index b76926d5a0..f713c420c8 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -190,6 +190,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); }; From dc73d2ccb614366dde316e84f3cfb2c2f723a44d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 Jul 2024 18:52:07 +0200 Subject: [PATCH 2/6] make sure to pass atleast one reference_and_state_interfaces --- pid_controller/src/pid_controller.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index f645738862..543e6c9696 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<>: 1, size_lt<>: 3, } } From b5553827768a4755dd77b8e7916cf1efd21c37f5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 Jul 2024 18:52:19 +0200 Subject: [PATCH 3/6] Export state interfaces from pid controller --- .../include/pid_controller/pid_controller.hpp | 2 ++ pid_controller/src/pid_controller.cpp | 21 +++++++++++++++++++ 2 files changed, 23 insertions(+) diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index f7b8cc4491..f95245918d 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -123,6 +123,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 f713c420c8..ad9aeeb7d4 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -403,6 +403,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 From d2e209966f7d3c3c417c5c04176f7f5cbd1b0514 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 Jul 2024 18:52:36 +0200 Subject: [PATCH 4/6] Fill in the data of the exported state interfaces --- pid_controller/src/pid_controller.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index ad9aeeb7d4..c4b2d35559 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -403,7 +403,7 @@ std::vector PidController::on_export_refer return reference_interfaces; } -std::vector PidController::on_export_state_interfaces() +std::vector PidController::on_export_state_interfaces() { std::vector state_interfaces; state_interfaces.reserve(state_interfaces_values_.size()); @@ -499,6 +499,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(); From bce6cac20af122e54b67fbf836fc629b92f94291 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 12 Jul 2024 11:12:47 +0200 Subject: [PATCH 5/6] fix the condition of reference_and_state_interfaces being atleast one --- pid_controller/src/pid_controller.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 543e6c9696..651cc1e7de 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -34,7 +34,7 @@ pid_controller: read_only: true, validation: { not_empty<>: null, - size_gt<>: 1, + size_gt<>: 0, size_lt<>: 3, } } From 0bccb505b9c434079eeeed1a4c0b40e063629aa9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 12 Jul 2024 11:37:50 +0200 Subject: [PATCH 6/6] fix the tests of the PID controller --- pid_controller/test/test_pid_controller.hpp | 1 + .../test/test_pid_controller_preceding.cpp | 18 ++++++++++++++++++ 2 files changed, 19 insertions(+) diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 1c356263e7..f0ea811339 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -78,6 +78,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 3e17e69286..8e147b1ab6 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -92,6 +92,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)