Add on_export_state_interfaces() to dynamics_linearizing_controller.cpp.

diff --git a/include/linearizing_controllers/dynamics_linearizing_controller.hpp b/include/linearizing_controllers/dynamics_linearizing_controller.hpp
index 3b2320f..e98503a 100644 (file)
--- 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<hardware_interface::CommandInterface> on_export_reference_interfaces(void) override;
+       
+               LINEARIZING_CONTROLLERS_PUBLIC  
+               std::vector<hardware_interface::StateInterface> 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 (file)
--- a/src/dynamics_linearizing_controller.cpp
+++ b/src/dynamics_linearizing_controller.cpp
@@ -212,6 +212,25 @@ namespace effort_controllers
                return interfaces;
        }
 
+       std::vector<hardware_interface::StateInterface> DynamicsLinearizingController::on_export_state_interfaces(void)
+       {
+               std::vector<hardware_interface::StateInterface> interfaces;
+
+               std::vector<std::string> 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]))
                {