diff --git a/templates/ros2_control/controller/dummy_chainable_controller.cpp b/templates/ros2_control/controller/dummy_chainable_controller.cpp
index 9a66450f..55b2c126 100644
--- a/templates/ros2_control/controller/dummy_chainable_controller.cpp
+++ b/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,
     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,32 @@ 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();
+          (*current_ref)->velocities[i] = std::numeric_limits<double>::quiet_NaN();
+          (*current_ref)->duration = 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();
+        (*current_ref)->velocities[i] = std::numeric_limits<double>::quiet_NaN();
+        (*current_ref)->duration = std::numeric_limits<double>::quiet_NaN();
+      }
     }
   }
   return controller_interface::return_type::OK;
@@ -238,16 +272,22 @@ 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();
   // 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) {
+    // send message only if there is no timeout
     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]);
-
-      reference_interfaces_[i] = std::numeric_limits<double>::quiet_NaN();
+      if (ref_timeout_ == rclcpp::Duration::from_seconds(0)) {
+        reference_interfaces_[i] = std::numeric_limits<double>::quiet_NaN();
+        (*current_ref)->displacements[i] = std::numeric_limits<double>::quiet_NaN();
+        (*current_ref)->velocities[i] = std::numeric_limits<double>::quiet_NaN();
+        (*current_ref)->duration = std::numeric_limits<double>::quiet_NaN();
+      }
     }
   }
 
@@ -258,6 +298,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;
 }
 
diff --git a/templates/ros2_control/controller/dummy_controller.cpp b/templates/ros2_control/controller/dummy_controller.cpp
index 66ecdcd0..015e2fc1 100644
--- a/templates/ros2_control/controller/dummy_controller.cpp
+++ b/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,
     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,17 +221,29 @@ 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();
+          (*current_ref)->velocities[i] = std::numeric_limits<double>::quiet_NaN();
+          (*current_ref)->duration = 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();
+      (*current_ref)->velocities[i] = std::numeric_limits<double>::quiet_NaN();
+      (*current_ref)->duration = std::numeric_limits<double>::quiet_NaN();
     }
   }
 
diff --git a/templates/ros2_control/controller/dummy_controller.yaml b/templates/ros2_control/controller/dummy_controller.yaml
index 84351bf9..67819d5b 100644
--- a/templates/ros2_control/controller/dummy_controller.yaml
+++ b/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.",
+  }
diff --git a/templates/ros2_control/controller/dummy_controller_params.yaml b/templates/ros2_control/controller/dummy_controller_params.yaml
index 69c14224..ec93d1fc 100644
--- a/templates/ros2_control/controller/dummy_controller_params.yaml
+++ b/templates/ros2_control/controller/dummy_controller_params.yaml
@@ -5,3 +5,5 @@ test_dummy_controller:
       - joint1
 
     interface_name: acceleration
+
+    reference_timeout: 0.1
diff --git a/templates/ros2_control/controller/dummy_controller_preceeding_params.yaml b/templates/ros2_control/controller/dummy_controller_preceeding_params.yaml
index 006570af..e945dce2 100644
--- a/templates/ros2_control/controller/dummy_controller_preceeding_params.yaml
+++ b/templates/ros2_control/controller/dummy_controller_preceeding_params.yaml
@@ -7,3 +7,5 @@ test_dummy_controller:
 
     state_joints:
       - joint1state
+
+    reference_timeout: 0.1
diff --git a/templates/ros2_control/controller/dummy_package_namespace/dummy_chainable_controller.hpp b/templates/ros2_control/controller/dummy_package_namespace/dummy_chainable_controller.hpp
index 53dd4d18..aded4a8e 100644
--- a/templates/ros2_control/controller/dummy_package_namespace/dummy_chainable_controller.hpp
+++ b/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_;
 
diff --git a/templates/ros2_control/controller/dummy_package_namespace/dummy_controller.hpp b/templates/ros2_control/controller/dummy_package_namespace/dummy_controller.hpp
index df7046e8..edb35598 100644
--- a/templates/ros2_control/controller/dummy_package_namespace/dummy_controller.hpp
+++ b/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_;
 
