diff --git a/dsr_agents/CMakeLists.txt b/dsr_agents/CMakeLists.txt
index 5d4ba75..dfabb91 100644
--- a/dsr_agents/CMakeLists.txt
+++ b/dsr_agents/CMakeLists.txt
@@ -56,6 +56,7 @@ find_package(nav2_util REQUIRED)
 find_package(opennav_docking_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_msgs REQUIRED)
@@ -70,7 +71,7 @@ find_package(tf2_geometry_msgs REQUIRED)
 set(dsr_LIBRARIES dsr_api dsr_core dsr_gui)
 set(fastrtps_LIBRARIES fastrtps fastcdr)
 
-# Docking agent executable node
+# Docking agent library node
 add_executable(docking_agent src/docking_agent.cpp)
 target_include_directories(docking_agent PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>"
@@ -143,6 +144,7 @@ target_link_libraries(topic_agent
   dsr_util::dsr_util
   PRIVATE
   ${fastrtps_LIBRARIES}
+  rclcpp_lifecycle::rclcpp_lifecycle
   ${sensor_msgs_TARGETS}
 )
 
@@ -189,6 +191,7 @@ ament_export_dependencies(
   opennav_docking_msgs
   rclcpp
   rclcpp_action
+  rclcpp_lifecycle
   sensor_msgs
   tf2
   tf2_msgs
diff --git a/dsr_agents/package.xml b/dsr_agents/package.xml
index 0b2cfcf..06f167a 100644
--- a/dsr_agents/package.xml
+++ b/dsr_agents/package.xml
@@ -19,6 +19,7 @@
   <depend>opennav_docking_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
+  <depend>rclcpp_lifecycle</depend>
   <depend>sensor_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_msgs</depend>
diff --git a/dsr_agents/src/docking_agent.cpp b/dsr_agents/src/docking_agent.cpp
index 9bb5a68..a7e5ae2 100644
--- a/dsr_agents/src/docking_agent.cpp
+++ b/dsr_agents/src/docking_agent.cpp
@@ -227,7 +227,7 @@ int main(int argc, char ** argv)
   auto node = std::make_shared<dsr_agents::DockingAgent>();
 
   dsr_util::QtExecutor exe;
-  exe.add_node(node);
+  exe.add_node(node->get_node_base_interface());
   exe.start();
 
   auto res = app.exec();
diff --git a/dsr_agents/src/nav_agent.cpp b/dsr_agents/src/nav_agent.cpp
index 64fb526..17c7a7a 100644
--- a/dsr_agents/src/nav_agent.cpp
+++ b/dsr_agents/src/nav_agent.cpp
@@ -255,7 +255,7 @@ int main(int argc, char ** argv)
   auto node = std::make_shared<dsr_agents::NavigationAgent>();
 
   dsr_util::QtExecutor exe;
-  exe.add_node(node);
+  exe.add_node(node->get_node_base_interface());
   exe.start();
 
   auto res = app.exec();
diff --git a/dsr_agents/src/tf_agent.cpp b/dsr_agents/src/tf_agent.cpp
index 013e15a..e6bf37e 100644
--- a/dsr_agents/src/tf_agent.cpp
+++ b/dsr_agents/src/tf_agent.cpp
@@ -120,7 +120,7 @@ int main(int argc, char ** argv)
   auto node = std::make_shared<dsr_agents::TFAgent>();
 
   dsr_util::QtExecutor exe;
-  exe.add_node(node);
+  exe.add_node(node->get_node_base_interface());
   exe.start();
 
   auto res = app.exec();
diff --git a/dsr_agents/src/topic_agent.cpp b/dsr_agents/src/topic_agent.cpp
index eeb521d..856969d 100644
--- a/dsr_agents/src/topic_agent.cpp
+++ b/dsr_agents/src/topic_agent.cpp
@@ -23,6 +23,7 @@
 #include "sensor_msgs/msg/image.hpp"
 #include "sensor_msgs/image_encodings.hpp"
 #include "sensor_msgs/msg/laser_scan.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
 
 // DSR
 #include "dsr_util/qt_executor.hpp"
@@ -41,7 +42,7 @@ TopicAgent::TopicAgent()
   std::this_thread::sleep_for(std::chrono::milliseconds(1000));
 
   // Subscriber to the topic with a generic subscription
-  auto data = rclcpp::Node::get_topic_names_and_types();
+  auto data = rclcpp_lifecycle::LifecycleNode::get_topic_names_and_types();
   for (auto type : data[ros_topic_]) {
     generic_sub_ = create_generic_subscription(
       ros_topic_, type, rclcpp::QoS(rclcpp::SystemDefaultsQoS()),
@@ -116,7 +117,7 @@ void TopicAgent::serial_callback(const std::shared_ptr<rclcpp::SerializedMessage
 {
   // In order to deserialize the message we have to manually create a ROS2
   // message in which we want to convert the serialized data.
-  auto data = rclcpp::Node::get_topic_names_and_types();
+  auto data = rclcpp_lifecycle::LifecycleNode::get_topic_names_and_types();
   const std::string topic_type = data[ros_topic_][0];
   RCLCPP_INFO_ONCE(
     this->get_logger(), "Subscribed to topic [%s] of type [%s]",
@@ -243,7 +244,7 @@ int main(int argc, char ** argv)
   auto node = std::make_shared<dsr_agents::TopicAgent>();
 
   dsr_util::QtExecutor exe;
-  exe.add_node(node);
+  exe.add_node(node->get_node_base_interface());
   exe.start();
 
   auto res = app.exec();
diff --git a/dsr_util/CMakeLists.txt b/dsr_util/CMakeLists.txt
index 669f493..bd67c72 100644
--- a/dsr_util/CMakeLists.txt
+++ b/dsr_util/CMakeLists.txt
@@ -54,6 +54,7 @@ find_package(fastcdr REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(Qt5 REQUIRED COMPONENTS Core)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 
 # ##########
@@ -96,6 +97,7 @@ target_link_libraries(${library_name}
   ${dsr_LIBRARIES}
   ${geometry_msgs_TARGETS}
   rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
   ${qt_LIBRARIES}
   PRIVATE
   Eigen3::Eigen
@@ -149,6 +151,7 @@ ament_export_dependencies(
   geometry_msgs
   Qt5Core
   rclcpp
+  rclcpp_lifecycle
   tf2_geometry_msgs
 )
 
diff --git a/dsr_util/include/dsr_util/agent_node.hpp b/dsr_util/include/dsr_util/agent_node.hpp
index 947173a..5af2c6c 100644
--- a/dsr_util/include/dsr_util/agent_node.hpp
+++ b/dsr_util/include/dsr_util/agent_node.hpp
@@ -30,22 +30,24 @@
 // ROS
 #include "geometry_msgs/msg/transform.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
 
 // DSR
 #include "dsr/api/dsr_api.h"
-
 #include "dsr_msgs/srv/save_dsr.hpp"
 #include "dsr_util/dsr_api_ext.hpp"
 
 namespace dsr_util
 {
 
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
 /**
  * @class dsr_util::AgentNode
  * @brief Base class to connect the DSR graph with ROS 2. It contains common methods and attributes
  * to send data from ROS 2 to the DSR graph and vice versa. All agents must inherit from this class.
  */
-class AgentNode : public QObject, public rclcpp::Node
+class AgentNode : public QObject, public rclcpp_lifecycle::LifecycleNode
 {
   Q_OBJECT
 
@@ -54,14 +56,56 @@ class AgentNode : public QObject, public rclcpp::Node
    * @brief Construct a new Agent Node object.
    *
    * @param ros_node_name Name of the ROS node and the DSR agent.
+   * @param options Node options
+   */
+  explicit AgentNode(
+    std::string ros_node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+
+  /**
+   * @brief Configure the node
+   *
+   * @param state State of the node
+   * @return CallbackReturn
+   */
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Activate the node
+   *
+   * @param state State of the node
+   * @return CallbackReturn
+   */
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Deactivate the node
+   *
+   * @param state State of the node
+   * @return CallbackReturn
    */
-  explicit AgentNode(std::string ros_node_name);
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Cleanup the node
+   *
+   * @param state State of the node
+   * @return CallbackReturn
+   */
+  CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Shutdown the node
+   *
+   * @param state State of the node
+   * @return CallbackReturn
+   */
+  CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
 
   /**
    * @brief Destroy the Agent Node object.
    *
    */
-  virtual ~AgentNode();
+  virtual ~AgentNode() = default;
 
 protected:
   /**
@@ -88,8 +132,7 @@ class AgentNode : public QObject, public rclcpp::Node
         "Inserted node [%s] successfully of type [%s]", name.c_str(), new_node.type().c_str());
     } else {
       RCLCPP_ERROR(
-        this->get_logger(),
-        "The node [%s] couldn't be inserted", name.c_str());
+        this->get_logger(), "The node [%s] couldn't be inserted", name.c_str());
     }
     return return_node;
   }
@@ -165,15 +208,13 @@ class AgentNode : public QObject, public rclcpp::Node
           RCLCPP_INFO(
             this->get_logger(),
             "Inserted edge [%s->%s] successfully of type [%s]",
-            parent_node.value().name().c_str(),
-            child_node.value().name().c_str(),
+            parent_node.value().name().c_str(), child_node.value().name().c_str(),
             new_edge.type().c_str());
         } else {
           RCLCPP_ERROR(
             this->get_logger(),
             "The edge [%s->%s] of type [%s] couldn't be inserted",
-            parent_node.value().name().c_str(),
-            child_node.value().name().c_str(),
+            parent_node.value().name().c_str(), child_node.value().name().c_str(),
             new_edge.type().c_str());
         }
       } else {
@@ -215,15 +256,13 @@ class AgentNode : public QObject, public rclcpp::Node
         RCLCPP_INFO(
           this->get_logger(),
           "Inserted edge [%s->%s] successfully of type [%s]",
-          parent_node.value().name().c_str(),
-          child_node.value().name().c_str(),
+          parent_node.value().name().c_str(), child_node.value().name().c_str(),
           new_edge.type().c_str());
       } else {
         RCLCPP_ERROR(
           this->get_logger(),
           "The edge [%s->%s] of type [%s] couldn't be inserted",
-          parent_node.value().name().c_str(),
-          child_node.value().name().c_str(),
+          parent_node.value().name().c_str(), child_node.value().name().c_str(),
           new_edge.type().c_str());
       }
     }
@@ -241,12 +280,13 @@ class AgentNode : public QObject, public rclcpp::Node
   bool delete_node(uint64_t id)
   {
     // Check if the node exists
+    bool success = false;
     if (auto node = G_->get_node(id); node.has_value()) {
       // Delete the node
       if (G_->delete_node(id)) {
         RCLCPP_INFO(
           this->get_logger(), "Deleted node [%s] successfully", node.value().name().c_str());
-        return true;
+        success = true;
       } else {
         RCLCPP_ERROR(
           this->get_logger(), "The node [%s] couldn't be deleted", node.value().name().c_str());
@@ -255,7 +295,7 @@ class AgentNode : public QObject, public rclcpp::Node
       RCLCPP_WARN(
         this->get_logger(), "The node [%lu] couldn't be deleted because it doesn't exists", id);
     }
-    return false;
+    return success;
   }
 
   /**
@@ -282,7 +322,7 @@ class AgentNode : public QObject, public rclcpp::Node
    */
   bool delete_edge(uint64_t from, uint64_t to, std::string edge_type)
   {
-    // Check if the parent and child nodes exist and if the edge existsd
+    // Check if the parent and child nodes exist and if the edge exists
     auto parent_node = G_->get_node(from);
     auto child_node = G_->get_node(to);
     if (parent_node.has_value() && child_node.has_value()) {
@@ -292,16 +332,14 @@ class AgentNode : public QObject, public rclcpp::Node
           RCLCPP_INFO(
             this->get_logger(),
             "Deleted edge [%s->%s] successfully of type [%s]",
-            parent_node.value().name().c_str(),
-            child_node.value().name().c_str(),
+            parent_node.value().name().c_str(), child_node.value().name().c_str(),
             edge_type.c_str());
           return true;
         } else {
           RCLCPP_ERROR(
             this->get_logger(),
             "The edge [%s->%s] of type [%s] couldn't be deleted",
-            parent_node.value().name().c_str(),
-            child_node.value().name().c_str(),
+            parent_node.value().name().c_str(), child_node.value().name().c_str(),
             edge_type.c_str());
         }
       } else {
@@ -373,26 +411,22 @@ class AgentNode : public QObject, public rclcpp::Node
               RCLCPP_INFO(
                 this->get_logger(),
                 "Replaced edge [%s->%s] of type [%s] with type [%s]",
-                parent_node.value().name().c_str(),
-                child_node.value().name().c_str(),
-                old_edge.c_str(),
-                new_edge.type().c_str());
+                parent_node.value().name().c_str(), child_node.value().name().c_str(),
+                old_edge.c_str(), new_edge.type().c_str());
               return true;
             }
           } else {
             RCLCPP_ERROR(
               this->get_logger(),
               "The edge [%s->%s] of type [%s] couldn't be deleted",
-              parent_node.value().name().c_str(),
-              child_node.value().name().c_str(),
+              parent_node.value().name().c_str(), child_node.value().name().c_str(),
               old_edge.c_str());
           }
         } else {
           RCLCPP_WARN(
             this->get_logger(),
             "The edge [%lu->%lu] of type [%s] couldn't be deleted "
-            "because it doesn't exists",
-            from, to, old_edge.c_str());
+            "because it doesn't exists", from, to, old_edge.c_str());
         }
       } else {
         RCLCPP_WARN(
@@ -478,12 +512,12 @@ class AgentNode : public QObject, public rclcpp::Node
     }
   }
 
-private:
   /**
    * @brief Initialize ROS parameters.
    */
   void get_common_params();
 
+private:
   /**
    * @brief Save the DSR graph into a file.
    *
@@ -555,6 +589,9 @@ class AgentNode : public QObject, public rclcpp::Node
   // Id of the DSR agent.
   int agent_id_;
 
+  // Name of the agent.
+  std::string agent_name_;
+
   // Name of the input file to load the DSR graph from.
   std::string dsr_input_file_;
 };
diff --git a/dsr_util/package.xml b/dsr_util/package.xml
index c64ee2c..28afeaf 100644
--- a/dsr_util/package.xml
+++ b/dsr_util/package.xml
@@ -17,6 +17,7 @@
   <depend>fastrtps</depend>
   <depend>geometry_msgs</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
   <depend>tf2_geometry_msgs</depend>
 
   <exec_depend>libqt5-core</exec_depend>
diff --git a/dsr_util/src/agent_node.cpp b/dsr_util/src/agent_node.cpp
index e70667a..884bbcc 100644
--- a/dsr_util/src/agent_node.cpp
+++ b/dsr_util/src/agent_node.cpp
@@ -21,14 +21,31 @@
 namespace dsr_util
 {
 
-AgentNode::AgentNode(std::string ros_node_name)
-: rclcpp::Node(ros_node_name)
+AgentNode::AgentNode(std::string ros_node_name, const rclcpp::NodeOptions & options)
+: rclcpp_lifecycle::LifecycleNode(ros_node_name, "", options)
+{
+  // Register types
+  qRegisterMetaType<uint64_t>("uint64_t");
+  qRegisterMetaType<std::string>("std::string");
+  qRegisterMetaType<std::vector<std::string>>("std::vector<std::string>");
+  qRegisterMetaType<DSR::Node>("Node");
+  qRegisterMetaType<DSR::Edge>("Edge");
+  qRegisterMetaType<DSR::SignalInfo>("DSR::SignalInfo");
+}
+
+CallbackReturn AgentNode::on_configure(const rclcpp_lifecycle::State & state)
 {
   // Get ROS parameters
   get_common_params();
 
   // Create graph
-  G_ = std::make_shared<dsr_util::DSRGraphExt>(ros_node_name, agent_id_, dsr_input_file_);
+  try {
+    G_ = std::make_shared<dsr_util::DSRGraphExt>(agent_name_, agent_id_, dsr_input_file_);
+  } catch (const DSR::DSRException & e) {
+    RCLCPP_ERROR(this->get_logger(), e.what());
+    on_cleanup(state);
+    return CallbackReturn::FAILURE;
+  }
 
   // Get RT API
   rt_ = G_->get_rt_api();
@@ -38,14 +55,6 @@ AgentNode::AgentNode(std::string ros_node_name)
     "save_dsr",
     std::bind(&AgentNode::save_dsr, this, std::placeholders::_1, std::placeholders::_2));
 
-  // Register types
-  qRegisterMetaType<uint64_t>("uint64_t");
-  qRegisterMetaType<std::string>("std::string");
-  qRegisterMetaType<std::vector<std::string>>("std::vector<std::string>");
-  qRegisterMetaType<DSR::Node>("Node");
-  qRegisterMetaType<DSR::Edge>("Edge");
-  qRegisterMetaType<DSR::SignalInfo>("DSR::SignalInfo");
-
   // Add connection signals
   QObject::connect(
     G_.get(), &DSR::DSRGraph::update_node_signal, this, &AgentNode::node_updated);
@@ -59,17 +68,61 @@ AgentNode::AgentNode(std::string ros_node_name)
     G_.get(), &DSR::DSRGraph::del_edge_signal, this, &AgentNode::edge_deleted);
   QObject::connect(
     G_.get(), &DSR::DSRGraph::del_node_signal, this, &AgentNode::node_deleted);
+
+  RCLCPP_INFO(this->get_logger(), "Configured agent node");
+  return CallbackReturn::SUCCESS;
 }
 
-AgentNode::~AgentNode()
+CallbackReturn AgentNode::on_activate(const rclcpp_lifecycle::State & state)
 {
+  LifecycleNode::on_activate(state);
+  RCLCPP_INFO(this->get_logger(), "Activating the node...");
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn AgentNode::on_deactivate(const rclcpp_lifecycle::State & state)
+{
+  LifecycleNode::on_deactivate(state);
+  RCLCPP_INFO(this->get_logger(), "Deactivating the node...");
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn AgentNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(this->get_logger(), "Cleaning the node...");
+
+  // Release the shared pointers
   G_.reset();
+  rt_.reset();
+  save_dsr_service_.reset();
+
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn AgentNode::on_shutdown(const rclcpp_lifecycle::State & state)
+{
+  RCLCPP_INFO(this->get_logger(), "Shutdown the node from state %s.", state.label().c_str());
+
+  // Release the shared pointers
+  G_.reset();
+  rt_.reset();
+  save_dsr_service_.reset();
+
+  return CallbackReturn::SUCCESS;
 }
 
-/* Initialize ROS parameters */
 void AgentNode::get_common_params()
 {
   // Agent parameters
+  declare_parameter_if_not_declared(
+    this, "agent_name", rclcpp::ParameterValue(""),
+    rcl_interfaces::msg::ParameterDescriptor()
+    .set__description("The name of the agent"));
+  this->get_parameter("agent_name", agent_name_);
+  RCLCPP_INFO(
+    this->get_logger(),
+    "The parameter agent_name is set to: [%s]", agent_name_.c_str());
+
   declare_parameter_if_not_declared(
     this, "agent_id", rclcpp::ParameterValue(0),
     rcl_interfaces::msg::ParameterDescriptor()
diff --git a/dsr_util/test/CMakeLists.txt b/dsr_util/test/CMakeLists.txt
index 5f0ccf9..94f5901 100644
--- a/dsr_util/test/CMakeLists.txt
+++ b/dsr_util/test/CMakeLists.txt
@@ -1,3 +1,14 @@
+find_package(tf2_geometry_msgs REQUIRED)
+
+# Test for agent node
+ament_add_gtest(test_agent_node test_agent_node.cpp)
+target_link_libraries(test_agent_node
+  ${fastrtps_LIBRARIES}
+  ${library_name}
+  ament_index_cpp::ament_index_cpp
+  ${tf2_geometry_msgs_TARGETS}
+)
+
 # Test for dsr_api ext
 ament_add_gtest(test_dsr_api_ext test_dsr_api_ext.cpp)
 target_link_libraries(test_dsr_api_ext
@@ -9,6 +20,5 @@ target_link_libraries(test_dsr_api_ext
 # Test for qt executor
 ament_add_gtest(test_qt_executor test_qt_executor.cpp)
 target_link_libraries(test_qt_executor
-  ${fastrtps_LIBRARIES}
   ${library_name}
 )
\ No newline at end of file
diff --git a/dsr_util/test/test_agent_node.cpp b/dsr_util/test/test_agent_node.cpp
new file mode 100644
index 0000000..555ddc9
--- /dev/null
+++ b/dsr_util/test/test_agent_node.cpp
@@ -0,0 +1,421 @@
+// Copyright (c) 2024 Alberto J. Tudela Roldán
+// Copyright (c) 2024 Grupo Avispa, DTE, Universidad de Málaga
+//
+// 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.
+
+#include "gtest/gtest.h"
+#include "dsr_util/agent_node.hpp"
+#include "dsr_util/qt_executor.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "test_dsr_setup.hpp"
+
+class AgentNodeFixture : public dsr_util::AgentNode
+{
+public:
+  explicit AgentNodeFixture(std::string name)
+  : dsr_util::AgentNode(name)
+  {
+  }
+  ~AgentNodeFixture() = default;
+
+  template<typename node_type>
+  std::optional<DSR::Node> add_node(const std::string & name)
+  {
+    return dsr_util::AgentNode::add_node<node_type>(name);
+  }
+
+  template<typename node_type, typename edge_type>
+  std::tuple<std::optional<DSR::Node>, std::optional<DSR::Edge>>
+  add_node_with_edge(
+    const std::string & name, const std::string & connecting_node_name, const bool reversed = false)
+  {
+    return dsr_util::AgentNode::add_node_with_edge<node_type, edge_type>(
+      name, connecting_node_name, reversed);
+  }
+
+  template<typename edge_type>
+  std::optional<DSR::Edge> add_edge(const std::string & from, const std::string & to)
+  {
+    return dsr_util::AgentNode::add_edge<edge_type>(from, to);
+  }
+
+  template<typename edge_type>
+  std::optional<DSR::Edge> add_edge(uint64_t from, uint64_t to)
+  {
+    return dsr_util::AgentNode::add_edge<edge_type>(from, to);
+  }
+
+  bool delete_node(uint64_t id)
+  {
+    return dsr_util::AgentNode::delete_node(id);
+  }
+
+  bool delete_node(const std::string & name)
+  {
+    return dsr_util::AgentNode::delete_node(name);
+  }
+
+  bool delete_edge(uint64_t from, uint64_t to, std::string edge_type)
+  {
+    return dsr_util::AgentNode::delete_edge(from, to, edge_type);
+  }
+
+  bool delete_edge(const std::string & from, const std::string & to, std::string edge_type)
+  {
+    return dsr_util::AgentNode::delete_edge(from, to, edge_type);
+  }
+
+  template<typename edge_type>
+  bool replace_edge(uint64_t from, uint64_t to, std::string old_edge)
+  {
+    return dsr_util::AgentNode::replace_edge<edge_type>(from, to, old_edge);
+  }
+
+  template<typename edge_type>
+  bool replace_edge(const std::string & from, const std::string & to, std::string old_edge)
+  {
+    return dsr_util::AgentNode::replace_edge<edge_type>(from, to, old_edge);
+  }
+
+  void update_rt_attributes(
+    DSR::Node & from, DSR::Node & to,
+    const geometry_msgs::msg::Transform & msg)
+  {
+    dsr_util::AgentNode::update_rt_attributes(from, to, msg);
+  }
+
+  std::shared_ptr<dsr_util::DSRGraphExt> get_graph()
+  {
+    return G_;
+  }
+};
+
+TEST_F(DsrUtilTest, agentNodeConfigure)
+{
+  // Create the node
+  auto agent_node = std::make_shared<AgentNodeFixture>("test_node");
+  agent_node->declare_parameter("dsr_input_file", rclcpp::ParameterValue(test_file_));
+  agent_node->configure();
+  agent_node->activate();
+  agent_node->deactivate();
+  agent_node->cleanup();
+  agent_node->shutdown();
+}
+
+TEST_F(DsrUtilTest, agentNodeConfigureEmpty)
+{
+  // Create the node
+  auto agent_node = std::make_shared<AgentNodeFixture>("test_node");
+  agent_node->configure();
+  agent_node->activate();
+  agent_node->deactivate();
+  agent_node->cleanup();
+  agent_node->shutdown();
+}
+
+TEST_F(DsrUtilTest, agentNodeAddNode)
+{
+  // Create the node
+  auto agent_node = std::make_shared<AgentNodeFixture>("test_node");
+  agent_node->declare_parameter("dsr_input_file", rclcpp::ParameterValue(test_file_));
+  agent_node->configure();
+  agent_node->activate();
+
+  // Add a node
+  auto node = agent_node->add_node<robot_node_type>("test_node");
+  EXPECT_TRUE(node.has_value());
+  EXPECT_EQ(node.value().name(), "test_node");
+  EXPECT_EQ(node.value().type(), "robot");
+  EXPECT_TRUE(agent_node->get_graph()->get_node("test_node").has_value());
+
+  agent_node->deactivate();
+  agent_node->cleanup();
+  agent_node->shutdown();
+}
+
+TEST_F(DsrUtilTest, agentNodeAddNodeWithEdge)
+{
+  // Create the node
+  auto agent_node = std::make_shared<AgentNodeFixture>("test_node");
+  agent_node->declare_parameter("dsr_input_file", rclcpp::ParameterValue(test_file_));
+  agent_node->configure();
+  agent_node->activate();
+
+  // Add a node with an edge
+  auto [node, edge] = agent_node->add_node_with_edge<robot_node_type, is_edge_type>(
+    "test_node", "world", false);
+  EXPECT_TRUE(node.has_value());
+  EXPECT_EQ(node.value().name(), "test_node");
+  EXPECT_EQ(node.value().type(), "robot");
+  EXPECT_TRUE(agent_node->get_graph()->get_node("test_node").has_value());
+  EXPECT_TRUE(edge.has_value());
+  EXPECT_EQ(agent_node->get_graph()->get_node(edge.value().from()).value().name(), "world");
+  EXPECT_EQ(agent_node->get_graph()->get_node(edge.value().to()).value().name(), "test_node");
+  EXPECT_EQ(edge.value().type(), "is");
+
+  agent_node->deactivate();
+  agent_node->cleanup();
+  agent_node->shutdown();
+}
+
+TEST_F(DsrUtilTest, agentNodeAddNodeWithEdgeReversed)
+{
+  // Create the node
+  auto agent_node = std::make_shared<AgentNodeFixture>("test_node");
+  agent_node->declare_parameter("dsr_input_file", rclcpp::ParameterValue(test_file_));
+  agent_node->configure();
+  agent_node->activate();
+
+  // Add a node with an edge
+  auto [node, edge] = agent_node->add_node_with_edge<robot_node_type, is_edge_type>(
+    "test_node", "world", true);
+  EXPECT_TRUE(node.has_value());
+  EXPECT_EQ(node.value().name(), "test_node");
+  EXPECT_EQ(node.value().type(), "robot");
+  EXPECT_TRUE(agent_node->get_graph()->get_node("test_node").has_value());
+  EXPECT_TRUE(edge.has_value());
+  EXPECT_EQ(agent_node->get_graph()->get_node(edge.value().from()).value().name(), "test_node");
+  EXPECT_EQ(agent_node->get_graph()->get_node(edge.value().to()).value().name(), "world");
+  EXPECT_EQ(edge.value().type(), "is");
+
+  agent_node->deactivate();
+  agent_node->cleanup();
+  agent_node->shutdown();
+}
+
+TEST_F(DsrUtilTest, agentNodeAddEdgeStr)
+{
+  // Create the node
+  auto agent_node = std::make_shared<AgentNodeFixture>("test_node");
+  agent_node->declare_parameter("dsr_input_file", rclcpp::ParameterValue(test_file_));
+  agent_node->configure();
+  agent_node->activate();
+
+  // Add two nodes and an edge
+  auto parent_node = agent_node->add_node<robot_node_type>("parent_node");
+  auto child_node = agent_node->add_node<robot_node_type>("child_node");
+  auto edge = agent_node->add_edge<is_edge_type>("parent_node", "child_node");
+  EXPECT_TRUE(edge.has_value());
+  EXPECT_EQ(agent_node->get_graph()->get_node(edge.value().from()).value().name(), "parent_node");
+  EXPECT_EQ(agent_node->get_graph()->get_node(edge.value().to()).value().name(), "child_node");
+  EXPECT_EQ(edge.value().type(), "is");
+
+  agent_node->deactivate();
+  agent_node->cleanup();
+  agent_node->shutdown();
+}
+
+TEST_F(DsrUtilTest, agentNodeAddEdgeId)
+{
+  // Create the node
+  auto agent_node = std::make_shared<AgentNodeFixture>("test_node");
+  agent_node->declare_parameter("dsr_input_file", rclcpp::ParameterValue(test_file_));
+  agent_node->configure();
+  agent_node->activate();
+
+  // Add two nodes and an edge
+  auto parent_node = agent_node->add_node<robot_node_type>("parent_node");
+  auto child_node = agent_node->add_node<robot_node_type>("child_node");
+  auto edge = agent_node->add_edge<is_edge_type>(parent_node.value().id(), child_node.value().id());
+  EXPECT_TRUE(edge.has_value());
+  EXPECT_EQ(agent_node->get_graph()->get_node(edge.value().from()).value().name(), "parent_node");
+  EXPECT_EQ(agent_node->get_graph()->get_node(edge.value().to()).value().name(), "child_node");
+  EXPECT_EQ(edge.value().type(), "is");
+
+  agent_node->deactivate();
+  agent_node->cleanup();
+  agent_node->shutdown();
+}
+
+TEST_F(DsrUtilTest, agentNodeDeleteNodeId)
+{
+  // Create the node
+  auto agent_node = std::make_shared<AgentNodeFixture>("test_node");
+  agent_node->declare_parameter("dsr_input_file", rclcpp::ParameterValue(test_file_));
+  agent_node->configure();
+  agent_node->activate();
+
+  // Add a node
+  auto node = agent_node->add_node<robot_node_type>("test_node");
+  EXPECT_TRUE(node.has_value());
+
+  // Delete the node
+  EXPECT_TRUE(agent_node->delete_node(node.value().id()));
+
+  agent_node->deactivate();
+  agent_node->cleanup();
+  agent_node->shutdown();
+}
+
+TEST_F(DsrUtilTest, agentNodeDeleteNodeStr)
+{
+  // Create the node
+  auto agent_node = std::make_shared<AgentNodeFixture>("test_node");
+  agent_node->declare_parameter("dsr_input_file", rclcpp::ParameterValue(test_file_));
+  agent_node->configure();
+  agent_node->activate();
+
+  // Add a node
+  auto node = agent_node->add_node<robot_node_type>("test_node");
+  EXPECT_TRUE(node.has_value());
+
+  // Delete the node
+  EXPECT_TRUE(agent_node->delete_node(node.value().name()));
+
+  agent_node->deactivate();
+  agent_node->cleanup();
+  agent_node->shutdown();
+}
+
+TEST_F(DsrUtilTest, agentNodeDeleteEdgeId)
+{
+  // Create the node
+  auto agent_node = std::make_shared<AgentNodeFixture>("test_node");
+  agent_node->declare_parameter("dsr_input_file", rclcpp::ParameterValue(test_file_));
+  agent_node->configure();
+  agent_node->activate();
+
+  // Add two nodes and an edge
+  auto parent_node = agent_node->add_node<robot_node_type>("parent_node");
+  auto child_node = agent_node->add_node<robot_node_type>("child_node");
+  auto edge = agent_node->add_edge<is_edge_type>(parent_node.value().id(), child_node.value().id());
+
+  // Delete the edge
+  EXPECT_TRUE(agent_node->delete_edge(parent_node.value().id(), child_node.value().id(), "is"));
+
+  agent_node->deactivate();
+  agent_node->cleanup();
+  agent_node->shutdown();
+}
+
+TEST_F(DsrUtilTest, agentNodeDeleteEdgeStr)
+{
+  // Create the node
+  auto agent_node = std::make_shared<AgentNodeFixture>("test_node");
+  agent_node->declare_parameter("dsr_input_file", rclcpp::ParameterValue(test_file_));
+  agent_node->configure();
+  agent_node->activate();
+
+  // Add two nodes and an edge
+  auto parent_node = agent_node->add_node<robot_node_type>("parent_node");
+  auto child_node = agent_node->add_node<robot_node_type>("child_node");
+  auto edge = agent_node->add_edge<is_edge_type>(parent_node.value().id(), child_node.value().id());
+
+  // Delete the edge
+  EXPECT_TRUE(agent_node->delete_edge("parent_node", "child_node", "is"));
+
+  agent_node->deactivate();
+  agent_node->cleanup();
+  agent_node->shutdown();
+}
+
+TEST_F(DsrUtilTest, agentNodeReplaceEdgeId)
+{
+  // Create the node
+  auto agent_node = std::make_shared<AgentNodeFixture>("test_node");
+  agent_node->declare_parameter("dsr_input_file", rclcpp::ParameterValue(test_file_));
+  agent_node->configure();
+  agent_node->activate();
+
+  // Add two nodes and an edge
+  auto parent_node = agent_node->add_node<robot_node_type>("parent_node");
+  auto child_node = agent_node->add_node<robot_node_type>("child_node");
+  auto edge = agent_node->add_edge<is_edge_type>(parent_node.value().id(), child_node.value().id());
+
+  // Replace the edge
+  EXPECT_TRUE(
+    agent_node->replace_edge<has_edge_type>(
+      parent_node.value().id(), child_node.value().id(), "is"));
+
+  agent_node->deactivate();
+  agent_node->cleanup();
+  agent_node->shutdown();
+}
+
+TEST_F(DsrUtilTest, agentNodeReplaceEdgeStr)
+{
+  // Create the node
+  auto agent_node = std::make_shared<AgentNodeFixture>("test_node");
+  agent_node->declare_parameter("dsr_input_file", rclcpp::ParameterValue(test_file_));
+  agent_node->configure();
+  agent_node->activate();
+
+  // Add two nodes and an edge
+  auto parent_node = agent_node->add_node<robot_node_type>("parent_node");
+  auto child_node = agent_node->add_node<robot_node_type>("child_node");
+  auto edge = agent_node->add_edge<is_edge_type>(parent_node.value().id(), child_node.value().id());
+
+  // Replace the edge
+  EXPECT_TRUE(agent_node->replace_edge<has_edge_type>("parent_node", "child_node", "is"));
+
+  agent_node->deactivate();
+  agent_node->cleanup();
+  agent_node->shutdown();
+}
+
+TEST_F(DsrUtilTest, agentNodeUpdateRTAttributes)
+{
+  // Create the node
+  auto agent_node = std::make_shared<AgentNodeFixture>("test_node");
+  agent_node->declare_parameter("dsr_input_file", rclcpp::ParameterValue(test_file_));
+  agent_node->configure();
+  agent_node->activate();
+
+  // Add two nodes
+  auto parent_node = agent_node->add_node<robot_node_type>("parent_node");
+  EXPECT_TRUE(parent_node.has_value());
+  auto child_node = agent_node->add_node<robot_node_type>("child_node");
+  EXPECT_TRUE(child_node.has_value());
+
+  // Update the RT attributes
+  geometry_msgs::msg::Transform msg;
+  msg.translation.x = 1.0;
+  msg.translation.y = 2.0;
+  msg.translation.z = 3.0;
+  msg.rotation.x = 0.0;
+  msg.rotation.y = 0.0;
+  msg.rotation.z = 0.0;
+  msg.rotation.w = 1.0;
+  agent_node->update_rt_attributes(parent_node.value(), child_node.value(), msg);
+
+  // Check the RT attributes
+  auto rt_edge = agent_node->get_graph()->get_edge(
+    parent_node.value().id(), child_node.value().id(), "RT");
+  EXPECT_TRUE(rt_edge.has_value());
+
+  auto attributes = rt_edge.value().attrs();
+  auto trans = std::get<std::vector<float>>(attributes["rt_translation"].value());
+  EXPECT_FLOAT_EQ(trans[0], 1.0);
+  EXPECT_FLOAT_EQ(trans[1], 2.0);
+  EXPECT_FLOAT_EQ(trans[2], 3.0);
+
+  auto rot = std::get<std::vector<float>>(attributes["rt_rotation_euler_xyz"].value());
+  EXPECT_FLOAT_EQ(rot[0], 0.0);
+  EXPECT_FLOAT_EQ(rot[1], 0.0);
+  EXPECT_FLOAT_EQ(rot[2], 0.0);
+
+  agent_node->deactivate();
+  agent_node->cleanup();
+  agent_node->shutdown();
+}
+
+
+int main(int argc, char ** argv)
+{
+  QCoreApplication app(argc, argv);
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+  bool success = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return success;
+}
diff --git a/dsr_util/test/test_dsr_setup.hpp b/dsr_util/test/test_dsr_setup.hpp
index 91aa84f..ced6553 100644
--- a/dsr_util/test/test_dsr_setup.hpp
+++ b/dsr_util/test/test_dsr_setup.hpp
@@ -54,17 +54,22 @@ class DsrUtilTest : public ::testing::Test
   void SetUp() override
   {
     auto pkg = ament_index_cpp::get_package_share_directory("dsr_util");
-    G_ = std::make_shared<DSRGraphExtFixture>("test", 2, pkg + "/test/test_dsr.json");
+    test_file_ = pkg + "/test/test_dsr.json";
+    G_ = std::make_shared<DSRGraphExtFixture>("test", 2, test_file_);
+    rt_ = G_->get_rt_api();
   }
 
   void TearDown() override
   {
     std::this_thread::sleep_for(std::chrono::milliseconds(10));
     G_.reset();
+    rt_.reset();
   }
 
 protected:
   std::shared_ptr<DSRGraphExtFixture> G_;
+  std::unique_ptr<DSR::RT_API> rt_;
+  std::string test_file_;
 };
 
 #endif  // TEST_DSR_SETUP_HPP_