From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Sun, 15 Dec 2024 06:08:53 +0000 (-0300)
Subject: Port to Jazzy.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=1b97def23839fba817e8d3f7a2eca4e7ad99d973;p=pid_plus_gravity_controller.git

Port to Jazzy.
---

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 <w.fetter@ieee.org>
+          Copyright (C) 2017..2024 Walter Fetter Lages <w.fetter@ieee.org>
 
         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 <sys/mman.h>
 
+#include <rclcpp/rclcpp.hpp>
 #include <rclcpp/logging.hpp>
 #include <hardware_interface/types/hardware_interface_type_values.hpp>
 #include <pluginlib/class_list_macros.hpp>
@@ -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;
 	}