From 0a6e8999fdd5477df1bda55a13ed8571870a8ede Mon Sep 17 00:00:00 2001
From: jyjblrd <jyjbird@gmail.com>
Date: Sat, 13 Apr 2024 19:15:28 +0100
Subject: [PATCH] add reliableImageTransport ros param

---
 src/orb_slam3_ros/src/ros_mono.cpp | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/src/orb_slam3_ros/src/ros_mono.cpp b/src/orb_slam3_ros/src/ros_mono.cpp
index 2661323..1cdd938 100644
--- a/src/orb_slam3_ros/src/ros_mono.cpp
+++ b/src/orb_slam3_ros/src/ros_mono.cpp
@@ -9,6 +9,7 @@
 #include <utility>
 
 #define BEST_EFFORT_QOS rclcpp::QoS(rclcpp::KeepLast(10), rmw_qos_profile_sensor_data)
+#define RELIABLE_QOS rclcpp::QoS(rclcpp::KeepLast(10), rmw_qos_profile_services_default)
 
 using namespace std;
 
@@ -20,10 +21,14 @@ class OrbSlam3Mono : public OrbSlam3Wrapper {
     this->declare_parameter("imageTopic", "robot" + to_string(agentId) + "/camera/image_color");
     string imageTopic = this->get_parameter("imageTopic").as_string();
 
-    image_subscriber_thread = std::thread([this, imageTopic]() {
+    this->declare_parameter("reliableImageTransport", true);
+    bool reliableImageTransport = this->get_parameter("reliableImageTransport").as_bool();
+
+    image_subscriber_thread = std::thread([this, imageTopic, reliableImageTransport]() {
       auto sub_node = rclcpp::Node::make_shared("image_subscriber_thread_node");
-      image_subscriber = sub_node->create_subscription<sensor_msgs::msg::Image>(
-        imageTopic, BEST_EFFORT_QOS, std::bind(&OrbSlam3Mono::grab_image, this, std::placeholders::_1));
+      image_subscriber = sub_node->create_subscription<sensor_msgs::msg::Image>(imageTopic,
+        reliableImageTransport ? RELIABLE_QOS : BEST_EFFORT_QOS,
+        std::bind(&OrbSlam3Mono::grab_image, this, std::placeholders::_1));
       rclcpp::spin(sub_node);
     });