zenoh: implemement experimental liveliness to get ROS2 graph to work

src/modules/zenoh/Kconfig

@@ -21,6 +21,14 @@ if MODULES_ZENOH
         default "tcp/127.0.0.1:7447" if PLATFORM_POSIX
         default "" if !PLATFORM_POSIX
 
+    config ZENOH_RMW_LIVELINESS
+        bool "[EXPERIMENTAL] rmw_zenoh liveliness implemenation"
+        default y
+        ---help---
+                Declares liveliness tokens with key expressions in the way rmw_zenoh expects them
+                Allowing to construct ROS2 graphs
+
+
     # Choose exactly one item
     choice ZENOH_PUBSUB_SELECTION
             prompt "Publishers/Subscribers selection"

src/modules/zenoh/zenoh.cpp

@@ -92,13 +92,25 @@ ZENOH::~ZENOH()
 
 }
 
-int ZENOH::generate_rmw_zenoh_keyexpr(const char *topic, char *type, char *keyexpr)
+int ZENOH::generate_rmw_zenoh_node_liveliness_keyexpr(const z_id_t *id, char *keyexpr)
 {
+	return snprintf(keyexpr, KEYEXPR_SIZE,
+			"@ros2_lv/0/%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x/0/0/NN/%%/%%/"
+			"px4_%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x",
+			id->id[0], id->id[1],  id->id[2], id->id[3], id->id[4], id->id[5], id->id[6],
+			id->id[7], id->id[8],  id->id[9], id->id[10], id->id[11], id->id[12], id->id[13],
+			id->id[14], id->id[15],
+			px4_guid[0], px4_guid[1], px4_guid[2], px4_guid[3],
+			px4_guid[4], px4_guid[5], px4_guid[6], px4_guid[7],
+			px4_guid[8], px4_guid[9], px4_guid[10], px4_guid[11],
+			px4_guid[12], px4_guid[13], px4_guid[14], px4_guid[15]);
+}
 
