Add reference timeout parameter to controllers templates

templates/ros2_control/controller/dummy_chainable_controller.cpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -41,8 +41,10 @@ using ControllerReferenceMsg = dummy_package_namespace::DummyClassName::Controll
 
 // called from RT control loop
 void reset_controller_reference_msg(
-  const std::shared_ptr<ControllerReferenceMsg> & msg, const std::vector<std::string> & joint_names)
+  const std::shared_ptr<ControllerReferenceMsg> & msg, const std::vector<std::string> & joint_names,
+  const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
 {
+  msg->header.stamp = node->now();
   msg->joint_names = joint_names;
   msg->displacements.resize(joint_names.size(), std::numeric_limits<double>::quiet_NaN());
   msg->velocities.resize(joint_names.size(), std::numeric_limits<double>::quiet_NaN());
@@ -93,13 +95,15 @@ controller_interface::CallbackReturn DummyClassName::on_configure(
   subscribers_qos.keep_last(1);
   subscribers_qos.best_effort();
 
+  ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout);
+
   // Reference Subscriber
   ref_subscriber_ = get_node()->create_subscription<ControllerReferenceMsg>(
-    "~/reference", subscribers_qos,
+    "~/commands", subscribers_qos,
     std::bind(&DummyClassName::reference_callback, this, std::placeholders::_1));
 
   std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
-  reset_controller_reference_msg(msg, params_.joints);
+  reset_controller_reference_msg(msg, params_.joints, get_node());
   input_ref_.writeFromNonRT(msg);
 
   auto set_slow_mode_service_callback =
@@ -139,6 +143,30 @@ controller_interface::CallbackReturn DummyClassName::on_configure(
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
+void DummyClassName::reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg)
+{
+  const auto age_of_last_command = get_node()->now() - msg->header.stamp;
+  if (msg->joint_names.size() == params_.joints.size()) {
+    if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) {
+      input_ref_.writeFromNonRT(msg);
+    } else {
+      RCLCPP_ERROR(
+        get_node()->get_logger(),
+        "Received message has timestamp %.10f older for %.10f which "
+        "is more then allowed timeout "
+        "(%.4f).",
+        rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(),
+        ref_timeout_.seconds());
+      reset_controller_reference_msg(msg, params_.joints, get_node());
+    }
+  } else {
+    RCLCPP_ERROR(
+      get_node()->get_logger(),
+      "Received %zu , but expected %zu joints in command. Ignoring message.",
+      msg->joint_names.size(), params_.joints.size());
+  }
+}
+
 controller_interface::InterfaceConfiguration DummyClassName::command_interface_configuration() const
 {
   controller_interface::InterfaceConfiguration command_interfaces_config;
@@ -165,18 +193,6 @@ controller_interface::InterfaceConfiguration DummyClassName::state_interface_con
   return state_interfaces_config;
 }
 
