Add Zenoh pico support

src/modules/zenoh/CMakeLists.txt

@@ -0,0 +1,100 @@
+############################################################################
+#
+#   Copyright (c) 2023 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.
+#
+############################################################################
+
+function(message)
+    if (NOT MESSAGE_QUIET)
+        _message(${ARGN})
+    endif()
+endfunction()
+
+set(POSIX_COMPATIBLE YES)
+set(BUILD_SHARED_LIBS OFF)
+set(BUILD_TESTING OFF)
+set(CHECK_THREADS NO)
+set(MESSAGE_QUIET ON)
+set(ZENOH_DEBUG ${CONFIG_ZENOH_DEBUG})
+
+px4_add_git_submodule(TARGET git_zenoh-pico PATH "zenoh-pico")
+add_subdirectory(zenoh-pico)
+unset(MESSAGE_QUIET)
+add_dependencies(zenohpico git_zenoh-pico px4_platform)
+target_compile_options(zenohpico PUBLIC -Wno-cast-align
+			-Wno-narrowing
+			-Wno-stringop-overflow
+			-Wno-unused-result
+			-DZ_BATCH_SIZE_RX=512
+			-DZ_BATCH_SIZE_TX=512
+			-DZ_FRAG_MAX_SIZE=1024)
+
+if(CONFIG_PLATFORM_NUTTX)
+    target_compile_options(zenohpico PRIVATE -DUNIX_NO_MULTICAST_IF)
+endif()
+
+if(CONFIG_ZENOH_SERIAL)
+    target_compile_options(zenohpico PRIVATE -DZ_LINK_SERIAL)
+endif()
+
+
+px4_add_module(
+		MODULE modules__zenoh
+		MAIN zenoh
+		SRCS
+		zenoh.cpp
+		zenoh_config.cpp
+		zenoh.h
+		publishers/zenoh_publisher.cpp
+		subscribers/zenoh_subscriber.cpp
+		MODULE_CONFIG
+			module.yaml
+		DEPENDS
+			cdr
+			uorb_msgs
+			px4_work_queue
+			zenohpico
+			zenoh_topics
+			git_zenoh-pico
+		INCLUDES
+			${PX4_BINARY_DIR}/msg
+			zenoh-pico/include
+			${CMAKE_CURRENT_LIST_DIR}
+			${PX4_BINARY_DIR}/src/modules/zenoh/
+		COMPILE_FLAGS
+			-Wno-pointer-compare
+			-Wno-cast-align
+			-Wno-address-of-packed-member
+			-Wno-double-promotion
+			-Wno-unused
+			-DZENOH_LINUX
+			-DZENOH_NO_STDATOMIC
+			-D_Bool=int8_t
+)

src/modules/zenoh/Kconfig

@@ -0,0 +1,73 @@
+menuconfig MODULES_ZENOH
+	bool "Zenoh"
+	default n
+	select LIB_CDRSTREAM
+	---help---
+		Enable support for Zenoh
+
+if MODULES_ZENOH
+    config ZENOH_CONFIG_PATH
+        string "Config path"
+        default "/fs/microsd/zenoh"
+        help
+            Path to store network, publishers and subscribers configuration
+
+    config ZENOH_SERIAL
+        bool "Zenoh serial transport"
+        default n
+        help
+            Enables transport over serial (Not yet supported on NuttX/Linux)
+
+    config ZENOH_DEBUG
+        int "Zenoh debug level"
+        default 0
+        help
+            Set Zenoh debug level
+            0: NONE
+            1: ERROR
+            2: INFO + ERROR
+            3: DEBUG + INFO + ERROR
+
+    # Choose exactly one item
+    choice ZENOH_PUBSUB_SELECTION
+            prompt "Publishers/Subscribers selection"
+            default ZENOH_PUBSUB_ALL
+
+    config ZENOH_PUBSUB_MINIMAL
+            bool "Minimal"
+            select ZENOH_PUBSUB_COLLISION_CONSTRAINTS
+            select ZENOH_PUBSUB_FAILSAFE_FLAGS
+            select ZENOH_PUBSUB_POSITION_SETPOINT_TRIPLET
+            select ZENOH_PUBSUB_SENSOR_COMBINED
+            select ZENOH_PUBSUB_TIMESYNC_STATUS
+            select ZENOH_PUBSUB_VEHICLE_ATTITUDE
+            select ZENOH_PUBSUB_VEHICLE_CONTROL_MODE
+            select ZENOH_PUBSUB_VEHICLE_GLOBAL_POSITION
+            select ZENOH_PUBSUB_SENSOR_GPS
+            select ZENOH_PUBSUB_VEHICLE_LOCAL_POSITION
+            select ZENOH_PUBSUB_VEHICLE_ODOMETRY
+            select ZENOH_PUBSUB_VEHICLE_STATUS
+            select ZENOH_PUBSUB_TRAJECTORY_WAYPOINT
+            select ZENOH_PUBSUB_OFFBOARD_CONTROL_MODE
+            select ZENOH_PUBSUB_ONBOARD_COMPUTER_STATUS
+            select ZENOH_PUBSUB_OBSTACLE_DISTANCE
+            select ZENOH_PUBSUB_SENSOR_OPTICAL_FLOW
+            select ZENOH_PUBSUB_TELEMETRY_STATUS
+            select ZENOH_PUBSUB_TRAJECTORY_SETPOINT
+            select ZENOH_PUBSUB_VEHICLE_ATTITUDE_SETPOINT
+            select ZENOH_PUBSUB_VEHICLE_ODOMETRY
+            select ZENOH_PUBSUB_VEHICLE_RATES_SETPOINT
+            select ZENOH_PUBSUB_VEHICLE_COMMAND
+            select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER
+            select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT
+
+    config ZENOH_PUBSUB_ALL
+            bool "All"
+
+    config ZENOH_PUBSUB_CUSTOM
+            bool "Custom"
+    endchoice
+
+endif
+
+rsource "Kconfig.topics"

src/modules/zenoh/Kconfig.topics

