From 2f2c0440c4fb96a4a9109031c8b832891d07896e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 16 Oct 2017 14:44:29 +0200
Subject: [PATCH] orb_exists: change semantics from (is published or
 subscribed) to (is published)

Existing users of orb_exists:
- logger (dynamic subscribe to multi-instances)
- mavlink (orb subscription)
- sdlog2
- preflightcheck (check for home_position)
- wait_for_topic shell command (it's not used)
- orb_group_count() (sensors: dynamic sensor addition)

All use-cases benefit from the changed semantics: they are really only
interested if there is a publisher, not another subscriber.
---
 src/drivers/drv_orb_dev.h        |  3 +++
 src/modules/uORB/uORBDevices.cpp |  9 +++++++++
 src/modules/uORB/uORBManager.cpp | 26 +++++++++++++++++++++++---
 src/modules/uORB/uORBManager.hpp |  5 ++---
 4 files changed, 37 insertions(+), 6 deletions(-)

diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h
index 2a0d5ca7d9..7947e68ec4 100644
--- a/src/drivers/drv_orb_dev.h
+++ b/src/drivers/drv_orb_dev.h
@@ -93,4 +93,7 @@
 /** Get the minimum interval at which the topic can be seen to be updated for this subscription */
 #define ORBIOCGETINTERVAL	_ORBIOC(16)
 
+/** Check whether the topic is published, sets *(unsigned long *)arg to 1 if published, 0 otherwise */
+#define ORBIOCISPUBLISHED	_ORBIOC(17)
+
 #endif /* _DRV_UORB_H */
diff --git a/src/modules/uORB/uORBDevices.cpp b/src/modules/uORB/uORBDevices.cpp
index 2cc25a3a14..1a59e0f4f3 100644
--- a/src/modules/uORB/uORBDevices.cpp
+++ b/src/modules/uORB/uORBDevices.cpp
@@ -170,6 +170,10 @@ uORB::DeviceNode::open(device::file_t *filp)
 		return ret;
 	}
 
+	if (FILE_FLAGS(filp) == 0) {
+		return CDev::open(filp);
+	}
+
 	/* can only be pub or sub, not both */
 	return -EINVAL;
 }
@@ -405,6 +409,11 @@ uORB::DeviceNode::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 
 		return OK;
 
+	case ORBIOCISPUBLISHED:
+		*(unsigned long *)arg = _published;
+
+		return OK;
+
 	default:
 		/* give it to the superclass */
 		return CDev::ioctl(filp, cmd, arg);
diff --git a/src/modules/uORB/uORBManager.cpp b/src/modules/uORB/uORBManager.cpp
index 332b2391d0..63f7880816 100644
--- a/src/modules/uORB/uORBManager.cpp
+++ b/src/modules/uORB/uORBManager.cpp
@@ -131,9 +131,9 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 		return ERROR;
 	}
 
-#if __PX4_NUTTX
+#if defined(__PX4_NUTTX)
 	struct stat buffer;
-	return stat(path, &buffer);
+	ret = stat(path, &buffer);
 #else
 	ret = px4_access(path, F_OK);
 
@@ -141,8 +141,28 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 		ret = (_remote_topics.find(meta->o_name) != _remote_topics.end()) ? OK : ERROR;
 	}
 
-	return ret;
 #endif
+
+	if (ret == 0) {
+		// we know the topic exists, but it's not necessarily advertised/published yet (for example
+		// if there is only a subscriber)
+		// The open() will not lead to memory allocations.
+		int fd = px4_open(path, 0);
+
+		if (fd >= 0) {
+			unsigned long is_published;
+
+			if (px4_ioctl(fd, ORBIOCISPUBLISHED, (unsigned long)&is_published) == 0) {
+				if (!is_published) {
+					ret = ERROR;
+				}
+			}
+
+			px4_close(fd);
+		}
+	}
+
+	return ret;
 }
 
 orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp
index b183e20dfc..e00e4a5770 100644
--- a/src/modules/uORB/uORBManager.hpp
+++ b/src/modules/uORB/uORBManager.hpp
@@ -298,12 +298,11 @@ public:
 	int  orb_stat(int handle, uint64_t *time) ;
 
 	/**
-	 * Check if a topic has already been created (a publisher or a subscriber exists with
-	 * the given instance).
+	 * Check if a topic has already been created and published (advertised)
 	 *
 	 * @param meta    ORB topic metadata.
 	 * @param instance  ORB instance
-	 * @return    OK if the topic exists, ERROR otherwise with errno set accordingly.
+	 * @return    OK if the topic exists, ERROR otherwise.
 	 */
 	int  orb_exists(const struct orb_metadata *meta, int instance) ;