diff --git a/src/modules/zenoh/default_topics.c.em b/src/modules/zenoh/default_topics.c.em index 0845c004e9..9979ede692 100644 --- a/src/modules/zenoh/default_topics.c.em +++ b/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]@ ; diff --git a/src/modules/zenoh/publishers/uorb_publisher.hpp b/src/modules/zenoh/publishers/uorb_publisher.hpp index c924d7aef5..6a90d435d5 100644 --- a/src/modules/zenoh/publishers/uorb_publisher.hpp +++ b/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) { - _uorb_sub = orb_subscribe(meta); + 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; diff --git a/src/modules/zenoh/subscribers/uorb_subscriber.hpp b/src/modules/zenoh/subscribers/uorb_subscriber.hpp index b9ba7f1c06..823a455776 100644 --- a/src/modules/zenoh/subscribers/uorb_subscriber.hpp +++ b/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; - _uorb_pub_handle = orb_advertise_multi(_uorb_meta, nullptr, &instance); + 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() diff --git a/src/modules/zenoh/zenoh.cpp b/src/modules/zenoh/zenoh.cpp index 318b770bd4..a2433d2341 100644 --- a/src/modules/zenoh/zenoh.cpp +++ b/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)) { - _zenoh_subscribers[i] = genSubscriber(type); + if (_config.getSubscriberMapping(topic, type, &instance_no)) { + _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)) { - _zenoh_publishers[i] = genPublisher(type); + if (_config.getPublisherMapping(topic, type, &instance)) { + _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 Publish uORB topic to Zenoh\n"); - PX4_INFO_RAW(" addsubscriber Publish Zenoh topic to uORB\n"); + PX4_INFO_RAW(" add publisher Publish uORB topic to Zenoh\n"); + PX4_INFO_RAW(" add subscriber Publish Zenoh topic to uORB\n"); + PX4_INFO_RAW(" delete publisher \n"); + PX4_INFO_RAW(" delete subscriber \n"); PX4_INFO_RAW(" net Zenoh network mode\n"); PX4_INFO_RAW(" values: client|peer \n"); PX4_INFO_RAW(" client: locator address e.g. tcp/10.41.10.1:7447#iface=eth0\n"); diff --git a/src/modules/zenoh/zenoh_config.cpp b/src/modules/zenoh/zenoh_config.cpp index 81daca2057..c2cacf54a8 100644 --- a/src/modules/zenoh/zenoh_config.cpp +++ b/src/modules/zenoh/zenoh_config.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include @@ -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 (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) { + 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; - - for ( - tok = strtok(line, ";"); - tok && *tok; - tok = strtok(NULL, ";\n")) { - if (!--num) { - return tok; - } + if (!line || !fields || max_fields <= 0) { + return 0; } - 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); - char *config_mode = (char *)get_csv_field(buffer, 1); + const char *fields[2]; + 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 *config_topic = get_csv_field(buffer, 1); + const char *fields[3]; + int nfields = parse_csv_line(buffer, fields, 3); - if (config_topic && config_type) { - strncpy(type, config_type, TOPIC_INFO_SIZE); - strncpy(topic, config_topic, TOPIC_INFO_SIZE); + + if (nfields >= 2) { + 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); } } } diff --git a/src/modules/zenoh/zenoh_config.hpp b/src/modules/zenoh/zenoh_config.hpp index 444275834b..e92abdb1c7 100644 --- a/src/modules/zenoh/zenoh_config.hpp +++ b/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 AddPubSub(char *topic, char *datatype, const char *filename); + int getPubSubMapping(char *topic, char *type, int *new_instance, 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();