@@ -0,0 +1,960 @@
+
+menu "Zenoh publishers/subscribers"
+	depends on MODULES_ZENOH
+	config ZENOH_PUBSUB_ACTION_REQUEST
+		bool "action_request"
+		default n
+
+	config ZENOH_PUBSUB_ACTUATOR_ARMED
+		bool "actuator_armed"
+		default n
+
+	config ZENOH_PUBSUB_ACTUATOR_CONTROLS_STATUS
+		bool "actuator_controls_status"
+		default n
+
+	config ZENOH_PUBSUB_ACTUATOR_MOTORS
+		bool "actuator_motors"
+		default n
+
+	config ZENOH_PUBSUB_ACTUATOR_OUTPUTS
+		bool "actuator_outputs"
+		default n
+
+	config ZENOH_PUBSUB_ACTUATOR_SERVOS
+		bool "actuator_servos"
+		default n
+
+	config ZENOH_PUBSUB_ACTUATOR_SERVOS_TRIM
+		bool "actuator_servos_trim"
+		default n
+
+	config ZENOH_PUBSUB_ACTUATOR_TEST
+		bool "actuator_test"
+		default n
+
+	config ZENOH_PUBSUB_ADC_REPORT
+		bool "adc_report"
+		default n
+
+	config ZENOH_PUBSUB_AIRSPEED
+		bool "airspeed"
+		default n
+
+	config ZENOH_PUBSUB_AIRSPEED_VALIDATED
+		bool "airspeed_validated"
+		default n
+
+	config ZENOH_PUBSUB_AIRSPEED_WIND
+		bool "airspeed_wind"
+		default n
+
+	config ZENOH_PUBSUB_AUTOTUNE_ATTITUDE_CONTROL_STATUS
+		bool "autotune_attitude_control_status"
+		default n
+
+	config ZENOH_PUBSUB_BATTERY_STATUS
+		bool "battery_status"
+		default n
+
+	config ZENOH_PUBSUB_BUTTON_EVENT
+		bool "button_event"
+		default n
+
+	config ZENOH_PUBSUB_CAMERA_CAPTURE
+		bool "camera_capture"
+		default n
+
+	config ZENOH_PUBSUB_CAMERA_STATUS
+		bool "camera_status"
+		default n
+
+	config ZENOH_PUBSUB_CAMERA_TRIGGER
+		bool "camera_trigger"
+		default n
+
+	config ZENOH_PUBSUB_CELLULAR_STATUS
+		bool "cellular_status"
+		default n
+
+	config ZENOH_PUBSUB_COLLISION_CONSTRAINTS
+		bool "collision_constraints"
+		default n
+
+	config ZENOH_PUBSUB_COLLISION_REPORT
+		bool "collision_report"
+		default n
+
+	config ZENOH_PUBSUB_CONTROL_ALLOCATOR_STATUS
+		bool "control_allocator_status"
+		default n
+
+	config ZENOH_PUBSUB_CPULOAD
+		bool "cpuload"
+		default n
+
+	config ZENOH_PUBSUB_DATAMAN_REQUEST
+		bool "dataman_request"
+		default n
+
+	config ZENOH_PUBSUB_DATAMAN_RESPONSE
+		bool "dataman_response"
+		default n
+
+	config ZENOH_PUBSUB_DEBUG_ARRAY
+		bool "debug_array"
+		default n
+
+	config ZENOH_PUBSUB_DEBUG_KEY_VALUE
+		bool "debug_key_value"
+		default n
+
+	config ZENOH_PUBSUB_DEBUG_VALUE
+		bool "debug_value"
+		default n
+
+	config ZENOH_PUBSUB_DEBUG_VECT
+		bool "debug_vect"
+		default n
+
+	config ZENOH_PUBSUB_DIFFERENTIAL_PRESSURE
+		bool "differential_pressure"
+		default n
+
+	config ZENOH_PUBSUB_DISTANCE_SENSOR
+		bool "distance_sensor"
+		default n
+
+	config ZENOH_PUBSUB_EKF2_TIMESTAMPS
+		bool "ekf2_timestamps"
+		default n
+
+	config ZENOH_PUBSUB_ESC_REPORT
+		bool "esc_report"
+		default n
+
+	config ZENOH_PUBSUB_ESC_STATUS
+		bool "esc_status"
+		default n
+
+	config ZENOH_PUBSUB_ESTIMATOR_AID_SOURCE1D
+		bool "estimator_aid_source1d"
+		default n
+
+	config ZENOH_PUBSUB_ESTIMATOR_AID_SOURCE2D
+		bool "estimator_aid_source2d"
+		default n
+
+	config ZENOH_PUBSUB_ESTIMATOR_AID_SOURCE3D
+		bool "estimator_aid_source3d"
+		default n
+
+	config ZENOH_PUBSUB_ESTIMATOR_BIAS
+		bool "estimator_bias"
+		default n
+
+	config ZENOH_PUBSUB_ESTIMATOR_BIAS3D
+		bool "estimator_bias3d"
+		default n
+
+	config ZENOH_PUBSUB_ESTIMATOR_EVENT_FLAGS
+		bool "estimator_event_flags"
+		default n
+
+	config ZENOH_PUBSUB_ESTIMATOR_GPS_STATUS
+		bool "estimator_gps_status"
+		default n
+
+	config ZENOH_PUBSUB_ESTIMATOR_INNOVATIONS
+		bool "estimator_innovations"
+		default n
+
+	config ZENOH_PUBSUB_ESTIMATOR_SELECTOR_STATUS
+		bool "estimator_selector_status"
+		default n
+
+	config ZENOH_PUBSUB_ESTIMATOR_SENSOR_BIAS
+		bool "estimator_sensor_bias"
+		default n
+
+	config ZENOH_PUBSUB_ESTIMATOR_STATES
+		bool "estimator_states"
+		default n
+
+	config ZENOH_PUBSUB_ESTIMATOR_STATUS
+		bool "estimator_status"
+		default n
+
+	config ZENOH_PUBSUB_ESTIMATOR_STATUS_FLAGS
+		bool "estimator_status_flags"
+		default n
+
+	config ZENOH_PUBSUB_EVENT
+		bool "event"
+		default n
+
+	config ZENOH_PUBSUB_FAILSAFE_FLAGS
+		bool "failsafe_flags"
+		default n
+
+	config ZENOH_PUBSUB_FAILURE_DETECTOR_STATUS
+		bool "failure_detector_status"
+		default n
+
+	config ZENOH_PUBSUB_FOLLOW_TARGET
+		bool "follow_target"
+		default n
+
+	config ZENOH_PUBSUB_FOLLOW_TARGET_ESTIMATOR
+		bool "follow_target_estimator"
+		default n
+
+	config ZENOH_PUBSUB_FOLLOW_TARGET_STATUS
+		bool "follow_target_status"
+		default n
+
+	config ZENOH_PUBSUB_GENERATOR_STATUS
+		bool "generator_status"
+		default n
+
+	config ZENOH_PUBSUB_GEOFENCE_RESULT
+		bool "geofence_result"
+		default n
+
+	config ZENOH_PUBSUB_GIMBAL_CONTROLS
+		bool "gimbal_controls"
+		default n
+
+	config ZENOH_PUBSUB_GIMBAL_DEVICE_ATTITUDE_STATUS
+		bool "gimbal_device_attitude_status"
+		default n
+
+	config ZENOH_PUBSUB_GIMBAL_DEVICE_INFORMATION
+		bool "gimbal_device_information"
+		default n
+
+	config ZENOH_PUBSUB_GIMBAL_DEVICE_SET_ATTITUDE
+		bool "gimbal_device_set_attitude"
+		default n
+
+	config ZENOH_PUBSUB_GIMBAL_MANAGER_INFORMATION
+		bool "gimbal_manager_information"
+		default n
+
+	config ZENOH_PUBSUB_GIMBAL_MANAGER_SET_ATTITUDE
+		bool "gimbal_manager_set_attitude"
+		default n
+
+	config ZENOH_PUBSUB_GIMBAL_MANAGER_SET_MANUAL_CONTROL
+		bool "gimbal_manager_set_manual_control"
+		default n
+
+	config ZENOH_PUBSUB_GIMBAL_MANAGER_STATUS
+		bool "gimbal_manager_status"
+		default n
+
+	config ZENOH_PUBSUB_GPIO_CONFIG
+		bool "gpio_config"
+		default n
+
+	config ZENOH_PUBSUB_GPIO_IN
+		bool "gpio_in"
+		default n
+
+	config ZENOH_PUBSUB_GPIO_OUT
+		bool "gpio_out"
+		default n
+
+	config ZENOH_PUBSUB_GPIO_REQUEST
+		bool "gpio_request"
+		default n
+
+	config ZENOH_PUBSUB_GPS_DUMP
+		bool "gps_dump"
+		default n
+
+	config ZENOH_PUBSUB_GPS_INJECT_DATA
+		bool "gps_inject_data"
+		default n
+
+	config ZENOH_PUBSUB_GRIPPER
+		bool "gripper"
+		default n
+
+	config ZENOH_PUBSUB_HEALTH_REPORT
+		bool "health_report"
+		default n
+
+	config ZENOH_PUBSUB_HEATER_STATUS
+		bool "heater_status"
+		default n
+
+	config ZENOH_PUBSUB_HOME_POSITION
+		bool "home_position"
+		default n
+
+	config ZENOH_PUBSUB_HOVER_THRUST_ESTIMATE
+		bool "hover_thrust_estimate"
+		default n
+
+	config ZENOH_PUBSUB_INPUT_RC
+		bool "input_rc"
+		default n
+
+	config ZENOH_PUBSUB_INTERNAL_COMBUSTION_ENGINE_STATUS
+		bool "internal_combustion_engine_status"
+		default n
+
+	config ZENOH_PUBSUB_IRIDIUMSBD_STATUS
+		bool "iridiumsbd_status"
+		default n
+
+	config ZENOH_PUBSUB_IRLOCK_REPORT
+		bool "irlock_report"
+		default n
+
+	config ZENOH_PUBSUB_LANDING_GEAR
+		bool "landing_gear"
+		default n
+
+	config ZENOH_PUBSUB_LANDING_GEAR_WHEEL
+		bool "landing_gear_wheel"
+		default n
+
+	config ZENOH_PUBSUB_LANDING_TARGET_INNOVATIONS
+		bool "landing_target_innovations"
+		default n
+
+	config ZENOH_PUBSUB_LANDING_TARGET_POSE
+		bool "landing_target_pose"
+		default n
+
+	config ZENOH_PUBSUB_LAUNCH_DETECTION_STATUS
+		bool "launch_detection_status"
+		default n
+
+	config ZENOH_PUBSUB_LED_CONTROL
+		bool "led_control"
+		default n
+
+	config ZENOH_PUBSUB_LOG_MESSAGE
+		bool "log_message"
+		default n
+
+	config ZENOH_PUBSUB_LOGGER_STATUS
+		bool "logger_status"
+		default n
+
+	config ZENOH_PUBSUB_MAG_WORKER_DATA
+		bool "mag_worker_data"
+		default n
+
+	config ZENOH_PUBSUB_MAGNETOMETER_BIAS_ESTIMATE
+		bool "magnetometer_bias_estimate"
+		default n
+
+	config ZENOH_PUBSUB_MANUAL_CONTROL_SETPOINT
+		bool "manual_control_setpoint"
+		default n
+
+	config ZENOH_PUBSUB_MANUAL_CONTROL_SWITCHES
+		bool "manual_control_switches"
+		default n
+
+	config ZENOH_PUBSUB_MAVLINK_LOG
+		bool "mavlink_log"
+		default n
+
+	config ZENOH_PUBSUB_MAVLINK_TUNNEL
+		bool "mavlink_tunnel"
+		default n
+
+	config ZENOH_PUBSUB_MISSION
+		bool "mission"
+		default n
+
+	config ZENOH_PUBSUB_MISSION_RESULT
+		bool "mission_result"
+		default n
+
+	config ZENOH_PUBSUB_MODE_COMPLETED
+		bool "mode_completed"
+		default n
+
+	config ZENOH_PUBSUB_MOUNT_ORIENTATION
+		bool "mount_orientation"
+		default n
+
+	config ZENOH_PUBSUB_NAVIGATOR_MISSION_ITEM
+		bool "navigator_mission_item"
+		default n
+
+	config ZENOH_PUBSUB_NORMALIZED_UNSIGNED_SETPOINT
+		bool "normalized_unsigned_setpoint"
+		default n
+
+	config ZENOH_PUBSUB_NPFG_STATUS
+		bool "npfg_status"
+		default n
+
+	config ZENOH_PUBSUB_OBSTACLE_DISTANCE
+		bool "obstacle_distance"
+		default n
+
+	config ZENOH_PUBSUB_OFFBOARD_CONTROL_MODE
+		bool "offboard_control_mode"
+		default n
+
+	config ZENOH_PUBSUB_ONBOARD_COMPUTER_STATUS
+		bool "onboard_computer_status"
+		default n
+
+	config ZENOH_PUBSUB_ORB_TEST
+		bool "orb_test"
+		default n
+
+	config ZENOH_PUBSUB_ORB_TEST_LARGE
+		bool "orb_test_large"
+		default n
+
+	config ZENOH_PUBSUB_ORB_TEST_MEDIUM
+		bool "orb_test_medium"
+		default n
+
+	config ZENOH_PUBSUB_ORBIT_STATUS
+		bool "orbit_status"
+		default n
+
+	config ZENOH_PUBSUB_PARAMETER_UPDATE
+		bool "parameter_update"
+		default n
+
+	config ZENOH_PUBSUB_PING
+		bool "ping"
+		default n
+
+	config ZENOH_PUBSUB_POSITION_CONTROLLER_LANDING_STATUS
+		bool "position_controller_landing_status"
+		default n
+
+	config ZENOH_PUBSUB_POSITION_CONTROLLER_STATUS
+		bool "position_controller_status"
+		default n
+
+	config ZENOH_PUBSUB_POSITION_SETPOINT
+		bool "position_setpoint"
+		default n
+
+	config ZENOH_PUBSUB_POSITION_SETPOINT_TRIPLET
+		bool "position_setpoint_triplet"
+		default n
+
+	config ZENOH_PUBSUB_POWER_BUTTON_STATE
+		bool "power_button_state"
+		default n
+
+	config ZENOH_PUBSUB_POWER_MONITOR
+		bool "power_monitor"
+		default n
+
+	config ZENOH_PUBSUB_PPS_CAPTURE
+		bool "pps_capture"
+		default n
+
+	config ZENOH_PUBSUB_PWM_INPUT
+		bool "pwm_input"
+		default n
+
+	config ZENOH_PUBSUB_PX4IO_STATUS
+		bool "px4io_status"
+		default n
+
+	config ZENOH_PUBSUB_QSHELL_REQ
+		bool "qshell_req"
+		default n
+
+	config ZENOH_PUBSUB_QSHELL_RETVAL
+		bool "qshell_retval"
+		default n
+
+	config ZENOH_PUBSUB_RADIO_STATUS
+		bool "radio_status"
+		default n
+
+	config ZENOH_PUBSUB_RATE_CTRL_STATUS
+		bool "rate_ctrl_status"
+		default n
+
+	config ZENOH_PUBSUB_RC_CHANNELS
+		bool "rc_channels"
+		default n
+
+	config ZENOH_PUBSUB_RC_PARAMETER_MAP
+		bool "rc_parameter_map"
+		default n
+
+	config ZENOH_PUBSUB_RPM
+		bool "rpm"
+		default n
+
+	config ZENOH_PUBSUB_RTL_TIME_ESTIMATE
+		bool "rtl_time_estimate"
+		default n
+
+	config ZENOH_PUBSUB_SATELLITE_INFO
+		bool "satellite_info"
+		default n
+
+	config ZENOH_PUBSUB_SENSOR_ACCEL
+		bool "sensor_accel"
+		default n
+
+	config ZENOH_PUBSUB_SENSOR_ACCEL_FIFO
+		bool "sensor_accel_fifo"
+		default n
+
+	config ZENOH_PUBSUB_SENSOR_BARO
+		bool "sensor_baro"
+		default n
+
+	config ZENOH_PUBSUB_SENSOR_COMBINED
+		bool "sensor_combined"
+		default n
+
+	config ZENOH_PUBSUB_SENSOR_CORRECTION
+		bool "sensor_correction"
+		default n
+
+	config ZENOH_PUBSUB_SENSOR_GNSS_RELATIVE
+		bool "sensor_gnss_relative"
+		default n
+
+	config ZENOH_PUBSUB_SENSOR_GPS
+		bool "sensor_gps"
+		default n
+
+	config ZENOH_PUBSUB_SENSOR_GYRO
+		bool "sensor_gyro"
+		default n
+
+	config ZENOH_PUBSUB_SENSOR_GYRO_FFT
+		bool "sensor_gyro_fft"
+		default n
+
+	config ZENOH_PUBSUB_SENSOR_GYRO_FIFO
+		bool "sensor_gyro_fifo"
+		default n
+
+	config ZENOH_PUBSUB_SENSOR_HYGROMETER
+		bool "sensor_hygrometer"
+		default n
+
+	config ZENOH_PUBSUB_SENSOR_MAG
+		bool "sensor_mag"
+		default n
+
+	config ZENOH_PUBSUB_SENSOR_OPTICAL_FLOW
+		bool "sensor_optical_flow"
+		default n
+
+	config ZENOH_PUBSUB_SENSOR_PREFLIGHT_MAG
+		bool "sensor_preflight_mag"
+		default n
+
+	config ZENOH_PUBSUB_SENSOR_SELECTION
+		bool "sensor_selection"
+		default n
+
+	config ZENOH_PUBSUB_SENSOR_UWB
+		bool "sensor_uwb"
+		default n
+
+	config ZENOH_PUBSUB_SENSORS_STATUS
+		bool "sensors_status"
+		default n
+
+	config ZENOH_PUBSUB_SENSORS_STATUS_IMU
+		bool "sensors_status_imu"
+		default n
+
+	config ZENOH_PUBSUB_SYSTEM_POWER
+		bool "system_power"
+		default n
+
+	config ZENOH_PUBSUB_TAKEOFF_STATUS
+		bool "takeoff_status"
+		default n
+
+	config ZENOH_PUBSUB_TASK_STACK_INFO
+		bool "task_stack_info"
+		default n
+
+	config ZENOH_PUBSUB_TECS_STATUS
+		bool "tecs_status"
+		default n
+
+	config ZENOH_PUBSUB_TELEMETRY_STATUS
+		bool "telemetry_status"
+		default n
+
+	config ZENOH_PUBSUB_TILTROTOR_EXTRA_CONTROLS
+		bool "tiltrotor_extra_controls"
+		default n
+
+	config ZENOH_PUBSUB_TIMESYNC_STATUS
+		bool "timesync_status"
+		default n
+
+	config ZENOH_PUBSUB_TRAJECTORY_BEZIER
+		bool "trajectory_bezier"
+		default n
+
+	config ZENOH_PUBSUB_TRAJECTORY_SETPOINT
+		bool "trajectory_setpoint"
+		default n
+
+	config ZENOH_PUBSUB_TRAJECTORY_WAYPOINT
+		bool "trajectory_waypoint"
+		default n
+
+	config ZENOH_PUBSUB_TRANSPONDER_REPORT
+		bool "transponder_report"
+		default n
+
+	config ZENOH_PUBSUB_TUNE_CONTROL
+		bool "tune_control"
+		default n
+
+	config ZENOH_PUBSUB_UAVCAN_PARAMETER_REQUEST
+		bool "uavcan_parameter_request"
+		default n
+
+	config ZENOH_PUBSUB_UAVCAN_PARAMETER_VALUE
+		bool "uavcan_parameter_value"
+		default n
+
+	config ZENOH_PUBSUB_ULOG_STREAM
+		bool "ulog_stream"
+		default n
+
+	config ZENOH_PUBSUB_ULOG_STREAM_ACK
+		bool "ulog_stream_ack"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_ACCELERATION
+		bool "vehicle_acceleration"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_AIR_DATA
+		bool "vehicle_air_data"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_ANGULAR_ACCELERATION_SETPOINT
+		bool "vehicle_angular_acceleration_setpoint"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_ANGULAR_VELOCITY
+		bool "vehicle_angular_velocity"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_ATTITUDE
+		bool "vehicle_attitude"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_ATTITUDE_SETPOINT
+		bool "vehicle_attitude_setpoint"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_COMMAND
+		bool "vehicle_command"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_COMMAND_ACK
+		bool "vehicle_command_ack"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_CONSTRAINTS
+		bool "vehicle_constraints"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_CONTROL_MODE
+		bool "vehicle_control_mode"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_GLOBAL_POSITION
+		bool "vehicle_global_position"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_IMU
+		bool "vehicle_imu"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_IMU_STATUS
+		bool "vehicle_imu_status"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_LAND_DETECTED
+		bool "vehicle_land_detected"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_LOCAL_POSITION
+		bool "vehicle_local_position"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_LOCAL_POSITION_SETPOINT
+		bool "vehicle_local_position_setpoint"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_MAGNETOMETER
+		bool "vehicle_magnetometer"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_ODOMETRY
+		bool "vehicle_odometry"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_OPTICAL_FLOW
+		bool "vehicle_optical_flow"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_OPTICAL_FLOW_VEL
+		bool "vehicle_optical_flow_vel"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_RATES_SETPOINT
+		bool "vehicle_rates_setpoint"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_ROI
+		bool "vehicle_roi"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_STATUS
+		bool "vehicle_status"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_THRUST_SETPOINT
+		bool "vehicle_thrust_setpoint"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_TORQUE_SETPOINT
+		bool "vehicle_torque_setpoint"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER
+		bool "vehicle_trajectory_bezier"
+		default n
+
+	config ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT
+		bool "vehicle_trajectory_waypoint"
+		default n
+
+	config ZENOH_PUBSUB_VTOL_VEHICLE_STATUS
+		bool "vtol_vehicle_status"
+		default n
+
+	config ZENOH_PUBSUB_WIND
+		bool "wind"
+		default n
+
+	config ZENOH_PUBSUB_YAW_ESTIMATOR_STATUS
+		bool "yaw_estimator_status"
+		default n
+
+
+
+config ZENOH_PUBSUB_ALL_SELECTION
+	bool
+	default y if ZENOH_PUBSUB_ALL
+	select ZENOH_PUBSUB_ACTION_REQUEST
+	select ZENOH_PUBSUB_ACTUATOR_ARMED
+	select ZENOH_PUBSUB_ACTUATOR_CONTROLS_STATUS
+	select ZENOH_PUBSUB_ACTUATOR_MOTORS
+	select ZENOH_PUBSUB_ACTUATOR_OUTPUTS
+	select ZENOH_PUBSUB_ACTUATOR_SERVOS
+	select ZENOH_PUBSUB_ACTUATOR_SERVOS_TRIM
+	select ZENOH_PUBSUB_ACTUATOR_TEST
+	select ZENOH_PUBSUB_ADC_REPORT
+	select ZENOH_PUBSUB_AIRSPEED
+	select ZENOH_PUBSUB_AIRSPEED_VALIDATED
+	select ZENOH_PUBSUB_AIRSPEED_WIND
+	select ZENOH_PUBSUB_AUTOTUNE_ATTITUDE_CONTROL_STATUS
+	select ZENOH_PUBSUB_BATTERY_STATUS
+	select ZENOH_PUBSUB_BUTTON_EVENT
+	select ZENOH_PUBSUB_CAMERA_CAPTURE
+	select ZENOH_PUBSUB_CAMERA_STATUS
+	select ZENOH_PUBSUB_CAMERA_TRIGGER
+	select ZENOH_PUBSUB_CELLULAR_STATUS
+	select ZENOH_PUBSUB_COLLISION_CONSTRAINTS
+	select ZENOH_PUBSUB_COLLISION_REPORT
+	select ZENOH_PUBSUB_CONTROL_ALLOCATOR_STATUS
+	select ZENOH_PUBSUB_CPULOAD
+	select ZENOH_PUBSUB_DATAMAN_REQUEST
+	select ZENOH_PUBSUB_DATAMAN_RESPONSE
+	select ZENOH_PUBSUB_DEBUG_ARRAY
+	select ZENOH_PUBSUB_DEBUG_KEY_VALUE
+	select ZENOH_PUBSUB_DEBUG_VALUE
+	select ZENOH_PUBSUB_DEBUG_VECT
+	select ZENOH_PUBSUB_DIFFERENTIAL_PRESSURE
+	select ZENOH_PUBSUB_DISTANCE_SENSOR
+	select ZENOH_PUBSUB_EKF2_TIMESTAMPS
+	select ZENOH_PUBSUB_ESC_REPORT
+	select ZENOH_PUBSUB_ESC_STATUS
+	select ZENOH_PUBSUB_ESTIMATOR_AID_SOURCE1D
+	select ZENOH_PUBSUB_ESTIMATOR_AID_SOURCE2D
+	select ZENOH_PUBSUB_ESTIMATOR_AID_SOURCE3D
+	select ZENOH_PUBSUB_ESTIMATOR_BIAS
+	select ZENOH_PUBSUB_ESTIMATOR_BIAS3D
+	select ZENOH_PUBSUB_ESTIMATOR_EVENT_FLAGS
+	select ZENOH_PUBSUB_ESTIMATOR_GPS_STATUS
+	select ZENOH_PUBSUB_ESTIMATOR_INNOVATIONS
+	select ZENOH_PUBSUB_ESTIMATOR_SELECTOR_STATUS
+	select ZENOH_PUBSUB_ESTIMATOR_SENSOR_BIAS
+	select ZENOH_PUBSUB_ESTIMATOR_STATES
+	select ZENOH_PUBSUB_ESTIMATOR_STATUS
+	select ZENOH_PUBSUB_ESTIMATOR_STATUS_FLAGS
+	select ZENOH_PUBSUB_EVENT
+	select ZENOH_PUBSUB_FAILSAFE_FLAGS
+	select ZENOH_PUBSUB_FAILURE_DETECTOR_STATUS
+	select ZENOH_PUBSUB_FOLLOW_TARGET
+	select ZENOH_PUBSUB_FOLLOW_TARGET_ESTIMATOR
+	select ZENOH_PUBSUB_FOLLOW_TARGET_STATUS
+	select ZENOH_PUBSUB_GENERATOR_STATUS
+	select ZENOH_PUBSUB_GEOFENCE_RESULT
+	select ZENOH_PUBSUB_GIMBAL_CONTROLS
+	select ZENOH_PUBSUB_GIMBAL_DEVICE_ATTITUDE_STATUS
+	select ZENOH_PUBSUB_GIMBAL_DEVICE_INFORMATION
+	select ZENOH_PUBSUB_GIMBAL_DEVICE_SET_ATTITUDE
+	select ZENOH_PUBSUB_GIMBAL_MANAGER_INFORMATION
+	select ZENOH_PUBSUB_GIMBAL_MANAGER_SET_ATTITUDE
+	select ZENOH_PUBSUB_GIMBAL_MANAGER_SET_MANUAL_CONTROL
+	select ZENOH_PUBSUB_GIMBAL_MANAGER_STATUS
+	select ZENOH_PUBSUB_GPIO_CONFIG
+	select ZENOH_PUBSUB_GPIO_IN
+	select ZENOH_PUBSUB_GPIO_OUT
+	select ZENOH_PUBSUB_GPIO_REQUEST
+	select ZENOH_PUBSUB_GPS_DUMP
+	select ZENOH_PUBSUB_GPS_INJECT_DATA
+	select ZENOH_PUBSUB_GRIPPER
+	select ZENOH_PUBSUB_HEALTH_REPORT
+	select ZENOH_PUBSUB_HEATER_STATUS
+	select ZENOH_PUBSUB_HOME_POSITION
+	select ZENOH_PUBSUB_HOVER_THRUST_ESTIMATE
+	select ZENOH_PUBSUB_INPUT_RC
+	select ZENOH_PUBSUB_INTERNAL_COMBUSTION_ENGINE_STATUS
+	select ZENOH_PUBSUB_IRIDIUMSBD_STATUS
+	select ZENOH_PUBSUB_IRLOCK_REPORT
+	select ZENOH_PUBSUB_LANDING_GEAR
+	select ZENOH_PUBSUB_LANDING_GEAR_WHEEL
+	select ZENOH_PUBSUB_LANDING_TARGET_INNOVATIONS
+	select ZENOH_PUBSUB_LANDING_TARGET_POSE
+	select ZENOH_PUBSUB_LAUNCH_DETECTION_STATUS
+	select ZENOH_PUBSUB_LED_CONTROL
+	select ZENOH_PUBSUB_LOG_MESSAGE
+	select ZENOH_PUBSUB_LOGGER_STATUS
+	select ZENOH_PUBSUB_MAG_WORKER_DATA
+	select ZENOH_PUBSUB_MAGNETOMETER_BIAS_ESTIMATE
+	select ZENOH_PUBSUB_MANUAL_CONTROL_SETPOINT
+	select ZENOH_PUBSUB_MANUAL_CONTROL_SWITCHES
+	select ZENOH_PUBSUB_MAVLINK_LOG
+	select ZENOH_PUBSUB_MAVLINK_TUNNEL
+	select ZENOH_PUBSUB_MISSION
+	select ZENOH_PUBSUB_MISSION_RESULT
+	select ZENOH_PUBSUB_MODE_COMPLETED
+	select ZENOH_PUBSUB_MOUNT_ORIENTATION
+	select ZENOH_PUBSUB_NAVIGATOR_MISSION_ITEM
+	select ZENOH_PUBSUB_NORMALIZED_UNSIGNED_SETPOINT
+	select ZENOH_PUBSUB_NPFG_STATUS
+	select ZENOH_PUBSUB_OBSTACLE_DISTANCE
+	select ZENOH_PUBSUB_OFFBOARD_CONTROL_MODE
+	select ZENOH_PUBSUB_ONBOARD_COMPUTER_STATUS
+	select ZENOH_PUBSUB_ORB_TEST
+	select ZENOH_PUBSUB_ORB_TEST_LARGE
+	select ZENOH_PUBSUB_ORB_TEST_MEDIUM
+	select ZENOH_PUBSUB_ORBIT_STATUS
+	select ZENOH_PUBSUB_PARAMETER_UPDATE
+	select ZENOH_PUBSUB_PING
+	select ZENOH_PUBSUB_POSITION_CONTROLLER_LANDING_STATUS
+	select ZENOH_PUBSUB_POSITION_CONTROLLER_STATUS
+	select ZENOH_PUBSUB_POSITION_SETPOINT
+	select ZENOH_PUBSUB_POSITION_SETPOINT_TRIPLET
+	select ZENOH_PUBSUB_POWER_BUTTON_STATE
+	select ZENOH_PUBSUB_POWER_MONITOR
+	select ZENOH_PUBSUB_PPS_CAPTURE
+	select ZENOH_PUBSUB_PWM_INPUT
+	select ZENOH_PUBSUB_PX4IO_STATUS
+	select ZENOH_PUBSUB_QSHELL_REQ
+	select ZENOH_PUBSUB_QSHELL_RETVAL
+	select ZENOH_PUBSUB_RADIO_STATUS
+	select ZENOH_PUBSUB_RATE_CTRL_STATUS
+	select ZENOH_PUBSUB_RC_CHANNELS
+	select ZENOH_PUBSUB_RC_PARAMETER_MAP
+	select ZENOH_PUBSUB_RPM
+	select ZENOH_PUBSUB_RTL_TIME_ESTIMATE
+	select ZENOH_PUBSUB_SATELLITE_INFO
+	select ZENOH_PUBSUB_SENSOR_ACCEL
+	select ZENOH_PUBSUB_SENSOR_ACCEL_FIFO
+	select ZENOH_PUBSUB_SENSOR_BARO
+	select ZENOH_PUBSUB_SENSOR_COMBINED
+	select ZENOH_PUBSUB_SENSOR_CORRECTION
+	select ZENOH_PUBSUB_SENSOR_GNSS_RELATIVE
+	select ZENOH_PUBSUB_SENSOR_GPS
+	select ZENOH_PUBSUB_SENSOR_GYRO
+	select ZENOH_PUBSUB_SENSOR_GYRO_FFT
+	select ZENOH_PUBSUB_SENSOR_GYRO_FIFO
+	select ZENOH_PUBSUB_SENSOR_HYGROMETER
+	select ZENOH_PUBSUB_SENSOR_MAG
+	select ZENOH_PUBSUB_SENSOR_OPTICAL_FLOW
+	select ZENOH_PUBSUB_SENSOR_PREFLIGHT_MAG
+	select ZENOH_PUBSUB_SENSOR_SELECTION
+	select ZENOH_PUBSUB_SENSOR_UWB
+	select ZENOH_PUBSUB_SENSORS_STATUS
+	select ZENOH_PUBSUB_SENSORS_STATUS_IMU
+	select ZENOH_PUBSUB_SYSTEM_POWER
+	select ZENOH_PUBSUB_TAKEOFF_STATUS
+	select ZENOH_PUBSUB_TASK_STACK_INFO
+	select ZENOH_PUBSUB_TECS_STATUS
+	select ZENOH_PUBSUB_TELEMETRY_STATUS
+	select ZENOH_PUBSUB_TILTROTOR_EXTRA_CONTROLS
+	select ZENOH_PUBSUB_TIMESYNC_STATUS
+	select ZENOH_PUBSUB_TRAJECTORY_BEZIER
+	select ZENOH_PUBSUB_TRAJECTORY_SETPOINT
+	select ZENOH_PUBSUB_TRAJECTORY_WAYPOINT
+	select ZENOH_PUBSUB_TRANSPONDER_REPORT
+	select ZENOH_PUBSUB_TUNE_CONTROL
+	select ZENOH_PUBSUB_UAVCAN_PARAMETER_REQUEST
+	select ZENOH_PUBSUB_UAVCAN_PARAMETER_VALUE
+	select ZENOH_PUBSUB_ULOG_STREAM
+	select ZENOH_PUBSUB_ULOG_STREAM_ACK
+	select ZENOH_PUBSUB_VEHICLE_ACCELERATION
+	select ZENOH_PUBSUB_VEHICLE_AIR_DATA
+	select ZENOH_PUBSUB_VEHICLE_ANGULAR_ACCELERATION_SETPOINT
+	select ZENOH_PUBSUB_VEHICLE_ANGULAR_VELOCITY
+	select ZENOH_PUBSUB_VEHICLE_ATTITUDE
+	select ZENOH_PUBSUB_VEHICLE_ATTITUDE_SETPOINT
+	select ZENOH_PUBSUB_VEHICLE_COMMAND
+	select ZENOH_PUBSUB_VEHICLE_COMMAND_ACK
+	select ZENOH_PUBSUB_VEHICLE_CONSTRAINTS
+	select ZENOH_PUBSUB_VEHICLE_CONTROL_MODE
+	select ZENOH_PUBSUB_VEHICLE_GLOBAL_POSITION
+	select ZENOH_PUBSUB_VEHICLE_IMU
+	select ZENOH_PUBSUB_VEHICLE_IMU_STATUS
+	select ZENOH_PUBSUB_VEHICLE_LAND_DETECTED
+	select ZENOH_PUBSUB_VEHICLE_LOCAL_POSITION
+	select ZENOH_PUBSUB_VEHICLE_LOCAL_POSITION_SETPOINT
+	select ZENOH_PUBSUB_VEHICLE_MAGNETOMETER
+	select ZENOH_PUBSUB_VEHICLE_ODOMETRY
+	select ZENOH_PUBSUB_VEHICLE_OPTICAL_FLOW
+	select ZENOH_PUBSUB_VEHICLE_OPTICAL_FLOW_VEL
+	select ZENOH_PUBSUB_VEHICLE_RATES_SETPOINT
+	select ZENOH_PUBSUB_VEHICLE_ROI
+	select ZENOH_PUBSUB_VEHICLE_STATUS
+	select ZENOH_PUBSUB_VEHICLE_THRUST_SETPOINT
+	select ZENOH_PUBSUB_VEHICLE_TORQUE_SETPOINT
+	select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER
+	select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT
+	select ZENOH_PUBSUB_VTOL_VEHICLE_STATUS
+	select ZENOH_PUBSUB_WIND
+	select ZENOH_PUBSUB_YAW_ESTIMATOR_STATUS
+
+endmenu

