diff --git a/micro_ros_agent/include/agent/Agent.hpp b/micro_ros_agent/include/agent/Agent.hpp
index f27f4d2..3f5c1c0 100644
--- a/micro_ros_agent/include/agent/Agent.hpp
+++ b/micro_ros_agent/include/agent/Agent.hpp
@@ -51,7 +51,7 @@ public:
     void stop();
 
 private:
-
+    bool initialized = false;
     eprosima::uxr::AgentInstance& xrce_dds_agent_instance_;
     std::map<eprosima::fastdds::dds::DomainId_t, std::shared_ptr<graph_manager::GraphManager>> graph_manager_map_;
 
diff --git a/micro_ros_agent/src/agent/Agent.cpp b/micro_ros_agent/src/agent/Agent.cpp
index 0dabe24..f8ca3fe 100644
--- a/micro_ros_agent/src/agent/Agent.cpp
+++ b/micro_ros_agent/src/agent/Agent.cpp
@@ -23,140 +23,6 @@ namespace agent {
 Agent::Agent()
     : xrce_dds_agent_instance_(xrce_dds_agent_instance_.getInstance())
 {
-    /**
-     * Add CREATE_PARTICIPANT callback.
-     */
-    std::function<void (
-        const eprosima::fastdds::dds::DomainParticipant *)> on_create_participant
-        ([&](
-            const eprosima::fastdds::dds::DomainParticipant* participant) -> void
-        {
-            auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
-            graph_manager_->add_participant(participant);
-        });
-    xrce_dds_agent_instance_.add_middleware_callback(
-        eprosima::uxr::Middleware::Kind::FASTDDS,
-        eprosima::uxr::middleware::CallbackKind::CREATE_PARTICIPANT,
-        std::move(on_create_participant));
-
-    /**
-     * Add REMOVE_PARTICIPANT callback.
-     */
-    std::function<void (
-        const eprosima::fastdds::dds::DomainParticipant *)> on_delete_participant
-        ([&](
-            const eprosima::fastdds::dds::DomainParticipant* participant) -> void
-        {
-            auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
-            graph_manager_->remove_participant(participant);
-        });
-    xrce_dds_agent_instance_.add_middleware_callback(
-        eprosima::uxr::Middleware::Kind::FASTDDS,
-        eprosima::uxr::middleware::CallbackKind::DELETE_PARTICIPANT,
-        std::move(on_delete_participant));
-
-    /**
-     * Add CREATE_DATAWRITER callback.
-     */
-    std::function<void (
-        const eprosima::fastdds::dds::DomainParticipant *,
-        const eprosima::fastdds::dds::DataWriter *)> on_create_datawriter
-        ([&](
-            const eprosima::fastdds::dds::DomainParticipant* participant,
-            const eprosima::fastdds::dds::DataWriter* datawriter) -> void
-        {
-            auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
-
-            // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
-            const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
-                datawriter->get_instance_handle();
-            const eprosima::fastrtps::rtps::GUID_t datawriter_guid =
-                iHandle2GUID(instance_handle);
-            graph_manager_->add_datawriter(datawriter_guid, participant, datawriter);
-            graph_manager_->associate_entity(
-                datawriter_guid, participant, dds::xrce::OBJK_DATAWRITER);
-        });
-    xrce_dds_agent_instance_.add_middleware_callback(
-        eprosima::uxr::Middleware::Kind::FASTDDS,
-        eprosima::uxr::middleware::CallbackKind::CREATE_DATAWRITER,
-        std::move(on_create_datawriter));
-
-    /**
-     * Add DELETE_DATAWRITER callback.
-     */
-    std::function<void (
-        const eprosima::fastdds::dds::DomainParticipant *,
-        const eprosima::fastdds::dds::DataWriter *)> on_delete_datawriter
-        ([&](
-            const eprosima::fastdds::dds::DomainParticipant* participant,
-            const eprosima::fastdds::dds::DataWriter* datawriter) -> void
-        {
-
-            auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
-
-            // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
-            const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
-                datawriter->get_instance_handle();
-            const eprosima::fastrtps::rtps::GUID_t datawriter_guid =
-                eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
-            graph_manager_->remove_datawriter(datawriter_guid);
-        });
-
-    xrce_dds_agent_instance_.add_middleware_callback(
-        eprosima::uxr::Middleware::Kind::FASTDDS,
-        eprosima::uxr::middleware::CallbackKind::DELETE_DATAWRITER,
-        std::move(on_delete_datawriter));
-
-    /**
-     * Add CREATE_DATAREADER callback.
-     */
-    std::function<void (
-        const eprosima::fastdds::dds::DomainParticipant *,
-        const eprosima::fastdds::dds::DataReader*)> on_create_datareader
-        ([&](
-            const eprosima::fastdds::dds::DomainParticipant* participant,
-            const eprosima::fastdds::dds::DataReader* datareader) -> void
-        {
-            auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
-
-            // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
-            const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
-                datareader->get_instance_handle();
-            const eprosima::fastrtps::rtps::GUID_t datareader_guid =
-                eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
-            graph_manager_->add_datareader(datareader_guid, participant, datareader);
-            graph_manager_->associate_entity(
-                datareader_guid, participant, dds::xrce::OBJK_DATAREADER);
-        });
-    xrce_dds_agent_instance_.add_middleware_callback(
-        eprosima::uxr::Middleware::Kind::FASTDDS,
-        eprosima::uxr::middleware::CallbackKind::CREATE_DATAREADER,
-        std::move(on_create_datareader));
-
-    /**
-     * Add DELETE_DATAREADER callback.
-     */
-    std::function<void (
-        const eprosima::fastdds::dds::DomainParticipant *,
-        const eprosima::fastdds::dds::DataReader *)> on_delete_datareader
-        ([&](
-            const eprosima::fastdds::dds::DomainParticipant* participant,
-            const eprosima::fastdds::dds::DataReader* datareader) -> void
-        {
-            auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
-
-            // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
-            const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
-                datareader->get_instance_handle();
-            const eprosima::fastrtps::rtps::GUID_t datareader_guid =
-                eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
-            graph_manager_->remove_datareader(datareader_guid);
-        });
-
-    xrce_dds_agent_instance_.add_middleware_callback(
-        eprosima::uxr::Middleware::Kind::FASTDDS,
-        eprosima::uxr::middleware::CallbackKind::DELETE_DATAREADER,
-        std::move(on_delete_datareader));
 }
 
 Agent& Agent::getInstance()
@@ -170,7 +36,150 @@ bool Agent::create(
         char** argv)
 {
     stop();
-    return xrce_dds_agent_instance_.create(argc, argv);
+
+    bool result = xrce_dds_agent_instance_.create(argc, argv);
+
+    if (result && !initialized)
+    {
+        initialized = true;
+
+        /**
+         * Add CREATE_PARTICIPANT callback.
+         */
+        std::function<void (
+            const eprosima::fastdds::dds::DomainParticipant *)> on_create_participant
+            ([&](
+                const eprosima::fastdds::dds::DomainParticipant* participant) -> void
+            {
+                auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
+                graph_manager_->add_participant(participant);
+            });
+        xrce_dds_agent_instance_.add_middleware_callback(
+            eprosima::uxr::Middleware::Kind::FASTDDS,
+            eprosima::uxr::middleware::CallbackKind::CREATE_PARTICIPANT,
+            std::move(on_create_participant));
+
+        /**
+         * Add REMOVE_PARTICIPANT callback.
+         */
+        std::function<void (
+            const eprosima::fastdds::dds::DomainParticipant *)> on_delete_participant
+            ([&](
+                const eprosima::fastdds::dds::DomainParticipant* participant) -> void
+            {
+                auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
+                graph_manager_->remove_participant(participant);
+            });
+        xrce_dds_agent_instance_.add_middleware_callback(
+            eprosima::uxr::Middleware::Kind::FASTDDS,
+            eprosima::uxr::middleware::CallbackKind::DELETE_PARTICIPANT,
+            std::move(on_delete_participant));
+
+        /**
+         * Add CREATE_DATAWRITER callback.
+         */
+        std::function<void (
+            const eprosima::fastdds::dds::DomainParticipant *,
+            const eprosima::fastdds::dds::DataWriter *)> on_create_datawriter
+            ([&](
+                const eprosima::fastdds::dds::DomainParticipant* participant,
+                const eprosima::fastdds::dds::DataWriter* datawriter) -> void
+            {
+                auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
+
+                // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
+                const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
+                    datawriter->get_instance_handle();
+                const eprosima::fastrtps::rtps::GUID_t datawriter_guid =
+                    iHandle2GUID(instance_handle);
+                graph_manager_->add_datawriter(datawriter_guid, participant, datawriter);
+                graph_manager_->associate_entity(
+                    datawriter_guid, participant, dds::xrce::OBJK_DATAWRITER);
+            });
+        xrce_dds_agent_instance_.add_middleware_callback(
+            eprosima::uxr::Middleware::Kind::FASTDDS,
+            eprosima::uxr::middleware::CallbackKind::CREATE_DATAWRITER,
+            std::move(on_create_datawriter));
+
+        /**
+         * Add DELETE_DATAWRITER callback.
+         */
+        std::function<void (
+            const eprosima::fastdds::dds::DomainParticipant *,
+            const eprosima::fastdds::dds::DataWriter *)> on_delete_datawriter
+            ([&](
+                const eprosima::fastdds::dds::DomainParticipant* participant,
+                const eprosima::fastdds::dds::DataWriter* datawriter) -> void
+            {
+
+                auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
+
+                // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
+                const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
+                    datawriter->get_instance_handle();
+                const eprosima::fastrtps::rtps::GUID_t datawriter_guid =
+                    eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
+                graph_manager_->remove_datawriter(datawriter_guid);
+            });
+
+        xrce_dds_agent_instance_.add_middleware_callback(
+            eprosima::uxr::Middleware::Kind::FASTDDS,
+            eprosima::uxr::middleware::CallbackKind::DELETE_DATAWRITER,
+            std::move(on_delete_datawriter));
+
+        /**
+         * Add CREATE_DATAREADER callback.
+         */
+        std::function<void (
+            const eprosima::fastdds::dds::DomainParticipant *,
+            const eprosima::fastdds::dds::DataReader*)> on_create_datareader
+            ([&](
+                const eprosima::fastdds::dds::DomainParticipant* participant,
+                const eprosima::fastdds::dds::DataReader* datareader) -> void
+            {
+                auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
+
+                // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
+                const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
+                    datareader->get_instance_handle();
+                const eprosima::fastrtps::rtps::GUID_t datareader_guid =
+                    eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
+                graph_manager_->add_datareader(datareader_guid, participant, datareader);
+                graph_manager_->associate_entity(
+                    datareader_guid, participant, dds::xrce::OBJK_DATAREADER);
+            });
+        xrce_dds_agent_instance_.add_middleware_callback(
+            eprosima::uxr::Middleware::Kind::FASTDDS,
+            eprosima::uxr::middleware::CallbackKind::CREATE_DATAREADER,
+            std::move(on_create_datareader));
+
+        /**
+         * Add DELETE_DATAREADER callback.
+         */
+        std::function<void (
+            const eprosima::fastdds::dds::DomainParticipant *,
+            const eprosima::fastdds::dds::DataReader *)> on_delete_datareader
+            ([&](
+                const eprosima::fastdds::dds::DomainParticipant* participant,
+                const eprosima::fastdds::dds::DataReader* datareader) -> void
+            {
+                auto graph_manager_ = find_or_create_graph_manager(participant->get_domain_id());
+
+                // TODO(jamoralp): Workaround for Fast-DDS bug #9977. Remove when fixed
+                const eprosima::fastrtps::rtps::InstanceHandle_t instance_handle =
+                    datareader->get_instance_handle();
+                const eprosima::fastrtps::rtps::GUID_t datareader_guid =
+                    eprosima::fastrtps::rtps::iHandle2GUID(instance_handle);
+                graph_manager_->remove_datareader(datareader_guid);
+            });
+
+        xrce_dds_agent_instance_.add_middleware_callback(
+            eprosima::uxr::Middleware::Kind::FASTDDS,
+            eprosima::uxr::middleware::CallbackKind::DELETE_DATAREADER,
+            std::move(on_delete_datareader));
+    }
+
+    return result;
 }
 
 void Agent::run()