From 4de05ed80887da12d865f6c6994e5cd00b4b269e Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Sun, 15 Jun 2025 18:15:53 -0300 Subject: [PATCH] Add on_export_state_interfaces() to dynamics_linearizing_controller.cpp. --- .../dynamics_linearizing_controller.hpp | 3 +++ src/dynamics_linearizing_controller.cpp | 24 ++++++++++++++++++++++ 2 files changed, 27 insertions(+) diff --git a/include/linearizing_controllers/dynamics_linearizing_controller.hpp b/include/linearizing_controllers/dynamics_linearizing_controller.hpp index 3b2320f..e98503a 100644 --- a/include/linearizing_controllers/dynamics_linearizing_controller.hpp +++ b/include/linearizing_controllers/dynamics_linearizing_controller.hpp @@ -66,6 +66,9 @@ namespace effort_controllers LINEARIZING_CONTROLLERS_PUBLIC std::vector on_export_reference_interfaces(void) override; + + LINEARIZING_CONTROLLERS_PUBLIC + std::vector on_export_state_interfaces() override; LINEARIZING_CONTROLLERS_PUBLIC bool on_set_chained_mode(bool chained_mode) override; diff --git a/src/dynamics_linearizing_controller.cpp b/src/dynamics_linearizing_controller.cpp index 8d2f645..748bf34 100644 --- a/src/dynamics_linearizing_controller.cpp +++ b/src/dynamics_linearizing_controller.cpp @@ -212,6 +212,25 @@ namespace effort_controllers return interfaces; } + std::vector DynamicsLinearizingController::on_export_state_interfaces(void) + { + std::vector interfaces; + + std::vector names; + names.push_back("linear/velocity"); + names.push_back("angular/velocity"); + + state_interfaces_values_.resize(names.size()); + + for(size_t i=0; i < names.size();i++) + { + // reference_interfaces_ is declared in the base class + interfaces.push_back(hardware_interface::StateInterface(std::string(get_node()->get_name()),names[i],&state_interfaces_values_[i])); + } + + return interfaces; + } + bool DynamicsLinearizingController::on_set_chained_mode(bool /*chained_mode*/) { return true; @@ -278,6 +297,11 @@ namespace effort_controllers Eigen::Vector2d u; odom_.getVelocity(u); + + for(unsigned int i=0;i < u.size() && i < state_interfaces_values_.size();i++) + { + state_interfaces_values_[i]=u[i]; + } if(!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { -- 2.12.0