src/modules/zenoh/module.yaml

@@ -0,0 +1,14 @@
+module_name: Zenoh bridge
+
+parameters:
+    - group: Zenoh
+      definitions:
+
+        ZENOH_ENABLE:
+            description:
+                short: Zenoh Enable
+                long: Zenoh
+            category: System
+            type: int32
+            reboot_required: true
+            default: 0

src/modules/zenoh/publishers/uorb_publisher.hpp

@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 uorb_publisher.hpp
+ *
+ * Defines generic, templatized uORB over Zenoh / ROS2
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include "zenoh_publisher.hpp"
+#include <uORB/Subscription.hpp>
+#include <dds_serializer.h>
+
+#define CDR_SAFETY_MARGIN 12
+
+class uORB_Zenoh_Publisher : public Zenoh_Publisher
+{
+public:
+	uORB_Zenoh_Publisher(const orb_metadata *meta, const uint32_t *ops) :
+		Zenoh_Publisher(true),
+		_uorb_meta{meta},
+		_cdr_ops(ops)
+	{
+		_uorb_sub = orb_subscribe(meta);
+	};
+
+	~uORB_Zenoh_Publisher() override = default;
+
+	// Update the uORB Subscription and broadcast a Zenoh ROS2 message
+	virtual int8_t update() override
+	{
+		uint8_t data[_uorb_meta->o_size];
+		orb_copy(_uorb_meta, _uorb_sub, data);
+
+		uint8_t buf[_uorb_meta->o_size + 4 + CDR_SAFETY_MARGIN];
+		memcpy(buf, ros2_header, sizeof(ros2_header));
+
+		dds_ostream_t os;
+		os.m_buffer = buf;
+		os.m_index = (uint32_t)sizeof(ros2_header);
+		os.m_size = (uint32_t)sizeof(ros2_header) + _uorb_meta->o_size + CDR_SAFETY_MARGIN;
+		os.m_xcdr_version = DDSI_RTPS_CDR_ENC_VERSION_2;
+
+		if (dds_stream_write(&os,
+				     &dds_allocator,
+				     (const char *)&data,
+				     _cdr_ops)) {
+			return publish((const uint8_t *)buf, os.m_size);
+
+		} else {
+			return _Z_ERR_MESSAGE_SERIALIZATION_FAILED;
+		}
+	};
+
+	void setPollFD(px4_pollfd_struct_t *pfd)
+	{
+		pfd->fd = _uorb_sub;
+		pfd->events = POLLIN;
+	}
+
+	void print()
+	{
+		printf("uORB %s -> ", _uorb_meta->o_name);
+		Zenoh_Publisher::print();
+	}
+
+private:
+	const orb_metadata *_uorb_meta;
+	int _uorb_sub;
+	const uint32_t *_cdr_ops;
+};

