From efc6a5036b72d0f4148c8ff50d09cfabddca7aa8 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 7 Dec 2023 21:24:59 -0500
Subject: [PATCH] uORB::Publication skip unadvertise on destruction if there
 are still subscribers

---
 platforms/common/uORB/Publication.hpp | 2 +-
 platforms/common/uORB/uORBManager.cpp | 5 +++++
 platforms/common/uORB/uORBManager.hpp | 2 ++
 3 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/platforms/common/uORB/Publication.hpp b/platforms/common/uORB/Publication.hpp
index 7547e3b7fa..d92310a5e1 100644
--- a/platforms/common/uORB/Publication.hpp
+++ b/platforms/common/uORB/Publication.hpp
@@ -84,7 +84,7 @@ protected:
 	{
 		if (_handle != nullptr) {
 			// don't automatically unadvertise queued publications (eg vehicle_command)
-			if (Manager::orb_get_queue_size(_handle) == 1) {
+			if ((Manager::orb_get_queue_size(_handle) == 1) && (Manager::orb_get_subscriber_count(_handle) <= 0)) {
 				unadvertise();
 			}
 		}
diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp
index 5b5530694b..d1d608633e 100644
--- a/platforms/common/uORB/uORBManager.cpp
+++ b/platforms/common/uORB/uORBManager.cpp
@@ -455,6 +455,11 @@ void uORB::Manager::orb_remove_internal_subscriber(void *node_handle)
 
 uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle) { return static_cast<const DeviceNode *>(node_handle)->get_queue_size(); }
 
+int8_t uORB::Manager::orb_get_subscriber_count(const void *node_handle)
+{
+	return static_cast<const DeviceNode *>(node_handle)->subscriber_count();
+}
+
 bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated)
 {
 	if (!is_advertised(node_handle)) {
diff --git a/platforms/common/uORB/uORBManager.hpp b/platforms/common/uORB/uORBManager.hpp
index 1c2870b1ee..ae235a896d 100644
--- a/platforms/common/uORB/uORBManager.hpp
+++ b/platforms/common/uORB/uORBManager.hpp
@@ -445,6 +445,8 @@ public:
 
 	static uint8_t orb_get_queue_size(const void *node_handle);
 
+	static int8_t orb_get_subscriber_count(const void *node_handle);
+
 	static bool orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated);
 
 	static bool register_callback(void *node_handle, SubscriptionCallback *callback_sub);