zenoh: update zenoh-pico from 0.7.0 to 1.0.0 (#23462)

* Update Zenoh-pico from 0.7.0 to 1.0.0
* Update the zenoh-pico version to use PX4/dev/1.0.0-px4
* Remove the rostopic and rt/ prefix
 * Unlike zenoh-bridge-dds we were using, zenoh-bridge-ros2dds is now adding the rt/ prefix automagically.

.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

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

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
 )

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

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)
 	{

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()

src/modules/zenoh/publishers/zenoh_publisher.hpp

@@ -51,10 +51,10 @@
 class Zenoh_Publisher : public ListNode<Zenoh_Publisher *>
 {
 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<Zenoh_Publisher *>
 	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
 };

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<int>(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<int>(len),
 				    .m_index = 4, .m_xcdr_version = DDSI_RTPS_CDR_ENC_VERSION_2
 				   };
 		dds_stream_read(&is, data, &dds_allocator, _cdr_ops);

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<Zenoh_Subscriber *>(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;

src/modules/zenoh/subscribers/zenoh_subscriber.hpp

@@ -55,14 +55,14 @@
 class Zenoh_Subscriber : public ListNode<Zenoh_Subscriber *>
 {
 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<Zenoh_Subscriber *>
 
 	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
 };

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 <px4_platform_common/log.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/cli.h>
@@ -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 <zenoh_topic> <uorb_topic>  Publish Zenoh topic to uORB\n");
 	PX4_INFO_RAW("     net           <mode> <locator>            Zenoh network mode\n");
 	PX4_INFO_RAW("          <mode>    values: client|peer   \n");
-	PX4_INFO_RAW("          <locator> 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("          <locator> 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;
 }