Update graph destructor

micro_ros_agent/include/agent/Agent.hpp

@@ -29,6 +29,11 @@ class Agent
 private:
     Agent();
 
+    ~Agent()
+    {
+        stop();
+    }
+    
     Agent(const Agent &) = delete;
 
     Agent(Agent &&) = delete;

micro_ros_agent/include/agent/graph_manager/graph_manager.hpp

@@ -80,15 +80,29 @@ public:
     /**
      * @brief   Default destructor.
      */
-    ~GraphManager()
+    ~GraphManager() = default;
-    {
+
-        if (microros_graph_publisher_.joinable())
+	void stop()
-        {
+	{
-            exit = true;
+		if (microros_graph_publisher_.joinable())
-            cv_.notify_one();
+		{
-            microros_graph_publisher_.join();
+			exit = true;
-        }
+			cv_.notify_one();
-    }
+			microros_graph_publisher_.join();
+		}
+
+		subscriber_->delete_datareader(ros_discovery_datareader_);
+		publisher_->delete_datawriter(ros_to_microros_graph_datawriter_);
+
+		participant_->delete_subscriber(subscriber_);
+		participant_->delete_publisher(publisher_);
+
+		// Delete topics
+		participant_->delete_topic(ros_discovery_topic_);
+		participant_->delete_topic(ros_to_microros_graph_topic_);
+
+		eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant(participant_);
+	}
 
     /**
      * @brief   Implementation of the notification logic that updates the micro-ROS graph.

micro_ros_agent/src/agent/Agent.cpp

@@ -188,6 +188,13 @@ void Agent::run()
 void Agent::stop()
 {
     xrce_dds_agent_instance_.stop();
+
+    for (auto & element : graph_manager_map_)
+    {
+        element.second.stop();
+    }
+
+    graph_manager_map_.clear();
 }
 
 graph_manager::GraphManager* Agent::find_or_create_graph_manager(eprosima::fastdds::dds::DomainId_t domain_id)