diff --git a/templates/ros2_control/controller/dummy_package_namespace/validate_dummy_controller_parameters.hpp b/templates/ros2_control/controller/dummy_package_namespace/validate_dummy_controller_parameters.hpp
index 7468a1f7..79ec1c45 100644
--- a/templates/ros2_control/controller/dummy_package_namespace/validate_dummy_controller_parameters.hpp
+++ b/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.
diff --git a/templates/ros2_control/controller/dummy_package_namespace/visibility_control.h b/templates/ros2_control/controller/dummy_package_namespace/visibility_control.h
index ab72b560..a5ad0c90 100644
--- a/templates/ros2_control/controller/dummy_package_namespace/visibility_control.h
+++ b/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.
diff --git a/templates/ros2_control/controller/test_dummy_chainable_controller.cpp b/templates/ros2_control/controller/test_dummy_chainable_controller.cpp
index 7a835ae0..7cd145d3 100644
--- a/templates/ros2_control/controller/test_dummy_chainable_controller.cpp
+++ b/templates/ros2_control/controller/test_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.
@@ -28,13 +28,14 @@ class DummyClassNameTest : public DummyClassNameFixture<TestableDummyClassName>
 {
 };
 
-TEST_F(DummyClassNameTest, all_parameters_set_configure_success)
+TEST_F(DummyClassNameTest, when_controller_is_configured_expect_all_parameters_set)
 {
   SetUpController();
 
   ASSERT_TRUE(controller_->params_.joints.empty());
   ASSERT_TRUE(controller_->params_.state_joints.empty());
   ASSERT_TRUE(controller_->params_.interface_name.empty());
+  ASSERT_EQ(controller_->params_.reference_timeout, 0.0);
 
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
 
@@ -42,9 +43,10 @@ TEST_F(DummyClassNameTest, all_parameters_set_configure_success)
   ASSERT_TRUE(controller_->params_.state_joints.empty());
   ASSERT_THAT(controller_->state_joints_, testing::ElementsAreArray(joint_names_));
   ASSERT_EQ(controller_->params_.interface_name, interface_name_);
+  ASSERT_EQ(controller_->params_.reference_timeout, 0.1);
 }
 
-TEST_F(DummyClassNameTest, check_exported_intefaces)
+TEST_F(DummyClassNameTest, when_controller_configured_expect_properly_exported_interfaces)
 {
   SetUpController();
 
@@ -180,6 +182,7 @@ TEST_F(DummyClassNameTest, test_update_logic_fast)
   // set command statically
   static constexpr double TEST_DISPLACEMENT = 23.24;
   std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
+  msg->header.stamp = controller_->get_node()->now();
   msg->joint_names = joint_names_;
   msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT);
   msg->velocities.resize(joint_names_.size(), std::numeric_limits<double>::quiet_NaN());
@@ -192,7 +195,7 @@ TEST_F(DummyClassNameTest, test_update_logic_fast)
 
   EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST);
   EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT);
-  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0]));
+  EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT);
   EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST);
   EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size());
   for (const auto & interface : controller_->reference_interfaces_) {
@@ -216,6 +219,7 @@ TEST_F(DummyClassNameTest, test_update_logic_slow)
   static constexpr double TEST_DISPLACEMENT = 23.24;
   std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
   // When slow mode is enabled command ends up being half of the value
+  msg->header.stamp = controller_->get_node()->now();
   msg->joint_names = joint_names_;
   msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT);
   msg->velocities.resize(joint_names_.size(), std::numeric_limits<double>::quiet_NaN());
@@ -229,8 +233,8 @@ TEST_F(DummyClassNameTest, test_update_logic_slow)
     controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
 
-  EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT / 2);
-  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0]));
+  EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT / 4);
+  EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT / 2);
   EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size());
   for (const auto & interface : controller_->reference_interfaces_) {
     EXPECT_TRUE(std::isnan(interface));
@@ -252,6 +256,7 @@ TEST_F(DummyClassNameTest, test_update_logic_chainable_fast)
   // set command statically
   static constexpr double TEST_DISPLACEMENT = 23.24;
   std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
+  msg->header.stamp = controller_->get_node()->now();
   msg->joint_names = joint_names_;
   msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT);
   msg->velocities.resize(joint_names_.size(), std::numeric_limits<double>::quiet_NaN());
@@ -292,6 +297,7 @@ TEST_F(DummyClassNameTest, test_update_logic_chainable_slow)
   static constexpr double TEST_DISPLACEMENT = 23.24;
   std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
   // When slow mode is enabled command ends up being half of the value
