From 08afdbaa2764b952750f70516d5a37c0ed920119 Mon Sep 17 00:00:00 2001
From: jorgenfj <jorgfje@stud.ntnu.no>
Date: Tue, 23 Jul 2024 21:13:31 +0200
Subject: [PATCH] qos fix

---
 .../landmark_server/src/landmark_server.cpp   | 19 ++++++++++++++-----
 1 file changed, 14 insertions(+), 5 deletions(-)

diff --git a/mission/landmark_server/src/landmark_server.cpp b/mission/landmark_server/src/landmark_server.cpp
index 899213b..70ce01f 100644
--- a/mission/landmark_server/src/landmark_server.cpp
+++ b/mission/landmark_server/src/landmark_server.cpp
@@ -12,23 +12,32 @@ LandmarkServerNode::LandmarkServerNode(const rclcpp::NodeOptions &options)
   // Define the quality of service profile for publisher and subscriber
   rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
   qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
-  auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1),
+  auto qos_best_effort = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1),
                          qos_profile);
 
+   // Set QoS profile
+    rmw_qos_profile_t qos_profile_sub = rmw_qos_profile_default;
+    qos_profile_sub.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+    qos_profile_sub.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST;
+    qos_profile_sub.depth = 10; // You can adjust this depth as needed for your use case
+
+    auto qos_reliable = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile_sub.history, qos_profile_sub.depth), qos_profile_sub);
+
+
   storedLandmarks_ = std::make_shared<LandmarkArray>();
 
   landmark_sub_ = this->create_subscription<LandmarkArray>(
-      "target_tracking/landmarks", qos,
+      "target_tracking/landmarks", qos_reliable,
       std::bind(&LandmarkServerNode::landmarksRecievedCallback, this, _1));
 
   landmarkPublisher_ =
-      this->create_publisher<LandmarkArray>("landmarks_out", qos);
+      this->create_publisher<LandmarkArray>("landmarks_out", qos_best_effort);
 
   posePublisher_ = this->create_publisher<geometry_msgs::msg::PoseArray>(
-      "landmark_poses_out", qos);
+      "landmark_poses_out", qos_best_effort);
 
   gridPublisher_ =
-      this->create_publisher<nav_msgs::msg::OccupancyGrid>("grid", qos);
+      this->create_publisher<nav_msgs::msg::OccupancyGrid>("grid", qos_best_effort);
 
   rmw_qos_profile_t qos_profile_sensor = rmw_qos_profile_sensor_data;
   qos_profile_sensor.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;