From 1b97def23839fba817e8d3f7a2eca4e7ad99d973 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Sun, 15 Dec 2024 03:08:53 -0300 Subject: [PATCH] Port to Jazzy. --- src/pid_plus_gravity_controller.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/pid_plus_gravity_controller.cpp b/src/pid_plus_gravity_controller.cpp index fd6d597..02580ab 100644 --- a/src/pid_plus_gravity_controller.cpp +++ b/src/pid_plus_gravity_controller.cpp @@ -1,7 +1,7 @@ /****************************************************************************** ROS 2 pid_plus_gravity_controller Package PID+Gravity Compensation Controller - Copyright (C) 2017..2022 Walter Fetter Lages + Copyright (C) 2017..2024 Walter Fetter Lages This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -21,6 +21,7 @@ #include +#include #include #include #include @@ -188,7 +189,8 @@ namespace effort_controllers auto time=get_node()->get_clock()->now(); for(unsigned int i=0;i < nJoints_;i++) - command_interfaces_[i].set_value(gravity_(i)); + if(!command_interfaces_[i].set_value(gravity_(i))) + RCLCPP_ERROR(get_node()->get_logger(),"Can't set command value."); return controller_interface::CallbackReturn::SUCCESS; } @@ -207,7 +209,8 @@ namespace effort_controllers RCLCPP_ERROR(get_node()->get_logger(),"KDL dynamics solver failed."); for(unsigned int i=0;i < nJoints_;i++) - command_interfaces_[i].set_value(gravity_(i)+pid_[i]->computeCommand(qr_(i)-q_(i),period)); + if(!command_interfaces_[i].set_value(gravity_(i)+pid_[i]->computeCommand(qr_(i)-q_(i),period))) + RCLCPP_ERROR(get_node()->get_logger(),"Can't set command value."); return controller_interface::return_type::OK; } -- 2.12.0