+  msg->header.stamp = controller_->get_node()->now();
   msg->joint_names = joint_names_;
   msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT);
   msg->velocities.resize(joint_names_.size(), std::numeric_limits<double>::quiet_NaN());
@@ -352,7 +358,7 @@ TEST_F(DummyClassNameTest, receive_message_and_publish_updated_status)
 
   ASSERT_EQ(msg.set_point, 101.101);
 
-  publish_commands();
+  publish_commands(controller_->get_node()->now());
   ASSERT_TRUE(controller_->wait_for_commands(executor));
 
   ASSERT_EQ(
@@ -366,6 +372,173 @@ TEST_F(DummyClassNameTest, receive_message_and_publish_updated_status)
   ASSERT_EQ(msg.set_point, 0.45);
 }
 
+// when too old msg is sent expect reference msg reset
+TEST_F(DummyClassNameTest, when_reference_msg_is_too_old_expect_unset_reference)
+{
+  SetUpController();
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(controller_->get_node()->get_node_base_interface());
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  auto reference = *(controller_->input_ref_.readFromNonRT());
+  auto old_timestamp = reference->header.stamp;
+  EXPECT_TRUE(std::isnan(reference->displacements[0]));
+
+  // reference_callback() is implicitly called when publish_commands() is called
+  // reference_msg is published with provided time stamp when publish_commands( time_stamp)
+  // is called
+  publish_commands(
+    controller_->get_node()->now() - controller_->ref_timeout_ -
+    rclcpp::Duration::from_seconds(0.1));
+  ASSERT_TRUE(controller_->wait_for_commands(executor));
+  ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp);
+  EXPECT_TRUE(std::isnan(reference->displacements[0]));
+}
+
+// when not in chainable mode and ref_msg_timedout expect
+// command_interfaces are set to 0.0 and when ref_msg is not timedout expect
+// command_interfaces are set to valid command values
+TEST_F(DummyClassNameTest, when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds)
+{
+  // 1. age>ref_timeout 2. age<ref_timeout
+  SetUpController();
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(controller_->get_node()->get_node_base_interface());
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  controller_->set_chained_mode(false);
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_FALSE(controller_->is_in_chained_mode());
+
+  for (const auto & interface : controller_->reference_interfaces_) {
+    EXPECT_TRUE(std::isnan(interface));
+  }
+
+  joint_command_values_[0] = TEST_DISPLACEMENT;
+  std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
+  msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ -
+                      rclcpp::Duration::from_seconds(0.1);
+  msg->joint_names = joint_names_;
+  msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT);
+  msg->velocities.resize(joint_names_.size(), std::numeric_limits<double>::quiet_NaN());
+  msg->duration = std::numeric_limits<double>::quiet_NaN();
+  controller_->input_ref_.writeFromNonRT(msg);
+  const auto age_of_last_command =
+    controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp;
+
+  // age_of_last_command > ref_timeout_
+  ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_);
+  ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+
+  EXPECT_EQ(joint_command_values_[0], 0.0);
+  for (const auto & interface : controller_->reference_interfaces_) {
+    EXPECT_TRUE(std::isnan(interface));
+  }
+  for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) {
+    EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0);
+  }
+
+  std::shared_ptr<ControllerReferenceMsg> msg_2 = std::make_shared<ControllerReferenceMsg>();
+  msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01);
+  msg_2->joint_names = joint_names_;
+  msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT);
+  msg_2->velocities.resize(joint_names_.size(), std::numeric_limits<double>::quiet_NaN());
+  msg_2->duration = std::numeric_limits<double>::quiet_NaN();
+  controller_->input_ref_.writeFromNonRT(msg_2);
+  const auto age_of_last_command_2 =
+    controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp;
+  EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST);
+  // age_of_last_command_2 < ref_timeout_
+  ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_);
+  ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+  EXPECT_EQ(joint_command_values_[0], TEST_DISPLACEMENT);
+  ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT);
+  for (const auto & interface : controller_->reference_interfaces_) {
+    EXPECT_TRUE(std::isnan(interface));
+  }
+
+  for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) {
+    EXPECT_EQ(controller_->command_interfaces_[i].get_value(), TEST_DISPLACEMENT);
+  }
+}
+
+// when ref_timeout = 0 expect reference_msg is accepted only once and command_interfaces
+// are calculated to valid values and reference_interfaces are unset
+TEST_F(DummyClassNameTest, when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once)
+{
+  SetUpController();
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(controller_->get_node()->get_node_base_interface());
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  joint_command_values_[0] = TEST_DISPLACEMENT;
+  controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0);
+  std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
+  msg->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.0);
+  msg->joint_names = joint_names_;
+  msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT);
+  msg->velocities.resize(joint_names_.size(), std::numeric_limits<double>::quiet_NaN());
+  msg->duration = std::numeric_limits<double>::quiet_NaN();
+  controller_->input_ref_.writeFromNonRT(msg);
+  const auto age_of_last_command =
+    controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp;
+
+  ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_);
+  ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+  EXPECT_FALSE(std::isnan(joint_command_values_[0]));
+  ASSERT_NE((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT);
+  EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT);
+  ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0]));
+}
+
+TEST_F(
+  DummyClassNameTest,
+  when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once)
+{
+  SetUpController();
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(controller_->get_node()->get_node_base_interface());
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size());
+  EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]);
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0]));
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0]));
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration));
+  controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0);
+  // reference_callback() is called implicitly when publish_commands() is called.
+  publish_commands(controller_->get_node()->now());
+  ASSERT_TRUE(controller_->wait_for_commands(executor));
+  EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size());
+  EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]);
+  EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0]));
+  EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0]));
+  EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration));
+  EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->displacements[0], 0.45);
+  EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->velocities[0], 0.0);
+  EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->duration, 1.25);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0]));
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0]));
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration));
+  
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
diff --git a/templates/ros2_control/controller/test_dummy_chainable_controller.hpp b/templates/ros2_control/controller/test_dummy_chainable_controller.hpp
index ace3db7b..e99779e3 100644
--- a/templates/ros2_control/controller/test_dummy_chainable_controller.hpp
+++ b/templates/ros2_control/controller/test_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.
@@ -48,7 +48,8 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR;
 // subclassing and friending so we can access member variables
 class TestableDummyClassName : public dummy_package_namespace::DummyClassName
 {
-  FRIEND_TEST(DummyClassNameTest, all_parameters_set_configure_success);
+  FRIEND_TEST(DummyClassNameTest, when_controller_is_configured_expect_all_parameters_set);
+  FRIEND_TEST(DummyClassNameTest, when_controller_configured_expect_properly_exported_interfaces);
   FRIEND_TEST(DummyClassNameTest, activate_success);
   FRIEND_TEST(DummyClassNameTest, reactivate_success);
   FRIEND_TEST(DummyClassNameTest, test_setting_slow_mode_service);
@@ -56,6 +57,14 @@ class TestableDummyClassName : public dummy_package_namespace::DummyClassName
   FRIEND_TEST(DummyClassNameTest, test_update_logic_slow);
   FRIEND_TEST(DummyClassNameTest, test_update_logic_chainable_fast);
   FRIEND_TEST(DummyClassNameTest, test_update_logic_chainable_slow);
+  FRIEND_TEST(DummyClassNameTest, when_reference_msg_is_too_old_expect_unset_reference);
+  FRIEND_TEST(
+    DummyClassNameTest, when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds);
+  FRIEND_TEST(
+    DummyClassNameTest, when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once);
+  FRIEND_TEST(
+    DummyClassNameTest,
+    when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once);
 
 public:
   controller_interface::CallbackReturn on_configure(
@@ -121,7 +130,7 @@ class DummyClassNameFixture : public ::testing::Test
 
     command_publisher_node_ = std::make_shared<rclcpp::Node>("command_publisher");
     command_publisher_ = command_publisher_node_->create_publisher<ControllerReferenceMsg>(
-      "/test_dummy_controller/commands", rclcpp::SystemDefaultsQoS());
+      "/test_dummy_controller/reference", rclcpp::SystemDefaultsQoS());
 
     service_caller_node_ = std::make_shared<rclcpp::Node>("service_caller");
     slow_control_service_client_ = service_caller_node_->create_client<ControllerModeSrvType>(
@@ -197,7 +206,7 @@ class DummyClassNameFixture : public ::testing::Test
 
   // TODO(anyone): add/remove arguments as it suites your command message type
   void publish_commands(
-    const std::vector<double> & displacements = {0.45},
+    const rclcpp::Time & stamp, const std::vector<double> & displacements = {0.45},
     const std::vector<double> & velocities = {0.0}, const double duration = 1.25)
   {
     auto wait_for_topic = [&](const auto topic_name) {
@@ -216,6 +225,7 @@ class DummyClassNameFixture : public ::testing::Test
     wait_for_topic(command_publisher_->get_topic_name());
 
     ControllerReferenceMsg msg;
+    msg.header.stamp = stamp;
     msg.joint_names = joint_names_;
     msg.displacements = displacements;
     msg.velocities = velocities;
@@ -255,6 +265,9 @@ class DummyClassNameFixture : public ::testing::Test
   std::vector<hardware_interface::StateInterface> state_itfs_;
   std::vector<hardware_interface::CommandInterface> command_itfs_;
 
+  double ref_timeout_ = 0.2;
+  static constexpr double TEST_DISPLACEMENT = 23.24;
+
   // Test related parameters
   std::unique_ptr<TestableDummyClassName> controller_;
   rclcpp::Node::SharedPtr command_publisher_node_;
diff --git a/templates/ros2_control/controller/test_dummy_chainable_controller_preceeding.cpp b/templates/ros2_control/controller/test_dummy_chainable_controller_preceeding.cpp
index d953cfc6..5ae070f3 100644
--- a/templates/ros2_control/controller/test_dummy_chainable_controller_preceeding.cpp
+++ b/templates/ros2_control/controller/test_dummy_chainable_controller_preceeding.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.
@@ -28,23 +28,24 @@ class DummyClassNameTest : public DummyClassNameFixture<TestableDummyClassName>
 {
 };
 
-TEST_F(DummyClassNameTest, all_parameters_set_configure_success)
+TEST_F(DummyClassNameTest, when_controller_is_configured_expect_all_parameters_set)
 {
   SetUpController();
 
   ASSERT_TRUE(controller_->params_.joints.empty());
   ASSERT_TRUE(controller_->params_.state_joints.empty());
   ASSERT_TRUE(controller_->params_.interface_name.empty());
+  ASSERT_EQ(controller_->params_.reference_timeout, 0.0);
 
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
 
   ASSERT_THAT(controller_->params_.joints, testing::ElementsAreArray(joint_names_));
-  ASSERT_THAT(controller_->params_.state_joints, testing::ElementsAreArray(state_joint_names_));
   ASSERT_THAT(controller_->state_joints_, testing::ElementsAreArray(state_joint_names_));
   ASSERT_EQ(controller_->params_.interface_name, interface_name_);
+  ASSERT_EQ(controller_->params_.reference_timeout, 0.1);
 }
 
-TEST_F(DummyClassNameTest, check_exported_intefaces)
+TEST_F(DummyClassNameTest, when_controller_configured_expect_properly_exported_interfaces)
 {
   SetUpController();
 
diff --git a/templates/ros2_control/controller/test_dummy_controller.cpp b/templates/ros2_control/controller/test_dummy_controller.cpp
index 57b2ea7a..b4beb011 100644
--- a/templates/ros2_control/controller/test_dummy_controller.cpp
+++ b/templates/ros2_control/controller/test_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.
@@ -28,7 +28,7 @@ class DummyClassNameTest : public DummyClassNameFixture<TestableDummyClassName>
 {
 };
 
-TEST_F(DummyClassNameTest, all_parameters_set_configure_success)
+TEST_F(DummyClassNameTest, when_controller_is_configured_expect_all_parameters_set)
 {
   SetUpController();
 
@@ -44,7 +44,7 @@ TEST_F(DummyClassNameTest, all_parameters_set_configure_success)
   ASSERT_EQ(controller_->params_.interface_name, interface_name_);
 }
 
-TEST_F(DummyClassNameTest, check_exported_intefaces)
+TEST_F(DummyClassNameTest, when_controller_configured_expect_properly_exported_interfaces)
 {
   SetUpController();
 
@@ -161,6 +161,7 @@ TEST_F(DummyClassNameTest, test_update_logic_fast)
   // set command statically
   static constexpr double TEST_DISPLACEMENT = 23.24;
   std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
+  msg->header.stamp = controller_->get_node()->now();
   msg->joint_names = joint_names_;
   msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT);
   msg->velocities.resize(joint_names_.size(), std::numeric_limits<double>::quiet_NaN());
@@ -173,7 +174,7 @@ TEST_F(DummyClassNameTest, test_update_logic_fast)
 
   EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST);
   EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT);
-  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0]));
+  EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT);
   EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST);
 }
 
