diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp
index a548cd5856..0f76713eae 100644
--- a/controller_interface/include/controller_interface/controller_interface_base.hpp
+++ b/controller_interface/include/controller_interface/controller_interface_base.hpp
@@ -23,6 +23,7 @@
 #include "realtime_tools/async_function_handler.hpp"
 
 #include "hardware_interface/handle.hpp"
+#include "hardware_interface/introspection.hpp"
 #include "hardware_interface/loaned_command_interface.hpp"
 #include "hardware_interface/loaned_state_interface.hpp"
 
@@ -305,6 +306,14 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
    */
   void wait_for_trigger_update_to_finish();
 
+  std::string get_name() const;
+
+  /// Enable or disable introspection of the controller.
+  /**
+   * \param[in] enable Enable introspection if true, disable otherwise.
+   */
+  void enable_introspection(bool enable);
+
 protected:
   std::vector<hardware_interface::LoanedCommandInterface> command_interfaces_;
   std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
@@ -316,6 +325,9 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   bool is_async_ = false;
   std::string urdf_ = "";
   ControllerUpdateStats trigger_stats_;
+
+protected:
+  pal_statistics::RegistrationsRAII stats_registrations_;
 };
 
 using ControllerInterfaceBaseSharedPtr = std::shared_ptr<ControllerInterfaceBase>;
diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp
index b8dd9f770d..a86339bed2 100644
--- a/controller_interface/src/controller_interface_base.cpp
+++ b/controller_interface/src/controller_interface_base.cpp
@@ -18,6 +18,7 @@
 #include <string>
 #include <vector>
 
+#include "hardware_interface/introspection.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 
 namespace controller_interface
