fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 05:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

zenoh: pubsub factory fix datatype naming convention

Browse Source
This commit is contained in:
Peter van der Perk
2025-08-22 08:22:59 +02:00
committed by Beat Küng
parent 575923b534
commit 5211d9c92e
3 changed files with 91 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/zenoh/zenoh.cpp
+41 -55
View File
@@ -80,6 +80,7 @@ void toCamelCase(char *input)
input[j] = '\0'; // Null-terminate the input string
}
ZENOH::ZENOH():
ModuleParams(nullptr)
{
@@ -91,6 +92,36 @@ ZENOH::~ZENOH()
}
int ZENOH::generate_rmw_zenoh_keyexpr(const char *topic, char *type, char *keyexpr)
{
const uint8_t *rihs_hash = getRIHS01_Hash(type);
const char *type_name = getTypeName(type);
if (rihs_hash && type_name) {
strcpy(type, type_name);
toCamelCase(type); // Convert uORB type to camel case
return snprintf(keyexpr, KEYEXPR_SIZE, "%" PRId32 "/%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",
_zenoh_domain_id.get(), topic, type,
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]
);
}
return -1;
}
void ZENOH::run()
{
char mode[NET_MODE_SIZE];
@@ -160,37 +191,15 @@ void ZENOH::run()
z_config.getSubscriberMapping(topic, type);
_zenoh_subscribers[i] = genSubscriber(type);
if (_zenoh_subscribers[i] != 0) {
const uint8_t *rihs_hash = getRIHS01_Hash(type);
const char *type_name = genTypeName(type, rihs_hash);
if (type_name) {
strcpy(type, type_name);
toCamelCase(type); // Convert uORB type to camel case
snprintf(keyexpr, KEYEXPR_SIZE, "%" PRId32 "/%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",
_zenoh_domain_id.get(), topic, type,
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]
);
_zenoh_subscribers[i]->declare_subscriber(s, keyexpr);
} else {
PX4_ERR("Could not create a subscriber for type %s", type);
}
if (_zenoh_subscribers[i] != 0 &&
generate_rmw_zenoh_keyexpr(topic, type, keyexpr) > 0) {
_zenoh_subscribers[i]->declare_subscriber(s, keyexpr);
} else {
PX4_ERR("Could not create a subscriber for type %s", type);
}
if (z_config.getSubscriberMapping(topic, type) < 0) {
PX4_WARN("Subscriber mapping parsing error");
}
@@ -214,33 +223,10 @@ void ZENOH::run()
z_config.getPublisherMapping(topic, type);
_zenoh_publishers[i] = genPublisher(type);
if (_zenoh_publishers[i] != 0) {
const uint8_t *rihs_hash = getRIHS01_Hash(type);
const char *type_name = genTypeName(type, rihs_hash);
if (type_name) {
strcpy(type, type_name);
toCamelCase(type); // Convert uORB type to camel case
snprintf(keyexpr, KEYEXPR_SIZE, "%" PRId32 "/%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",
_zenoh_domain_id.get(), topic, type,
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]
);
_zenoh_publishers[i]->declare_publisher(s, keyexpr, (uint8_t *)&px4_guid);
_zenoh_publishers[i]->setPollFD(&pfds[i]);
}
if (_zenoh_publishers[i] != 0 &&
generate_rmw_zenoh_keyexpr(topic, type, keyexpr) > 0) {
_zenoh_publishers[i]->declare_publisher(s, keyexpr, (uint8_t *)&px4_guid);
_zenoh_publishers[i]->setPollFD(&pfds[i]);
} else {
PX4_ERR("Could not create a publisher for type %s", type);
src/modules/zenoh/zenoh.h
+2
View File
@@ -86,6 +86,8 @@ private:
(ParamInt<px4::params::ZENOH_DOMAIN_ID>) _zenoh_domain_id
)
int generate_rmw_zenoh_keyexpr(const char *topic, char *type, char *keyexpr);
Zenoh_Config _config;
int _pub_count;