From 21dc537ac4375795679ac71ae4c9b16455744a25 Mon Sep 17 00:00:00 2001
From: Wolfgang Merkt <w.merkt@gmail.com>
Date: Thu, 11 Nov 2021 11:22:27 +0000
Subject: [PATCH 1/2] [forward_command_controller] Add parameter
 command_timeout - relates to #363

---
 .../forward_joint_group_command_controller.h         | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h
index a0f9e166a..e8783aada 100644
--- a/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h
+++ b/forward_command_controller/include/forward_command_controller/forward_joint_group_command_controller.h
@@ -103,6 +103,13 @@ class ForwardJointGroupCommandController: public controller_interface::Controlle
       }
     }
 
+    // Check if timeout parameter has been set. Read it if set, otherwise warn about unsafe default behaviour.
+    if(n.hasParam("command_timeout"))
+    {
+      n.getParam("command_timeout", command_timeout_);
+      ROS_INFO_STREAM("Using command timeout: " << command_timeout_);
+    }
+
     commands_buffer_.writeFromNonRT(std::vector<double>(n_joints_, 0.0));
 
     sub_command_ = n.subscribe<std_msgs::Float64MultiArray>("command", 1, &ForwardJointGroupCommandController::commandCB, this);
@@ -120,6 +127,8 @@ class ForwardJointGroupCommandController: public controller_interface::Controlle
   std::vector< std::string > joint_names_;
   std::vector< hardware_interface::JointHandle > joints_;
   realtime_tools::RealtimeBuffer<std::vector<double> > commands_buffer_;
+  realtime_tools::RealtimeBuffer<ros::Time> last_received_command_time_buffer_;
+  double command_timeout_ = -1.0;
   unsigned int n_joints_;
 
 private:
@@ -132,6 +141,9 @@ class ForwardJointGroupCommandController: public controller_interface::Controlle
       return;
     }
     commands_buffer_.writeFromNonRT(msg->data);
+
+    // Record time of last received command
+    last_received_command_time_buffer_.writeFromNonRT(ros::Time::now());
   }
 };
 

From 48b796ebad827a4f02f3e2beefd84c8599dc6b59 Mon Sep 17 00:00:00 2001
From: Wolfgang Merkt <w.merkt@gmail.com>
Date: Thu, 11 Nov 2021 11:23:08 +0000
Subject: [PATCH 2/2] [velocity_controllers] JointGroupVelocityController: Add
 command_timeout feature. Resolves #363

---
 .../src/joint_group_velocity_controller.cpp   | 25 +++++++++++++++++--
 1 file changed, 23 insertions(+), 2 deletions(-)

diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp
index d6e64068f..2668f9a39 100644
--- a/velocity_controllers/src/joint_group_velocity_controller.cpp
+++ b/velocity_controllers/src/joint_group_velocity_controller.cpp
@@ -38,12 +38,33 @@
 #include <velocity_controllers/joint_group_velocity_controller.h>
 #include <pluginlib/class_list_macros.hpp>
 
-template <class T>
-void forward_command_controller::ForwardJointGroupCommandController<T>::starting(const ros::Time& time)
+template<>
+void velocity_controllers::JointGroupVelocityController::starting(const ros::Time& /*time*/)
 {
   // Start controller with 0.0 velocities
   commands_buffer_.readFromRT()->assign(n_joints_, 0.0);
 }
 
+template<>
+void velocity_controllers::JointGroupVelocityController::update(const ros::Time& time, const ros::Duration& /*period*/)
+{
+  std::vector<double> & commands = *commands_buffer_.readFromRT();
+
+  // Check timeout
+  if (command_timeout_ > 0.0)
+  {
+    ros::Time& last_received_command_time = *last_received_command_time_buffer_.readFromRT();
+    const double command_age = (time - last_received_command_time).toSec();
+    if (std::abs(command_age) > command_timeout_)
+    {
+      ROS_WARN_STREAM_THROTTLE(10, "Commands timed out, setting to zero.");
+      for (std::size_t i = 0; i < commands.size(); ++i)
+      {  commands[i] = 0;  }
+    }
+  }
+
+  for(unsigned int i=0; i<n_joints_; i++)
+  {  joints_[i].setCommand(commands[i]);  }
+}
 
 PLUGINLIB_EXPORT_CLASS(velocity_controllers::JointGroupVelocityController,controller_interface::ControllerBase)