From 892a23882114711150f0512b852b45651c7900a2 Mon Sep 17 00:00:00 2001
From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Thu, 23 Sep 2021 03:52:14 -0300
Subject: [PATCH] Add realtime_buffer to command callback.

---
 CMakeLists.txt                                     |  2 ++
 .../computed_torque_controller.hpp                 |  4 ++++
 package.xml                                        |  1 +
 src/computed_torque_controller.cpp                 | 22 +++++++++++++++-------
 4 files changed, 22 insertions(+), 7 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 660815e..1b5042e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -15,6 +15,7 @@ find_package(trajectory_msgs REQUIRED)
 find_package(urdf REQUIRED)
 find_package(kdl_parser REQUIRED)
 find_package(orocos_kdl REQUIRED)
+find_package(realtime_tools REQUIRED)
 
 add_library(computed_torque_controller SHARED src/computed_torque_controller.cpp)
 target_compile_features(computed_torque_controller PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
@@ -28,6 +29,7 @@ ament_target_dependencies(
   "urdf"
   "kdl_parser"
   "orocos_kdl"
+  "realtime_tools"
 )
 
 # Causes the visibility macros to use dllexport rather than dllimport,
diff --git a/include/computed_torque_controller/computed_torque_controller.hpp b/include/computed_torque_controller/computed_torque_controller.hpp
index 669f53b..f63d7c5 100644
--- a/include/computed_torque_controller/computed_torque_controller.hpp
+++ b/include/computed_torque_controller/computed_torque_controller.hpp
@@ -30,6 +30,8 @@
 #include <kdl_parser/kdl_parser.hpp>
 #include <kdl/chainidsolver_recursive_newton_euler.hpp>
 
+#include <realtime_tools/realtime_buffer.h>
+
 #include <computed_torque_controller/visibility_control.h>
 
 namespace effort_controllers
@@ -64,6 +66,8 @@ namespace effort_controllers
 		
 		int priority_;
 		
+		realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint>> referencePoint_;
+		
 		void commandCB(const trajectory_msgs::msg::JointTrajectoryPoint::SharedPtr referencePoint);
 		void robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription);
 		
diff --git a/package.xml b/package.xml
index 0fa433d..9242b75 100644
--- a/package.xml
+++ b/package.xml
@@ -16,6 +16,7 @@
   <depend>urdf</depend>
   <depend>kdl_parser</depend>
   <depend>orocos_kdl</depend>
+  <depend>realtime_tools</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/src/computed_torque_controller.cpp b/src/computed_torque_controller.cpp
index a36154a..0b00f63 100644
--- a/src/computed_torque_controller.cpp
+++ b/src/computed_torque_controller.cpp
@@ -30,7 +30,7 @@
 namespace effort_controllers
 {	
 	ComputedTorqueController::ComputedTorqueController(void):
-		q_(0),dq_(0),v_(0),qr_(0),dqr_(0),ddqr_(0),torque_(0),fext_(0)
+		q_(0),dq_(0),v_(0),qr_(0),dqr_(0),ddqr_(0),torque_(0),fext_(0),referencePoint_(nullptr)
 	{
 	}
 
@@ -231,6 +231,19 @@ namespace effort_controllers
 
 	controller_interface::return_type ComputedTorqueController::update(void)
 	{
+		auto referencePoint=referencePoint_.readFromRT();
+		if(referencePoint && *referencePoint)
+		{
+			if((*referencePoint)->positions.size() >= nJoints_)
+				qr_.data=Eigen::VectorXd::Map((*referencePoint)->positions.data(),nJoints_);
+				
+			if((*referencePoint)->velocities.size() >= nJoints_)
+				dqr_.data=Eigen::VectorXd::Map((*referencePoint)->velocities.data(),nJoints_);
+				
+			if((*referencePoint)->accelerations.size() >= nJoints_)
+				ddqr_.data=Eigen::VectorXd::Map((*referencePoint)->accelerations.data(),nJoints_);
+		}
+
 		for(int i=0;i < nJoints_;i++)
 		{
 			q_(i)=state_interfaces_[2*i].get_value();
@@ -251,12 +264,7 @@ namespace effort_controllers
 
 	void ComputedTorqueController::commandCB(const trajectory_msgs::msg::JointTrajectoryPoint::SharedPtr referencePoint)
 	{
-		for(int i=0;i < nJoints_;i++)
-		{
-			qr_(i)=referencePoint->positions[i];
-			dqr_(i)=referencePoint->velocities[i];
-			ddqr_(i)=referencePoint->accelerations[i];
-		}
+		referencePoint_.writeFromNonRT(referencePoint);
 	}
 
 	void ComputedTorqueController::robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription)
-- 
2.12.0