From 10c97cd15a6cb306ace6a34d76dcb9a353ee58c7 Mon Sep 17 00:00:00 2001
From: Vladimir Ermakov <vooon341@gmail.com>
Date: Fri, 7 Jun 2024 13:52:13 +0200
Subject: [PATCH] sys_status: replace rmw_qos too

---
 mavros/src/plugins/sys_status.cpp | 14 ++++++++++----
 1 file changed, 10 insertions(+), 4 deletions(-)

diff --git a/mavros/src/plugins/sys_status.cpp b/mavros/src/plugins/sys_status.cpp
index d7aa9066f..a8a6bea0f 100644
--- a/mavros/src/plugins/sys_status.cpp
+++ b/mavros/src/plugins/sys_status.cpp
@@ -588,25 +588,31 @@ class SystemStatusPlugin : public plugin::Plugin
 
     srv_cg = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 
+#ifdef USE_OLD_RMW_QOS
+    auto services_qos = rmw_qos_profile_services_default;
+#else
+    auto services_qos = rclcpp::ServicesQoS();
+#endif
+
     mode_srv = node->create_service<mavros_msgs::srv::SetMode>(
       "set_mode",
       std::bind(
         &SystemStatusPlugin::set_mode_cb, this, _1,
-        _2), rmw_qos_profile_services_default, srv_cg);
+        _2), services_qos, srv_cg);
     stream_rate_srv = node->create_service<mavros_msgs::srv::StreamRate>(
       "set_stream_rate",
       std::bind(
         &SystemStatusPlugin::set_rate_cb, this, _1,
-        _2), rmw_qos_profile_services_default, srv_cg);
+        _2), services_qos, srv_cg);
     message_interval_srv = node->create_service<mavros_msgs::srv::MessageInterval>(
       "set_message_interval",
       std::bind(
         &SystemStatusPlugin::set_message_interval_cb, this, _1,
-        _2), rmw_qos_profile_services_default, srv_cg);
+        _2), services_qos, srv_cg);
     vehicle_info_get_srv = node->create_service<mavros_msgs::srv::VehicleInfoGet>(
       "vehicle_info_get", std::bind(
         &SystemStatusPlugin::vehicle_info_get_cb, this, _1,
-        _2), rmw_qos_profile_services_default, srv_cg);
+        _2), services_qos, srv_cg);
 
     uas->diagnostic_updater.add(hb_diag);