-	const uint8_t *rihs_hash = getRIHS01_Hash(type);
+int ZENOH::generate_rmw_zenoh_topic_keyexpr(const char *topic, const uint8_t *rihs_hash, char *type, char *keyexpr)
+{
 	const char *type_name = getTypeName(type);
 
-	if (rihs_hash && type_name) {
+	if (type_name) {
 		strcpy(type, type_name);
 		toCamelCase(type); // Convert uORB type to camel case
 		return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRId32 "/%s/"
@@ -122,24 +134,50 @@ int ZENOH::generate_rmw_zenoh_keyexpr(const char *topic, char *type, char *keyex
 	return -1;
 }
 
-void ZENOH::run()
+int ZENOH::generate_rmw_zenoh_topic_liveliness_keyexpr(const z_id_t *id, const char *topic, const uint8_t *rihs_hash,
+		char *type_camel_case, char *keyexpr, const char *entity_str)
+{
+	// NOT REALLY COMPLIANT WITH RMW_ZENOH_CPP but get's the job done
+	// TODO build a correct keyexpr
+
+	return snprintf(keyexpr, KEYEXPR_SIZE,
+			"@ros2_lv/%" PRId32 "/"
+			"%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x/"
+			"0/11/%s/%%/%%/px4_%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x/%%%s/"
+			KEYEXPR_MSG_NAME "%s_/RIHS01_"
+			"%02x%02x%02x%02x%02x%02x%02x%02x"
+			"%02x%02x%02x%02x%02x%02x%02x%02x"
+			"%02x%02x%02x%02x%02x%02x%02x%02x"
+			"%02x%02x%02x%02x%02x%02x%02x%02x"
+			"/::,7:,:,:,,",
+			_zenoh_domain_id.get(),
+			id->id[0], id->id[1],  id->id[2], id->id[3], id->id[4], id->id[5], id->id[6],
+			id->id[7], id->id[8],  id->id[9], id->id[10], id->id[11], id->id[12], id->id[13],
+			id->id[14], id->id[15],
+			entity_str,
+			px4_guid[0], px4_guid[1], px4_guid[2], px4_guid[3],
+			px4_guid[4], px4_guid[5], px4_guid[6], px4_guid[7],
+			px4_guid[8], px4_guid[9], px4_guid[10], px4_guid[11],
+			px4_guid[12], px4_guid[13], px4_guid[14], px4_guid[15],
+			topic, type_camel_case,
+			rihs_hash[0], rihs_hash[1], rihs_hash[2], rihs_hash[3],
+			rihs_hash[4], rihs_hash[5], rihs_hash[6], rihs_hash[7],
+			rihs_hash[8], rihs_hash[9], rihs_hash[10], rihs_hash[11],
+			rihs_hash[12], rihs_hash[13], rihs_hash[14], rihs_hash[15],
+			rihs_hash[16], rihs_hash[17], rihs_hash[18], rihs_hash[19],
+			rihs_hash[20], rihs_hash[21], rihs_hash[22], rihs_hash[23],
+			rihs_hash[24], rihs_hash[25], rihs_hash[26], rihs_hash[27],
+			rihs_hash[28], rihs_hash[29], rihs_hash[30], rihs_hash[31]
+		       );
+}
+
+int ZENOH::setupSession()
 {
 	char mode[NET_MODE_SIZE];
 	char locator[NET_LOCATOR_SIZE];
-	int8_t ret;
+	int ret = 0;
-	int i;
 
-#ifndef BOARD_HAS_NO_UUID
+	_config.getNetworkConfig(mode, locator);
-	px4_guid_t px4_guid;
-	board_get_px4_guid(px4_guid);
-#else
-	// TODO Fill ID with something reasonable
-	px4_guid_t px4_guid = {0xAA, 0xBB, 0xAA};
-#endif
-
-	Zenoh_Config z_config;
-
-	z_config.getNetworkConfig(mode, locator);
 
 	z_owned_config_t config;
 	z_config_default(&config);
@@ -153,7 +191,6 @@ void ZENOH::run()
 	}
 
 	PX4_INFO("Opening session...");
-	z_owned_session_t s;
 	ret = z_open(&s, z_move(config), NULL);
 
 	if (ret < 0) {
@@ -167,40 +204,95 @@ void ZENOH::run()
 			PX4_ERR("Unable to open session, ret: %d", ret);
 		}
 
-		return;
+		return ret;
 	}
 
-	PX4_INFO("Starting reading/writing tasks...");
+	return ret;
+}
+
+int ZENOH::setupTopics(px4_pollfd_struct_t *pfds)
+{
+	char keyexpr[KEYEXPR_SIZE];
+	int i;
+	int ret = 0;
+
+#ifndef BOARD_HAS_NO_UUID
+	board_get_px4_guid(px4_guid);
+#else
+	// TODO Fill ID with something reasonable
+	px4_guid[0] = 0xAA;
+	px4_guid[1] = 0xBB;
+	px4_guid[2] = 0xCC;
+#endif
 
 	// Start read and lease tasks for zenoh-pico
 	if (zp_start_read_task(z_loan_mut(s), NULL) < 0 || zp_start_lease_task(z_loan_mut(s), NULL) < 0) {
 		PX4_ERR("Unable to start read and lease tasks");
 		z_drop(z_move(s));
-		return;
+		return -EINVAL;
 	}
 
+#ifdef CONFIG_ZENOH_RMW_LIVELINESS
+	z_id_t self_id = z_info_zid(z_loan(s));
+
+	if (generate_rmw_zenoh_node_liveliness_keyexpr(&self_id, keyexpr)) {
+		z_view_keyexpr_t ke;
+
+		if (z_view_keyexpr_from_str(&ke, keyexpr) < 0) {
+			printf("%s is not a valid key expression\n", keyexpr);
+			return 1;
+		}
+
+		z_owned_liveliness_token_t token;
+
+		if (z_liveliness_declare_token(z_loan(s), &token, z_loan(ke), NULL) < 0) {
+			printf("Unable to create liveliness token!\n");
+			return -1;
+		}
+	}
+
+#endif
+
 #ifdef Z_SUBSCRIBE
-	_sub_count =  z_config.getSubCount();
 	_zenoh_subscribers = (Zenoh_Subscriber **)malloc(sizeof(Zenoh_Subscriber *)*_sub_count);
 	{
 		char topic[TOPIC_INFO_SIZE];
 		char type[TOPIC_INFO_SIZE];
-		char keyexpr[KEYEXPR_SIZE];
 
 		for (i = 0; i < _sub_count; i++) {
-			z_config.getSubscriberMapping(topic, type);
+			_config.getSubscriberMapping(topic, type);
 			_zenoh_subscribers[i] = genSubscriber(type);
+			const uint8_t *rihs_hash = getRIHS01_Hash(type);
 
-			if (_zenoh_subscribers[i] != 0 &&
+			if (rihs_hash != NULL && _zenoh_subscribers[i] != 0 &&
-			    generate_rmw_zenoh_keyexpr(topic, type, keyexpr) > 0) {
+			    generate_rmw_zenoh_topic_keyexpr(topic, rihs_hash, type, keyexpr) > 0) {
 				_zenoh_subscribers[i]->declare_subscriber(s, keyexpr);
+#ifdef CONFIG_ZENOH_RMW_LIVELINESS
+
+				if (generate_rmw_zenoh_topic_liveliness_keyexpr(&self_id, topic, rihs_hash, type, keyexpr, "MS") > 0) {
+					z_view_keyexpr_t ke;
+
+					if (z_view_keyexpr_from_str(&ke, keyexpr) < 0) {
+						printf("%s is not a valid key expression\n", keyexpr);
+						return -1;
+					}
+
+					z_owned_liveliness_token_t token;
+
+					if (z_liveliness_declare_token(z_loan(s), &token, z_loan(ke), NULL) < 0) {
+						printf("Unable to create liveliness token!\n");
+						return -1;
+					}
+				}
+
+#endif
 
 			} else {
 				PX4_ERR("Could not create a subscriber for type %s", type);
 			}
 
 
-			if (z_config.getSubscriberMapping(topic, type) < 0) {
+			if (_config.getSubscriberMapping(topic, type) < 0) {
 				PX4_WARN("Subscriber mapping parsing error");
 			}
 		}
@@ -209,34 +301,74 @@ void ZENOH::run()
 #endif
 
 #ifdef Z_PUBLISH
-
-	_pub_count =  z_config.getPubCount();
 	_zenoh_publishers = (uORB_Zenoh_Publisher **)malloc(_pub_count * sizeof(uORB_Zenoh_Publisher *));
-	px4_pollfd_struct_t pfds[_pub_count];
 
 	{
 		char topic[TOPIC_INFO_SIZE];
 		char type[TOPIC_INFO_SIZE];
-		char keyexpr[KEYEXPR_SIZE];
 
 		for (i = 0; i < _pub_count; i++) {
-			z_config.getPublisherMapping(topic, type);
+			_config.getPublisherMapping(topic, type);
 			_zenoh_publishers[i] = genPublisher(type);
+			const uint8_t *rihs_hash = getRIHS01_Hash(type);
 
-			if (_zenoh_publishers[i] != 0 &&
+			if (rihs_hash && _zenoh_publishers[i] != 0 &&
-			    generate_rmw_zenoh_keyexpr(topic, type, keyexpr) > 0) {
+			    generate_rmw_zenoh_topic_keyexpr(topic, rihs_hash, type, keyexpr) > 0) {
 				_zenoh_publishers[i]->declare_publisher(s, keyexpr, (uint8_t *)&px4_guid);
 				_zenoh_publishers[i]->setPollFD(&pfds[i]);
+#ifdef CONFIG_ZENOH_RMW_LIVELINESS
+
+				if (generate_rmw_zenoh_topic_liveliness_keyexpr(&self_id, topic, rihs_hash, type, keyexpr, "MP") > 0) {
+					z_view_keyexpr_t ke;
+
+					if (z_view_keyexpr_from_str(&ke, keyexpr) < 0) {
+						printf("%s is not a valid key expression\n", keyexpr);
+						return -1;
+					}
+
+					z_owned_liveliness_token_t token;
+
+					if (z_liveliness_declare_token(z_loan(s), &token, z_loan(ke), NULL) < 0) {
+						printf("Unable to create liveliness token!\n");
+						return -1;
+					}
+				}
+
+#endif
 
 			} else {
 				PX4_ERR("Could not create a publisher for type %s", type);
 			}
 		}
 
-		if (z_config.getSubscriberMapping(topic, type) < 0) {
+		if (_config.getSubscriberMapping(topic, type) < 0) {
 			PX4_WARN("Publisher mapping parsing error");
 		}
 	}
+#endif
+
+	return ret;
+}
+
+void ZENOH::run()
+{
+	int8_t ret;
+	int i;
+	_pub_count =  _config.getPubCount();
+	_sub_count =  _config.getSubCount();
+	px4_pollfd_struct_t pfds[_pub_count];
+
+	if (setupSession() < 0) {
+		PX4_ERR("Failed to setup Zenoh session");
+		return;
+	}
+
+	PX4_INFO("Starting reading/writing tasks...");
+
+	if (setupTopics(pfds) < 0) {
+		PX4_ERR("Failed to setup topics");
+		return;
+	}
 
 	if (_pub_count == 0) {
 		// Nothing to publish but we don't want to stop this thread
@@ -265,8 +397,6 @@ void ZENOH::run()
 		}
 	}
 
-#endif
-
 	// Exiting cleaning up publisher and subscribers
 	for (i = 0; i < _sub_count; i++) {
 		if (_zenoh_subscribers[i]) {

src/modules/zenoh/zenoh.h

@@ -49,6 +49,7 @@
 #include "publishers/uorb_publisher.hpp"
 #include "subscribers/uorb_subscriber.hpp"
 
+
 class ZENOH : public ModuleBase<ZENOH>, public ModuleParams
 {
 public:
@@ -86,7 +87,12 @@ private:
 		(ParamInt<px4::params::ZENOH_DOMAIN_ID>) _zenoh_domain_id
 	)
 
-	int generate_rmw_zenoh_keyexpr(const char *topic, char *type, char *keyexpr);
+	int generate_rmw_zenoh_node_liveliness_keyexpr(const z_id_t *id, char *keyexpr);
+	int generate_rmw_zenoh_topic_keyexpr(const char *topic, const uint8_t *rihs_hash, char *type, char *keyexpr);
+	int generate_rmw_zenoh_topic_liveliness_keyexpr(const z_id_t *id, const char *topic, const uint8_t *rihs_hash,
+			char *type, char *keyexpr, const char *entity_str);
+	int setupSession();
+	int setupTopics(px4_pollfd_struct_t *pfds);
 
 	Zenoh_Config _config;
 
@@ -95,6 +101,10 @@ private:
 	int _sub_count;
 	Zenoh_Subscriber **_zenoh_subscribers;
 
+	z_owned_session_t s;
+
+	px4_guid_t px4_guid;
+
 };
 
 #endif //ZENOH_MODULE_H

src/modules/zenoh/zenoh_config.hpp

@@ -62,7 +62,7 @@
 #define KEYEXPR_MSG_NAME_SIZE sizeof(KEYEXPR_MSG_NAME)
 #define TOPIC_INFO_SIZE (64)
 #define MAX_LINE_SIZE (2 * TOPIC_INFO_SIZE)
-#define KEYEXPR_SIZE (MAX_LINE_SIZE + KEYEXPR_MSG_NAME_SIZE + KEYEXPR_RIHS01_SIZE)
+#define KEYEXPR_SIZE (MAX_LINE_SIZE + KEYEXPR_MSG_NAME_SIZE + KEYEXPR_RIHS01_SIZE + 128)
 
 class Zenoh_Config
 {