src/modules/zenoh/publishers/zenoh_publisher.cpp

@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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_publisher.cpp
+ *
+ * Zenoh publisher
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#include "zenoh_publisher.hpp"
+
+
+Zenoh_Publisher::Zenoh_Publisher(bool rostopic)
+{
+	this->_rostopic = rostopic;
+	this->_topic[0] = 0x0;
+}
+
+Zenoh_Publisher::~Zenoh_Publisher()
+{
+	undeclare_publisher();
+}
+
+int Zenoh_Publisher::undeclare_publisher()
+{
+	z_undeclare_publisher(z_publisher_move(&_pub));
+	return 0;
+}
+
+int Zenoh_Publisher::declare_publisher(z_session_t s, const char *keyexpr)
+{
+	if (_rostopic) {
+		strncpy(this->_topic, (char *)_rt_prefix, _rt_prefix_offset);
+
+		if (keyexpr[0] == '/') {
+			strncpy(this->_topic + _rt_prefix_offset, keyexpr + 1, sizeof(this->_topic) - _rt_prefix_offset);
+
+		} else {
+			strncpy(this->_topic + _rt_prefix_offset, keyexpr, sizeof(this->_topic) - _rt_prefix_offset);
+		}
+
+	} else {
+		strncpy(this->_topic, keyexpr, sizeof(this->_topic));
+	}
+
+	_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;
+	}
+
+	return 0;
+}
+
+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);
+}
+
+void Zenoh_Publisher::print()
+{
+	printf("Topic: %s\n", this->_topic);
+}

