From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Tue, 3 Jun 2025 14:56:18 +0000 (-0300)
Subject: Fix update_reference_from_subscribers() in dynamics_linearizing controller.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=360f7d578c07384c051dd7498251d63230ef4022;p=linearizing_controllers.git

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
--- 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();