fix(zenoh): Zenoh various minor fixes and improvements regarding failed setup (#27147)

* fix(zenoh) : zenoh_subscirber / zenoh_publisher: print() method checks if key expression exists before printing

* fix(zenoh) : added cleanupSession method, called when setup steps fail and when module is closing

src/modules/zenoh/publishers/zenoh_publisher.cpp

@@ -109,7 +109,14 @@ z_result_t Zenoh_Publisher::publish(const uint8_t *buf, int size)
 
 void Zenoh_Publisher::print()
 {
+	const z_loaned_keyexpr_t *ke = z_publisher_keyexpr(z_loan(_pub));
+
+	if (ke == NULL) {
+		printf("Topic: (unavailable)\n");
+		return;
+	}
+
 	z_view_string_t keystr;
-	z_keyexpr_as_view_string(z_publisher_keyexpr(z_loan(_pub)), &keystr);
+	z_keyexpr_as_view_string(ke, &keystr);
 	printf("Topic: %.*s\n", (int)z_string_len(z_loan(keystr)), z_string_data(z_loan(keystr)));
 }

src/modules/zenoh/subscribers/zenoh_subscriber.cpp

@@ -94,8 +94,15 @@ void Zenoh_Subscriber::print()
 
 void Zenoh_Subscriber::print(const char *type_string, const char *topic_string)
 {
+	const z_loaned_keyexpr_t *ke = z_subscriber_keyexpr(z_loan(_sub));
+
+	if (ke == NULL) {
+		printf("Topic: (unavailable) -> %s %s\n", type_string, topic_string);
+		return;
+	}
+
 	z_view_string_t keystr;
-	z_keyexpr_as_view_string(z_subscriber_keyexpr(z_loan(_sub)), &keystr);
+	z_keyexpr_as_view_string(ke, &keystr);
 	printf("Topic: %.*s -> %s %s \n", (int)z_string_len(z_loan(keystr)), z_string_data(z_loan(keystr)), type_string,
 	       topic_string);
 }

src/modules/zenoh/zenoh.cpp

@@ -254,7 +254,6 @@ int ZENOH::setupSession()
 	// 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));
 		ret = -EINVAL;
 	}
 
@@ -415,6 +414,42 @@ int ZENOH::setupTopics(px4_pollfd_struct_t *pfds)
 	return ret;
 }
 
+void ZENOH::cleanupSession()
+{
+	PX4_INFO("Cleaning up Zenoh session...");
+
+	for (int i = 0; i < _sub_count; i++) {
+		if (_zenoh_subscribers && _zenoh_subscribers[i]) {
+			delete _zenoh_subscribers[i];
+		}
+	}
+
+	if (_zenoh_subscribers) {
+		free(_zenoh_subscribers);
+		_zenoh_subscribers = nullptr;
+	}
+
+	for (int i = 0; i < _pub_count; i++) {
+		if (_zenoh_publishers && _zenoh_publishers[i]) {
+			delete _zenoh_publishers[i];
+		}
+	}
+
+	if (_zenoh_publishers) {
+		free(_zenoh_publishers);
+		_zenoh_publishers = nullptr;
+	}
+
+	if (z_internal_check(_s)) {
+		zp_stop_read_task(z_session_loan_mut(&_s));
+		zp_stop_lease_task(z_session_loan_mut(&_s));
+
+		z_drop(z_session_move(&_s));
+	}
+
+	connected = false;
+}
+
 void ZENOH::run()
 {
 	z_result_t ret;
@@ -425,6 +460,8 @@ void ZENOH::run()
 
 	if (setupSession() < 0) {
 		PX4_ERR("Failed to setup Zenoh session");
+		cleanupSession();
+		exit_and_cleanup(desc);
 		return;
 	}
 
@@ -434,6 +471,8 @@ void ZENOH::run()
 
 	if (setupTopics(pfds) < 0) {
 		PX4_ERR("Failed to setup topics");
+		cleanupSession();
+		exit_and_cleanup(desc);
 		return;
 	}
 
@@ -464,30 +503,7 @@ void ZENOH::run()
 		}
 	}
 
-	// Exiting cleaning up publisher and subscribers
+	cleanupSession();
-	for (i = 0; i < _sub_count; i++) {
-		if (_zenoh_subscribers[i]) {
-			delete _zenoh_subscribers[i];
-		}
-	}
-
-	free(_zenoh_subscribers);
-
-	for (i = 0; i < _pub_count; i++) {
-		if (_zenoh_publishers[i]) {
-			delete _zenoh_publishers[i];
-		}
-	}
-
-	free(_zenoh_publishers);
-
-	// Stop read and lease tasks for zenoh-pico
-	zp_stop_read_task(z_session_loan_mut(&_s));
-	zp_stop_lease_task(z_session_loan_mut(&_s));
-
-	z_drop(z_session_move(&_s));
-
-	connected = false;
 	exit_and_cleanup(desc);
 }
 

src/modules/zenoh/zenoh.h

@@ -97,6 +97,7 @@ private:
 			char *type, char *keyexpr, const char *entity_str);
 	int setupSession();
 	int setupTopics(px4_pollfd_struct_t *pfds);
+	void cleanupSession();
 
 	Zenoh_Config _config;