src/modules/zenoh/publishers/zenoh_publisher.hpp

@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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_publisher.hpp
+ *
+ * Defines basic functionality of Zenoh publisher class
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/log.h>
+
+#include <lib/parameters/param.h>
+#include <containers/List.hpp>
+#include <zenoh-pico.h>
+
+class Zenoh_Publisher : public ListNode<Zenoh_Publisher *>
+{
+public:
+	Zenoh_Publisher(bool rostopic = true);
+	virtual ~Zenoh_Publisher();
+
+	virtual int declare_publisher(z_session_t s, const char *keyexpr);
+
+	virtual int undeclare_publisher();
+
+	virtual int8_t update() = 0;
+
+	virtual void print();
+
+protected:
+	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

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 uorb_subscriber.hpp
+ *
+ * Defines generic, templatized uORB over Zenoh / ROS2
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include "zenoh_subscriber.hpp"
+#include <uORB/topics/input_rc.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/actuator_outputs.h>
+
+class uORB_Zenoh_Subscriber : public Zenoh_Subscriber
+{
+public:
+	uORB_Zenoh_Subscriber(const orb_metadata *meta, const uint32_t *ops) :
+		Zenoh_Subscriber(true),
+		_uorb_meta{meta},
+		_cdr_ops(ops)
+	{
+		int instance = 0;
+		_uorb_pub_handle = orb_advertise_multi_queue(_uorb_meta, nullptr, &instance, 1); //FIXME template magic qsize
+	};
+
+	~uORB_Zenoh_Subscriber() override = default;
+
+	// Update the uORB Subscription and broadcast a Zenoh ROS2 message
+	void data_handler(const z_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),
+				    .m_index = 4, .m_xcdr_version = DDSI_RTPS_CDR_ENC_VERSION_2
+				   };
+		dds_stream_read(&is, data, &dds_allocator, _cdr_ops);
+
+		// As long as we don't have timesynchronization between Zenoh nodes
+		// we've to manually set the timestamp
+		fix_timestamp(data);
+
+		// ORB_ID::input_rc needs additional timestamp fixup
+		if (static_cast<ORB_ID>(_uorb_meta->o_id) == ORB_ID::input_rc) {
+			memcpy(&data[8], data, sizeof(hrt_abstime));
+		}
+
+		orb_publish(_uorb_meta, _uorb_pub_handle, &data);
+	};
+
+	void fix_timestamp(char *data)
+	{
+		hrt_abstime now = hrt_absolute_time();
+		memcpy(data, &now, sizeof(hrt_abstime));
+	}
+
+	void print()
+	{
+		Zenoh_Subscriber::print("uORB", _uorb_meta->o_name);
+	}
+
+protected:
+	// Default payload-size function -- can specialize in derived class
+	size_t get_payload_size()
+	{
+		return _uorb_meta->o_size;
+	}
+
+private:
+	const orb_metadata *_uorb_meta;
+	orb_advert_t _uorb_pub_handle;
+	const uint32_t *_cdr_ops;
+};

