mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Implement instance selection & pub/sub deletion for Zenoh
This commit is contained in:
Benjamin Chung
Beat Küng
parent
7a98c87fcb
commit
70054fc567
@ -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]@
|
||||
;
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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 <zenoh_topic> <uorb_topic> Publish uORB topic to Zenoh\n");
|
||||
PX4_INFO_RAW(" addsubscriber <zenoh_topic> <uorb_topic> Publish Zenoh topic to uORB\n");
|
||||
PX4_INFO_RAW(" add publisher <zenoh_topic> <uorb_topic> <optional uorb_instance> Publish uORB topic to Zenoh\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");
|
||||
|
||||
@ -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 (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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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();
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user