@@ -82,6 +83,9 @@ return_type ControllerInterfaceBase::init(
   node_->register_on_cleanup(
     [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
     {
+      // make sure introspection is disabled on controller cleanup as users may manually enable
+      // it in `on_configure` and `on_deactivate` - see the docs for details
+      enable_introspection(false);
       if (is_async() && async_handler_ && async_handler_->is_running())
       {
         async_handler_->stop_thread();
@@ -92,6 +96,7 @@ return_type ControllerInterfaceBase::init(
   node_->register_on_activate(
     [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
     {
+      enable_introspection(true);
       if (is_async() && async_handler_ && async_handler_->is_running())
       {
         // This is needed if it is disabled due to a thrown exception in the async callback thread
@@ -101,7 +106,11 @@ return_type ControllerInterfaceBase::init(
     });
 
   node_->register_on_deactivate(
-    std::bind(&ControllerInterfaceBase::on_deactivate, this, std::placeholders::_1));
+    [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
+    {
+      enable_introspection(false);
+      return on_deactivate(previous_state);
+    });
 
   node_->register_on_shutdown(
     std::bind(&ControllerInterfaceBase::on_shutdown, this, std::placeholders::_1));
@@ -158,6 +167,8 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
       thread_priority);
     async_handler_->start_thread();
   }
+  REGISTER_ROS2_CONTROL_INTROSPECTION("total_triggers", &trigger_stats_.total_triggers);
+  REGISTER_ROS2_CONTROL_INTROSPECTION("failed_triggers", &trigger_stats_.failed_triggers);
   trigger_stats_.reset();
 
   return get_node()->configure();
@@ -258,4 +269,19 @@ void ControllerInterfaceBase::wait_for_trigger_update_to_finish()
     async_handler_->wait_for_trigger_cycle_to_finish();
   }
 }
+
+std::string ControllerInterfaceBase::get_name() const { return get_node()->get_name(); }
+
+void ControllerInterfaceBase::enable_introspection(bool enable)
+{
+  if (enable)
+  {
+    stats_registrations_.enableAll();
+  }
+  else
+  {
+    stats_registrations_.disableAll();
+  }
+}
+
 }  // namespace controller_interface
diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp
index 7acbe20166..ae1be62b9f 100644
--- a/controller_manager/include/controller_manager/controller_manager.hpp
+++ b/controller_manager/include/controller_manager/controller_manager.hpp
@@ -80,7 +80,7 @@ class ControllerManager : public rclcpp::Node
     const std::string & node_namespace = "",
     const rclcpp::NodeOptions & options = get_cm_node_options());
 
-  virtual ~ControllerManager() = default;
+  virtual ~ControllerManager();
 
   void robot_description_callback(const std_msgs::msg::String & msg);
 
@@ -317,6 +317,13 @@ class ControllerManager : public rclcpp::Node
 
   void initialize_parameters();
 
+  /**
+   * Call shutdown to change the given controller lifecycle node to the finalized state.
+   *
+   * \param[in] controller controller to be shutdown.
+   */
+  void shutdown_controller(controller_manager::ControllerSpec & controller) const;
+
   /**
    * Clear request lists used when switching controllers. The lists are shared between "callback"
    * and "control loop" threads.
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
index fd59c89ea8..d3848c2bb2 100644
--- a/controller_manager/src/controller_manager.cpp
+++ b/controller_manager/src/controller_manager.cpp
@@ -22,6 +22,7 @@
 
 #include "controller_interface/controller_interface_base.hpp"
 #include "controller_manager_msgs/msg/hardware_component_state.hpp"
+#include "hardware_interface/introspection.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 #include "rcl/arguments.h"
@@ -58,6 +59,13 @@ static const rmw_qos_profile_t qos_services = {
   false};
 #endif
 
+inline bool is_controller_unconfigured(
+  const controller_interface::ControllerInterfaceBase & controller)
+{
+  return controller.get_lifecycle_state().id() ==
+         lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED;
+}
+
 inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller)
 {
   return controller.get_lifecycle_state().id() ==
@@ -284,6 +292,8 @@ ControllerManager::ControllerManager(
   init_controller_manager();
 }
 
+ControllerManager::~ControllerManager() { CLEAR_ALL_ROS2_CONTROL_INTROSPECTION_REGISTRIES(); }
+
 void ControllerManager::init_controller_manager()
 {
   // Get parameters needed for RT "update" loop to work
@@ -322,6 +332,10 @@ void ControllerManager::init_controller_manager()
   diagnostics_updater_.add(
     "Controller Manager Activity", this,
     &ControllerManager::controller_manager_diagnostic_callback);
+  INITIALIZE_ROS2_CONTROL_INTROSPECTION_REGISTRY(
+    this, hardware_interface::DEFAULT_INTROSPECTION_TOPIC,
+    hardware_interface::DEFAULT_REGISTRY_KEY);
+  START_ROS2_CONTROL_INTROSPECTION_PUBLISHER_THREAD(hardware_interface::DEFAULT_REGISTRY_KEY);
 }
 
 void ControllerManager::initialize_parameters()
@@ -695,39 +709,17 @@ controller_interface::return_type ControllerManager::unload_controller(
     return controller_interface::return_type::ERROR;
   }
 
-  RCLCPP_DEBUG(get_logger(), "Cleanup controller");
+  RCLCPP_DEBUG(get_logger(), "Shutdown controller");
   controller_chain_spec_cleanup(controller_chain_spec_, controller_name);
   // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
   // cleaning-up controllers?
-  if (is_controller_inactive(*controller.c))
+  if (is_controller_inactive(*controller.c) || is_controller_unconfigured(*controller.c))
   {
     RCLCPP_DEBUG(
-      get_logger(), "Controller '%s' is cleaned-up before unloading!", controller_name.c_str());
+      get_logger(), "Controller '%s' is shutdown before unloading!", controller_name.c_str());
     // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
     // cleaning-up controllers?
-    try
-    {
-      const auto new_state = controller.c->get_node()->cleanup();
-      if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
-      {
-        RCLCPP_WARN(
-          get_logger(), "Failed to clean-up the controller '%s' before unloading!",
-          controller_name.c_str());
-      }
-    }
-    catch (const std::exception & e)
-    {
-      RCLCPP_ERROR(
-        get_logger(),
-        "Caught exception of type : %s while cleaning up the controller '%s' before unloading: %s",
-        typeid(e).name(), controller_name.c_str(), e.what());
-    }
-    catch (...)
-    {
-      RCLCPP_ERROR(
-        get_logger(), "Failed to clean-up the controller '%s' before unloading",
-        controller_name.c_str());
-    }
+    shutdown_controller(controller);
   }
   executor_->remove_node(controller.c->get_node()->get_node_base_interface());
   to.erase(found_it);
@@ -745,6 +737,33 @@ controller_interface::return_type ControllerManager::unload_controller(
   return controller_interface::return_type::OK;
 }
 
+void ControllerManager::shutdown_controller(controller_manager::ControllerSpec & controller) const
+{
+  try
+  {
+    const auto new_state = controller.c->get_node()->shutdown();
+    if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
+    {
+      RCLCPP_WARN(
+        get_logger(), "Failed to shutdown the controller '%s' before unloading!",
+        controller.info.name.c_str());
+    }
+  }
+  catch (const std::exception & e)
+  {
+    RCLCPP_ERROR(
+      get_logger(),
+      "Caught exception of type : %s while shutdown the controller '%s' before unloading: %s",
+      typeid(e).name(), controller.info.name.c_str(), e.what());
+  }
+  catch (...)
+  {
+    RCLCPP_ERROR(
+      get_logger(), "Failed to shutdown the controller '%s' before unloading",
+      controller.info.name.c_str());
+  }
+}
+
 std::vector<ControllerSpec> ControllerManager::get_loaded_controllers() const
 {
   std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);
@@ -2606,6 +2625,8 @@ controller_interface::return_type ControllerManager::update(
     manage_switch();
   }
 
+  PUBLISH_ROS2_CONTROL_INTROSPECTION_DATA_ASYNC(hardware_interface::DEFAULT_REGISTRY_KEY);
+
   return ret;
 }
 
diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp
index ee8377f2a3..306799a66e 100644
--- a/controller_manager/test/test_controller/test_controller.cpp
+++ b/controller_manager/test/test_controller/test_controller.cpp
@@ -160,6 +160,15 @@ CallbackReturn TestController::on_cleanup(const rclcpp_lifecycle::State & /*prev
   return CallbackReturn::SUCCESS;
 }
 
+CallbackReturn TestController::on_shutdown(const rclcpp_lifecycle::State &)
+{
+  if (shutdown_calls)
+  {
+    (*shutdown_calls)++;
+  }
+  return CallbackReturn::SUCCESS;
+}
+
 void TestController::set_command_interface_configuration(
   const controller_interface::InterfaceConfiguration & cfg)
 {
diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp
index 0728be877b..a28dfa420e 100644
--- a/controller_manager/test/test_controller/test_controller.hpp
+++ b/controller_manager/test/test_controller/test_controller.hpp
@@ -54,6 +54,8 @@ class TestController : public controller_interface::ControllerInterface
 
   CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
 
+  CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override;
+
   void set_command_interface_configuration(
     const controller_interface::InterfaceConfiguration & cfg);
 
@@ -66,9 +68,10 @@ class TestController : public controller_interface::ControllerInterface
   rclcpp::Service<example_interfaces::srv::SetBool>::SharedPtr service_;
   unsigned int internal_counter = 0;
   bool simulate_cleanup_failure = false;
-  // Variable where we store when cleanup was called, pointer because the controller
-  // is usually destroyed after cleanup
+  // Variable where we store when shutdown was called, pointer because the controller
+  // is usually destroyed after shutdown
   size_t * cleanup_calls = nullptr;
+  size_t * shutdown_calls = nullptr;
   controller_interface::InterfaceConfiguration cmd_iface_cfg_;
   controller_interface::InterfaceConfiguration state_iface_cfg_;
 
diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp
index 49c02d28ac..4e7065b094 100644
--- a/controller_manager/test/test_controller_manager.cpp
+++ b/controller_manager/test/test_controller_manager.cpp
@@ -228,7 +228,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
   EXPECT_EQ(controller_interface::return_type::OK, unload_future.get());
 
   EXPECT_EQ(
-    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
     test_controller->get_lifecycle_state().id());
   EXPECT_EQ(1, test_controller.use_count());
 }
@@ -429,7 +429,7 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle)
   EXPECT_EQ(controller_interface::return_type::OK, unload_future.get());
 
   EXPECT_EQ(
-    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
     test_controller->get_lifecycle_state().id());
   EXPECT_EQ(1, test_controller.use_count());
 }
diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp
index 26a5299a07..3a1ea5877b 100644
--- a/controller_manager/test/test_controller_manager_srvs.cpp
+++ b/controller_manager/test/test_controller_manager_srvs.cpp
@@ -395,14 +395,14 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv)
   ASSERT_GT(test_controller.use_count(), 1)
     << "Controller manager should have have a copy of this shared ptr";
 
-  cleanup_calls = 0;
-  test_controller->cleanup_calls = &cleanup_calls;
+  size_t shutdown_calls = 0;
+  test_controller->shutdown_calls = &shutdown_calls;
   test_controller.reset();  // destroy our copy of the controller
 
   request->force_kill = false;
   result = call_service_and_wait(*client, request, srv_executor, true);
   ASSERT_TRUE(result->ok);
-  ASSERT_EQ(cleanup_calls, 1u);
+  ASSERT_EQ(shutdown_calls, 1u);
   ASSERT_EQ(test_controller.use_count(), 0)
     << "No more references to the controller after reloading.";
   test_controller.reset();
@@ -428,8 +428,8 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv)
     << "Controller manager should still have have a copy of "
        "this shared ptr, no unloading was performed";
 
-  cleanup_calls = 0;
-  test_controller->cleanup_calls = &cleanup_calls;
+  shutdown_calls = 0;
+  test_controller->shutdown_calls = &shutdown_calls;
   test_controller.reset();  // destroy our copy of the controller
 
   // Force stop active controller
@@ -439,7 +439,7 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv)
 
   ASSERT_EQ(test_controller_weak.use_count(), 0)
     << "No more references to the controller after reloading.";
-  ASSERT_EQ(cleanup_calls, 1u)
+  ASSERT_EQ(shutdown_calls, 1u)
     << "Controller should have been stopped and cleaned up with force_kill = true";
 }
 
@@ -491,7 +491,7 @@ TEST_F(TestControllerManagerSrvs, unload_controller_srv)
   result = call_service_and_wait(*client, request, srv_executor, true);
   ASSERT_TRUE(result->ok);
   EXPECT_EQ(
-    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
     test_controller->get_lifecycle_state().id());
   EXPECT_EQ(0u, cm_->get_loaded_controllers().size());
 }
@@ -526,7 +526,7 @@ TEST_F(TestControllerManagerSrvs, robot_description_on_load_and_unload_controlle
   unload_request->name = test_controller::TEST_CONTROLLER_NAME;
   auto result = call_service_and_wait(*unload_client, unload_request, srv_executor, true);
   EXPECT_EQ(
-    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
     test_controller->get_lifecycle_state().id());
   EXPECT_EQ(0u, cm_->get_loaded_controllers().size());
 
@@ -578,7 +578,7 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv)
   unload_request->name = test_controller::TEST_CONTROLLER_NAME;
   ASSERT_TRUE(call_service_and_wait(*unload_client, unload_request, srv_executor, true)->ok);
   EXPECT_EQ(
-    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
     test_controller->get_lifecycle_state().id());
   EXPECT_EQ(0u, cm_->get_loaded_controllers().size());
 }
diff --git a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp
index d5695e41e5..efe903be6f 100644
--- a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp
+++ b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp
@@ -45,6 +45,12 @@ TestControllerWithInterfaces::on_cleanup(const rclcpp_lifecycle::State & /*previ
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+TestControllerWithInterfaces::on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
 }  // namespace test_controller_with_interfaces
 
 #include "pluginlib/class_list_macros.hpp"
diff --git a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp
index 751cb4f3a8..8227b62cb2 100644
--- a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp
+++ b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp
@@ -54,6 +54,9 @@ class TestControllerWithInterfaces : public controller_interface::ControllerInte
 
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(
     const rclcpp_lifecycle::State & previous_state) override;
+
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(
+    const rclcpp_lifecycle::State & previous_state) override;
 };
 
 }  // namespace test_controller_with_interfaces
diff --git a/doc/images/plotjuggler.png b/doc/images/plotjuggler.png
new file mode 100644
index 0000000000..708a476cb1
Binary files /dev/null and b/doc/images/plotjuggler.png differ
diff --git a/doc/images/plotjuggler_select_topics.png b/doc/images/plotjuggler_select_topics.png
new file mode 100644
index 0000000000..7f18ae4da7
Binary files /dev/null and b/doc/images/plotjuggler_select_topics.png differ
diff --git a/doc/images/plotjuggler_visualizing_data.png b/doc/images/plotjuggler_visualizing_data.png
new file mode 100644
index 0000000000..a52732c442
Binary files /dev/null and b/doc/images/plotjuggler_visualizing_data.png differ
diff --git a/doc/index.rst b/doc/index.rst
index 09a2ddf745..5aea45d713 100644
--- a/doc/index.rst
+++ b/doc/index.rst
@@ -37,3 +37,4 @@ Guidelines and Best Practices
    :titlesonly:
 
    Debugging the Controller Manager and Plugins <debugging.rst>
+   Introspecting Controllers and Hardware Components <introspection.rst>
diff --git a/doc/introspection.rst b/doc/introspection.rst
new file mode 100644
index 0000000000..d3cccf7446
--- /dev/null
+++ b/doc/introspection.rst
@@ -0,0 +1,81 @@
+
+Introspection of the ros2_control setup
+***************************************
+
+With the integration of the ``pal_statistics`` package, the ``controller_manager`` node publishes the registered variables within the same process to the ``~/introspection_data`` topics.
+By default, all ``State`` and ``Command`` interfaces in the ``controller_manager`` are registered when they are added, and are unregistered when they are removed from the ``ResourceManager``.
+The state of the all the registered entities are published at the end of every ``update`` cycle of the ``controller_manager``. For instance, In a complete synchronous ros2_control setup (with synchronous controllers and hardware components), this data in the ``Command`` interface is the command used by the hardware components to command the hardware.
+
+All the registered variables are published over 3 topics: ``~/introspection_data/full``, ``~/introspection_data/names``, and ``~/introspection_data/values``.
+- The ``~/introspection_data/full`` topic publishes the full introspection data along with names and values in a single message. This can be useful to track or view variables and information from command line.
+- The ``~/introspection_data/names`` topic publishes the names of the registered variables. This topic contains the names of the variables registered. This is only published every time a a variables is registered and unregistered.
+- The ``~/introspection_data/values`` topic publishes the values of the registered variables. This topic contains the values of the variables registered.
+
+The topics ``~/introspection_data/full`` and ``~/introspection_data/values`` are always published on every update cycle asynchronously, provided that there is at least one subscriber to these topics.
+
+The topic ``~/introspection_data/full`` can be used to integrate with your custom visualization tools or to track the variables from the command line. The topic ``~/introspection_data/names`` and ``~/introspection_data/values`` are to be used for visualization tools like `PlotJuggler <https://plotjuggler.io/>`_ or `RQT plot <http://wiki.ros.org/rqt_plot>`_ to visualize the data.
+
+.. note::
+  If you have a high frequency of data, it is recommended to use the ``~/introspection_data/names`` and ``~/introspection_data/values`` topic. So, that the data transferred and stored is minimized.
+
+How to introspect internal variables of controllers and hardware components
+============================================================================
+
+Any member variable of a controller or hardware component can be registered for the introspection. It is very important that the lifetime of this variable exists as long as the controller or hardware component is available.
+
+.. note::
+  If a variable's lifetime is not properly managed, it may be attempted to read, which in the worst case scenario will cause a segmentation fault.
+
+How to register a variable for introspection
+---------------------------------------------
+
+1. Include the necessary headers in the controller or hardware component header file.
+
+   .. code-block:: cpp
+
+     #include <hardware_interface/introspection.hpp>
+
+2. Register the variable in the configure method of the controller or hardware component.
+
+   .. code-block:: cpp
+
+     void MyController::on_configure()
+     {
+       ...
+       // Register the variable for introspection (disabled by default)
+       // The variable is introspected only when the controller is active and
+       // then deactivated when the controller is deactivated.
+       REGISTER_ROS2_CONTROL_INTROSPECTION("my_variable_name", &my_variable_);
+       ...
+     }
+
+3. By default, the introspection of all the registered variables of the controllers and the hardware components is only activated, when they are active and it is deactivated when the controller or hardware component is deactivated.
+
+   .. note::
+      If you want to keep the introspection active even when the controller or hardware component is not active, you can do that by calling ``this->enable_introspection(true)`` in the ``on_configure`` and ``on_deactivate`` method of the controller or hardware component after registering the variables.
+
+Types of entities that can be introspected
+-------------------------------------------
+
+- Any variable that can be cast to a double is suitable for registration.
+- A function that returns a value that can be cast to a double is also suitable for registration.
+- Variables of complex structures can be registered by having defined introspection for their every internal variable.
+- Introspection of custom types can be done by defining a `custom introspection function <https://github.com/pal-robotics/pal_statistics/blob/humble-devel/pal_statistics/include/pal_statistics/registration_utils.hpp>`_.
+
+  .. note::
+    Registering the variables for introspection is not real-time safe. It is recommended to register the variables in the ``on_configure`` method only.
+
+Data Visualization
+*******************
+
+Data can be visualized with any tools that display ROS topics, but we recommend `PlotJuggler <https://plotjuggler.io/>`_ for viewing high resolution live data, or data in bags.
+
+1. Open ``PlotJuggler`` running ``ros2 run plotjuggler plotjuggler``.
+   .. image:: images/plotjuggler.png
+2. Visualize the data:
+   - Importing from the ros2bag
+   - Subscribing to the ROS2 topics live with the ``ROS2 Topic Subscriber`` option under ``Streaming`` header.
+3. Choose the topics ``~/introspection_data/names`` and ``~/introspection_data/values`` from the popup window.
+   .. image:: images/plotjuggler_select_topics.png
+4. Now, select the variables that are of your interest and drag them to the plot.
+   .. image:: images/plotjuggler_visualizing_data.png
diff --git a/doc/release_notes.rst b/doc/release_notes.rst
index 58930ec6c4..a08fdfbf5a 100644
--- a/doc/release_notes.rst
+++ b/doc/release_notes.rst
@@ -27,6 +27,7 @@ For details see the controller_manager section.
 * The ``assign_interfaces`` and ``release_interfaces`` methods are now virtual, so that the user can override them to store the interfaces into custom variable types, so that the user can have the flexibility to take the ownership of the loaned interfaces to the controller (`#1743 <https://github.com/ros-controls/ros2_control/pull/1743>`_)
 * The new ``PoseSensor`` semantic component provides a standard interface for hardware providing cartesian poses (`#1775 <https://github.com/ros-controls/ros2_control/pull/1775>`_)
 * The controllers now support the fallback controllers (a list of controllers that will be activated, when the spawned controllers fails by throwing an exception or returning ``return_type::ERROR`` during the ``update`` cycle) (`#1789 <https://github.com/ros-controls/ros2_control/pull/1789>`_)
+* The controllers can be easily introspect the internal member variables using the macro ``REGISTER_ROS2_CONTROL_INTROSPECTION`` (`#1918 <https://github.com/ros-controls/ros2_control/pull/1918>`_)
 
 controller_manager
 ******************
@@ -85,6 +86,7 @@ controller_manager
 * The ``ros2_control_node`` node has a new ``cpu_affinity`` parameter to bind the process to a specific CPU core. By default, this is not enabled. (`#1852 <https://github.com/ros-controls/ros2_control/pull/1852>`_).
 * The ``--service-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 <https://github.com/ros-controls/ros2_control/pull/1808>`_).
 * The ``cpu_affinity`` parameter can now accept of types ``int`` or ``int_array`` to bind the process to a specific CPU core or multiple CPU cores. (`#1915 <https://github.com/ros-controls/ros2_control/pull/1915>`_).
+* The ``pal_statistics`` is now integrated into the controller_manager, so that the controllers, hardware components and the controller_manager can be easily introspected and monitored using the topics ``~/introspection_data/names`` and ``~/introspection_data/values`` (`#1918 <https://github.com/ros-controls/ros2_control/pull/1918>`_).
 * A python module ``test_utils`` was added to the ``controller_manager`` package to help with integration testing (`#1955 <https://github.com/ros-controls/ros2_control/pull/1955>`_).
 
 hardware_interface
@@ -159,6 +161,7 @@ hardware_interface
 * With (`#1421 <https://github.com/ros-controls/ros2_control/pull/1421>`_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file.
 * With (`#1763 <https://github.com/ros-controls/ros2_control/pull/1763>`_) parsing for SDF published to ``robot_description`` topic is now also supported.
 * With (`#1567 <https://github.com/ros-controls/ros2_control/pull/1567>`_) all the Hardware components now have a fully functional asynchronous functionality, by simply adding ``is_async`` tag to the ros2_control tag in the URDF. This will allow the hardware components to run in a separate thread, and the controller manager will be able to run the controllers in parallel with the hardware components.
+* The hardware components can be easily introspect the internal member variables using the macro ``REGISTER_ROS2_CONTROL_INTROSPECTION`` (`#1918 <https://github.com/ros-controls/ros2_control/pull/1918>`_)
 
 joint_limits
 ************
diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt
index 673704c868..e3cf55df6d 100644
--- a/hardware_interface/CMakeLists.txt
+++ b/hardware_interface/CMakeLists.txt
@@ -22,6 +22,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   tinyxml2_vendor
   joint_limits
   urdf
+  pal_statistics
 )
 
 find_package(ament_cmake REQUIRED)
diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp
index 6f4dfa4402..c786a94892 100644
--- a/hardware_interface/include/hardware_interface/actuator_interface.hpp
+++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp
@@ -25,6 +25,7 @@
 #include "hardware_interface/component_parser.hpp"
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/introspection.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "hardware_interface/types/trigger_type.hpp"
@@ -93,7 +94,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
    */
   ActuatorInterface(const ActuatorInterface & other) = delete;
 
-  ActuatorInterface(ActuatorInterface && other) = default;
+  ActuatorInterface(ActuatorInterface && other) = delete;
 
   virtual ~ActuatorInterface() = default;
 
@@ -522,6 +523,22 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
    */
   const HardwareInfo & get_hardware_info() const { return info_; }
 
+  /// Enable or disable introspection of the hardware.
+  /**
+   * \param[in] enable Enable introspection if true, disable otherwise.
+   */
+  void enable_introspection(bool enable)
+  {
+    if (enable)
+    {
+      stats_registrations_.enableAll();
+    }
+    else
+    {
+      stats_registrations_.disableAll();
+    }
+  }
+
 protected:
   HardwareInfo info_;
   // interface names to InterfaceDescription
@@ -548,6 +565,9 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
   std::unordered_map<std::string, StateInterface::SharedPtr> actuator_states_;
   std::unordered_map<std::string, CommandInterface::SharedPtr> actuator_commands_;
   std::atomic<TriggerType> next_trigger_ = TriggerType::READ;
+
+protected:
+  pal_statistics::RegistrationsRAII stats_registrations_;
 };
 
 }  // namespace hardware_interface
diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp
index 1dfd499c2c..c3584f114b 100644
--- a/hardware_interface/include/hardware_interface/handle.hpp
+++ b/hardware_interface/include/hardware_interface/handle.hpp
@@ -24,6 +24,7 @@
 #include <variant>
 
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/introspection.hpp"
 #include "hardware_interface/macros.hpp"
 
 namespace hardware_interface
@@ -207,6 +208,24 @@ class StateInterface : public Handle
   {
   }
 
+  void registerIntrospection() const
+  {
+    if (std::holds_alternative<double>(value_))
+    {
+      std::function<double()> f = [this]()
+      { return value_ptr_ ? *value_ptr_ : std::numeric_limits<double>::quiet_NaN(); };
+      DEFAULT_REGISTER_ROS2_CONTROL_INTROSPECTION("state_interface." + get_name(), f);
+    }
+  }
+
+  void unregisterIntrospection() const
+  {
+    if (std::holds_alternative<double>(value_))
+    {
+      DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION("state_interface." + get_name());
+    }
+  }
+
   StateInterface(const StateInterface & other) = default;
 
   StateInterface(StateInterface && other) = default;
@@ -234,6 +253,24 @@ class CommandInterface : public Handle
 
   CommandInterface(CommandInterface && other) = default;
 
+  void registerIntrospection() const
+  {
+    if (std::holds_alternative<double>(value_))
+    {
+      std::function<double()> f = [this]()
+      { return value_ptr_ ? *value_ptr_ : std::numeric_limits<double>::quiet_NaN(); };
+      DEFAULT_REGISTER_ROS2_CONTROL_INTROSPECTION("command_interface." + get_name(), f);
+    }
+  }
+
+  void unregisterIntrospection() const
+  {
+    if (std::holds_alternative<double>(value_))
+    {
+      DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION("command_interface." + get_name());
+    }
+  }
+
   using Handle::Handle;
 
   using SharedPtr = std::shared_ptr<CommandInterface>;
diff --git a/hardware_interface/include/hardware_interface/introspection.hpp b/hardware_interface/include/hardware_interface/introspection.hpp
new file mode 100644
index 0000000000..d25b71c2d8
--- /dev/null
+++ b/hardware_interface/include/hardware_interface/introspection.hpp
@@ -0,0 +1,54 @@
+// Copyright 2024 PAL Robotics S.L.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \author Sai Kishor Kothakota
+
+#ifndef HARDWARE_INTERFACE__INTROSPECTION_HPP_
+#define HARDWARE_INTERFACE__INTROSPECTION_HPP_
+
+#include "pal_statistics/pal_statistics_macros.hpp"
+#include "pal_statistics/pal_statistics_utils.hpp"
+
+namespace hardware_interface
+{
+constexpr char DEFAULT_REGISTRY_KEY[] = "ros2_control";
+constexpr char DEFAULT_INTROSPECTION_TOPIC[] = "~/introspection_data";
+
+#define REGISTER_ROS2_CONTROL_INTROSPECTION(ID, ENTITY)                      \
+  REGISTER_ENTITY(                                                           \
+    hardware_interface::DEFAULT_REGISTRY_KEY, get_name() + "." + ID, ENTITY, \
+    &stats_registrations_, false);
+
+#define UNREGISTER_ROS2_CONTROL_INTROSPECTION(ID) \
+  UNREGISTER_ENTITY(DEFAULT_REGISTRY_KEY, get_name() + "." + ID);
+
+#define CLEAR_ALL_ROS2_CONTROL_INTROSPECTION_REGISTRIES() CLEAR_ALL_REGISTRIES();
+
+#define INITIALIZE_ROS2_CONTROL_INTROSPECTION_REGISTRY(node, topic, registry_key) \
+  INITIALIZE_REGISTRY(node, topic, registry_key);
+
+#define START_ROS2_CONTROL_INTROSPECTION_PUBLISHER_THREAD(registry_key) \
+  START_PUBLISH_THREAD(registry_key);
+
+#define PUBLISH_ROS2_CONTROL_INTROSPECTION_DATA_ASYNC(registry_key) \
+  PUBLISH_ASYNC_STATISTICS(registry_key);
+
+#define DEFAULT_REGISTER_ROS2_CONTROL_INTROSPECTION(ID, ENTITY) \
+  REGISTER_ENTITY(DEFAULT_REGISTRY_KEY, ID, ENTITY);
+
+#define DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION(ID) \
+  UNREGISTER_ENTITY(DEFAULT_REGISTRY_KEY, ID);
+}  // namespace hardware_interface
+
+#endif  // HARDWARE_INTERFACE__INTROSPECTION_HPP_
diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp
index 93de2a6494..693ad3a45c 100644
--- a/hardware_interface/include/hardware_interface/sensor_interface.hpp
+++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp
@@ -25,6 +25,7 @@
 #include "hardware_interface/component_parser.hpp"
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/introspection.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
@@ -92,7 +93,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    */
   SensorInterface(const SensorInterface & other) = delete;
 
-  SensorInterface(SensorInterface && other) = default;
+  SensorInterface(SensorInterface && other) = delete;
 
   virtual ~SensorInterface() = default;
 
@@ -326,6 +327,22 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    */
   const HardwareInfo & get_hardware_info() const { return info_; }
 
+  /// Enable or disable introspection of the sensor hardware.
+  /**
+   * \param[in] enable Enable introspection if true, disable otherwise.
+   */
+  void enable_introspection(bool enable)
+  {
+    if (enable)
+    {
+      stats_registrations_.enableAll();
+    }
+    else
+    {
+      stats_registrations_.disableAll();
+    }
+  }
+
 protected:
   HardwareInfo info_;
   // interface names to InterfaceDescription
@@ -346,6 +363,9 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   rclcpp::Logger sensor_logger_;
   // interface names to Handle accessed through getters/setters
   std::unordered_map<std::string, StateInterface::SharedPtr> sensor_states_map_;
+
+protected:
+  pal_statistics::RegistrationsRAII stats_registrations_;
 };
 
 }  // namespace hardware_interface
diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp
index e145deae5b..b0cc18f785 100644
--- a/hardware_interface/include/hardware_interface/system_interface.hpp
+++ b/hardware_interface/include/hardware_interface/system_interface.hpp
@@ -25,6 +25,7 @@
 #include "hardware_interface/component_parser.hpp"
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/introspection.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
@@ -96,7 +97,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    */
   SystemInterface(const SystemInterface & other) = delete;
 
-  SystemInterface(SystemInterface && other) = default;
+  SystemInterface(SystemInterface && other) = delete;
 
   virtual ~SystemInterface() = default;
 
@@ -551,6 +552,22 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    */
   const HardwareInfo & get_hardware_info() const { return info_; }
 
+  /// Enable or disable introspection of the hardware.
+  /**
+   * \param[in] enable Enable introspection if true, disable otherwise.
+   */
+  void enable_introspection(bool enable)
+  {
+    if (enable)
+    {
+      stats_registrations_.enableAll();
+    }
+    else
+    {
+      stats_registrations_.disableAll();
+    }
+  }
+
 protected:
   HardwareInfo info_;
   // interface names to InterfaceDescription
@@ -587,6 +604,9 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   std::unordered_map<std::string, StateInterface::SharedPtr> system_states_;
   std::unordered_map<std::string, CommandInterface::SharedPtr> system_commands_;
   std::atomic<TriggerType> next_trigger_ = TriggerType::READ;