src/modules/zenoh/subscribers/zenoh_subscriber.cpp

@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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_subscriber.cpp
+ *
+ * Zenoh subscriber
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#include "zenoh_subscriber.hpp"
+
+static void data_handler_cb(const z_sample_t *sample, void *arg)
+{
+	static_cast<Zenoh_Subscriber *>(arg)->data_handler(sample);
+}
+
+void Zenoh_Subscriber::data_handler(const z_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));
+}
+
+
+Zenoh_Subscriber::Zenoh_Subscriber(bool rostopic)
+{
+	this->_rostopic = rostopic;
+	this->_topic[0] = 0x0;
+}
+
+Zenoh_Subscriber::~Zenoh_Subscriber()
+{
+	undeclare_subscriber();
+}
+
+int Zenoh_Subscriber::undeclare_subscriber()
+{
+	z_undeclare_subscriber(z_subscriber_move(&_sub));
+	return 0;
+}
+
+int Zenoh_Subscriber::declare_subscriber(z_session_t s, const char *keyexpr)
+{
+	z_owned_closure_sample_t callback = z_closure_sample(data_handler_cb, NULL, this);
+
+	if (_rostopic) {
+		strncpy(this->_topic, (char *)_rt_prefix, _rt_prefix_offset);
+
+		if (keyexpr[0] == '/') {
+			strncpy(this->_topic + _rt_prefix_offset, keyexpr + 1, sizeof(this->_topic) - _rt_prefix_offset);
+
+		} else {
+			strncpy(this->_topic + _rt_prefix_offset, keyexpr, sizeof(this->_topic) - _rt_prefix_offset);
+		}
+
+	} else {
+		strncpy(this->_topic, (char *)keyexpr, sizeof(this->_topic));
+	}
+
+	_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;
+	}
+
+	return 0;
+}
+
+void Zenoh_Subscriber::print()
+{
+	printf("Topic: %s\n", this->_topic);
+}
+
+void Zenoh_Subscriber::print(const char *type_string, const char *topic_string)
+{
+	printf("Topic: %s -> %s %s \n", this->_topic, type_string, topic_string);
+}

src/modules/zenoh/subscribers/zenoh_subscriber.hpp

