diff --git a/.gitmodules b/.gitmodules index e443da19a290..a783b1693843 100644 --- a/.gitmodules +++ b/.gitmodules @@ -71,7 +71,7 @@ [submodule "src/modules/zenoh/zenoh-pico"] path = src/modules/zenoh/zenoh-pico url = https://github.com/px4/zenoh-pico - branch = pr-zubf-werror-fix + branch = dev/1.0.0-px4 [submodule "src/lib/heatshrink/heatshrink"] path = src/lib/heatshrink/heatshrink url = https://github.com/PX4/heatshrink.git diff --git a/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig index 333609282e00..ec6b1c7e2b0e 100644 --- a/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig @@ -93,6 +93,7 @@ CONFIG_DEFAULT_SMALL=y CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 +CONFIG_DEV_URANDOM=y CONFIG_ETH0_PHY_LAN8742A=y CONFIG_EXPERIMENTAL=y CONFIG_FAT_DMAMEMORY=y diff --git a/src/modules/zenoh/CMakeLists.txt b/src/modules/zenoh/CMakeLists.txt index 889a1994a427..e3a8166dff73 100644 --- a/src/modules/zenoh/CMakeLists.txt +++ b/src/modules/zenoh/CMakeLists.txt @@ -57,6 +57,7 @@ target_compile_options(zenohpico PUBLIC -Wno-cast-align -DZ_BATCH_SIZE_TX=512 -DZ_FRAG_MAX_SIZE=1024) +target_compile_options(zenohpico PRIVATE -Wno-missing-prototypes) if(CONFIG_PLATFORM_NUTTX) target_compile_options(zenohpico PRIVATE -DUNIX_NO_MULTICAST_IF) endif() @@ -72,7 +73,6 @@ px4_add_module( SRCS zenoh.cpp zenoh_config.cpp - zenoh.h publishers/zenoh_publisher.cpp subscribers/zenoh_subscriber.cpp MODULE_CONFIG @@ -96,6 +96,5 @@ px4_add_module( -Wno-double-promotion -Wno-unused -DZENOH_LINUX - -DZENOH_NO_STDATOMIC -D_Bool=int8_t ) diff --git a/src/modules/zenoh/Kconfig.topics b/src/modules/zenoh/Kconfig.topics index e1c2ea4ada5b..ee9fe7ce7362 100644 --- a/src/modules/zenoh/Kconfig.topics +++ b/src/modules/zenoh/Kconfig.topics @@ -573,6 +573,10 @@ menu "Zenoh publishers/subscribers" bool "rover_ackermann_guidance_status" default n + config ZENOH_PUBSUB_ROVER_ACKERMANN_STATUS + bool "rover_ackermann_status" + default n + config ZENOH_PUBSUB_RPM bool "rpm" default n @@ -1009,6 +1013,7 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_REGISTER_EXT_COMPONENT_REPLY select ZENOH_PUBSUB_REGISTER_EXT_COMPONENT_REQUEST select ZENOH_PUBSUB_ROVER_ACKERMANN_GUIDANCE_STATUS + select ZENOH_PUBSUB_ROVER_ACKERMANN_STATUS select ZENOH_PUBSUB_RPM select ZENOH_PUBSUB_RTL_STATUS select ZENOH_PUBSUB_RTL_TIME_ESTIMATE diff --git a/src/modules/zenoh/publishers/uorb_publisher.hpp b/src/modules/zenoh/publishers/uorb_publisher.hpp index b541bfd1dd7c..fd41e4f2e88c 100644 --- a/src/modules/zenoh/publishers/uorb_publisher.hpp +++ b/src/modules/zenoh/publishers/uorb_publisher.hpp @@ -51,7 +51,7 @@ class uORB_Zenoh_Publisher : public Zenoh_Publisher { public: uORB_Zenoh_Publisher(const orb_metadata *meta, const uint32_t *ops) : - Zenoh_Publisher(true), + Zenoh_Publisher(), _uorb_meta{meta}, _cdr_ops(ops) { diff --git a/src/modules/zenoh/publishers/zenoh_publisher.cpp b/src/modules/zenoh/publishers/zenoh_publisher.cpp index 1e312ffea502..8ce688d2b787 100644 --- a/src/modules/zenoh/publishers/zenoh_publisher.cpp +++ b/src/modules/zenoh/publishers/zenoh_publisher.cpp @@ -42,9 +42,8 @@ #include "zenoh_publisher.hpp" -Zenoh_Publisher::Zenoh_Publisher(bool rostopic) +Zenoh_Publisher::Zenoh_Publisher() { - this->_rostopic = rostopic; this->_topic[0] = 0x0; } @@ -59,24 +58,18 @@ int Zenoh_Publisher::undeclare_publisher() return 0; } -int Zenoh_Publisher::declare_publisher(z_session_t s, const char *keyexpr) +int Zenoh_Publisher::declare_publisher(z_owned_session_t s, const char *keyexpr) { - if (_rostopic) { - strncpy(this->_topic, (char *)_rt_prefix, _rt_prefix_offset); + strncpy(this->_topic, keyexpr, sizeof(this->_topic)); - if (keyexpr[0] == '/') { - strncpy(this->_topic + _rt_prefix_offset, keyexpr + 1, sizeof(this->_topic) - _rt_prefix_offset); + z_view_keyexpr_t ke; + z_view_keyexpr_from_str(&ke, this->_topic); - } else { - strncpy(this->_topic + _rt_prefix_offset, keyexpr, sizeof(this->_topic) - _rt_prefix_offset); - } - - } else { - strncpy(this->_topic, keyexpr, sizeof(this->_topic)); + if (z_declare_publisher(&_pub, z_loan(s), z_loan(ke), NULL) < 0) { + printf("Unable to declare publisher for key expression!\n"); + return -1; } - _pub = z_declare_publisher(s, z_keyexpr(this->_topic), NULL); - if (!z_publisher_check(&_pub)) { printf("Unable to declare publisher for key expression!\n"); return -1; @@ -87,9 +80,13 @@ int Zenoh_Publisher::declare_publisher(z_session_t s, const char *keyexpr) int8_t Zenoh_Publisher::publish(const uint8_t *buf, int size) { - z_publisher_put_options_t options = z_publisher_put_options_default(); - options.encoding = z_encoding(Z_ENCODING_PREFIX_APP_CUSTOM, NULL); - return z_publisher_put(z_publisher_loan(&_pub), buf, size, &options); + z_publisher_put_options_t options; + z_publisher_put_options_default(&options); + options.encoding = NULL; + + z_owned_bytes_t payload; + z_bytes_serialize_from_slice(&payload, buf, size); + return z_publisher_put(z_loan(_pub), z_move(payload), &options); } void Zenoh_Publisher::print() diff --git a/src/modules/zenoh/publishers/zenoh_publisher.hpp b/src/modules/zenoh/publishers/zenoh_publisher.hpp index 1d88a453e2f7..a12849c78682 100644 --- a/src/modules/zenoh/publishers/zenoh_publisher.hpp +++ b/src/modules/zenoh/publishers/zenoh_publisher.hpp @@ -51,10 +51,10 @@ class Zenoh_Publisher : public ListNode { public: - Zenoh_Publisher(bool rostopic = true); + Zenoh_Publisher(); virtual ~Zenoh_Publisher(); - virtual int declare_publisher(z_session_t s, const char *keyexpr); + virtual int declare_publisher(z_owned_session_t s, const char *keyexpr); virtual int undeclare_publisher(); @@ -66,11 +66,5 @@ class Zenoh_Publisher : public ListNode int8_t publish(const uint8_t *, int size); z_owned_publisher_t _pub; - char _topic[60]; // The Topic name is somewhere is the Zenoh stack as well but no good api to fetch it. - - // Indicates ROS2 Topic namespace - bool _rostopic; - const char *_rt_prefix = "rt/"; - const size_t _rt_prefix_offset = 3; // "rt/" are 3 chars }; diff --git a/src/modules/zenoh/subscribers/uorb_subscriber.hpp b/src/modules/zenoh/subscribers/uorb_subscriber.hpp index 550c1a3f4584..bd59adb84606 100644 --- a/src/modules/zenoh/subscribers/uorb_subscriber.hpp +++ b/src/modules/zenoh/subscribers/uorb_subscriber.hpp @@ -50,7 +50,7 @@ class uORB_Zenoh_Subscriber : public Zenoh_Subscriber { public: uORB_Zenoh_Subscriber(const orb_metadata *meta, const uint32_t *ops) : - Zenoh_Subscriber(true), + Zenoh_Subscriber(), _uorb_meta{meta}, _cdr_ops(ops) { @@ -61,11 +61,14 @@ class uORB_Zenoh_Subscriber : public Zenoh_Subscriber ~uORB_Zenoh_Subscriber() override = default; // Update the uORB Subscription and broadcast a Zenoh ROS2 message - void data_handler(const z_sample_t *sample) + void data_handler(const z_loaned_sample_t *sample) { char data[_uorb_meta->o_size]; - dds_istream_t is = {.m_buffer = (unsigned char *)(sample->payload.start), .m_size = static_cast(sample->payload.len), + const z_loaned_bytes_t *payload = z_sample_payload(sample); + size_t len = z_bytes_len(payload); + + dds_istream_t is = {.m_buffer = (unsigned char *)(payload), .m_size = static_cast(len), .m_index = 4, .m_xcdr_version = DDSI_RTPS_CDR_ENC_VERSION_2 }; dds_stream_read(&is, data, &dds_allocator, _cdr_ops); diff --git a/src/modules/zenoh/subscribers/zenoh_subscriber.cpp b/src/modules/zenoh/subscribers/zenoh_subscriber.cpp index aab66ee5bdba..79f37d89812a 100644 --- a/src/modules/zenoh/subscribers/zenoh_subscriber.cpp +++ b/src/modules/zenoh/subscribers/zenoh_subscriber.cpp @@ -41,22 +41,23 @@ #include "zenoh_subscriber.hpp" -static void data_handler_cb(const z_sample_t *sample, void *arg) +static void data_handler_cb(const z_loaned_sample_t *sample, void *arg) { static_cast(arg)->data_handler(sample); } -void Zenoh_Subscriber::data_handler(const z_sample_t *sample) +void Zenoh_Subscriber::data_handler(const z_loaned_sample_t *sample) { - z_owned_str_t keystr = z_keyexpr_to_string(sample->keyexpr); - printf(">> [Subscriber] Received ('%s' size '%d')\n", z_str_loan(&keystr), (int)sample->payload.len); - z_str_drop(z_str_move(&keystr)); + z_view_string_t keystr; + z_keyexpr_as_view_string(z_sample_keyexpr(sample), &keystr); + z_owned_slice_t value; + z_bytes_deserialize_into_slice(z_sample_payload(sample), &value); + printf(">> [Subscriber] Received ('%s' size '%d')\n", z_string_data(z_loan(keystr)), (int)z_slice_len(z_loan(value))); } -Zenoh_Subscriber::Zenoh_Subscriber(bool rostopic) +Zenoh_Subscriber::Zenoh_Subscriber() { - this->_rostopic = rostopic; this->_topic[0] = 0x0; } @@ -71,27 +72,21 @@ int Zenoh_Subscriber::undeclare_subscriber() return 0; } -int Zenoh_Subscriber::declare_subscriber(z_session_t s, const char *keyexpr) +int Zenoh_Subscriber::declare_subscriber(z_owned_session_t s, const char *keyexpr) { - z_owned_closure_sample_t callback = z_closure_sample(data_handler_cb, NULL, this); + z_owned_closure_sample_t callback; + z_closure_sample(&callback, data_handler_cb, NULL, this); - if (_rostopic) { - strncpy(this->_topic, (char *)_rt_prefix, _rt_prefix_offset); + strncpy(this->_topic, keyexpr, sizeof(this->_topic)); - if (keyexpr[0] == '/') { - strncpy(this->_topic + _rt_prefix_offset, keyexpr + 1, sizeof(this->_topic) - _rt_prefix_offset); + z_view_keyexpr_t ke; + z_view_keyexpr_from_str(&ke, this->_topic); - } else { - strncpy(this->_topic + _rt_prefix_offset, keyexpr, sizeof(this->_topic) - _rt_prefix_offset); - } - - } else { - strncpy(this->_topic, (char *)keyexpr, sizeof(this->_topic)); + if (z_declare_subscriber(&_sub, z_loan(s), z_loan(ke), z_closure_sample_move(&callback), NULL) < 0) { + printf("Unable to declare subscriber.\n"); + exit(-1); } - _sub = z_declare_subscriber(s, z_keyexpr(this->_topic), z_closure_sample_move(&callback), NULL); - - if (!z_subscriber_check(&_sub)) { printf("Unable to declare subscriber for key expression!\n %s\n", keyexpr); return -1; diff --git a/src/modules/zenoh/subscribers/zenoh_subscriber.hpp b/src/modules/zenoh/subscribers/zenoh_subscriber.hpp index e3f1200e9245..1b6d10922045 100644 --- a/src/modules/zenoh/subscribers/zenoh_subscriber.hpp +++ b/src/modules/zenoh/subscribers/zenoh_subscriber.hpp @@ -55,14 +55,14 @@ class Zenoh_Subscriber : public ListNode { public: - Zenoh_Subscriber(bool rostopic = true); + Zenoh_Subscriber(); virtual ~Zenoh_Subscriber(); - virtual int declare_subscriber(z_session_t s, const char *keyexpr); + virtual int declare_subscriber(z_owned_session_t s, const char *keyexpr); virtual int undeclare_subscriber(); - virtual void data_handler(const z_sample_t *sample); + virtual void data_handler(const z_loaned_sample_t *sample); virtual void print(); @@ -71,10 +71,4 @@ class Zenoh_Subscriber : public ListNode z_owned_subscriber_t _sub; char _topic[60]; // The Topic name is somewhere is the Zenoh stack as well but no good api to fetch it. - - - // Indicates ROS2 Topic namespace - bool _rostopic; - const char *_rt_prefix = "rt/"; - const size_t _rt_prefix_offset = 3; // "rt/" are 3 chars }; diff --git a/src/modules/zenoh/zenoh-pico b/src/modules/zenoh/zenoh-pico index 22bbfc215092..fff1a8c168e1 160000 --- a/src/modules/zenoh/zenoh-pico +++ b/src/modules/zenoh/zenoh-pico @@ -1 +1 @@ -Subproject commit 22bbfc215092a051341c234fdcf68f5baa267dec +Subproject commit fff1a8c168e10d4ed9cbf2aa7d67ede07f239c52 diff --git a/src/modules/zenoh/zenoh.cpp b/src/modules/zenoh/zenoh.cpp index 0caf2c5eaa5f..6411734a2918 100644 --- a/src/modules/zenoh/zenoh.cpp +++ b/src/modules/zenoh/zenoh.cpp @@ -32,6 +32,8 @@ ****************************************************************************/ #include "zenoh.h" +#include "zenoh-pico/api/macros.h" +#include "zenoh-pico/api/primitives.h" #include #include #include @@ -78,27 +80,39 @@ void ZENOH::run() z_config.getNetworkConfig(mode, locator); - z_owned_config_t config = z_config_default(); - zp_config_insert(z_config_loan(&config), Z_CONFIG_MODE_KEY, z_string_make(mode)); + z_owned_config_t config; + z_config_default(&config); + zp_config_insert(z_loan_mut(config), Z_CONFIG_MODE_KEY, mode); if (locator[0] != 0) { - zp_config_insert(z_config_loan(&config), Z_CONFIG_PEER_KEY, z_string_make(locator)); + zp_config_insert(z_loan_mut(config), Z_CONFIG_CONNECT_KEY, locator); } else if (strcmp(Z_CONFIG_MODE_PEER, mode) == 0) { - zp_config_insert(z_config_loan(&config), Z_CONFIG_PEER_KEY, z_string_make(Z_CONFIG_MULTICAST_LOCATOR_DEFAULT)); + zp_config_insert(z_loan_mut(config), Z_CONFIG_CONNECT_KEY, Z_CONFIG_MULTICAST_LOCATOR_DEFAULT); } PX4_INFO("Opening session..."); - z_owned_session_t s = z_open(z_config_move(&config)); + z_owned_session_t s; + ret = z_open(&s, z_move(config)); + + if (ret < 0) { + PX4_ERR("Unable to open session, ret: %d", ret); + return; + } + + PX4_INFO("Checking session..."); if (!z_session_check(&s)) { - PX4_ERR("Unable to open session!"); + PX4_ERR("Unable to check session!"); return; } + PX4_INFO("Starting reading/writing tasks..."); + // Start read and lease tasks for zenoh-pico - if (zp_start_read_task(z_session_loan(&s), NULL) < 0 || zp_start_lease_task(z_session_loan(&s), NULL) < 0) { + if (zp_start_read_task(z_loan_mut(s), NULL) < 0 || zp_start_lease_task(z_loan_mut(s), NULL) < 0) { PX4_ERR("Unable to start read and lease tasks"); + z_close(z_move(s)); return; } @@ -114,7 +128,7 @@ void ZENOH::run() _zenoh_subscribers[i] = genSubscriber(type); if (_zenoh_subscribers[i] != 0) { - _zenoh_subscribers[i]->declare_subscriber(z_session_loan(&s), topic); + _zenoh_subscribers[i]->declare_subscriber(s, topic); } @@ -141,7 +155,7 @@ void ZENOH::run() _zenoh_publishers[i] = genPublisher(type); if (_zenoh_publishers[i] != 0) { - _zenoh_publishers[i]->declare_publisher(z_session_loan(&s), topic); + _zenoh_publishers[i]->declare_publisher(s, topic); _zenoh_publishers[i]->setPollFD(&pfds[i]); } } @@ -154,7 +168,7 @@ void ZENOH::run() if (_pub_count == 0) { // Nothing to publish but we don't want to stop this thread while (!should_exit()) { - sleep(2); + usleep(1000); } } @@ -194,8 +208,8 @@ void ZENOH::run() free(_zenoh_publishers); // Stop read and lease tasks for zenoh-pico - zp_stop_read_task(z_session_loan(&s)); - zp_stop_lease_task(z_session_loan(&s)); + zp_stop_read_task(z_session_loan_mut(&s)); + zp_stop_lease_task(z_session_loan_mut(&s)); z_close(z_session_move(&s)); exit_and_cleanup(); @@ -234,8 +248,8 @@ Zenoh demo bridge PX4_INFO_RAW(" addsubscriber Publish Zenoh topic to uORB\n"); PX4_INFO_RAW(" net Zenoh network mode\n"); PX4_INFO_RAW(" values: client|peer \n"); - PX4_INFO_RAW(" client: locator address for router\n"); - PX4_INFO_RAW(" peer: multicast address e.g. udp/224.0.0.225:7447#iface=eth0\n"); + PX4_INFO_RAW(" client: locator address e.g. tcp/10.41.10.1:7447#iface=eth0\n"); + PX4_INFO_RAW(" peer: multicast address e.g. udp/224.0.0.224:7446#iface=eth0\n"); return 0; }