@@ -191,6 +192,7 @@ TEST_F(DummyClassNameTest, test_update_logic_slow)
   static constexpr double TEST_DISPLACEMENT = 23.24;
   std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
   // When slow mode is enabled command ends up being half of the value
+  msg->header.stamp = controller_->get_node()->now();
   msg->joint_names = joint_names_;
   msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT);
   msg->velocities.resize(joint_names_.size(), std::numeric_limits<double>::quiet_NaN());
@@ -205,7 +207,7 @@ TEST_F(DummyClassNameTest, test_update_logic_slow)
     controller_interface::return_type::OK);
 
   EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT / 2);
-  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0]));
+  EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT / 2);
 }
 
 TEST_F(DummyClassNameTest, publish_status_success)
@@ -243,7 +245,7 @@ TEST_F(DummyClassNameTest, receive_message_and_publish_updated_status)
 
   ASSERT_EQ(msg.set_point, 101.101);
 
-  publish_commands();
+  publish_commands(controller_->get_node()->now());
   ASSERT_TRUE(controller_->wait_for_commands(executor));
 
   ASSERT_EQ(
@@ -257,6 +259,163 @@ TEST_F(DummyClassNameTest, receive_message_and_publish_updated_status)
   ASSERT_EQ(msg.set_point, 0.45);
 }
 