@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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_subscriber.hpp
+ *
+ * Defines basic functionality of Zenoh subscriber class
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/log.h>
+
+#include <lib/parameters/param.h>
+#include <containers/List.hpp>
+#include <zenoh-pico.h>
+
+// CycloneDDS CDR Deserializer
+#include <dds/cdr/dds_cdrstream.h>
+#include <dds_serializer.h>
+
+class Zenoh_Subscriber : public ListNode<Zenoh_Subscriber *>
+{
+public:
+	Zenoh_Subscriber(bool rostopic = true);
+	virtual ~Zenoh_Subscriber();
+
+	virtual int declare_subscriber(z_session_t s, const char *keyexpr);
+
+	virtual int undeclare_subscriber();
+
+	virtual void data_handler(const z_sample_t *sample);
+
+	virtual void print();
+
+protected:
+	virtual void  print(const char *type_string, const char *topic_string);
+
+	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

@@ -0,0 +1,290 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+#include "zenoh.h"
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/cli.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <ctype.h>
+#include <string.h>
+
+#include <zenoh-pico.h>
+
+// CycloneDDS CDR Deserializer
+#include <dds/cdr/dds_cdrstream.h>
+
+// Auto-generated header to all uORB <-> CDR conversions
+#include <uorb_pubsub_factory.hpp>
+
+
+#define Z_PUBLISH
+#define Z_SUBSCRIBE
+
+extern "C" __EXPORT int zenoh_main(int argc, char *argv[]);
+
+ZENOH::ZENOH():
+	ModuleParams(nullptr)
+{
+
+}
+
+ZENOH::~ZENOH()
+{
+
+}
+
+void ZENOH::run()
+{
+	char mode[NET_MODE_SIZE];
+	char locator[NET_LOCATOR_SIZE];
+	int8_t ret;
+	int i;
+
+	Zenoh_Config z_config;
+
+	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));
+
+	if (locator[0] != 0) {
+		zp_config_insert(z_config_loan(&config), Z_CONFIG_PEER_KEY, z_string_make(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));
+	}
+
+	PX4_INFO("Opening session...");
+	z_owned_session_t s = z_open(z_config_move(&config));
+
+	if (!z_session_check(&s)) {
+		PX4_ERR("Unable to open session!");
+		return;
+	}
+
+	// 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) {
+		PX4_ERR("Unable to start read and lease tasks");
+		return;
+	}
+
+#ifdef Z_SUBSCRIBE
+	_sub_count =  z_config.getSubCount();
+	_zenoh_subscribers = (Zenoh_Subscriber **)malloc(sizeof(Zenoh_Subscriber *)*_sub_count);
+	{
+		char topic[TOPIC_INFO_SIZE];
+		char type[TOPIC_INFO_SIZE];
+
+		for (i = 0; i < _sub_count; i++) {
+			z_config.getSubscriberMapping(topic, type);
+			_zenoh_subscribers[i] = genSubscriber(type);
+
+			if (_zenoh_subscribers[i] != 0) {
+				_zenoh_subscribers[i]->declare_subscriber(z_session_loan(&s), topic);
+			}
+
+
+		}
+
+		if (z_config.getSubscriberMapping(topic, type) < 0) {
+			PX4_WARN("Subscriber mapping parsing error");
+		}
+	}
+#endif
+
+#ifdef Z_PUBLISH
+
+	_pub_count =  z_config.getPubCount();
+	_zenoh_publishers = (uORB_Zenoh_Publisher **)malloc(_pub_count * sizeof(uORB_Zenoh_Publisher *));
+	px4_pollfd_struct_t pfds[_pub_count];
+
+	{
+		char topic[TOPIC_INFO_SIZE];
+		char type[TOPIC_INFO_SIZE];
+
+		for (i = 0; i < _pub_count; i++) {
+			z_config.getPublisherMapping(topic, type);
+			_zenoh_publishers[i] = genPublisher(type);
+
+			if (_zenoh_publishers[i] != 0) {
+				_zenoh_publishers[i]->declare_publisher(z_session_loan(&s), topic);
+				_zenoh_publishers[i]->setPollFD(&pfds[i]);
+			}
+		}
+
+		if (z_config.getSubscriberMapping(topic, type) < 0) {
+			PX4_WARN("Publisher mapping parsing error");
+		}
+	}
+
+	if (_pub_count == 0) {
+		// Nothing to publish but we don't want to stop this thread
+		while (!should_exit()) {
+			sleep(2);
+		}
+	}
+
+	while (!should_exit()) {
+		int pret = px4_poll(pfds, _pub_count, 100);
+
+		if (pret == 0) {
+			//PX4_INFO("Zenoh poll timeout\n");
+
+		} else {
+			for (i = 0; i < _pub_count; i++) {
+				if (pfds[i].revents & POLLIN) {
+					ret = _zenoh_publishers[i]->update();
+
+					if (ret < 0) {
+						PX4_WARN("Publisher error %i", ret);
+
+					}
+				}
+			}
+		}
+	}
+
+#endif
+
+	// Exiting cleaning up publisher and subscribers
+	for (i = 0; i < _sub_count; i++) {
+		delete _zenoh_subscribers[i];
+	}
+
+	free(_zenoh_subscribers);
+
+	for (i = 0; i < _pub_count; i++) {
+		delete _zenoh_publishers[i];
+	}
+
+	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));
+
+	z_close(z_session_move(&s));
+	exit_and_cleanup();
+}
+
+int ZENOH::custom_command(int argc, char *argv[])
+{
+	if (argc > 0 && strcmp("config", argv[0]) == 0) {
+		Zenoh_Config z_config;
+
+		if (z_config.cli(argc, argv) == 0) {
+			return 0;
+		}
+	}
+
+	return print_usage("Unrecognized command.");
+}
+
+int ZENOH::print_usage(const char *reason)
+{
+	if (reason) {
+		printf("%s\n\n", reason);
+	}
+
+	PRINT_MODULE_USAGE_NAME("zenoh", "driver");
+	PRINT_MODULE_DESCRIPTION(R"DESC_STR(
+### Description
+
+Zenoh demo bridge
+	)DESC_STR");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_COMMAND("stop");
+	PRINT_MODULE_USAGE_COMMAND("status");
+	PRINT_MODULE_USAGE_COMMAND("config");
+	PX4_INFO_RAW("     addpublisher  <zenoh_topic> <uorb_topic>  Publish uORB topic to Zenoh\n");
+	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");
+	return 0;
+}
+
+int ZENOH::print_status()
+{
+	PX4_INFO("running");
+
+	PX4_INFO("Publishers");
+
+	for (int i = 0; i < _pub_count; i++) {
+		_zenoh_publishers[i]->print();
+	}
+
+	PX4_INFO("Subscribers");
+
+	for (int i = 0; i < _sub_count; i++) {
+		_zenoh_subscribers[i]->print();
+	}
+
+	return 0;
+}
+
+int ZENOH::task_spawn(int argc, char *argv[])
+{
+
+	int task_id = px4_task_spawn_cmd(
+			      "zenoh",
+			      SCHED_DEFAULT,
+			      SCHED_PRIORITY_DEFAULT,
+			      4096,
+			      &run_trampoline,
+			      argv
+		      );
+
+	if (task_id < 0) {
+		return -errno;
+
+	} else {
+		_task_id = task_id;
+		return 0;
+	}
+}
+
+ZENOH *ZENOH::instantiate(int argc, char *argv[])
+{
+	return new ZENOH();
+}
+
+int zenoh_main(int argc, char *argv[])
+{
+	return ZENOH::main(argc, argv);
+}

src/modules/zenoh/zenoh.h

@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+#ifndef ZENOH_MODULE_H
+#define ZENOH_MODULE_H
+
+#include <termios.h>
+#include <poll.h>
+#include <sys/select.h>
+#include <sys/time.h>
+
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/module.h>
+#include <perf/perf_counter.h>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/parameter_update.h>
+
+#include "zenoh_config.hpp"
+#include "publishers/uorb_publisher.hpp"
+#include "subscribers/uorb_subscriber.hpp"
+
+class ZENOH : public ModuleBase<ZENOH>, public ModuleParams
+{
+public:
+	ZENOH();
+
+	~ZENOH();
+
+
+	/**
+	 * @see ModuleBase::custom_command
+	 */
+	static int custom_command(int argc, char *argv[]);
+
+	/**
+	 * @see ModuleBase::print_usage
+	 */
+	static int print_usage(const char *reason = nullptr);
+
+	/**
+	 * @see ModuleBase::print_usage
+	 */
+	int print_status();
+
+	/**
+	 * @see ModuleBase::task_spawn
+	 */
+	static int task_spawn(int argc, char *argv[]);
+
+	static ZENOH *instantiate(int argc, char *argv[]);
+
+	void run() override;
+
+private:
+
+	Zenoh_Config _config;
+
+	int _pub_count;
+	uORB_Zenoh_Publisher **_zenoh_publishers;
+	int _sub_count;
+	Zenoh_Subscriber **_zenoh_subscribers;
+
+};
+
+#endif //ZENOH_MODULE_H

src/modules/zenoh/zenoh_config.cpp

