Fix update_reference_from_subscribers() in dynamics_linearizing controller.

diff --git a/src/dynamics_linearizing_controller.cpp b/src/dynamics_linearizing_controller.cpp
index 32cf878..8d2f645 100644 (file)
--- a/src/dynamics_linearizing_controller.cpp
+++ b/src/dynamics_linearizing_controller.cpp
@@ -198,8 +198,8 @@ namespace effort_controllers
                std::vector<hardware_interface::CommandInterface> interfaces;
 
                std::vector<std::string> names;
-               names.push_back("linear/acceleration");
-               names.push_back("angular/acceleration");
+               names.push_back("linear/accel");
+               names.push_back("angular/accel");
 
                reference_interfaces_.resize(names.size());
 
@@ -248,9 +248,12 @@ namespace effort_controllers
 
        controller_interface::return_type DynamicsLinearizingController::update_reference_from_subscribers(const rclcpp::Time & /*time*/,const rclcpp::Duration &/*period*/)
        {
-               // Move functionality to the `update_and_write_commands` because of the missing arguments in
-               // humble - otherwise issues with multiple time-sources might happen when working with simulators
-
+               auto command=rt_command_.readFromRT();
+               if(command && *command)
+               {
+                       reference_interfaces_[0]=(*command)->linear.x;
+                       reference_interfaces_[1]=(*command)->angular.z;
+               }
                return controller_interface::return_type::OK;
        }
 
@@ -276,22 +279,10 @@ namespace effort_controllers
                Eigen::Vector2d u;
                odom_.getVelocity(u);
 
-               if(is_in_chained_mode())
+               if(!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1]))
                {
-                       if(!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1]))
-                       {
-                               v_[0]=reference_interfaces_[0];
-                               v_[1]=reference_interfaces_[1];
-                       }
-               }
-               else
-               {
-                       auto command=rt_command_.readFromRT();
-                       if(command && *command)
-                       {
-                               v_[0]=(*command)->linear.x;
-                               v_[1]=(*command)->angular.z;
-                       }
+                       v_[0]=reference_interfaces_[0];
+                       v_[1]=reference_interfaces_[1];
                }
 
                auto param=rt_param_.readFromRT();