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,7 +80,9 @@ public:
     /**
      * @brief   Default destructor.
      */
-    ~GraphManager()
+    ~GraphManager() = default;
+
+	void stop()
 	{
 		if (microros_graph_publisher_.joinable())
 		{
@@ -88,6 +90,18 @@ public:
 			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_);
 	}
 
     /**

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)