@@ -0,0 +1,409 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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_config.cpp
+ *
+ * Zenoh configuration backend
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#include "zenoh_config.hpp"
+
+#include <dirent.h>
+#include <sys/stat.h>
+#include <errno.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include <uORB/topics/uORBTopics.hpp>
+
+
+const char *default_net_config = Z_CONFIG_MODE_DEFAULT;
+const char *default_pub_config = "";
+const char *default_sub_config = ""; //TODO maybe use YAML
+
+
+Zenoh_Config::Zenoh_Config()
+{
+	bool correct_config = true;
+	DIR *dir = opendir(ZENOH_SD_ROOT_PATH);
+	fp_mapping = NULL;
+
+	if (dir) {
+		/* Directory exists. */
+		closedir(dir);
+
+		if (access(ZENOH_NET_CONFIG_PATH, F_OK) != 0) {
+			correct_config = false;
+
+		} else if (access(ZENOH_PUB_CONFIG_PATH, F_OK) != 0) {
+			correct_config = false;
+
+		} else if (access(ZENOH_SUB_CONFIG_PATH, F_OK) != 0) {
+			correct_config = false;
+		}
+
+	} else {
+		/* opendir() failed */
+		correct_config = false;
+	}
+
+	if (!correct_config) {
+		generate_clean_config();
+	}
+}
+
+Zenoh_Config::~Zenoh_Config()
+{
+	if (fp_mapping != NULL) {
+		fclose(fp_mapping);
+	}
+}
+
+int Zenoh_Config::AddPubSub(char *topic, char *datatype, const char *filename)
+{
+	{
+		char f_topic[TOPIC_INFO_SIZE];
+		char f_type[TOPIC_INFO_SIZE];
+
+		while (getPubSubMapping(f_topic, f_type, filename) > 0) {
+			if (strcmp(topic, f_topic) == 0
+			    || strcmp(datatype, f_type) == 0) {
+				printf("Already mapped to uORB %s -> %s\n", f_type, f_topic);
+				return 0;
+			}
+
+		}
+	}
+
+	for (size_t i = 0; i < orb_topics_count(); i++) {
+		const struct orb_metadata *meta = get_orb_meta((ORB_ID)i);
+
+		if (meta != NULL &&
+		    strcmp(meta->o_name, datatype) == 0) {
+			FILE *fp = fopen(filename, "a");
+
+			if (fp) {
+				fprintf(fp, "%s;%s\n", topic, datatype);
+
+			} else {
+				return -1;
+			}
+
+			fclose(fp);
+			return 1;
+		}
+	}
+
+	printf("%s not found\n", datatype);
+	return 0;
+}
+
+
+int Zenoh_Config::SetNetworkConfig(char *mode, char *locator)
+{
+
+	FILE *fp = fopen(ZENOH_NET_CONFIG_PATH, "w");
+
+	if (fp) {
+		if (locator == 0) {
+			fprintf(fp, "%s\n", mode);
+
+		} else {
+			fprintf(fp, "%s;%s\n", mode, locator);
+		}
+
+	} else {
+		return -1;
+	}
+
+	fclose(fp);
+	return 0;
+}
+
+int Zenoh_Config::cli(int argc, char *argv[])
+{
+	if (argc == 1) {
+		dump_config();
+
+	} else if (argc == 3) {
+		if (strcmp(argv[1], "net") == 0) {
+			SetNetworkConfig(argv[2], 0);
+		}
+
+	} else if (argc == 4) {
+		if (strcmp(argv[1], "addpublisher") == 0) {
+			if (AddPubSub(argv[2], argv[3], ZENOH_PUB_CONFIG_PATH) > 0) {
+				printf("Added %s %s to publishers\n", argv[2], argv[3]);
+
+			} else {
+				printf("Could not add uORB %s -> %s to publishers\n",  argv[3], argv[2]);
+			}
+
+		} else if (strcmp(argv[1], "addsubscriber") == 0) {
+			if (AddPubSub(argv[2], argv[3], ZENOH_SUB_CONFIG_PATH) > 0) {
+				printf("Added %s -> uORB %s to subscribers\n", argv[2], argv[3]);
+
+			} else {
+				printf("Could not add %s -> uORB %s to subscribers\n",  argv[2], argv[3]);
+			}
+
+		} else if (strcmp(argv[1], "net") == 0) {
+			SetNetworkConfig(argv[2], argv[3]);
+		}
+	}
+
+	//TODO make CLI to modify configuration now you would have to manually modify the files
+	return 0;
+}
+
+const char *Zenoh_Config::get_csv_field(char *line, int num)
+{
+	const char *tok;
+
+	for (
+		tok = strtok(line, ";");
+		tok && *tok;
+		tok = strtok(NULL, ";\n")) {
+		if (!--num) {
+			return tok;
+		}
+	}
+
+	return NULL;
+}
+
+void Zenoh_Config::getNetworkConfig(char *mode, char *locator)
+{
+	FILE *fp;
+	char buffer[NET_CONFIG_LINE_SIZE];
+
+	fp = fopen(ZENOH_NET_CONFIG_PATH, "r");
+
+	// If file opened successfully, then read the file
+	if (fp) {
+		fgets(buffer, NET_CONFIG_LINE_SIZE, fp);
+		const char *config_locator = get_csv_field(buffer, 2);
+		char *config_mode = (char *)get_csv_field(buffer, 1);
+
+		if (config_mode) {
+			config_mode[strcspn(config_mode, "\n")] = 0;
+			strncpy(mode, config_mode, NET_MODE_SIZE);
+
+		} else {
+			mode[0] = 0;
+		}
+
+		if (config_locator) {
+			strncpy(locator, config_locator, NET_LOCATOR_SIZE);
+
+		} else {
+			locator[0] = 0;
+		}
+
+	} else {
+		printf("Failed to open the file\n");
+	}
+
+	//Close the file
+	fclose(fp);
+}
+
+int Zenoh_Config::getLineCount(const char *filename)
+{
+	int lines = 0;
+	int ch;
+
+	// Open file in write mode
+	FILE *fp = fopen(filename, "r");
+
+	while ((ch = fgetc(fp)) != EOF) {
+		if (ch == '\n') {
+			lines++;
+		}
+	}
+
+	//Close the file
+	fclose(fp);
+
+	return lines;
+}
+
+// Very rudamentary here but we've to wait for a more advanced param system
+int Zenoh_Config::getPubSubMapping(char *topic, char *type, const char *filename)
+{
+	char buffer[MAX_LINE_SIZE];
+
+	if (fp_mapping == NULL) {
+		fp_mapping = fopen(filename, "r");
+	}
+
+	if (fp_mapping) {
+		while (fgets(buffer, MAX_LINE_SIZE, fp_mapping) != NULL) {
+			if (buffer[0] != '\n') {
+				const char *config_type = get_csv_field(buffer, 2);
+				const char *config_topic = get_csv_field(buffer, 1);
+
+				strncpy(type, config_type, TOPIC_INFO_SIZE);
+				strncpy(topic, config_topic, TOPIC_INFO_SIZE);
+				return 1;
+			}
+
+		}
+
+	} else {
+		printf("Failed to open the file\n");
+		return -1;
+	}
+
+	//Close the file
+	fclose(fp_mapping);
+	fp_mapping = NULL;
+	return 0;
+
+}
+
+
+void Zenoh_Config::dump_config()
+{
+	printf("Network config:\n");
+	{
+		char mode[NET_MODE_SIZE];
+		char locator[NET_LOCATOR_SIZE];
+		getNetworkConfig(mode, locator);
+
+		printf("Mode: %s\n", mode);
+
+		if (locator[0] == 0) {
+			printf("Locator: scout\n");
+
+		} else {
+			printf("Locator: %s\n", locator);
+		}
+
+		printf("\n");
+	}
+
+	{
+		char topic[TOPIC_INFO_SIZE];
+		char type[TOPIC_INFO_SIZE];
+
+		printf("Publisher config:\n");
+
+		while (getPubSubMapping(topic, type, ZENOH_PUB_CONFIG_PATH) > 0) {
+			printf("Topic: %s\n", topic);
+			printf("Type: %s\n", type);
+		}
+
+		printf("\nSubscriber config:\n");
+
+		while (getPubSubMapping(topic, type, ZENOH_SUB_CONFIG_PATH) > 0) {
+			printf("Topic: %s\n", topic);
+			printf("Type: %s\n", type);
+		}
+	}
+}
+
+
+void Zenoh_Config::generate_clean_config()
+{
+	printf("Generate clean\n");
+	FILE *fp;
+
+	DIR *dir = opendir(ZENOH_SD_ROOT_PATH);
+
+	if (dir) {
+		printf("Zenoh directory exists\n");
+
+	} else {
+		/* Create zenoh dir. */
+		if (mkdir(ZENOH_SD_ROOT_PATH, 0700) < 0) {
+			printf("Failed to create Zenoh directory\n");
+			return;
+		}
+
+	}
+
+	if (access(ZENOH_NET_CONFIG_PATH, F_OK) != 0) {
+		// Open file in write mode
+		fp = fopen(ZENOH_NET_CONFIG_PATH, "w");
+
+		// If file opened successfully, then write the string to file
+		if (fp) {
+			fputs(default_net_config, fp);
+
+		} else {
+			printf("Failed to open the file\n");
+			return;
+		}
+
+		//Close the file
+		fclose(fp);
+	}
+
+	if (access(ZENOH_PUB_CONFIG_PATH, F_OK) != 0) {
+		// Open file in write mode
+		fp = fopen(ZENOH_PUB_CONFIG_PATH, "w");
+
+		// If file opened successfully, then write the string to file
+		if (fp) {
+			fputs(default_pub_config, fp);
+
+		} else {
+			printf("Failed to open the file\n");
+			return;
+		}
+
+		//Close the file
+		fclose(fp);
+	}
+
+	if (access(ZENOH_SUB_CONFIG_PATH, F_OK) != 0) {
+		// Open file in write mode
+		fp = fopen(ZENOH_SUB_CONFIG_PATH, "w");
+
+		// If file opened successfully, then write the string to file
+		if (fp) {
+			fputs(default_sub_config, fp);
+
+		} else {
+			printf("Failed to open the file\n");
+			return;
+		}
+
+		//Close the file
+		fclose(fp);
+	}
+}

src/modules/zenoh/zenoh_config.hpp

@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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_config.hpp
+ *
+ * Defines Zenoh configuration backend
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/log.h>
+
+#include <lib/parameters/param.h>
+#include <containers/List.hpp>
+#include <zenoh-pico.h>
+
+#define ZENOH_MAX_PATH_LENGTH (128 + 40)
+#define ZENOH_SD_ROOT_PATH    CONFIG_ZENOH_CONFIG_PATH
+#define ZENOH_PUB_CONFIG_PATH ZENOH_SD_ROOT_PATH"/pub.csv"
+#define ZENOH_SUB_CONFIG_PATH ZENOH_SD_ROOT_PATH"/sub.csv"
+#define ZENOH_NET_CONFIG_PATH ZENOH_SD_ROOT_PATH"/net.txt"
+
+#define NET_MODE_SIZE sizeof("client")
+#define NET_LOCATOR_SIZE 64
+#define NET_CONFIG_LINE_SIZE NET_MODE_SIZE + NET_LOCATOR_SIZE
+#define TOPIC_INFO_SIZE 64
+#define MAX_LINE_SIZE 2*TOPIC_INFO_SIZE
+
+class Zenoh_Config
+{
+public:
+	Zenoh_Config();
+	~Zenoh_Config();
+
+	int cli(int argc, char *argv[]);
+
+	void getNetworkConfig(char *mode, char *locator);
+	int getPubCount()
+	{
+		return getLineCount(ZENOH_PUB_CONFIG_PATH);
+	}
+	int getSubCount()
+	{
+		return getLineCount(ZENOH_SUB_CONFIG_PATH);
+	}
+	int getPublisherMapping(char *topic, char *type)
+	{
+		return getPubSubMapping(topic, type, ZENOH_PUB_CONFIG_PATH);
+	}
+	int getSubscriberMapping(char *topic, char *type)
+	{
+		return getPubSubMapping(topic, type, ZENOH_SUB_CONFIG_PATH);
+	}
+
+
+private:
+	int getPubSubMapping(char *topic, char *type, const char *filename);
+	int AddPubSub(char *topic, char *datatype, const char *filename);
+	int SetNetworkConfig(char *mode, char *locator);
+	int getLineCount(const char *filename);
+
+	const char *get_csv_field(char *line, int num);
+	void generate_clean_config();
+	void dump_config();
+
+	FILE *fp_mapping;
+
+
+};