From 019d232911d55383aed200050a36f29de173c175 Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Wed, 5 Jul 2023 11:01:30 +0200 Subject: [PATCH] Add Zenoh pico support --- .gitmodules | 10 + Kconfig | 2 + ROMFS/px4fmu_common/init.d/rcS | 4 + Tools/astyle/files_to_check_code_style.sh | 3 + Tools/msg/px_generate_uorb_topic_files.py | 16 +- .../templates/cdrstream/uorb_idl_header.h.em | 65 ++ Tools/zenoh/px_generate_zenoh_topic_files.py | 175 ++++ Tools/zenoh/templates/zenoh/Kconfig.topics.em | 25 + .../zenoh/uorb_pubsub_factory.hpp.em | 154 +++ .../mr-canhubk3/nuttx-config/nsh/defconfig | 2 + boards/nxp/mr-canhubk3/zenoh.px4board | 58 ++ boards/px4/fmu-v6x/zenoh.px4board | 4 + boards/px4/sitl/zenoh.px4board | 3 + msg/CMakeLists.txt | 125 +++ platforms/common/uORB/CMakeLists.txt | 4 + src/lib/CMakeLists.txt | 1 + src/lib/cdrstream/CMakeLists.txt | 49 + src/lib/cdrstream/Kconfig | 5 + src/lib/cdrstream/cyclonedds | 1 + src/lib/cdrstream/dds_serializer.c | 53 + src/lib/cdrstream/dds_serializer.h | 43 + src/lib/cdrstream/msg2idl.py | 57 ++ src/lib/cdrstream/rosidl | 1 + src/modules/zenoh/CMakeLists.txt | 100 ++ src/modules/zenoh/Kconfig | 73 ++ src/modules/zenoh/Kconfig.topics | 960 ++++++++++++++++++ src/modules/zenoh/module.yaml | 14 + .../zenoh/publishers/uorb_publisher.hpp | 105 ++ .../zenoh/publishers/zenoh_publisher.cpp | 98 ++ .../zenoh/publishers/zenoh_publisher.hpp | 76 ++ .../zenoh/subscribers/uorb_subscriber.hpp | 107 ++ .../zenoh/subscribers/zenoh_subscriber.cpp | 111 ++ .../zenoh/subscribers/zenoh_subscriber.hpp | 80 ++ src/modules/zenoh/zenoh-pico | 1 + src/modules/zenoh/zenoh.cpp | 290 ++++++ src/modules/zenoh/zenoh.h | 95 ++ src/modules/zenoh/zenoh_config.cpp | 409 ++++++++ src/modules/zenoh/zenoh_config.hpp | 103 ++ 38 files changed, 3479 insertions(+), 3 deletions(-) create mode 100644 Tools/msg/templates/cdrstream/uorb_idl_header.h.em create mode 100755 Tools/zenoh/px_generate_zenoh_topic_files.py create mode 100644 Tools/zenoh/templates/zenoh/Kconfig.topics.em create mode 100644 Tools/zenoh/templates/zenoh/uorb_pubsub_factory.hpp.em create mode 100644 boards/nxp/mr-canhubk3/zenoh.px4board create mode 100644 boards/px4/fmu-v6x/zenoh.px4board create mode 100644 boards/px4/sitl/zenoh.px4board create mode 100644 src/lib/cdrstream/CMakeLists.txt create mode 100644 src/lib/cdrstream/Kconfig create mode 160000 src/lib/cdrstream/cyclonedds create mode 100644 src/lib/cdrstream/dds_serializer.c create mode 100644 src/lib/cdrstream/dds_serializer.h create mode 100644 src/lib/cdrstream/msg2idl.py create mode 160000 src/lib/cdrstream/rosidl create mode 100644 src/modules/zenoh/CMakeLists.txt create mode 100644 src/modules/zenoh/Kconfig create mode 100644 src/modules/zenoh/Kconfig.topics create mode 100644 src/modules/zenoh/module.yaml create mode 100644 src/modules/zenoh/publishers/uorb_publisher.hpp create mode 100644 src/modules/zenoh/publishers/zenoh_publisher.cpp create mode 100644 src/modules/zenoh/publishers/zenoh_publisher.hpp create mode 100644 src/modules/zenoh/subscribers/uorb_subscriber.hpp create mode 100644 src/modules/zenoh/subscribers/zenoh_subscriber.cpp create mode 100644 src/modules/zenoh/subscribers/zenoh_subscriber.hpp create mode 160000 src/modules/zenoh/zenoh-pico create mode 100644 src/modules/zenoh/zenoh.cpp create mode 100644 src/modules/zenoh/zenoh.h create mode 100644 src/modules/zenoh/zenoh_config.cpp create mode 100644 src/modules/zenoh/zenoh_config.hpp diff --git a/.gitmodules b/.gitmodules index 78c25c2335..8b26803957 100644 --- a/.gitmodules +++ b/.gitmodules @@ -62,3 +62,13 @@ path = src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client url = https://github.com/PX4/Micro-XRCE-DDS-Client.git branch = px4 +[submodule "src/lib/cdrstream/cyclonedds"] + path = src/lib/cdrstream/cyclonedds + url = https://github.com/px4/cyclonedds +[submodule "src/lib/cdrstream/rosidl"] + path = src/lib/cdrstream/rosidl + url = https://github.com/px4/rosidl +[submodule "src/modules/zenoh/zenoh-pico"] + path = src/modules/zenoh/zenoh-pico + url = https://github.com/px4/zenoh-pico + branch = pr-zubf-werror-fix diff --git a/Kconfig b/Kconfig index 0bcdf14ff9..6427534f91 100644 --- a/Kconfig +++ b/Kconfig @@ -205,3 +205,5 @@ menu "platforms" depends on PLATFORM_QURT || PLATFORM_POSIX source "platforms/common/Kconfig" endmenu + +source "src/lib/*/Kconfig" diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 7d53e5b71d..1d75bf12a2 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -537,6 +537,10 @@ else cyphal start fi fi + if param greater -s ZENOH_ENABLE 0 + then + zenoh start + fi # # End of autostart. diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index 6704ac262f..895ffe6b33 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -26,4 +26,7 @@ exec find boards msg src platforms test \ -path src/lib/crypto/libtomcrypt -prune -o \ -path src/lib/crypto/libtommath -prune -o \ -path src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client -prune -o \ + -path src/lib/cdrstream/cyclonedds -prune -o \ + -path src/lib/cdrstream/rosidl -prune -o \ + -path src/drivers/zenoh/zenoh-pico -prune -o \ -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN diff --git a/Tools/msg/px_generate_uorb_topic_files.py b/Tools/msg/px_generate_uorb_topic_files.py index 4a26cbf10a..fe2a0e0d33 100755 --- a/Tools/msg/px_generate_uorb_topic_files.py +++ b/Tools/msg/px_generate_uorb_topic_files.py @@ -70,9 +70,9 @@ __license__ = "BSD" __email__ = "thomasgubler@gmail.com" -TEMPLATE_FILE = ['msg.h.em', 'msg.cpp.em'] +TEMPLATE_FILE = ['msg.h.em', 'msg.cpp.em', 'uorb_idl_header.h.em'] TOPICS_LIST_TEMPLATE_FILE = ['uORBTopics.hpp.em', 'uORBTopics.cpp.em'] -OUTPUT_FILE_EXT = ['.h', '.cpp'] +OUTPUT_FILE_EXT = ['.h', '.cpp', '.h'] INCL_DEFAULT = ['std_msgs:./msg/std_msgs'] PACKAGE = 'px4' TOPICS_TOKEN = '# TOPICS ' @@ -150,6 +150,7 @@ def generate_output_from_file(format_idx, filename, outputdir, package, template em_globals = { "name_snake_case": full_type_name_snake, "file_name_in": filename, + "file_base_name": file_base_name, "search_path": search_path, "msg_context": msg_context, "spec": spec, @@ -161,7 +162,10 @@ def generate_output_from_file(format_idx, filename, outputdir, package, template os.makedirs(outputdir) template_file = os.path.join(templatedir, TEMPLATE_FILE[format_idx]) - output_file = os.path.join(outputdir, full_type_name_snake + OUTPUT_FILE_EXT[format_idx]) + if format_idx == 2: + output_file = os.path.join(outputdir, file_base_name + OUTPUT_FILE_EXT[format_idx]) + else: + output_file = os.path.join(outputdir, full_type_name_snake + OUTPUT_FILE_EXT[format_idx]) return generate_by_template(output_file, template_file, em_globals) @@ -217,6 +221,7 @@ if __name__ == "__main__": parser = argparse.ArgumentParser(description='Convert msg files to uorb headers/sources') parser.add_argument('--headers', help='Generate header files', action='store_true') parser.add_argument('--sources', help='Generate source files', action='store_true') + parser.add_argument('--uorb-idl-header', help='Generate uORB compatible idl header', action='store_true') parser.add_argument('-f', dest='file', help="files to convert (use only without -d)", nargs="+") @@ -241,6 +246,11 @@ if __name__ == "__main__": generate_idx = 0 elif args.sources: generate_idx = 1 + elif args.uorb_idl_header: + for f in args.file: + print(f) + generate_output_from_file(2, f, args.outputdir, args.package, args.templatedir, INCL_DEFAULT) + exit(0) else: print('Error: either --headers or --sources must be specified') exit(-1) diff --git a/Tools/msg/templates/cdrstream/uorb_idl_header.h.em b/Tools/msg/templates/cdrstream/uorb_idl_header.h.em new file mode 100644 index 0000000000..4ad097a54c --- /dev/null +++ b/Tools/msg/templates/cdrstream/uorb_idl_header.h.em @@ -0,0 +1,65 @@ +@{ +import genmsg.msgs +import re + +from px_generate_uorb_topic_helper import * # this is in Tools/ + +uorb_struct = '%s_s'%name_snake_case +uorb_struct_upper = name_snake_case.upper() +}@ + +/**************************************************************** + + PX4 Cyclone DDS IDL to C Translator compatible idl struct + Source: @file_name_in + Compatible with Cyclone DDS: V0.11.0 + +*****************************************************************/ +#ifndef DDSC_IDL_UORB_@(uorb_struct_upper)_H +#define DDSC_IDL_UORB_@(uorb_struct_upper)_H + +#include "dds/ddsc/dds_public_impl.h" +#include "dds/cdr/dds_cdrstream.h" +#include + +@############################## +@# Includes for dependencies +@############################## +@{ +for field in spec.parsed_fields(): + if (not field.is_builtin): + if (not field.is_header): + (package, name) = genmsg.names.package_resource_name(field.base_type) + package = package or spec.package # convert '' to package + + print('#include "%s.h"'%(name)) + name = re.sub(r'(?'%(name)) +}@ + + +#ifdef __cplusplus +extern "C" { +#endif + +@{ +for field in spec.parsed_fields(): + if (not field.is_builtin): + if (not field.is_header): + (package, name) = genmsg.names.package_resource_name(field.base_type) + package = package or spec.package # convert '' to package + + print('typedef px4_msg_%s px4_msg_px4__msg__%s;' % (name,name)) +}@ + + + +typedef struct @uorb_struct px4_msg_@(file_base_name); + +extern const struct dds_cdrstream_desc px4_msg_@(file_base_name)_cdrstream_desc; + +#ifdef __cplusplus +} +#endif + +#endif /* DDSC_IDL_UORB_@(uorb_struct_upper)_H */ diff --git a/Tools/zenoh/px_generate_zenoh_topic_files.py b/Tools/zenoh/px_generate_zenoh_topic_files.py new file mode 100755 index 0000000000..b9110dbea8 --- /dev/null +++ b/Tools/zenoh/px_generate_zenoh_topic_files.py @@ -0,0 +1,175 @@ +#!/usr/bin/env python3 +############################################################################# +# +# Copyright (C) 2013-2022 PX4 Pro 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. +# +############################################################################# + +""" +px_generate_zenoh_topic_files.py +Generates c/cpp header/source files for use with zenoh +message files +""" + +import os +import argparse +import re +import sys + +try: + import em +except ImportError as e: + print("Failed to import em: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user empy") + print("") + sys.exit(1) + +try: + import genmsg.template_tools +except ImportError as e: + print("Failed to import genmsg: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user pyros-genmsg") + print("") + sys.exit(1) + + +__author__ = "Sergey Belash, Thomas Gubler, Beat Kueng" +__copyright__ = "Copyright (C) 2013-2022 PX4 Development Team." +__license__ = "BSD" +__email__ = "thomasgubler@gmail.com" + +ZENOH_TEMPLATE_FILE = ['Kconfig.topics.em', 'uorb_pubsub_factory.hpp.em'] +TOPICS_TOKEN = '# TOPICS ' + + +def get_topics(filename): + """ + Get TOPICS names from a "# TOPICS" line + """ + ofile = open(filename, 'r') + text = ofile.read() + result = [] + for each_line in text.split('\n'): + if each_line.startswith(TOPICS_TOKEN): + topic_names_str = each_line.strip() + topic_names_str = topic_names_str.replace(TOPICS_TOKEN, "") + topic_names_list = topic_names_str.split(" ") + for topic in topic_names_list: + # topic name PascalCase (file name) to snake_case (topic name) + topic_name = re.sub(r'(? +#include +@[for idx, topic_name in enumerate(full_base_names)]@ +#include +@[end for] + +@[for idx, topic_name in enumerate(datatypes)]@ +@{ +type_topic_count = len([e for e in topic_names_all if e.startswith(topic_name)]) +}@ +#ifdef CONFIG_ZENOH_PUBSUB_@(topic_name.upper()) +# define CONFIG_ZENOH_PUBSUB_@(topic_name.upper())_COUNT @(type_topic_count) +#else +# define CONFIG_ZENOH_PUBSUB_@(topic_name.upper())_COUNT 0 +#endif +@[end for] + +#define ZENOH_PUBSUB_COUNT \ +@[for idx, topic_name in enumerate(datatypes)]@ + CONFIG_ZENOH_PUBSUB_@(topic_name.upper())_COUNT + \ +@[end for] 0 + +typedef struct { + const uint32_t *ops; + const orb_metadata* orb_meta; +} UorbPubSubTopicBinder; + +const UorbPubSubTopicBinder _topics[ZENOH_PUBSUB_COUNT] { +@{ +uorb_id_idx = 0 +}@ +@[for idx, topic_name in enumerate(datatypes)]@ +#ifdef CONFIG_ZENOH_PUBSUB_@(topic_name.upper()) +@{ +topic_names = [e for e in topic_names_all if e.startswith(topic_name)] +}@ +@[for topic_name_inst in topic_names]@ + { + px4_msg_@(topic_dict[topic_name])_cdrstream_desc.ops.ops, + ORB_ID(@(topic_name_inst)) + }, +@{ +uorb_id_idx += 1 +}@ +@[end for]#endif +@[end for] +}; + +uORB_Zenoh_Publisher* genPublisher(const orb_metadata *meta) { + for (auto &pub : _topics) { + if(pub.orb_meta->o_id == meta->o_id) { + return new uORB_Zenoh_Publisher(meta, pub.ops); + } + } + return NULL; +} + + +uORB_Zenoh_Publisher* genPublisher(const char *name) { + for (auto &pub : _topics) { + if(strcmp(pub.orb_meta->o_name, name) == 0) { + return new uORB_Zenoh_Publisher(pub.orb_meta, pub.ops); + } + } + return NULL; +} + + +Zenoh_Subscriber* genSubscriber(const orb_metadata *meta) { + for (auto &sub : _topics) { + if(sub.orb_meta->o_id == meta->o_id) { + return new uORB_Zenoh_Subscriber(meta, sub.ops); + } + } + return NULL; +} + + +Zenoh_Subscriber* genSubscriber(const char *name) { + for (auto &sub : _topics) { + if(strcmp(sub.orb_meta->o_name, name) == 0) { + return new uORB_Zenoh_Subscriber(sub.orb_meta, sub.ops); + } + } + return NULL; +} diff --git a/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig b/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig index f306f5eb9d..77861ee597 100644 --- a/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig +++ b/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig @@ -85,6 +85,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_TJA1103=y CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y @@ -175,6 +176,7 @@ CONFIG_NET_CAN_SOCK_OPTS=y CONFIG_NET_ETH_PKTSIZE=1518 CONFIG_NET_ICMP=y CONFIG_NET_ICMP_SOCKET=y +CONFIG_NET_IGMP=y CONFIG_NET_NACTIVESOCKETS=16 CONFIG_NET_SOLINGER=y CONFIG_NET_TCP=y diff --git a/boards/nxp/mr-canhubk3/zenoh.px4board b/boards/nxp/mr-canhubk3/zenoh.px4board new file mode 100644 index 0000000000..28de5ea5cc --- /dev/null +++ b/boards/nxp/mr-canhubk3/zenoh.px4board @@ -0,0 +1,58 @@ +# CONFIG_BOARD_ROMFSROOT is not set +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS5" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3" +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_COMMON_LIGHT=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y +CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_EXAMPLES_FAKE_GPS=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_MODULES_ZENOH=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_NETMAN=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REFLECT=y +CONFIG_SYSTEMCMDS_SERIAL_TEST=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y diff --git a/boards/px4/fmu-v6x/zenoh.px4board b/boards/px4/fmu-v6x/zenoh.px4board new file mode 100644 index 0000000000..cb14fde935 --- /dev/null +++ b/boards/px4/fmu-v6x/zenoh.px4board @@ -0,0 +1,4 @@ +# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set +CONFIG_DRIVERS_UAVCAN=n +CONFIG_MODULES_UXRCE_DDS_CLIENT=n +CONFIG_MODULES_ZENOH=y diff --git a/boards/px4/sitl/zenoh.px4board b/boards/px4/sitl/zenoh.px4board new file mode 100644 index 0000000000..58a29ad52f --- /dev/null +++ b/boards/px4/sitl/zenoh.px4board @@ -0,0 +1,3 @@ +CONFIG_MODULES_UXRCE_DDS_CLIENT=n +CONFIG_MODULES_ZENOH=y +CONFIG_ZENOH_CONFIG_PATH="./zenoh" diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 70b09f9cfe..b1d787c721 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -336,3 +336,128 @@ add_custom_command( add_library(uorb_msgs ${uorb_headers} ${msg_out_path}/uORBTopics.hpp ${uorb_sources} ${msg_source_out_path}/uORBTopics.cpp) target_link_libraries(uorb_msgs PRIVATE m) add_dependencies(uorb_msgs prebuild_targets uorb_headers) + +if(CONFIG_LIB_CDRSTREAM) + set(uorb_cdr_idl) + set(uorb_cdr_msg) + set(uorb_cdr_idl_uorb) + set(idl_include_path ${PX4_BINARY_DIR}/uORB/idl) + set(idl_out_path ${idl_include_path}/px4/msg) + set(idl_uorb_path ${PX4_BINARY_DIR}/msg/px4/msg) + + # Make sure that CycloneDDS has been checkout out + execute_process(COMMAND git submodule sync src/lib/cdrstream/cyclonedds + WORKING_DIRECTORY ${PX4_SOURCE_DIR} ) + execute_process(COMMAND git submodule update --init --force src/lib/cdrstream/cyclonedds + WORKING_DIRECTORY ${PX4_SOURCE_DIR} ) + + # CycloneDDS-tools doesn't ship with the cdrstream-desc feature thus we've to compile idlc from source + MESSAGE(STATUS "Configuring idlc :" ${CMAKE_CURRENT_BINARY_DIR}/idlc) + file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/idlc) + execute_process(COMMAND ${CMAKE_COMMAND} ${PX4_SOURCE_DIR}/src/lib/cdrstream/cyclonedds + -DCMAKE_C_COMPILER=/usr/bin/gcc + -DBUILD_EXAMPLES=OFF + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/idlc + RESULT_VARIABLE CMD_ERROR + OUTPUT_FILE CMD_OUTPUT ) + MESSAGE(STATUS "Building idlc :" ${CMAKE_CURRENT_BINARY_DIR}/idlc) + execute_process(COMMAND ${CMAKE_COMMAND} --build . --target idlc + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/idlc + RESULT_VARIABLE CMD_ERROR + OUTPUT_FILE CMD_OUTPUT ) + list(APPEND CMAKE_PROGRAM_PATH "${CMAKE_CURRENT_BINARY_DIR}/idlc/bin") + + # Copy .msg files + foreach(msg_file ${msg_files}) + get_filename_component(msg ${msg_file} NAME_WE) + configure_file(${PX4_SOURCE_DIR}/msg/${msg}.msg ${idl_out_path}/${msg}.msg COPYONLY) + list(APPEND uorb_cdr_idl ${idl_out_path}/${msg}.idl) + list(APPEND uorb_cdr_msg ${idl_out_path}/${msg}.msg) + list(APPEND uorb_cdr_idl_uorb ${idl_uorb_path}/${msg}.h) + endforeach() + + # Generate IDL from .msg using rosidl_adapter + # Note this a submodule inside PX4 hence no ROS2 installation required + add_custom_command( + OUTPUT ${uorb_cdr_idl} + COMMAND ${CMAKE_COMMAND} + -E env "PYTHONPATH=${PX4_SOURCE_DIR}/src/lib/cdrstream/rosidl/rosidl_adapter:${PX4_SOURCE_DIR}/src/lib/cdrstream/rosidl/rosidl_cli" + ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/cdrstream/msg2idl.py + ${uorb_cdr_msg} + DEPENDS + ${uorb_cdr_msg} + git_cyclonedds + COMMENT "Generating IDL from uORB topic headers" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + VERBATIM + ) + + # Generate C definitions from IDL + set(CYCLONEDDS_DIR ${PX4_SOURCE_DIR}/src/lib/cdrstream/cyclonedds) + include("${CYCLONEDDS_DIR}/cmake/Modules/Generate.cmake") + idlc_generate(TARGET uorb_cdrstream + FEATURES "cdrstream-desc" + FILES ${uorb_cdr_idl} + INCLUDES ${idl_include_path} + BASE_DIR ${idl_include_path} + WARNINGS no-implicit-extensibility) + target_link_libraries(uorb_cdrstream INTERFACE cdr) + + # Generate and overwrite IDL header with custom headers for uORB operatability + # We typedef the IDL struct the uORB struct so that the IDL offset calculate + # the offset of internal uORB struct for serialization/deserialization + + # In the future we might want to turn this around let the IDL struct be the leading ABI + # However we need to remove the padding for logging and remove the re-ordering of fields + + add_custom_target( + uorb_idl_header + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py + --uorb-idl-header + -f ${msg_files} + -i ${CMAKE_CURRENT_SOURCE_DIR} + -o ${idl_uorb_path} + -e ${PX4_SOURCE_DIR}/Tools/msg/templates/cdrstream + DEPENDS + uorb_cdrstream + ${msg_files} + ${PX4_SOURCE_DIR}/Tools/msg/templates/cdrstream/uorb_idl_header.h.em + ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py + ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_helper.py + COMMENT "Generating uORB compatible IDL headers" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + VERBATIM + ) + add_dependencies(uorb_msgs uorb_idl_header) + + # Compile all CDR compatible message defnitions + target_link_libraries(uorb_msgs PRIVATE uorb_cdrstream ) +endif() + +if(CONFIG_MODULES_ZENOH) + # Update kconfig file for topics + execute_process(COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py + --zenoh-config + -f ${msg_files} + -o ${PX4_SOURCE_DIR}/src/modules/zenoh/ + -e ${PX4_SOURCE_DIR}/Tools/zenoh/templates/zenoh + ) + add_custom_command( + OUTPUT + ${PX4_BINARY_DIR}/src/modules/zenoh/uorb_pubsub_factory.hpp + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py + --zenoh-pub-sub + -f ${msg_files} + -o ${PX4_BINARY_DIR}/src/modules/zenoh/ + -e ${PX4_SOURCE_DIR}/Tools/zenoh/templates/zenoh + DEPENDS + ${msg_files} + ${PX4_SOURCE_DIR}/Tools/zenoh/templates/zenoh/uorb_pubsub_factory.hpp.em + ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py + COMMENT "Generating Zenoh Topic Code" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + VERBATIM + ) + add_library(zenoh_topics ${PX4_BINARY_DIR}/src/modules/zenoh/uorb_pubsub_factory.hpp) + set_target_properties(zenoh_topics PROPERTIES LINKER_LANGUAGE CXX) +endif() diff --git a/platforms/common/uORB/CMakeLists.txt b/platforms/common/uORB/CMakeLists.txt index c151d051c7..2ab49b3bd5 100644 --- a/platforms/common/uORB/CMakeLists.txt +++ b/platforms/common/uORB/CMakeLists.txt @@ -34,6 +34,10 @@ # this includes the generated topics directory include_directories(${CMAKE_CURRENT_BINARY_DIR}) +if(CONFIG_LIB_CDRSTREAM) + include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/lib/cdrstream/cyclonedds/src/core/ddsc/include) +endif() + set(SRCS) set(SRCS_COMMON diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 4a28408b04..8210be8280 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -38,6 +38,7 @@ add_subdirectory(battery EXCLUDE_FROM_ALL) add_subdirectory(bezier EXCLUDE_FROM_ALL) add_subdirectory(button EXCLUDE_FROM_ALL) add_subdirectory(cdev EXCLUDE_FROM_ALL) +add_subdirectory(cdrstream EXCLUDE_FROM_ALL) add_subdirectory(circuit_breaker EXCLUDE_FROM_ALL) add_subdirectory(collision_prevention EXCLUDE_FROM_ALL) add_subdirectory(component_information EXCLUDE_FROM_ALL) diff --git a/src/lib/cdrstream/CMakeLists.txt b/src/lib/cdrstream/CMakeLists.txt new file mode 100644 index 0000000000..cfdb6ab090 --- /dev/null +++ b/src/lib/cdrstream/CMakeLists.txt @@ -0,0 +1,49 @@ +############################################################################ +# +# 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. +# +############################################################################ + +if(CONFIG_LIB_CDRSTREAM) + set(BUILD_SHARED_LIBS NO) + + # CycloneDDS CDR serializer + include(${CMAKE_CURRENT_LIST_DIR}/cyclonedds/src/core/cdr/CMakeLists.txt) + target_compile_options(cdr PUBLIC + -Wno-cast-align + -Wno-double-promotion + $<$:-Wno-implicit-function-declaration -Wno-nested-externs> + -DNDEBUG) + px4_add_git_submodule(TARGET git_cyclonedds PATH "cyclonedds") + px4_add_git_submodule(TARGET git_rosidl PATH "rosidl") + add_dependencies(cdr git_cyclonedds git_rosidl uorb_headers parameters px4_platform) + target_sources(cdr PRIVATE dds_serializer.c) + target_include_directories(cdr PUBLIC ${CMAKE_CURRENT_LIST_DIR}) +endif() diff --git a/src/lib/cdrstream/Kconfig b/src/lib/cdrstream/Kconfig new file mode 100644 index 0000000000..311443075a --- /dev/null +++ b/src/lib/cdrstream/Kconfig @@ -0,0 +1,5 @@ +config LIB_CDRSTREAM + bool + default n + ---help--- + Enable CDR stream serializer library diff --git a/src/lib/cdrstream/cyclonedds b/src/lib/cdrstream/cyclonedds new file mode 160000 index 0000000000..314887ca40 --- /dev/null +++ b/src/lib/cdrstream/cyclonedds @@ -0,0 +1 @@ +Subproject commit 314887ca403c2fb0a0316add22672102936ed36c diff --git a/src/lib/cdrstream/dds_serializer.c b/src/lib/cdrstream/dds_serializer.c new file mode 100644 index 0000000000..b17cc105ea --- /dev/null +++ b/src/lib/cdrstream/dds_serializer.c @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * 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 "dds_serializer.h" + + +void *dds_malloc(size_t size) +{ + return NULL; +} +void *dds_realloc(void *ptr, size_t new_size) +{ + printf("Error CDR buffer is too small\n"); + return NULL; +} +void dds_free(void *pt) +{ + +} +const struct dds_cdrstream_allocator dds_allocator = { dds_malloc, dds_realloc, dds_free }; + +// CDR Xtypes header {0x00, 0x01} indicates it's Little Endian (CDR_LE representation) +const uint8_t ros2_header[4] = {0x00, 0x01, 0x00, 0x00}; diff --git a/src/lib/cdrstream/dds_serializer.h b/src/lib/cdrstream/dds_serializer.h new file mode 100644 index 0000000000..ac89dfb0b0 --- /dev/null +++ b/src/lib/cdrstream/dds_serializer.h @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * 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 DDS_CDRSTREAM_SERDER_H +#define DDS_CDRSTREAM_SERDER_H + +#include +#include + +extern const struct dds_cdrstream_allocator dds_allocator; +extern const uint8_t ros2_header[4]; + +#endif //DDS_CDRSTREAM_SERDER_H diff --git a/src/lib/cdrstream/msg2idl.py b/src/lib/cdrstream/msg2idl.py new file mode 100644 index 0000000000..74a464ab3d --- /dev/null +++ b/src/lib/cdrstream/msg2idl.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python3 + +############################################################################ +# +# 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. +# +############################################################################ + +import argparse +import pathlib +import sys + +from rosidl_adapter.msg import convert_msg_to_idl + +if __name__ == '__main__': + parser = argparse.ArgumentParser( + description=f'Convert px4 .msg files to .idl') + parser.add_argument( + 'interface_files', nargs='+', + help='The interface files to convert') + args = parser.parse_args(sys.argv[1:]) + + for interface_file in args.interface_files: + interface_file = pathlib.Path(interface_file) + package_dir = interface_file.parent.absolute() + + convert_msg_to_idl( + package_dir, 'px4', + interface_file.absolute().relative_to(package_dir), + interface_file.parent) diff --git a/src/lib/cdrstream/rosidl b/src/lib/cdrstream/rosidl new file mode 160000 index 0000000000..7790c70717 --- /dev/null +++ b/src/lib/cdrstream/rosidl @@ -0,0 +1 @@ +Subproject commit 7790c70717e09c003711f6f65015666c223fc283 diff --git a/src/modules/zenoh/CMakeLists.txt b/src/modules/zenoh/CMakeLists.txt new file mode 100644 index 0000000000..204bad04a7 --- /dev/null +++ b/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 +) diff --git a/src/modules/zenoh/Kconfig b/src/modules/zenoh/Kconfig new file mode 100644 index 0000000000..d868dae392 --- /dev/null +++ b/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" diff --git a/src/modules/zenoh/Kconfig.topics b/src/modules/zenoh/Kconfig.topics new file mode 100644 index 0000000000..643109d93f --- /dev/null +++ b/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 diff --git a/src/modules/zenoh/module.yaml b/src/modules/zenoh/module.yaml new file mode 100644 index 0000000000..25e658f9f3 --- /dev/null +++ b/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 diff --git a/src/modules/zenoh/publishers/uorb_publisher.hpp b/src/modules/zenoh/publishers/uorb_publisher.hpp new file mode 100644 index 0000000000..b541bfd1dd --- /dev/null +++ b/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 + */ + +#pragma once + +#include "zenoh_publisher.hpp" +#include +#include + +#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; +}; diff --git a/src/modules/zenoh/publishers/zenoh_publisher.cpp b/src/modules/zenoh/publishers/zenoh_publisher.cpp new file mode 100644 index 0000000000..1e312ffea5 --- /dev/null +++ b/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 + */ + +#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); +} diff --git a/src/modules/zenoh/publishers/zenoh_publisher.hpp b/src/modules/zenoh/publishers/zenoh_publisher.hpp new file mode 100644 index 0000000000..1d88a453e2 --- /dev/null +++ b/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 + */ + +#pragma once + +#include +#include + +#include +#include +#include + +class Zenoh_Publisher : public ListNode +{ +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 +}; diff --git a/src/modules/zenoh/subscribers/uorb_subscriber.hpp b/src/modules/zenoh/subscribers/uorb_subscriber.hpp new file mode 100644 index 0000000000..bd16b64398 --- /dev/null +++ b/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 + */ + +#pragma once + +#include "zenoh_subscriber.hpp" +#include +#include +#include + +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(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(_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; +}; diff --git a/src/modules/zenoh/subscribers/zenoh_subscriber.cpp b/src/modules/zenoh/subscribers/zenoh_subscriber.cpp new file mode 100644 index 0000000000..aab66ee5bd --- /dev/null +++ b/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 + */ + +#include "zenoh_subscriber.hpp" + +static void data_handler_cb(const z_sample_t *sample, void *arg) +{ + static_cast(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); +} diff --git a/src/modules/zenoh/subscribers/zenoh_subscriber.hpp b/src/modules/zenoh/subscribers/zenoh_subscriber.hpp new file mode 100644 index 0000000000..e3f1200e92 --- /dev/null +++ b/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 + */ + +#pragma once + +#include +#include + +#include +#include +#include + +// CycloneDDS CDR Deserializer +#include +#include + +class Zenoh_Subscriber : public ListNode +{ +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 +}; diff --git a/src/modules/zenoh/zenoh-pico b/src/modules/zenoh/zenoh-pico new file mode 160000 index 0000000000..22bbfc2150 --- /dev/null +++ b/src/modules/zenoh/zenoh-pico @@ -0,0 +1 @@ +Subproject commit 22bbfc215092a051341c234fdcf68f5baa267dec diff --git a/src/modules/zenoh/zenoh.cpp b/src/modules/zenoh/zenoh.cpp new file mode 100644 index 0000000000..0caf2c5eaa --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +// CycloneDDS CDR Deserializer +#include + +// Auto-generated header to all uORB <-> CDR conversions +#include + + +#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 Publish uORB topic to Zenoh\n"); + 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"); + 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); +} diff --git a/src/modules/zenoh/zenoh.h b/src/modules/zenoh/zenoh.h new file mode 100644 index 0000000000..a7cb0465a9 --- /dev/null +++ b/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 +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "zenoh_config.hpp" +#include "publishers/uorb_publisher.hpp" +#include "subscribers/uorb_subscriber.hpp" + +class ZENOH : public ModuleBase, 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 diff --git a/src/modules/zenoh/zenoh_config.cpp b/src/modules/zenoh/zenoh_config.cpp new file mode 100644 index 0000000000..8069956585 --- /dev/null +++ b/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 + */ + +#include "zenoh_config.hpp" + +#include +#include +#include +#include +#include + +#include + + +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); + } +} diff --git a/src/modules/zenoh/zenoh_config.hpp b/src/modules/zenoh/zenoh_config.hpp new file mode 100644 index 0000000000..72ae22d391 --- /dev/null +++ b/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 + */ + +#pragma once + +#include +#include + +#include +#include +#include + +#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; + + +};