Implement instance selection & pub/sub deletion for Zenoh

src/modules/zenoh/default_topics.c.em

@@ -18,15 +18,15 @@ import os
 
 const char* default_pub_config =
 @[    for pub in publications]@
-	"@(pub['topic']);@(pub['simple_base_type'])\n"
+	"@(pub['topic']);@(pub['simple_base_type']);0\n"
 @[    end for]@
 ;
 
 const char* default_sub_config =
 @[    for sub in subscriptions]@
-	"@(sub['topic']);@(sub['simple_base_type'])\n"
+	"@(sub['topic']);@(sub['simple_base_type']);0\n"
 @[    end for]@
 @[    for sub in subscriptions_multi]@
-	"@(sub['topic']);@(sub['simple_base_type'])\n"
+	"@(sub['topic']);@(sub['simple_base_type']);-1\n"
 @[    end for]@
 ;

src/modules/zenoh/publishers/uorb_publisher.hpp

@@ -50,12 +50,17 @@
 class uORB_Zenoh_Publisher : public Zenoh_Publisher
 {
 public:
-	uORB_Zenoh_Publisher(const orb_metadata *meta, const uint32_t *ops) :
+	uORB_Zenoh_Publisher(const orb_metadata *meta, const uint32_t *ops, int instance) :
 		Zenoh_Publisher(),
 		_uorb_meta{meta},
 		_cdr_ops(ops)
 	{
+		if (instance <= 0) { // default (<0) or =0
 			_uorb_sub = orb_subscribe(meta);
+
+		} else { // otherwise
+			_uorb_sub = orb_subscribe_multi(meta, instance);
+		}
 	};
 
 	~uORB_Zenoh_Publisher() override = default;

src/modules/zenoh/subscribers/uorb_subscriber.hpp

@@ -49,13 +49,19 @@
 class uORB_Zenoh_Subscriber : public Zenoh_Subscriber
 {
 public:
-	uORB_Zenoh_Subscriber(const orb_metadata *meta, const uint32_t *ops) :
+	// d_instance: < (default if not in CSV) if we should create a new instance (safe), nonzero if we should use the 0 instance
+	uORB_Zenoh_Subscriber(const orb_metadata *meta, const uint32_t *ops, int d_instance) :
 		Zenoh_Subscriber(),
 		_uorb_meta{meta},
 		_cdr_ops(ops)
 	{
-		int instance = 0;
+		if (d_instance < 0) { // default=-1; allocate a new instance
+			int instance;
 			_uorb_pub_handle = orb_advertise_multi(_uorb_meta, nullptr, &instance);
+
+		} else {
+			_uorb_pub_handle = orb_advertise(_uorb_meta, nullptr);
+		}
 	};
 
 	~uORB_Zenoh_Subscriber()

src/modules/zenoh/zenoh.cpp

@@ -273,10 +273,11 @@ int ZENOH::setupTopics(px4_pollfd_struct_t *pfds)
 	{
 		char topic[TOPIC_INFO_SIZE];
 		char type[TOPIC_INFO_SIZE];
+		int instance_no;
 
 		for (i = 0; i < _sub_count; i++) {
-			if (_config.getSubscriberMapping(topic, type)) {
+			if (_config.getSubscriberMapping(topic, type, &instance_no)) {
-				_zenoh_subscribers[i] = genSubscriber(type);
+				_zenoh_subscribers[i] = genSubscriber(type, instance_no);
 				const uint8_t *rihs_hash = getRIHS01_Hash(type);
 
 				if (rihs_hash != NULL && _zenoh_subscribers[i] != 0 &&
@@ -312,7 +313,7 @@ int ZENOH::setupTopics(px4_pollfd_struct_t *pfds)
 			}
 		}
 
-		if (_config.getSubscriberMapping(topic, type) < 0) {
+		if (_config.getSubscriberMapping(topic, type, &instance_no) < 0) {
 			PX4_WARN("Subscriber mapping parsing error");
 		}
 
@@ -327,10 +328,11 @@ int ZENOH::setupTopics(px4_pollfd_struct_t *pfds)
 	{
 		char topic[TOPIC_INFO_SIZE];
 		char type[TOPIC_INFO_SIZE];
+		int instance;
 
 		for (i = 0; i < _pub_count; i++) {
-			if (_config.getPublisherMapping(topic, type)) {
+			if (_config.getPublisherMapping(topic, type, &instance)) {
-				_zenoh_publishers[i] = genPublisher(type);
+				_zenoh_publishers[i] = genPublisher(type, instance);
 				const uint8_t *rihs_hash = getRIHS01_Hash(type);
 
 				if (rihs_hash && _zenoh_publishers[i] != 0 &&
@@ -367,7 +369,7 @@ int ZENOH::setupTopics(px4_pollfd_struct_t *pfds)
 			}
 		}
 
-		if (_config.getSubscriberMapping(topic, type) < 0) {
+		if (_config.getPublisherMapping(topic, type, &instance) < 0) {
 			PX4_WARN("Publisher mapping parsing error");
 		}
 
@@ -487,8 +489,10 @@ Zenoh demo bridge
 	PRINT_MODULE_USAGE_COMMAND("stop");
 	PRINT_MODULE_USAGE_COMMAND("status");
 	PRINT_MODULE_USAGE_COMMAND("config");
-	PX4_INFO_RAW("     addpublisher  <zenoh_topic> <uorb_topic>  Publish uORB topic to Zenoh\n");
+	PX4_INFO_RAW("     add publisher  <zenoh_topic> <uorb_topic> <optional uorb_instance>  Publish uORB topic to Zenoh\n");
-	PX4_INFO_RAW("     addsubscriber <zenoh_topic> <uorb_topic>  Publish Zenoh topic to uORB\n");
+	PX4_INFO_RAW("     add subscriber <zenoh_topic> <uorb_topic> <optional uorb_instance>  Publish Zenoh topic to uORB\n");
+	PX4_INFO_RAW("     delete publisher  <zenoh_topic>\n");
+	PX4_INFO_RAW("     delete subscriber <zenoh_topic>\n");
 	PX4_INFO_RAW("     net           <mode> <locator>            Zenoh network mode\n");
 	PX4_INFO_RAW("          <mode>    values: client|peer   \n");
 	PX4_INFO_RAW("          <locator> client: locator address e.g. tcp/10.41.10.1:7447#iface=eth0\n");

src/modules/zenoh/zenoh_config.cpp

@@ -46,6 +46,7 @@
 #include <errno.h>
 #include <string.h>
 #include <stdlib.h>
+#include <ctype.h>
 
 #include <uORB/topics/uORBTopics.hpp>
 
@@ -94,13 +95,14 @@ Zenoh_Config::~Zenoh_Config()
 	}
 }
 
-int Zenoh_Config::AddPubSub(char *topic, char *datatype, const char *filename)
+int Zenoh_Config::AddPubSub(char *topic, char *datatype, int instance_no, const char *filename)
 {
 	{
 		char f_topic[TOPIC_INFO_SIZE];
 		char f_type[TOPIC_INFO_SIZE];
+		int f_new_instance;
 
-		while (getPubSubMapping(f_topic, f_type, filename) > 0) {
+		while (getPubSubMapping(f_topic, f_type, &f_new_instance, filename) > 0) {
 			if (strcmp(topic, f_topic) == 0
 			    || strcmp(datatype, f_type) == 0) {
 				printf("Already mapped to uORB %s -> %s\n", f_type, f_topic);
@@ -118,7 +120,7 @@ int Zenoh_Config::AddPubSub(char *topic, char *datatype, const char *filename)
 			FILE *fp = fopen(filename, "a");
 
 			if (fp) {
-				fprintf(fp, "%s;%s\n", topic, datatype);
+				fprintf(fp, "%s;%s;%d\n", topic, datatype, instance_no);
 
 			} else {
 				return -1;
@@ -134,6 +136,79 @@ int Zenoh_Config::AddPubSub(char *topic, char *datatype, const char *filename)
 }
 
 
+int Zenoh_Config::DeletePubSub(char *topic, const char *filename)
+{
+	if (!filename || !topic) {
+		return -1;
+	}
+
+	FILE *file = fopen(filename, "r");
+
+	if (!file) {
+		return -1;
+	}
+
+	// Create a temporary file for writing
+	char temp_filename[256];
+	snprintf(temp_filename, sizeof(temp_filename), "%s.tmp", filename);
+	FILE *temp_file = fopen(temp_filename, "w");
+
+	if (!temp_file) {
+		fclose(file);
+		return -1;
+	}
+
+	char line[TOPIC_INFO_SIZE];
+	char line_copy[TOPIC_INFO_SIZE];
+	const char *fields[1];  // We only need the topic
+	int found = 0;
+
+	while (fgets(line, sizeof(line), file)) {
+		// Remove newline if present
+		size_t len = strlen(line);
+
+		if (len > 0 && line[len - 1] == '\n') {
+			line[len - 1] = '\0';
+		}
+
+		// Make a copy of the line before parsing since parse_csv_line will replace ;s with \0s
+		strncpy(line_copy, line, sizeof(line_copy) - 1);
+		line_copy[sizeof(line_copy) - 1] = '\0';
+		int num_fields = parse_csv_line(line_copy, fields, 1);
+
+		// If the topic doesn't match the topic, write the line to temp file
+		if (num_fields == 0 || strcmp(fields[0], topic) != 0) {
+			fprintf(temp_file, "%s\n", line);
+
+		} else {
+			found = 1;
+		}
+	}
+
+	fclose(file);
+	fclose(temp_file);
+
+	// replace the original file if deletion was successful
+	if (found) {
+		if (remove(filename) != 0) {
+			remove(temp_filename);
+			return -1;
+		}
+
+		if (rename(temp_filename, filename) != 0) {
+			remove(temp_filename);
+			return -1;
+		}
+
+	} else {
+		// Otherwise, if no line was deleted, remove the temp file
+		remove(temp_filename);
+	}
+
+	return 0;
+}
+
+
 int Zenoh_Config::SetNetworkConfig(char *mode, char *locator)
 {
 
@@ -166,24 +241,63 @@ int Zenoh_Config::cli(int argc, char *argv[])
 		}
 
 	} else if (argc == 4) {
-		if (strcmp(argv[1], "addpublisher") == 0) {
+		if (strcmp(argv[1], "net") == 0) {
-			if (AddPubSub(argv[2], argv[3], ZENOH_PUB_CONFIG_PATH) > 0) {
-				printf("Added %s %s to publishers\n", argv[2], argv[3]);
-
-			} else {
-				printf("Could not add uORB %s -> %s to publishers\n",  argv[3], argv[2]);
-			}
-
-		} else if (strcmp(argv[1], "addsubscriber") == 0) {
-			if (AddPubSub(argv[2], argv[3], ZENOH_SUB_CONFIG_PATH) > 0) {
-				printf("Added %s -> uORB %s to subscribers\n", argv[2], argv[3]);
-
-			} else {
-				printf("Could not add %s -> uORB %s to subscribers\n",  argv[2], argv[3]);
-			}
-
-		} else if (strcmp(argv[1], "net") == 0) {
 			SetNetworkConfig(argv[2], argv[3]);
+
+		} else if (strcmp(argv[1], "delete") == 0) {
+			if (strcmp(argv[2], "publisher") == 0) {
+				int res = DeletePubSub(argv[3], ZENOH_PUB_CONFIG_PATH);
+
+				if (res < 0) {
+					printf("Could not delete publisher topic %s\n", argv[3]);
+				}
+
+			} else if (strcmp(argv[2], "subscriber") == 0) {
+				int res = DeletePubSub(argv[3], ZENOH_SUB_CONFIG_PATH);
+
+				if (res < 0) {
+					printf("Could not delete subscriber topic %s\n", argv[3]);
+				}
+			}
+		}
+
+	} else if (argc >= 5) {
+		if (strcmp(argv[1], "add") == 0) {
+			if (strcmp(argv[2], "publisher") == 0) {
+				int instance = 0;
+
+				if (argc == 6) {
+					if (sscanf(argv[5], "%d", &instance) != 1 || instance < 0) {
+						printf("Invalid instance %s (must be an integer, 0 for the default instance or a specific instance's index)\n",
+						       argv[5]);
+						return 0;
+					}
+				}
+
+				if (AddPubSub(argv[3], argv[4], instance, ZENOH_PUB_CONFIG_PATH) > 0) {
+					printf("Added %s %s to publishers (instance %d)\n", argv[3], argv[4], instance);
+
+				} else {
+					printf("Could not add uORB %s:%d -> %s to publishers\n",  argv[3], instance, argv[4]);
+				}
+
+			} else if (strcmp(argv[2], "subscriber") == 0) {
+				int instance = 0;
+
+				if (argc == 6) {
+					if (sscanf(argv[5], "%d", &instance) != 1 || instance == 0 || instance == -1) {
+						printf("Invalid instance %s (must be an integer, 0 for the default instance or -1 for a new uOrb instance)\n", argv[5]);
+						return 0;
+					}
+				}
+
+				if (AddPubSub(argv[3], argv[4], instance, ZENOH_SUB_CONFIG_PATH) > 0) {
+					printf("Added %s -> uORB %s:%d to subscribers\n", argv[3], argv[4], instance);
+
+				} else {
+					printf("Could not add %s -> uORB %s to subscribers\n",  argv[3], argv[4]);
+				}
+			}
 		}
 	}
 
@@ -191,22 +305,34 @@ int Zenoh_Config::cli(int argc, char *argv[])
 	return 0;
 }
 
-const char *Zenoh_Config::get_csv_field(char *line, int num)
+int Zenoh_Config::parse_csv_line(char *line, const char **fields, int max_fields)
 {
-	const char *tok;
+	if (!line || !fields || max_fields <= 0) {
-
+		return 0;
-	for (
-		tok = strtok(line, ";");
-		tok && *tok;
-		tok = strtok(NULL, ";\n")) {
-		if (!--num) {
-			return tok;
-		}
 	}
 
-	return NULL;
+	int field_count = 0;
+	char *token = strtok(line, ";");
+
+	while (token && field_count < max_fields) {
+		// Trim leading whitespace
+		while (isspace((unsigned char)*token)) { token++; }
+
+		// Trim trailing whitespace
+		char *end = token + strlen(token) - 1;
+
+		while (end > token && isspace((unsigned char)*end)) {
+			*end = '\0';
+			end--;
 		}
 
+		fields[field_count] = token;
+		field_count++;
+		token = strtok(NULL, ";\n\r");
+	}
+
+	return field_count;
+}
 void Zenoh_Config::getNetworkConfig(char *mode, char *locator)
 {
 	FILE *fp;
@@ -217,8 +343,17 @@ void Zenoh_Config::getNetworkConfig(char *mode, char *locator)
 	// If file opened successfully, then read the file
 	if (fp) {
 		fgets(buffer, NET_CONFIG_LINE_SIZE, fp);
-		const char *config_locator = get_csv_field(buffer, 2);
+		const char *fields[2];
-		char *config_mode = (char *)get_csv_field(buffer, 1);
+		int nfields = parse_csv_line(buffer, fields, 2);
+
+		if (nfields < 2) {
+			PX4_ERR("Invalid Zenoh net config file (must contain the mode and locator separated by a ;).");
+			fclose(fp);
+			return;
+		}
+
+		const char *config_locator = fields[1];
+		char *config_mode = (char *)fields[0];
 
 		if (config_mode) {
 			config_mode[strcspn(config_mode, "\n")] = 0;
@@ -277,7 +412,7 @@ int Zenoh_Config::closePubSubMapping()
 
 
 // Very rudamentary here but we've to wait for a more advanced param system
-int Zenoh_Config::getPubSubMapping(char *topic, char *type, const char *filename)
+int Zenoh_Config::getPubSubMapping(char *topic, char *type, int *instance, const char *filename)
 {
 	char buffer[MAX_LINE_SIZE];
 
@@ -289,12 +424,23 @@ int Zenoh_Config::getPubSubMapping(char *topic, char *type, const char *filename
 		while (fgets(buffer, MAX_LINE_SIZE, fp_mapping) != NULL) {
 
 			if (buffer[0] != '\n') {
-				const char *config_type = get_csv_field(buffer, 2);
+				const char *fields[3];
-				const char *config_topic = get_csv_field(buffer, 1);
+				int nfields = parse_csv_line(buffer, fields, 3);
 
-				if (config_topic && config_type) {
+
-					strncpy(type, config_type, TOPIC_INFO_SIZE);
+				if (nfields >= 2) {
-					strncpy(topic, config_topic, TOPIC_INFO_SIZE);
+					if (nfields == 3) {
+						if (sscanf(fields[2], "%d", instance) != 1) {
+							PX4_WARN("Malformed zenoh config instance %s (instance field should be an integer following the type)\n", fields[2]);
+							return -1;
+						}
+
+					} else {
+						*instance = -1;
+					}
+
+					strncpy(type, fields[1], TOPIC_INFO_SIZE);
+					strncpy(topic, fields[0], TOPIC_INFO_SIZE);
 					return 1;
 
 				} else {
@@ -338,19 +484,22 @@ void Zenoh_Config::dump_config()
 	{
 		char topic[TOPIC_INFO_SIZE];
 		char type[TOPIC_INFO_SIZE];
+		int instance_no;
 
 		printf("Publisher config:\n");
 
-		while (getPubSubMapping(topic, type, ZENOH_PUB_CONFIG_PATH) > 0) {
+		while (getPubSubMapping(topic, type, &instance_no, ZENOH_PUB_CONFIG_PATH) > 0) {
 			printf("Topic: %s\n", topic);
 			printf("Type: %s\n", type);
+			printf("Instance: %d\n", instance_no);
 		}
 
 		printf("\nSubscriber config:\n");
 
-		while (getPubSubMapping(topic, type, ZENOH_SUB_CONFIG_PATH) > 0) {
+		while (getPubSubMapping(topic, type, &instance_no, ZENOH_SUB_CONFIG_PATH) > 0) {
 			printf("Topic: %s\n", topic);
 			printf("Type: %s\n", type);
+			printf("Instance: %d\n", instance_no);
 		}
 	}
 }

src/modules/zenoh/zenoh_config.hpp

@@ -81,24 +81,26 @@ public:
 	{
 		return getLineCount(ZENOH_SUB_CONFIG_PATH);
 	}
-	int getPublisherMapping(char *topic, char *type)
+	int getPublisherMapping(char *topic, char *type, int *instance)
 	{
-		return getPubSubMapping(topic, type, ZENOH_PUB_CONFIG_PATH);
+		return getPubSubMapping(topic, type, instance, ZENOH_PUB_CONFIG_PATH);
 	}
-	int getSubscriberMapping(char *topic, char *type)
+	// existing_instance will be either 0 (should create a new instance) or nonzero (should reuse the existing 0 instance)
+	int getSubscriberMapping(char *topic, char *type, int *existing_instance)
 	{
-		return getPubSubMapping(topic, type, ZENOH_SUB_CONFIG_PATH);
+		return getPubSubMapping(topic, type, existing_instance, ZENOH_SUB_CONFIG_PATH);
 	}
 	int closePubSubMapping();
 
 
 private:
-	int getPubSubMapping(char *topic, char *type, const char *filename);
+	int getPubSubMapping(char *topic, char *type, int *new_instance, const char *filename);
-	int AddPubSub(char *topic, char *datatype, const char *filename);
+	int AddPubSub(char *topic, char *datatype, int new_instance, const char *filename);
+	int DeletePubSub(char *topic, const char *filename);
 	int SetNetworkConfig(char *mode, char *locator);
 	int getLineCount(const char *filename);
 
-	const char *get_csv_field(char *line, int num);
+	int parse_csv_line(char *line, const char **fields, int max_fields);
 	void generate_clean_config();
 	void dump_config();