+// when too old msg is sent expect reference msg reset
+TEST_F(DummyClassNameTest, when_reference_msg_is_too_old_expect_unset_reference)
+{
+  SetUpController();
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(controller_->get_node()->get_node_base_interface());
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  auto reference = *(controller_->input_ref_.readFromNonRT());
+  auto old_timestamp = reference->header.stamp;
+  EXPECT_TRUE(std::isnan(reference->displacements[0]));
+
+  // reference_callback() is implicitly called when publish_commands() is called
+  // reference_msg is published with provided time stamp when publish_commands( time_stamp)
+  // is called
+  publish_commands(
+    controller_->get_node()->now() - controller_->ref_timeout_ -
+    rclcpp::Duration::from_seconds(0.1));
+  ASSERT_TRUE(controller_->wait_for_commands(executor));
+  ASSERT_EQ(old_timestamp, (*(controller_->input_ref_.readFromNonRT()))->header.stamp);
+  EXPECT_TRUE(std::isnan(reference->displacements[0]));
+}
+
+// when not in chainable mode and ref_msg_timedout expect
+// command_interfaces are set to 0.0 and when ref_msg is not timedout expect
+// command_interfaces are set to valid command values
+TEST_F(DummyClassNameTest, when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds)
+{
+  // 1. age>ref_timeout 2. age<ref_timeout
+  SetUpController();
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(controller_->get_node()->get_node_base_interface());
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  controller_->set_chained_mode(false);
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_FALSE(controller_->is_in_chained_mode());
+
+  joint_command_values_[0] = TEST_DISPLACEMENT;
+  std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
+  msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ -
+                      rclcpp::Duration::from_seconds(0.1);
+  msg->joint_names = joint_names_;
+  msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT);
+  msg->velocities.resize(joint_names_.size(), std::numeric_limits<double>::quiet_NaN());
+  msg->duration = std::numeric_limits<double>::quiet_NaN();
+  controller_->input_ref_.writeFromNonRT(msg);
+  const auto age_of_last_command =
+    controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp;
+
+  // age_of_last_command > ref_timeout_
+  ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_);
+  ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+
+  EXPECT_EQ(joint_command_values_[0], 0.0);
+
+  for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) {
+    EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0);
+  }
+
+  std::shared_ptr<ControllerReferenceMsg> msg_2 = std::make_shared<ControllerReferenceMsg>();
+  msg_2->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.01);
+  msg_2->joint_names = joint_names_;
+  msg_2->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT);
+  msg_2->velocities.resize(joint_names_.size(), std::numeric_limits<double>::quiet_NaN());
+  msg_2->duration = std::numeric_limits<double>::quiet_NaN();
+  controller_->input_ref_.writeFromNonRT(msg_2);
+  const auto age_of_last_command_2 =
+    controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp;
+  EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST);
+  // age_of_last_command_2 < ref_timeout_
+  ASSERT_TRUE(age_of_last_command_2 <= controller_->ref_timeout_);
+  ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+  EXPECT_EQ(joint_command_values_[0], TEST_DISPLACEMENT);
+  ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT);
+
+  for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) {
+    EXPECT_EQ(controller_->command_interfaces_[i].get_value(), TEST_DISPLACEMENT);
+  }
+}
+
+// when ref_timeout = 0 expect reference_msg is accepted only once and command_interfaces
+// are calculated to valid values and reference_interfaces are unset
+TEST_F(DummyClassNameTest, when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once)
+{
+  SetUpController();
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(controller_->get_node()->get_node_base_interface());
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  joint_command_values_[0] = TEST_DISPLACEMENT;
+  controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0);
+  std::shared_ptr<ControllerReferenceMsg> msg = std::make_shared<ControllerReferenceMsg>();
+  msg->header.stamp = controller_->get_node()->now() - rclcpp::Duration::from_seconds(0.0);
+  msg->joint_names = joint_names_;
+  msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT);
+  msg->velocities.resize(joint_names_.size(), std::numeric_limits<double>::quiet_NaN());
+  msg->duration = std::numeric_limits<double>::quiet_NaN();
+  controller_->input_ref_.writeFromNonRT(msg);
+  const auto age_of_last_command =
+    controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp;
+
+  ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_);
+  ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+  EXPECT_FALSE(std::isnan(joint_command_values_[0]));
+  ASSERT_NE((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT);
+  EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT);
+  ASSERT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0]));
+}
+
+TEST_F(
+  DummyClassNameTest,
+  when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once)
+{
+  SetUpController();
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(controller_->get_node()->get_node_base_interface());
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size());
+  EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]);
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0]));
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0]));
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration));
+  controller_->ref_timeout_ = rclcpp::Duration::from_seconds(0.0);
+  // reference_callback() is called implicitly when publish_commands() is called.
+  publish_commands(controller_->get_node()->now());
+  ASSERT_TRUE(controller_->wait_for_commands(executor));
+  EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names.size(), joint_names_.size());
+  EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->joint_names[0], joint_names_[0]);
+  EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0]));
+  EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0]));
+  EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration));
+  EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->displacements[0], 0.45);
+  EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->velocities[0], 0.0);
+  EXPECT_EQ((*(controller_->input_ref_.readFromNonRT()))->duration, 1.25);
+  ASSERT_EQ(
+    controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
+    controller_interface::return_type::OK);
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->displacements[0]));
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->velocities[0]));
+  EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->duration));
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
diff --git a/templates/ros2_control/controller/test_dummy_controller.hpp b/templates/ros2_control/controller/test_dummy_controller.hpp
index 741e0a8e..7b1b7390 100644
--- a/templates/ros2_control/controller/test_dummy_controller.hpp
+++ b/templates/ros2_control/controller/test_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.
@@ -47,12 +47,21 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR;
 // subclassing and friending so we can access member variables
 class TestableDummyClassName : public dummy_package_namespace::DummyClassName
 {
-  FRIEND_TEST(DummyClassNameTest, all_parameters_set_configure_success);
+  FRIEND_TEST(DummyClassNameTest, when_controller_is_configured_expect_all_parameters_set);
+  FRIEND_TEST(DummyClassNameTest, when_controller_configured_expect_properly_exported_interfaces);
   FRIEND_TEST(DummyClassNameTest, activate_success);
   FRIEND_TEST(DummyClassNameTest, reactivate_success);
   FRIEND_TEST(DummyClassNameTest, test_setting_slow_mode_service);
   FRIEND_TEST(DummyClassNameTest, test_update_logic_fast);
   FRIEND_TEST(DummyClassNameTest, test_update_logic_slow);
+  FRIEND_TEST(DummyClassNameTest, when_reference_msg_is_too_old_expect_unset_reference);
+  FRIEND_TEST(
+    DummyClassNameTest, when_ref_msg_old_expect_cmnd_itfs_set_to_zero_otherwise_to_valid_cmnds);
+  FRIEND_TEST(
+    DummyClassNameTest, when_reference_timeout_is_zero_expect_reference_msg_being_used_only_once);
+  FRIEND_TEST(
+    DummyClassNameTest,
+    when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once);
 
 public:
   controller_interface::CallbackReturn on_configure(
@@ -66,6 +75,12 @@ class TestableDummyClassName : public dummy_package_namespace::DummyClassName
     return ret;
   }
 
+  controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override
+  {
+    return dummy_package_namespace::DummyClassName::on_activate(previous_state);
+  }
+
   /**
    * @brief wait_for_command blocks until a new ControllerReferenceMsg is received.
    * Requires that the executor is not spinned elsewhere between the
@@ -111,7 +126,7 @@ class DummyClassNameFixture : public ::testing::Test
 
     command_publisher_node_ = std::make_shared<rclcpp::Node>("command_publisher");
     command_publisher_ = command_publisher_node_->create_publisher<ControllerReferenceMsg>(
-      "/test_dummy_controller/commands", rclcpp::SystemDefaultsQoS());
+      "/test_dummy_controller/reference", rclcpp::SystemDefaultsQoS());
 
     service_caller_node_ = std::make_shared<rclcpp::Node>("service_caller");
     slow_control_service_client_ = service_caller_node_->create_client<ControllerModeSrvType>(
@@ -187,7 +202,7 @@ class DummyClassNameFixture : public ::testing::Test
 
   // TODO(anyone): add/remove arguments as it suites your command message type
   void publish_commands(
-    const std::vector<double> & displacements = {0.45},
+    const rclcpp::Time & stamp, const std::vector<double> & displacements = {0.45},
     const std::vector<double> & velocities = {0.0}, const double duration = 1.25)
   {
     auto wait_for_topic = [&](const auto topic_name) {
@@ -206,6 +221,7 @@ class DummyClassNameFixture : public ::testing::Test
     wait_for_topic(command_publisher_->get_topic_name());
 
     ControllerReferenceMsg msg;
+    msg.header.stamp = stamp;
     msg.joint_names = joint_names_;
     msg.displacements = displacements;
     msg.velocities = velocities;
@@ -245,6 +261,9 @@ class DummyClassNameFixture : public ::testing::Test
   std::vector<hardware_interface::StateInterface> state_itfs_;
   std::vector<hardware_interface::CommandInterface> command_itfs_;
 
+  double ref_timeout_ = 0.2;
+  static constexpr double TEST_DISPLACEMENT = 23.24;
+
   // Test related parameters
   std::unique_ptr<TestableDummyClassName> controller_;
   rclcpp::Node::SharedPtr command_publisher_node_;
diff --git a/templates/ros2_control/controller/test_dummy_controller_preceeding.cpp b/templates/ros2_control/controller/test_dummy_controller_preceeding.cpp
index a3bc99ce..0219fe17 100644
--- a/templates/ros2_control/controller/test_dummy_controller_preceeding.cpp
+++ b/templates/ros2_control/controller/test_dummy_controller_preceeding.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.
@@ -28,23 +28,26 @@ class DummyClassNameTest : public DummyClassNameFixture<TestableDummyClassName>
 {
 };
 
-TEST_F(DummyClassNameTest, all_parameters_set_configure_success)
+TEST_F(DummyClassNameTest, when_controller_is_configured_expect_all_parameters_set)
 {
   SetUpController();
 
+  ASSERT_EQ(controller_->params_.reference_timeout, 0.0);
   ASSERT_TRUE(controller_->params_.joints.empty());
   ASSERT_TRUE(controller_->params_.state_joints.empty());
   ASSERT_TRUE(controller_->params_.interface_name.empty());
+  ASSERT_EQ(controller_->params_.reference_timeout, 0.0);
 
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
 
+  ASSERT_EQ(controller_->params_.reference_timeout, 0.1);
   ASSERT_THAT(controller_->params_.joints, testing::ElementsAreArray(joint_names_));
-  ASSERT_THAT(controller_->params_.state_joints, testing::ElementsAreArray(state_joint_names_));
   ASSERT_THAT(controller_->state_joints_, testing::ElementsAreArray(state_joint_names_));
   ASSERT_EQ(controller_->params_.interface_name, interface_name_);
+  ASSERT_EQ(controller_->params_.reference_timeout, 0.1);
 }
 
-TEST_F(DummyClassNameTest, check_exported_intefaces)
+TEST_F(DummyClassNameTest, when_controller_configured_expect_properly_exported_interfaces)
 {
   SetUpController();
 
diff --git a/templates/ros2_control/controller/test_load_dummy_controller.cpp b/templates/ros2_control/controller/test_load_dummy_controller.cpp
index e1ea359c..00b340a7 100644
--- a/templates/ros2_control/controller/test_load_dummy_controller.cpp
+++ b/templates/ros2_control/controller/test_load_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.