diff --git a/Tools/zenoh/templates/zenoh/uorb_pubsub_factory.hpp.em b/Tools/zenoh/templates/zenoh/uorb_pubsub_factory.hpp.em index c6808cecbc..678285ea97 100644 --- a/Tools/zenoh/templates/zenoh/uorb_pubsub_factory.hpp.em +++ b/Tools/zenoh/templates/zenoh/uorb_pubsub_factory.hpp.em @@ -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; diff --git a/src/modules/zenoh/zenoh.cpp b/src/modules/zenoh/zenoh.cpp index 0b30a5a7ba..63e0bc6d95 100644 --- a/src/modules/zenoh/zenoh.cpp +++ b/src/modules/zenoh/zenoh.cpp @@ -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); diff --git a/src/modules/zenoh/zenoh.h b/src/modules/zenoh/zenoh.h index fc6fc64578..1e400ca87d 100644 --- a/src/modules/zenoh/zenoh.h +++ b/src/modules/zenoh/zenoh.h @@ -86,6 +86,8 @@ private: (ParamInt) _zenoh_domain_id ) + int generate_rmw_zenoh_keyexpr(const char *topic, char *type, char *keyexpr); + Zenoh_Config _config; int _pub_count;