From 360f7d578c07384c051dd7498251d63230ef4022 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Tue, 3 Jun 2025 11:56:18 -0300 Subject: [PATCH] Fix update_reference_from_subscribers() in dynamics_linearizing controller. --- src/dynamics_linearizing_controller.cpp | 31 +++++++++++-------------------- 1 file changed, 11 insertions(+), 20 deletions(-) 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 interfaces; std::vector 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(); -- 2.12.0