diff --git a/src/modules/zenoh/publishers/zenoh_publisher.cpp b/src/modules/zenoh/publishers/zenoh_publisher.cpp index 9ca45f573d..f67e81a93a 100644 --- a/src/modules/zenoh/publishers/zenoh_publisher.cpp +++ b/src/modules/zenoh/publishers/zenoh_publisher.cpp @@ -57,10 +57,12 @@ int Zenoh_Publisher::undeclare_publisher() return 0; } -int Zenoh_Publisher::declare_publisher(z_owned_session_t s, const char *keyexpr) +int Zenoh_Publisher::declare_publisher(z_owned_session_t s, const char *keyexpr, uint8_t *gid) { z_view_keyexpr_t ke; + this->rmw_gid = gid; + if (z_view_keyexpr_from_str(&ke, keyexpr) < 0) { printf("%s is not a valid key expression\n", keyexpr); return -1; @@ -93,11 +95,8 @@ int8_t Zenoh_Publisher::publish(const uint8_t *buf, int size) ze_serializer_serialize_str(z_loan_mut(serializer), "source_timestamp"); ze_serializer_serialize_int64(z_loan_mut(serializer), hrt_absolute_time()); - px4_guid_t px4_guid; - board_get_px4_guid(px4_guid); - ze_serializer_serialize_str(z_loan_mut(serializer), "source_gid"); - ze_serializer_serialize_buf(z_loan_mut(serializer), px4_guid, 16); + ze_serializer_serialize_buf(z_loan_mut(serializer), rmw_gid, 16); ze_serializer_finish(z_move(serializer), &attachment); options.attachment = z_move(attachment); diff --git a/src/modules/zenoh/publishers/zenoh_publisher.hpp b/src/modules/zenoh/publishers/zenoh_publisher.hpp index 920a623e09..bd76c9a0be 100644 --- a/src/modules/zenoh/publishers/zenoh_publisher.hpp +++ b/src/modules/zenoh/publishers/zenoh_publisher.hpp @@ -54,7 +54,7 @@ public: Zenoh_Publisher(); virtual ~Zenoh_Publisher(); - virtual int declare_publisher(z_owned_session_t s, const char *keyexpr); + virtual int declare_publisher(z_owned_session_t s, const char *keyexpr, uint8_t *gid); virtual int undeclare_publisher(); @@ -68,4 +68,7 @@ protected: z_owned_publisher_t _pub; int64_t sequence_number; z_timestamp_t ts; + + /* 16 bytes DDS-Like GID */ + uint8_t *rmw_gid; }; diff --git a/src/modules/zenoh/subscribers/uorb_subscriber.hpp b/src/modules/zenoh/subscribers/uorb_subscriber.hpp index bd59adb846..3df9868324 100644 --- a/src/modules/zenoh/subscribers/uorb_subscriber.hpp +++ b/src/modules/zenoh/subscribers/uorb_subscriber.hpp @@ -65,6 +65,8 @@ public: { char data[_uorb_meta->o_size]; + // TODO process rmw_zenoh attachment + const z_loaned_bytes_t *payload = z_sample_payload(sample); size_t len = z_bytes_len(payload); diff --git a/src/modules/zenoh/subscribers/zenoh_subscriber.cpp b/src/modules/zenoh/subscribers/zenoh_subscriber.cpp index 56001f8421..7f6f71c71a 100644 --- a/src/modules/zenoh/subscribers/zenoh_subscriber.cpp +++ b/src/modules/zenoh/subscribers/zenoh_subscriber.cpp @@ -54,7 +54,6 @@ void Zenoh_Subscriber::data_handler(const z_loaned_sample_t *sample) (int)z_bytes_len(z_sample_payload(sample))); } - Zenoh_Subscriber::Zenoh_Subscriber() { } diff --git a/src/modules/zenoh/subscribers/zenoh_subscriber.hpp b/src/modules/zenoh/subscribers/zenoh_subscriber.hpp index 5359fec291..66d0daafdb 100644 --- a/src/modules/zenoh/subscribers/zenoh_subscriber.hpp +++ b/src/modules/zenoh/subscribers/zenoh_subscriber.hpp @@ -67,7 +67,7 @@ public: virtual void print(); protected: - virtual void print(const char *type_string, const char *topic_string); + virtual void print(const char *type_string, const char *topic_string); z_owned_subscriber_t _sub; }; diff --git a/src/modules/zenoh/zenoh.cpp b/src/modules/zenoh/zenoh.cpp index 38dc9529cb..ea93a27ecc 100644 --- a/src/modules/zenoh/zenoh.cpp +++ b/src/modules/zenoh/zenoh.cpp @@ -52,9 +52,6 @@ // Auto-generated header to all uORB <-> CDR conversions #include -// FIXME make this a parameter -const uint8_t ros_domain_id = 0; - #define Z_PUBLISH #define Z_SUBSCRIBE @@ -101,6 +98,9 @@ void ZENOH::run() int8_t ret; int i; + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + Zenoh_Config z_config; z_config.getNetworkConfig(mode, locator); @@ -149,13 +149,13 @@ void ZENOH::run() if (_zenoh_subscribers[i] != 0) { const uint8_t *rihs_hash = getRIHS01_Hash(type); toCamelCase(type); // Convert uORB type to camel case - snprintf(keyexpr, KEYEXPR_SIZE, "%i/%s/" + snprintf(keyexpr, KEYEXPR_SIZE, "%li/%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", - ros_domain_id, topic, type, + _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], @@ -194,13 +194,13 @@ void ZENOH::run() if (_zenoh_publishers[i] != 0) { const uint8_t *rihs_hash = getRIHS01_Hash(type); toCamelCase(type); // Convert uORB type to camel case - snprintf(keyexpr, KEYEXPR_SIZE, "%i/%s/" + snprintf(keyexpr, KEYEXPR_SIZE, "%li/%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", - ros_domain_id, topic, type, + _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], @@ -210,7 +210,7 @@ void ZENOH::run() 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); + _zenoh_publishers[i]->declare_publisher(s, keyexpr, (uint8_t *)&px4_guid); _zenoh_publishers[i]->setPollFD(&pfds[i]); } } diff --git a/src/modules/zenoh/zenoh.h b/src/modules/zenoh/zenoh.h index a7cb0465a9..fc6fc64578 100644 --- a/src/modules/zenoh/zenoh.h +++ b/src/modules/zenoh/zenoh.h @@ -82,6 +82,9 @@ public: void run() override; private: + DEFINE_PARAMETERS( + (ParamInt) _zenoh_domain_id + ) Zenoh_Config _config; diff --git a/src/modules/zenoh/zenoh_params.c b/src/modules/zenoh/zenoh_params.c new file mode 100644 index 0000000000..1eddfb63f8 --- /dev/null +++ b/src/modules/zenoh/zenoh_params.c @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file zenoh_params.c + * + * Parameters defined by Zenoh + * + * @author Peter van der Perk + */ + +/** + * ROS2 RMW_ZENOH_CPP Domain id + * + * @min 0 + * @max 232 + */ +PARAM_DEFINE_INT32(ZENOH_DOMAIN_ID, 0);