From b9c1c43cf6fe84930f881dab90092c3ce2a7ed28 Mon Sep 17 00:00:00 2001
From: mhubii <m.huber_1994@hotmail.de>
Date: Sun, 20 Oct 2024 17:21:39 +0100
Subject: [PATCH] turn off controller in case to command are received

---
 .../controllers/twist_controller.hpp                 |  5 +++++
 .../src/controllers/twist_controller.cpp             | 12 +++++++++++-
 2 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp
index ed364a25..b1503a2d 100644
--- a/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp
+++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp
@@ -3,6 +3,7 @@
 
 #include <algorithm>
 #include <array>
+#include <atomic>
 #include <functional>
 #include <memory>
 #include <stdexcept>
@@ -80,6 +81,10 @@ class TwistController : public controller_interface::ControllerInterface {
   void configure_joint_names_();
   void configure_twist_impl_();
 
+  // some safety checks
+  std::atomic<int> updates_since_last_command_ = 0;
+  double max_time_without_command_ = 0.2;
+
   // joint veloctiy computation
   std::unique_ptr<TwistImpl> twist_impl_ptr_;
   lbr_fri_ros2::Kinematics::jnt_pos_array_t q_, dq_;
diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp
index e01d03e7..f93f0981 100644
--- a/lbr_ros2_control/src/controllers/twist_controller.cpp
+++ b/lbr_ros2_control/src/controllers/twist_controller.cpp
@@ -66,6 +66,7 @@ controller_interface::CallbackReturn TwistController::on_init() {
     twist_subscription_ptr_ = this->get_node()->create_subscription<geometry_msgs::msg::Twist>(
         "command/twist", 1, [this](const geometry_msgs::msg::Twist::SharedPtr msg) {
           rt_twist_ptr_.writeFromNonRT(msg);
+          updates_since_last_command_ = 0;
         });
     this->get_node()->declare_parameter("robot_name", "lbr");
     this->get_node()->declare_parameter("chain_root", "lbr_link_0");
@@ -85,7 +86,7 @@ controller_interface::CallbackReturn TwistController::on_init() {
 }
 
 controller_interface::return_type TwistController::update(const rclcpp::Time & /*time*/,
-                                                          const rclcpp::Duration & /*period*/) {
+                                                          const rclcpp::Duration &period) {
   auto twist_command = rt_twist_ptr_.readFromRT();
   if (!twist_command || !(*twist_command)) {
     return controller_interface::return_type::OK;
@@ -98,6 +99,13 @@ controller_interface::return_type TwistController::update(const rclcpp::Time & /
       KUKA::FRI::ESessionState::COMMANDING_ACTIVE) {
     return controller_interface::return_type::OK;
   }
+  if (updates_since_last_command_ >
+      static_cast<int>(max_time_without_command_ / period.seconds())) {
+    RCLCPP_ERROR(this->get_node()->get_logger(),
+                 "No twist command received within time %f. Stopping the controller.",
+                 max_time_without_command_);
+    return controller_interface::return_type::ERROR;
+  }
 
   // pass joint positions to q_
   std::for_each(q_.begin(), q_.end(), [&, i = 0](double &q_i) mutable {
@@ -115,6 +123,8 @@ controller_interface::return_type TwistController::update(const rclcpp::Time & /
     ++i;
   });
 
+  ++updates_since_last_command_;
+
   return controller_interface::return_type::OK;
 }