-void DummyClassName::reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg)
-{
-  if (msg->joint_names.size() == params_.joints.size()) {
-    input_ref_.writeFromNonRT(msg);
-  } else {
-    RCLCPP_ERROR(
-      get_node()->get_logger(),
-      "Received %zu , but expected %zu joints in command. Ignoring message.",
-      msg->joint_names.size(), params_.joints.size());
-  }
-}
-
 std::vector<hardware_interface::CommandInterface> DummyClassName::on_export_reference_interfaces()
 {
   reference_interfaces_.resize(state_joints_.size(), std::numeric_limits<double>::quiet_NaN());
@@ -203,7 +219,7 @@ controller_interface::CallbackReturn DummyClassName::on_activate(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
   // Set default value in command
-  reset_controller_reference_msg(*(input_ref_.readFromRT()), state_joints_);
+  reset_controller_reference_msg(*(input_ref_.readFromRT()), state_joints_, get_node());
 
   return controller_interface::CallbackReturn::SUCCESS;
 }
@@ -222,14 +238,28 @@ controller_interface::CallbackReturn DummyClassName::on_deactivate(
 controller_interface::return_type DummyClassName::update_reference_from_subscribers()
 {
   auto current_ref = input_ref_.readFromRT();
+  const auto age_of_last_command = get_node()->now() - (*current_ref)->header.stamp;
 
   // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`,
   // instead of a loop
   for (size_t i = 0; i < reference_interfaces_.size(); ++i) {
-    if (!std::isnan((*current_ref)->displacements[i])) {
-      reference_interfaces_[i] = (*current_ref)->displacements[i];
+    // send message only if there is no timeout
+    if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) {
+      if (!std::isnan((*current_ref)->displacements[i])) {
+        if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) {
+          (*current_ref)->displacements[i] /= 2;
+        }
+        reference_interfaces_[i] = (*current_ref)->displacements[i];
+        if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) {
+          (*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
+        }
+      }
+    } else {
+      if (!std::isnan((*current_ref)->displacements[i])) {
+        reference_interfaces_[i] = 0.0;
 
-      (*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
+        (*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
+      }
     }
   }
   return controller_interface::return_type::OK;
@@ -238,16 +268,25 @@ controller_interface::return_type DummyClassName::update_reference_from_subscrib
 controller_interface::return_type DummyClassName::update_and_write_commands(
   const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
+  auto current_ref = input_ref_.readFromRT();
+  const auto age_of_last_command = get_node()->now() - (*current_ref)->header.stamp;
   // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`,
   // instead of a loop
   for (size_t i = 0; i < command_interfaces_.size(); ++i) {
-    if (!std::isnan(reference_interfaces_[i])) {
-      if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) {
-        reference_interfaces_[i] /= 2;
+    // send message only if there is no timeout
+    if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) {
+      if (!std::isnan(reference_interfaces_[i])) {
+        if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) {
+          reference_interfaces_[i] /= 2;
+        }
+        command_interfaces_[i].set_value(reference_interfaces_[i]);
+        if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) {
+          reference_interfaces_[i] = std::numeric_limits<double>::quiet_NaN();
+        }
       }
-      command_interfaces_[i].set_value(reference_interfaces_[i]);
-
-      reference_interfaces_[i] = std::numeric_limits<double>::quiet_NaN();
+    } else {
+      command_interfaces_[i].set_value(0.0);
+      (*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
     }
   }
 
@@ -258,6 +297,9 @@ controller_interface::return_type DummyClassName::update_and_write_commands(
     state_publisher_->unlockAndPublish();
   }
 
+  for (size_t i = 0; i < reference_interfaces_.size(); ++i) {
+    reference_interfaces_[i] = std::numeric_limits<double>::quiet_NaN();
+  }
   return controller_interface::return_type::OK;
 }
 

templates/ros2_control/controller/dummy_controller.cpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -41,8 +41,10 @@ using ControllerReferenceMsg = dummy_package_namespace::DummyClassName::Controll
 
 // called from RT control loop
 void reset_controller_reference_msg(
-  std::shared_ptr<ControllerReferenceMsg> & msg, const std::vector<std::string> & joint_names)
+  const std::shared_ptr<ControllerReferenceMsg> & msg, const std::vector<std::string> & joint_names,
+  const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
 {
+  msg->header.stamp = node->now();
   msg->joint_names = joint_names;
   msg->displacements.resize(joint_names.size(), std::numeric_limits<double>::quiet_NaN());
   msg->velocities.resize(joint_names.size(), std::numeric_limits<double>::quiet_NaN());
@@ -93,13 +95,15 @@ controller_interface::CallbackReturn DummyClassName::on_configure(
   subscribers_qos.keep_last(1);
   subscribers_qos.best_effort();
 
+  ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout);
+
   // Reference Subscriber
   ref_subscriber_ = get_node()->create_subscription<ControllerReferenceMsg>(
-    "~/reference", subscribers_qos,
+    "~/commands", subscribers_qos,
     std::bind(&DummyClassName::reference_callback, this, std::placeholders::_1));
 
   std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
-  reset_controller_reference_msg(msg, params_.joints);
+  reset_controller_reference_msg(msg, params_.joints, get_node());
   input_ref_.writeFromNonRT(msg);
 
   auto set_slow_mode_service_callback =
@@ -141,8 +145,20 @@ controller_interface::CallbackReturn DummyClassName::on_configure(
 
 void DummyClassName::reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg)
 {
+  const auto age_of_last_command = get_node()->now() - msg->header.stamp;
   if (msg->joint_names.size() == params_.joints.size()) {
-    input_ref_.writeFromNonRT(msg);
+    if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) {
+      input_ref_.writeFromNonRT(msg);
+    } else {
+      RCLCPP_ERROR(
+        get_node()->get_logger(),
+        "Received message has timestamp %.10f older for %.10f which "
+        "is more then allowed timeout "
+        "(%.4f).",
+        rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(),
+        ref_timeout_.seconds());
+      reset_controller_reference_msg(msg, params_.joints, get_node());
+    }
   } else {
     RCLCPP_ERROR(
       get_node()->get_logger(),
@@ -185,7 +201,7 @@ controller_interface::CallbackReturn DummyClassName::on_activate(
   // `controller_interface::get_ordered_interfaces` helper function
 
   // Set default value in command
-  reset_controller_reference_msg(*(input_ref_.readFromRT)(), params_.joints);
+  reset_controller_reference_msg(*(input_ref_.readFromRT)(), params_.joints, get_node());
 
   return controller_interface::CallbackReturn::SUCCESS;
 }
@@ -205,16 +221,24 @@ controller_interface::return_type DummyClassName::update(
   const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
   auto current_ref = input_ref_.readFromRT();
+  const auto age_of_last_command = get_node()->now() - (*current_ref)->header.stamp;
 
   // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`,
   // instead of a loop
   for (size_t i = 0; i < command_interfaces_.size(); ++i) {
-    if (!std::isnan((*current_ref)->displacements[i])) {
-      if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) {
-        (*current_ref)->displacements[i] /= 2;
+    // send message only if there is no timeout
+    if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) {
+      if (!std::isnan((*current_ref)->displacements[i])) {
+        if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) {
+          (*current_ref)->displacements[i] /= 2;
+        }
+        command_interfaces_[i].set_value((*current_ref)->displacements[i]);
+        if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) {
+          (*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
+        }
       }
-      command_interfaces_[i].set_value((*current_ref)->displacements[i]);
-
+    } else {
+      command_interfaces_[i].set_value(0.0);
       (*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
     }
   }

templates/ros2_control/controller/dummy_controller.yaml

@@ -29,3 +29,10 @@ dummy_controller:
       forbidden_interface_name_prefix: null
     }
   }
+  reference_timeout: {
+    type: double,
+    default_value: 0.0,
+    description: "Timeout for controller references after which they will be reset.
+    This is especially useful for controllers that can cause unwanted and dangerous behaviour
+    if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.",
+  }

templates/ros2_control/controller/dummy_controller_params.yaml

@@ -5,3 +5,5 @@ test_dummy_controller:
       - joint1
 
     interface_name: acceleration
+
+    reference_timeout: 0.1

templates/ros2_control/controller/dummy_controller_preceeding_params.yaml

@@ -7,3 +7,5 @@ test_dummy_controller:
 
     state_joints:
       - joint1state
+
+    reference_timeout: 0.1

templates/ros2_control/controller/dummy_package_namespace/dummy_chainable_controller.hpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -95,6 +95,8 @@ class DummyClassName : public controller_interface::ChainableControllerInterface
   rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ = nullptr;
   realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>> input_ref_;
 
+  rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0);  // 0ms
+
   rclcpp::Service<ControllerModeSrvType>::SharedPtr set_slow_control_mode_service_;
   realtime_tools::RealtimeBuffer<control_mode_type> control_mode_;
 

templates/ros2_control/controller/dummy_package_namespace/dummy_controller.hpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -92,6 +92,8 @@ class DummyClassName : public controller_interface::ControllerInterface
   rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ = nullptr;
   realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>> input_ref_;
 
+  rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0);  // 0ms
+
   rclcpp::Service<ControllerModeSrvType>::SharedPtr set_slow_control_mode_service_;
   realtime_tools::RealtimeBuffer<control_mode_type> control_mode_;
 

templates/ros2_control/controller/dummy_package_namespace/validate_dummy_controller_parameters.hpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.

templates/ros2_control/controller/dummy_package_namespace/visibility_control.h

@@ -1,4 +1,4 @@
-// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.