Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 20:03:54 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

zenoh: pubsub factory fix datatype naming convention

Browse Source
This commit is contained in:
Peter van der Perk
2025-03-27 21:29:00 +01: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
Tools/zenoh/templates/zenoh/uorb_pubsub_factory.hpp.em
+48 -28
View File
@@ -92,10 +92,24 @@ type_topic_count = len(datatypes_with_topics[topic_name])
const uint8_t @(topic_name)_hash[32] = @(rihs01_hash)
@[end for]
@[for idx, topic_name in enumerate(datatypes)]@
#ifdef CONFIG_ZENOH_PUBSUB_@(topic_name.upper())
@{
topic_names = datatypes_with_topics[topic_name]
}@
const orb_metadata* @(topic_name)_topic_meta[@(len(topic_names))] = {
@[for topic_name_inst in topic_names]@
ORB_ID(@(topic_name_inst)),
@[end for]};
#endif
@[end for]
typedef struct {
const char *data_type_name;
const uint32_t *ops;
const orb_metadata* orb_meta;
const uint8_t *hash;
const orb_metadata** orb_topic;
const uint8_t orb_topics_size;
} UorbPubSubTopicBinder;
const UorbPubSubTopicBinder _topics[ZENOH_PUBSUB_COUNT] {
@@ -107,23 +121,23 @@ uorb_id_idx = 0
@{
topic_names = datatypes_with_topics[topic_name]
}@
@[for topic_name_inst in topic_names]@
{
"@(topic_name)",
px4_msgs_msg_@(topic_dict[topic_name])_cdrstream_desc.ops.ops,
ORB_ID(@(topic_name_inst)),
@(topic_dict[topic_name])_hash
@(topic_dict[topic_name])_hash,
@(topic_name)_topic_meta,
@(len(topic_names)),
},
@{
uorb_id_idx += 1
}@
@[end for]#endif
#endif
@[end for]
};
uORB_Zenoh_Publisher* genPublisher(const orb_metadata *meta) {
for (auto &pub : _topics) {
if(pub.orb_meta->o_id == meta->o_id) {
return new uORB_Zenoh_Publisher(meta, pub.ops);
for(int i = 0; i < pub.orb_topics_size; i++) {
if(pub.orb_topic[i]->o_id == meta->o_id) {
return new uORB_Zenoh_Publisher(meta, pub.ops);
}
}
}
return NULL;
@@ -132,8 +146,10 @@ uORB_Zenoh_Publisher* genPublisher(const orb_metadata *meta) {
uORB_Zenoh_Publisher* genPublisher(const char *name) {
for (auto &pub : _topics) {
if(strcmp(pub.orb_meta->o_name, name) == 0) {
return new uORB_Zenoh_Publisher(pub.orb_meta, pub.ops);
for(int i = 0; i < pub.orb_topics_size; i++) {
if(strcmp(pub.orb_topic[i]->o_name, name) == 0) {
return new uORB_Zenoh_Publisher(pub.orb_topic[i], pub.ops);
}
}
}
return NULL;
@@ -142,8 +158,10 @@ uORB_Zenoh_Publisher* genPublisher(const char *name) {
Zenoh_Subscriber* genSubscriber(const orb_metadata *meta) {
for (auto &sub : _topics) {
if(sub.orb_meta->o_id == meta->o_id) {
return new uORB_Zenoh_Subscriber(meta, sub.ops);
for(int i = 0; i < sub.orb_topics_size; i++) {
if(sub.orb_topic[i]->o_id == meta->o_id) {
return new uORB_Zenoh_Subscriber(meta, sub.ops);
}
}
}
return NULL;
@@ -152,22 +170,20 @@ Zenoh_Subscriber* genSubscriber(const orb_metadata *meta) {
Zenoh_Subscriber* genSubscriber(const char *name) {
for (auto &sub : _topics) {
if(strcmp(sub.orb_meta->o_name, name) == 0) {
return new uORB_Zenoh_Subscriber(sub.orb_meta, sub.ops);
for(int i = 0; i < sub.orb_topics_size; i++) {
if(strcmp(sub.orb_topic[i]->o_name, name) == 0) {
return new uORB_Zenoh_Subscriber(sub.orb_topic[i], sub.ops);
}
}
}
return NULL;
}
const char* genTypeName(const char *name, const uint8_t* hash) {
//FIXME this isn't fully clean because we've got topics that never matches datatype name
// i.e. ESTIMATOR_AID_SOURCE2D however storing all strings in flash isn't ideal either
const char *pch;
const char* getTypeName(const char *name) {
for (auto &sub : _topics) {
pch = strstr(name, sub.orb_meta->o_name);
if(pch) {
if(hash == sub.hash) {
return sub.orb_meta->o_name;
for(int i = 0; i < sub.orb_topics_size; i++) {
if(strcmp(sub.orb_topic[i]->o_name, name) == 0) {
return sub.data_type_name;
}
}
}
@@ -177,8 +193,10 @@ const char* genTypeName(const char *name, const uint8_t* hash) {
const uint8_t* getRIHS01_Hash(const orb_metadata *meta) {
for (auto &sub : _topics) {
if(sub.orb_meta->o_id == meta->o_id) {
return sub.hash;
for(int i = 0; i < sub.orb_topics_size; i++) {
if(sub.orb_topic[i]->o_id == meta->o_id) {
return sub.hash;
}
}
}
return NULL;
@@ -186,8 +204,10 @@ const uint8_t* getRIHS01_Hash(const orb_metadata *meta) {
const uint8_t* getRIHS01_Hash(const char *name) {
for (auto &sub : _topics) {
if(strcmp(sub.orb_meta->o_name, name) == 0) {
return sub.hash;
for(int i = 0; i < sub.orb_topics_size; i++) {
if(strcmp(sub.orb_topic[i]->o_name, name) == 0) {
return sub.hash;
}
}
}
return NULL;
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;