+
+protected:
+  pal_statistics::RegistrationsRAII stats_registrations_;
 };
 
 }  // namespace hardware_interface
diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml
index 45a7c6a2f4..e85aef20f7 100644
--- a/hardware_interface/package.xml
+++ b/hardware_interface/package.xml
@@ -12,15 +12,16 @@
 
   <depend>backward_ros</depend>
   <depend>control_msgs</depend>
+  <depend>joint_limits</depend>
   <depend>lifecycle_msgs</depend>
+  <depend>pal_statistics</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>rcpputils</depend>
   <depend>realtime_tools</depend>
+  <depend>sdformat_urdf</depend>
   <depend>tinyxml2_vendor</depend>
-  <depend>joint_limits</depend>
   <depend>urdf</depend>
-  <depend>sdformat_urdf</depend>
 
   <build_depend>rcutils</build_depend>
   <exec_depend>rcutils</exec_depend>
diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp
index 81bebf1182..1bba005fd7 100644
--- a/hardware_interface/src/actuator.cpp
+++ b/hardware_interface/src/actuator.cpp
@@ -98,6 +98,7 @@ const rclcpp_lifecycle::State & Actuator::configure()
 const rclcpp_lifecycle::State & Actuator::cleanup()
 {
   std::unique_lock<std::recursive_mutex> lock(actuators_mutex_);
+  impl_->enable_introspection(false);
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
     switch (impl_->on_cleanup(impl_->get_lifecycle_state()))
@@ -119,6 +120,7 @@ const rclcpp_lifecycle::State & Actuator::cleanup()
 const rclcpp_lifecycle::State & Actuator::shutdown()
 {
   std::unique_lock<std::recursive_mutex> lock(actuators_mutex_);
+  impl_->enable_introspection(false);
   if (
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
@@ -146,6 +148,7 @@ const rclcpp_lifecycle::State & Actuator::activate()
     switch (impl_->on_activate(impl_->get_lifecycle_state()))
     {
       case CallbackReturn::SUCCESS:
+        impl_->enable_introspection(true);
         impl_->set_lifecycle_state(rclcpp_lifecycle::State(
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE));
         break;
@@ -164,6 +167,7 @@ const rclcpp_lifecycle::State & Actuator::activate()
 const rclcpp_lifecycle::State & Actuator::deactivate()
 {
   std::unique_lock<std::recursive_mutex> lock(actuators_mutex_);
+  impl_->enable_introspection(false);
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
   {
     switch (impl_->on_deactivate(impl_->get_lifecycle_state()))
@@ -187,6 +191,7 @@ const rclcpp_lifecycle::State & Actuator::deactivate()
 const rclcpp_lifecycle::State & Actuator::error()
 {
   std::unique_lock<std::recursive_mutex> lock(actuators_mutex_);
+  impl_->enable_introspection(false);
   if (
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp
index 7a616d890d..4f027b7f70 100644
--- a/hardware_interface/src/resource_manager.cpp
+++ b/hardware_interface/src/resource_manager.cpp
@@ -611,6 +611,7 @@ class ResourceStorage
         command_interface->get_name() + "]");
       throw std::runtime_error(msg);
     }
+    command_interface->registerIntrospection();
   }
 
   // BEGIN (Handle export change): for backward compatibility, can be removed if
@@ -668,6 +669,7 @@ class ResourceStorage
         interface->get_name() + "]");
       throw std::runtime_error(msg);
     }
+    interface->registerIntrospection();
     return interface_name;
   }
   /// Adds exported state interfaces into internal storage.
@@ -715,6 +717,7 @@ class ResourceStorage
   {
     for (const auto & interface : interface_names)
     {
+      state_interface_map_[interface]->unregisterIntrospection();
       state_interface_map_.erase(interface);
     }
   }
@@ -775,6 +778,7 @@ class ResourceStorage
   {
     for (const auto & interface : interface_names)
     {
+      command_interface_map_[interface]->unregisterIntrospection();
       command_interface_map_.erase(interface);
       claimed_command_interface_map_.erase(interface);
     }
diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp
index ae4d930bbe..08d60f3c25 100644
--- a/hardware_interface/src/sensor.cpp
+++ b/hardware_interface/src/sensor.cpp
@@ -95,6 +95,7 @@ const rclcpp_lifecycle::State & Sensor::configure()
 const rclcpp_lifecycle::State & Sensor::cleanup()
 {
   std::unique_lock<std::recursive_mutex> lock(sensors_mutex_);
+  impl_->enable_introspection(false);
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
     switch (impl_->on_cleanup(impl_->get_lifecycle_state()))
@@ -116,6 +117,7 @@ const rclcpp_lifecycle::State & Sensor::cleanup()
 const rclcpp_lifecycle::State & Sensor::shutdown()
 {
   std::unique_lock<std::recursive_mutex> lock(sensors_mutex_);
+  impl_->enable_introspection(false);
   if (
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
@@ -143,6 +145,7 @@ const rclcpp_lifecycle::State & Sensor::activate()
     switch (impl_->on_activate(impl_->get_lifecycle_state()))
     {
       case CallbackReturn::SUCCESS:
+        impl_->enable_introspection(true);
         impl_->set_lifecycle_state(rclcpp_lifecycle::State(
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE));
         break;
@@ -161,6 +164,7 @@ const rclcpp_lifecycle::State & Sensor::activate()
 const rclcpp_lifecycle::State & Sensor::deactivate()
 {
   std::unique_lock<std::recursive_mutex> lock(sensors_mutex_);
+  impl_->enable_introspection(false);
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
   {
     switch (impl_->on_deactivate(impl_->get_lifecycle_state()))
@@ -184,6 +188,7 @@ const rclcpp_lifecycle::State & Sensor::deactivate()
 const rclcpp_lifecycle::State & Sensor::error()
 {
   std::unique_lock<std::recursive_mutex> lock(sensors_mutex_);
+  impl_->enable_introspection(false);
   if (
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp
index 89d6afd42e..7ff830bac5 100644
--- a/hardware_interface/src/system.cpp
+++ b/hardware_interface/src/system.cpp
@@ -96,6 +96,7 @@ const rclcpp_lifecycle::State & System::configure()
 const rclcpp_lifecycle::State & System::cleanup()
 {
   std::unique_lock<std::recursive_mutex> lock(system_mutex_);
+  impl_->enable_introspection(false);
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
     switch (impl_->on_cleanup(impl_->get_lifecycle_state()))
@@ -117,6 +118,7 @@ const rclcpp_lifecycle::State & System::cleanup()
 const rclcpp_lifecycle::State & System::shutdown()
 {
   std::unique_lock<std::recursive_mutex> lock(system_mutex_);
+  impl_->enable_introspection(false);
   if (
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
@@ -144,6 +146,7 @@ const rclcpp_lifecycle::State & System::activate()
     switch (impl_->on_activate(impl_->get_lifecycle_state()))
     {
       case CallbackReturn::SUCCESS:
+        impl_->enable_introspection(true);
         impl_->set_lifecycle_state(rclcpp_lifecycle::State(
           lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE));
         break;
@@ -162,6 +165,7 @@ const rclcpp_lifecycle::State & System::activate()
 const rclcpp_lifecycle::State & System::deactivate()
 {
   std::unique_lock<std::recursive_mutex> lock(system_mutex_);
+  impl_->enable_introspection(false);
   if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
   {
     switch (impl_->on_deactivate(impl_->get_lifecycle_state()))
@@ -185,6 +189,7 @@ const rclcpp_lifecycle::State & System::deactivate()
 const rclcpp_lifecycle::State & System::error()
 {
   std::unique_lock<std::recursive_mutex> lock(system_mutex_);
+  impl_->enable_introspection(false);
   if (
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
     impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp
index cbe42999ab..27883b8937 100644
--- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp
+++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp
@@ -201,7 +201,7 @@ class JointLimiterInterface
    * \returns true if limits are enforced, otherwise false.
    */
   virtual bool enforce(
-    JointLimitsStateDataType & current_joint_states,
+    const JointLimitsStateDataType & current_joint_states,
     JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt)
   {
     joint_limits_ = *(updated_limits_.readFromRT());
@@ -234,7 +234,7 @@ class JointLimiterInterface
    * \returns true if limits are enforced, otherwise false.
    */
   virtual bool on_enforce(
-    JointLimitsStateDataType & current_joint_states,
+    const JointLimitsStateDataType & current_joint_states,
     JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) = 0;
 
   /** \brief Checks if the logging interface is set.
diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp
index 8a17896139..b84c2662c8 100644
--- a/joint_limits/include/joint_limits/joint_limits_helpers.hpp
+++ b/joint_limits/include/joint_limits/joint_limits_helpers.hpp
@@ -49,7 +49,7 @@ bool is_limited(double value, double min, double max);
  */
 PositionLimits compute_position_limits(
   const joint_limits::JointLimits & limits, const std::optional<double> & act_vel,
-  const std::optional<double> & prev_command_pos, double dt);
+  const std::optional<double> & act_pos, const std::optional<double> & prev_command_pos, double dt);
 
 /**
  * @brief Computes the velocity limits based on the position and acceleration limits.
diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp
index 9af574078d..3b7451e2ee 100644
--- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp
+++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp
@@ -67,7 +67,7 @@ class JointSaturationLimiter : public JointLimiterInterface<JointLimitsStateData
    * \returns true if limits are enforced, otherwise false.
    */
   bool on_enforce(
-    JointLimitsStateDataType & current_joint_states,
+    const JointLimitsStateDataType & current_joint_states,
     JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) override;
 
 protected:
diff --git a/joint_limits/include/joint_limits/joint_soft_limiter.hpp b/joint_limits/include/joint_limits/joint_soft_limiter.hpp
index ebc9d99934..6940c03c97 100644
--- a/joint_limits/include/joint_limits/joint_soft_limiter.hpp
+++ b/joint_limits/include/joint_limits/joint_soft_limiter.hpp
@@ -46,7 +46,7 @@ class JointSoftLimiter : public JointSaturationLimiter<JointControlInterfacesDat
   }
 
   bool on_enforce(
-    JointControlInterfacesData & actual, JointControlInterfacesData & desired,
+    const JointControlInterfacesData & actual, JointControlInterfacesData & desired,
     const rclcpp::Duration & dt) override;
 
   bool has_soft_position_limits(const joint_limits::SoftJointLimits & soft_joint_limits)
diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp
index 11ccaf7738..61a5e5b313 100644
--- a/joint_limits/src/joint_limits_helpers.cpp
+++ b/joint_limits/src/joint_limits_helpers.cpp
@@ -39,7 +39,7 @@ bool is_limited(double value, double min, double max) { return value < min || va
 
 PositionLimits compute_position_limits(
   const joint_limits::JointLimits & limits, const std::optional<double> & act_vel,
-  const std::optional<double> & prev_command_pos, double dt)
+  const std::optional<double> & act_pos, const std::optional<double> & prev_command_pos, double dt)
 {
   PositionLimits pos_limits(limits.min_position, limits.max_position);
   if (limits.has_velocity_limits)
@@ -50,8 +50,10 @@ PositionLimits compute_position_limits(
                                : limits.max_velocity;
     const double max_vel = std::min(limits.max_velocity, delta_vel);
     const double delta_pos = max_vel * dt;
-    pos_limits.lower_limit = std::max(prev_command_pos.value() - delta_pos, pos_limits.lower_limit);
-    pos_limits.upper_limit = std::min(prev_command_pos.value() + delta_pos, pos_limits.upper_limit);
+    const double position_reference =
+      act_pos.has_value() ? act_pos.value() : prev_command_pos.value();
+    pos_limits.lower_limit = std::max(position_reference - delta_pos, pos_limits.lower_limit);
+    pos_limits.upper_limit = std::min(position_reference + delta_pos, pos_limits.upper_limit);
   }
   internal::check_and_swap_limits(pos_limits.lower_limit, pos_limits.upper_limit);
   return pos_limits;
diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp
index 2279b910ae..c2ed78fcd9 100644
--- a/joint_limits/src/joint_range_limiter.cpp
+++ b/joint_limits/src/joint_range_limiter.cpp
@@ -43,7 +43,7 @@ bool JointSaturationLimiter<JointControlInterfacesData>::on_init()
 
 template <>
 bool JointSaturationLimiter<JointControlInterfacesData>::on_enforce(
-  JointControlInterfacesData & actual, JointControlInterfacesData & desired,
+  const JointControlInterfacesData & actual, JointControlInterfacesData & desired,
   const rclcpp::Duration & dt)
 {
   bool limits_enforced = false;
@@ -112,8 +112,8 @@ bool JointSaturationLimiter<JointControlInterfacesData>::on_enforce(
 
   if (desired.has_position())
   {
-    const auto limits =
-      compute_position_limits(joint_limits, actual.velocity, prev_command_.position, dt_seconds);
+    const auto limits = compute_position_limits(
+      joint_limits, actual.velocity, actual.position, prev_command_.position, dt_seconds);
     limits_enforced = is_limited(desired.position.value(), limits.lower_limit, limits.upper_limit);
     desired.position = std::clamp(desired.position.value(), limits.lower_limit, limits.upper_limit);
   }
diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp
index 47a88e658d..e61cb429ec 100644
--- a/joint_limits/src/joint_saturation_limiter.cpp
+++ b/joint_limits/src/joint_saturation_limiter.cpp
@@ -27,7 +27,7 @@ namespace joint_limits
 {
 template <>
 bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enforce(
-  trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states,
+  const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states,
   trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt)
 {
   bool limits_enforced = false;
@@ -44,23 +44,21 @@ bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enfo
   // velocity max is implicitly already violated due to max_acc * dt > 2.0
 
   // check for required inputs combination
-  bool has_desired_position = (desired_joint_states.positions.size() == number_of_joints_);
-  bool has_desired_velocity = (desired_joint_states.velocities.size() == number_of_joints_);
-  bool has_desired_acceleration = (desired_joint_states.accelerations.size() == number_of_joints_);
-  bool has_current_position = (current_joint_states.positions.size() == number_of_joints_);
-  bool has_current_velocity = (current_joint_states.velocities.size() == number_of_joints_);
+  const bool has_desired_position = (desired_joint_states.positions.size() == number_of_joints_);
+  const bool has_desired_velocity = (desired_joint_states.velocities.size() == number_of_joints_);
+  const bool has_desired_acceleration =
+    (desired_joint_states.accelerations.size() == number_of_joints_);
+  const bool has_current_position = (current_joint_states.positions.size() == number_of_joints_);
+  const bool has_current_velocity = (current_joint_states.velocities.size() == number_of_joints_);
 
   // pos state and vel or pos cmd is required, vel state is optional
   if (!(has_current_position && (has_desired_position || has_desired_velocity)))
   {
     return false;
   }
-
-  if (!has_current_velocity)
-  {
-    // First update() after activating does not have velocity available, assume 0
-    current_joint_states.velocities.resize(number_of_joints_, 0.0);
-  }
+  const std::vector<double> & current_joint_velocities =
+    has_current_velocity ? current_joint_states.velocities
+                         : std::vector<double>(number_of_joints_, 0.0);
 
   // TODO(destogl): please check if we get too much malloc from this initialization,
   // if so then we should use members instead local variables and initialize them in other method
@@ -140,8 +138,7 @@ bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enfo
             current_joint_states.positions[index] + desired_vel[index] * dt_seconds;
         }
 
-        desired_acc[index] =
-          (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds;
+        desired_acc[index] = (desired_vel[index] - current_joint_velocities[index]) / dt_seconds;
       }
     }
 
@@ -174,16 +171,15 @@ bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enfo
         if (
           std::fabs(desired_acc[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_acc[index]))
         {
-          desired_acc[index] =
-            (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds;
+          desired_acc[index] = (desired_vel[index] - current_joint_velocities[index]) / dt_seconds;
         }
 
         // check if decelerating - if velocity is changing toward 0
         bool deceleration_limit_applied = false;
         bool limit_applied = false;
         if (
-          (desired_acc[index] < 0 && current_joint_states.velocities[index] > 0) ||
-          (desired_acc[index] > 0 && current_joint_states.velocities[index] < 0))
+          (desired_acc[index] < 0 && current_joint_velocities[index] > 0) ||
+          (desired_acc[index] > 0 && current_joint_velocities[index] < 0))
         {
           // limit deceleration
           if (joint_limits_[index].has_deceleration_limits)
@@ -204,13 +200,12 @@ bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enfo
         if (limit_applied)
         {
           // vel_cmd from integration of desired_acc, needed even if no vel output
-          desired_vel[index] =
-            current_joint_states.velocities[index] + desired_acc[index] * dt_seconds;
+          desired_vel[index] = current_joint_velocities[index] + desired_acc[index] * dt_seconds;
           if (has_desired_position)
           {
             // pos_cmd from from double integration of desired_acc
             desired_pos[index] = current_joint_states.positions[index] +
-                                 current_joint_states.velocities[index] * dt_seconds +
+                                 current_joint_velocities[index] * dt_seconds +
                                  0.5 * desired_acc[index] * dt_seconds * dt_seconds;
           }
         }
@@ -315,7 +310,7 @@ bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enfo
       // Compute accel to stop
       // Here we aren't explicitly maximally decelerating, but for joints near their limits this
       // should still result in max decel being used
-      desired_acc[index] = -current_joint_states.velocities[index] / dt_seconds;
+      desired_acc[index] = -current_joint_velocities[index] / dt_seconds;
       if (joint_limits_[index].has_deceleration_limits)
       {
         desired_acc[index] = std::copysign(
@@ -332,13 +327,12 @@ bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enfo
       // Recompute velocity and position
       if (has_desired_velocity)
       {
-        desired_vel[index] =
-          current_joint_states.velocities[index] + desired_acc[index] * dt_seconds;
+        desired_vel[index] = current_joint_velocities[index] + desired_acc[index] * dt_seconds;
       }
       if (has_desired_position)
       {
         desired_pos[index] = current_joint_states.positions[index] +
-                             current_joint_states.velocities[index] * dt_seconds +
+                             current_joint_velocities[index] * dt_seconds +
                              0.5 * desired_acc[index] * dt_seconds * dt_seconds;
       }
     }
diff --git a/joint_limits/src/joint_soft_limiter.cpp b/joint_limits/src/joint_soft_limiter.cpp
index a2292cb033..14eab8e41a 100644
--- a/joint_limits/src/joint_soft_limiter.cpp
+++ b/joint_limits/src/joint_soft_limiter.cpp
@@ -13,13 +13,14 @@
 // limitations under the License.
 
 /// \author Adrià Roig Moreno
+#define _USE_MATH_DEFINES
 #include "joint_limits/joint_soft_limiter.hpp"
 
 namespace joint_limits
 {
 
 bool JointSoftLimiter::on_enforce(
-  JointControlInterfacesData & actual, JointControlInterfacesData & desired,
+  const JointControlInterfacesData & actual, JointControlInterfacesData & desired,
   const rclcpp::Duration & dt)
 {
   bool limits_enforced = false;
@@ -141,8 +142,8 @@ bool JointSoftLimiter::on_enforce(
 
   if (desired.has_position())
   {
-    const auto position_limits =
-      compute_position_limits(hard_limits, actual.velocity, prev_command_.position, dt_seconds);
+    const auto position_limits = compute_position_limits(
+      hard_limits, actual.velocity, actual.position, prev_command_.position, dt_seconds);
 
     double pos_low = -std::numeric_limits<double>::infinity();
     double pos_high = std::numeric_limits<double>::infinity();
diff --git a/joint_limits/test/test_joint_range_limiter.cpp b/joint_limits/test/test_joint_range_limiter.cpp
index 083646d84f..41ab569289 100644
--- a/joint_limits/test/test_joint_range_limiter.cpp
+++ b/joint_limits/test/test_joint_range_limiter.cpp
@@ -142,6 +142,32 @@ TEST_F(JointSaturationLimiterTest, check_desired_position_only_cases)
   EXPECT_FALSE(desired_state_.has_effort());
   EXPECT_FALSE(desired_state_.has_jerk());
 
+  desired_state_ = {};
+  actual_state_ = {};
+  actual_state_.position = 0.0;
+  desired_state_.position = 5.0 * M_PI;
+  const rclcpp::Duration test_period(0, 100);  // 0.1 second
+  for (size_t i = 0; i < 2000; i++)
+  {
+    desired_state_.position = 5.0 * M_PI;
+    SCOPED_TRACE(
+      "Testing for actual position: " + std::to_string(actual_state_.position.value()) +
+      ", desired position: " + std::to_string(desired_state_.position.value()) +
+      " for the joint limits : " + limits.to_string() + " Iteration : " + std::to_string(i));
+    ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, test_period));
+    EXPECT_NEAR(
+      desired_state_.position.value(),
+      std::min(
+        actual_state_.position.value() + (limits.max_velocity * test_period.seconds()), M_PI),
+      COMMON_THRESHOLD);
+    EXPECT_TRUE(desired_state_.has_position());
+    EXPECT_FALSE(desired_state_.has_velocity());
+    EXPECT_FALSE(desired_state_.has_acceleration());
+    EXPECT_FALSE(desired_state_.has_effort());
+    EXPECT_FALSE(desired_state_.has_jerk());
+    actual_state_.position = desired_state_.position.value() / 2.0;
+  }
+
   // Now test when there are no position limits, then the desired position is not saturated
   limits = joint_limits::JointLimits();
   ASSERT_TRUE(Init(limits));
@@ -652,19 +678,40 @@ TEST_F(JointSaturationLimiterTest, check_all_desired_references_limiting)
   test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true);
   test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 1.0, 0.5, 0.5, 3.0, 1.0, 0.5, 0.5, false);
 
+  ASSERT_TRUE(Init(limits));
+  ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_));
+
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, -6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 1.5, 0.5, 0.5, 0.5, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 2.0, 1.0, 0.5, 0.5, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 2.5, 1.0, 0.5, 0.5, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 3.5, 1.0, 0.5, 0.5, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 4.0, 1.0, 0.5, 0.5, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 4.5, 1.0, 0.5, 0.5, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true);
+
   // Now enforce the limits with actual position and velocity
   ASSERT_TRUE(Init(limits));
   // Desired position and velocity affected due to the acceleration limits
   test_limit_enforcing(0.5, 0.0, 6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true);
   test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true);
-  test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true);
-  test_limit_enforcing(1.0, 0.0, -6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true);
-  test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.0, 0.5, 0.5, 0.5, true);
+  test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(1.2, 0.0, 6.0, 0.0, 0.0, 0.0, 1.7, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(1.5, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(1.5, 0.0, -6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(1.0, 0.0, -6.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.5, 0.5, 0.5, 0.5, true);
   test_limit_enforcing(2.0, 0.5, 6.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true);
   test_limit_enforcing(3.0, 0.5, 6.0, 2.0, 1.0, 0.5, 4.0, 1.0, 0.5, 0.5, true);
-  test_limit_enforcing(4.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true);
+  test_limit_enforcing(4.0, 1.5, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true);
   test_limit_enforcing(4.8, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 0.5, 0.5, 0.5, true);
   test_limit_enforcing(5.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 0.0, 0.5, 0.5, true);
+  test_limit_enforcing(5.0, 1.0, 6.0, 3.0, 1.0, 0.5, 5.0, 0.0, 0.5, 0.5, true);
 }
 
 int main(int argc, char ** argv)
diff --git a/joint_limits/test/test_joint_soft_limiter.cpp b/joint_limits/test/test_joint_soft_limiter.cpp
index 7deb2796ea..3659be4048 100644
--- a/joint_limits/test/test_joint_soft_limiter.cpp
+++ b/joint_limits/test/test_joint_soft_limiter.cpp
@@ -162,6 +162,7 @@ TEST_F(JointSoftLimiterTest, check_desired_position_only_cases)
   soft_limits.max_position = 1.5;
   soft_limits.min_position = -1.5;
   ASSERT_TRUE(Init(limits, soft_limits));
+  actual_state_.position = 0.0;
   desired_state_.position = 2.0;
   ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period));
   EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD);
@@ -170,6 +171,7 @@ TEST_F(JointSoftLimiterTest, check_desired_position_only_cases)
   soft_limits.min_position = -0.75;
   ASSERT_TRUE(Init(limits, soft_limits));
   desired_state_.position = 2.0;
+  actual_state_ = {};
   ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period));
   EXPECT_NEAR(desired_state_.position.value(), soft_limits.max_position, COMMON_THRESHOLD);
   desired_state_.position = -3.0;
@@ -286,6 +288,32 @@ TEST_F(JointSoftLimiterTest, check_desired_position_only_cases)
     actual_state_.position = expected_pos;
   }
 
+  // More generic test case to mock a slow system
+  soft_limits.k_position = 0.5;
+  soft_limits.max_position = 2.0;
+  soft_limits.min_position = -2.0;
+  ASSERT_TRUE(Init(limits, soft_limits));
+  desired_state_.position = 2.0;
+  ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period));
+  EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD);
+  actual_state_.position = -0.1;
+  for (auto i = 0; i < 10000; i++)
+  {
+    SCOPED_TRACE(
+      "Testing for iteration: " + std::to_string(i) +
+      " for desired position: " + std::to_string(desired_state_.position.value()) +
+      " and actual position : " + std::to_string(actual_state_.position.value()) +
+      " for the joint limits : " + limits.to_string());
+    desired_state_.position = 4.0;
+    const double delta_pos = std::min(
+      (soft_limits.max_position - actual_state_.position.value()) * soft_limits.k_position,
+      limits.max_velocity * period.seconds());
+    const double expected_pos = actual_state_.position.value() + delta_pos;
+    ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period));
+    EXPECT_NEAR(desired_state_.position.value(), expected_pos, COMMON_THRESHOLD);
+    actual_state_.position = expected_pos / 1.27;
+  }
+
   // Now test when there are no position limits and soft limits, then the desired position is not
   // saturated
   limits = joint_limits::JointLimits();
@@ -1021,9 +1049,13 @@ TEST_F(JointSoftLimiterTest, check_all_desired_references_limiting)
   // Desired position and velocity affected due to the acceleration limits
   test_limit_enforcing(0.5, 0.0, 6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true);
   test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true);
-  test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true);
-  test_limit_enforcing(1.0, 0.0, -6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true);
-  test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.0, 0.5, 0.5, 0.5, true);
+  // If the actual position doesn't change, the desired position should not change
+  test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(1.2, 0.0, 6.0, 0.0, 0.0, 0.0, 1.7, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(1.5, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(1.5, 0.0, -6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.5, 0.5, 0.5, 0.5, true);
   test_limit_enforcing(2.0, 0.5, 6.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true);
   test_limit_enforcing(3.0, 0.5, 6.0, 2.0, 1.0, 0.5, 4.0, 1.0, 0.5, 0.5, true);
   test_limit_enforcing(4.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true);