Add realtime_buffer to command callback.

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 660815e..1b5042e 100644 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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)