Spiri
/
PX4-Autopilot
3
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add Zenoh pico support

This commit is contained in:
Peter van der Perk 2023-07-05 11:01:30 +02:00 committed by Daniel Agar
parent 5137ca1ccc
commit 019d232911
38 changed files with 3479 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
.gitmodules vendored
View File

@ -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

2
Kconfig
View File

@ -205,3 +205,5 @@ menu "platforms"
depends on PLATFORM_QURT || PLATFORM_POSIX
source "platforms/common/Kconfig"
endmenu
source "src/lib/*/Kconfig"

4
ROMFS/px4fmu_common/init.d/rcS
View File

@ -537,6 +537,10 @@ else
cyphal start
fi
fi
if param greater -s ZENOH_ENABLE 0
then
zenoh start
fi
#
# End of autostart.

3
Tools/astyle/files_to_check_code_style.sh
View File

@ -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

16
Tools/msg/px_generate_uorb_topic_files.py
View File

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

65
Tools/msg/templates/cdrstream/uorb_idl_header.h.em Normal file
View File

@ -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 <uORB/topics/@(name_snake_case).h>
@##############################
@# 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'(?<!^)(?=[A-Z])', '_', name).lower()
print('#include <uORB/topics/%s.h>'%(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 */

175
Tools/zenoh/px_generate_zenoh_topic_files.py Executable file
View File

@ -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'(?<!^)(?=[A-Z])', '_', topic).lower()
result.append(topic_name)
ofile.close()
if len(result) == 0:
# topic name PascalCase (file name) to snake_case (topic name)
file_base_name = os.path.basename(filename).replace(".msg", "")
topic_name = re.sub(r'(?<!^)(?=[A-Z])', '_', file_base_name).lower()
result.append(topic_name)
return result
def generate_by_template(output_file, template_file, em_globals):
"""
Invokes empy intepreter to geneate output_file by the
given template_file and predefined em_globals dict
"""
# check if folder exists:
folder_name = os.path.dirname(output_file)
if not os.path.exists(folder_name):
os.makedirs(folder_name)
ofile = open(output_file, 'w')
# todo, reuse interpreter
interpreter = em.Interpreter(output=ofile, globals=em_globals, options={
em.RAW_OPT: True, em.BUFFERED_OPT: True})
try:
interpreter.file(open(template_file))
except OSError as e:
ofile.close()
os.remove(output_file)
raise
interpreter.shutdown()
ofile.close()
return True
def generate_topics_list_file_from_files(files, outputdir, template_filename, templatedir):
# generate cpp file with topics list
filenames = []
for filename in [os.path.basename(p) for p in files if os.path.basename(p).endswith(".msg")]:
filenames.append(re.sub(r'(?<!^)(?=[A-Z])', '_', filename).lower())
datatypes = []
for filename in [os.path.basename(p) for p in files if os.path.basename(p).endswith(".msg")]:
datatypes.append(re.sub(r'(?<!^)(?=[A-Z])', '_', filename).lower().replace(".msg",""))
full_base_names = []
for filename in [os.path.basename(p) for p in files if os.path.basename(p).endswith(".msg")]:
full_base_names.append(filename.replace(".msg",""))
topics = []
for msg_filename in files:
topics.extend(get_topics(msg_filename))
tl_globals = {"msgs": filenames, "topics": topics, "datatypes": datatypes, "full_base_names": full_base_names}
tl_template_file = os.path.join(templatedir, template_filename)
tl_out_file = os.path.join(outputdir, template_filename.replace(".em", ""))
generate_by_template(tl_out_file, tl_template_file, tl_globals)
if __name__ == "__main__":
parser = argparse.ArgumentParser(description='Convert msg files to uorb headers/sources')
parser.add_argument('--zenoh-config', help='Generate Zenoh Kconfig file', action='store_true')
parser.add_argument('--zenoh-pub-sub', help='Generate Zenoh Pubsub factory', action='store_true')
parser.add_argument('-f', dest='file',
help="files to convert (use only without -d)",
nargs="+")
parser.add_argument('-e', dest='templatedir',
help='directory with template files',)
parser.add_argument('-o', dest='outputdir',
help='output directory for header files')
parser.add_argument('-p', dest='prefix', default='',
help='string added as prefix to the output file '
' name when converting directories')
args = parser.parse_args()
if args.zenoh_config:
generate_topics_list_file_from_files(args.file, args.outputdir, ZENOH_TEMPLATE_FILE[0], args.templatedir)
exit(0)
elif args.zenoh_pub_sub:
generate_topics_list_file_from_files(args.file, args.outputdir, ZENOH_TEMPLATE_FILE[1], args.templatedir)
exit(0)
else:
print('Error: either --headers or --sources must be specified')
exit(-1)

25
Tools/zenoh/templates/zenoh/Kconfig.topics.em Normal file
View File

@ -0,0 +1,25 @@
@{
topics_count = len(topics)
topic_names_all = list(set(topics)) # set() filters duplicates
topic_names_all.sort()
datatypes = list(set(datatypes)) # set() filters duplicates
datatypes.sort()
}@
menu "Zenoh publishers/subscribers"
depends on MODULES_ZENOH
@[for idx, topic_name in enumerate(datatypes)]@
config ZENOH_PUBSUB_@(topic_name.upper())
bool "@(topic_name)"
default n
@[end for]
config ZENOH_PUBSUB_ALL_SELECTION
bool
default y if ZENOH_PUBSUB_ALL
@[for idx, topic_name in enumerate(datatypes)]@
select ZENOH_PUBSUB_@(topic_name.upper())
@[end for]
endmenu

154
Tools/zenoh/templates/zenoh/uorb_pubsub_factory.hpp.em Normal file
View File

@ -0,0 +1,154 @@
@###############################################
@#
@# EmPy template for generating u.hpp file
@# for logging purposes
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - topics (List) list of all topic names
@###############################################
@{
topic_dict = dict(zip(datatypes, full_base_names))
topics_count = len(topics)
topic_names_all = list(set(topics)) # set() filters duplicates
topic_names_all.sort()
datatypes = list(set(datatypes)) # set() filters duplicates
datatypes.sort()
full_base_names = list(set(full_base_names)) # set() filters duplicates
full_base_names.sort()
}@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* uorb_pubsub_factory.hpp
*
* Defines generic, templatized uORB over Zenoh / ROS2
*/
#pragma once
#include <publishers/uorb_publisher.hpp>
#include <uORB/topics/uORBTopics.hpp>
@[for idx, topic_name in enumerate(full_base_names)]@
#include <px4/msg/@(topic_name).h>
@[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;
}

2
boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig
View File

@ -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

58
boards/nxp/mr-canhubk3/zenoh.px4board Normal file
View File

@ -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

4
boards/px4/fmu-v6x/zenoh.px4board Normal file
View File

@ -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

3
boards/px4/sitl/zenoh.px4board Normal file
View File

@ -0,0 +1,3 @@
CONFIG_MODULES_UXRCE_DDS_CLIENT=n
CONFIG_MODULES_ZENOH=y
CONFIG_ZENOH_CONFIG_PATH="./zenoh"

125
msg/CMakeLists.txt
View File

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

4
platforms/common/uORB/CMakeLists.txt
View File

@ -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

1
src/lib/CMakeLists.txt
View File

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

49
src/lib/cdrstream/CMakeLists.txt Normal file
View File

@ -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
$<$<COMPILE_LANGUAGE:C>:-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()

5
src/lib/cdrstream/Kconfig Normal file
View File

@ -0,0 +1,5 @@
config LIB_CDRSTREAM
bool
default n
---help---
Enable CDR stream serializer library

1
src/lib/cdrstream/cyclonedds Submodule

@ -0,0 +1 @@
Subproject commit 314887ca403c2fb0a0316add22672102936ed36c

53
src/lib/cdrstream/dds_serializer.c Normal file
View File

@ -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};

43
src/lib/cdrstream/dds_serializer.h Normal file
View File

@ -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 <stdlib.h>
#include <dds/cdr/dds_cdrstream.h>
extern const struct dds_cdrstream_allocator dds_allocator;
extern const uint8_t ros2_header[4];
#endif //DDS_CDRSTREAM_SERDER_H

57
src/lib/cdrstream/msg2idl.py Normal file
View File

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

1
src/lib/cdrstream/rosidl Submodule

@ -0,0 +1 @@
Subproject commit 7790c70717e09c003711f6f65015666c223fc283

100
src/modules/zenoh/CMakeLists.txt Normal file
View File

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

73
src/modules/zenoh/Kconfig Normal file
View File

@ -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"

960
src/modules/zenoh/Kconfig.topics Normal file
View File

@ -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

14
src/modules/zenoh/module.yaml Normal file
View File

@ -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

105
src/modules/zenoh/publishers/uorb_publisher.hpp Normal file
View File

@ -0,0 +1,105 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file uorb_publisher.hpp
*
* Defines generic, templatized uORB over Zenoh / ROS2
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
#pragma once
#include "zenoh_publisher.hpp"
#include <uORB/Subscription.hpp>
#include <dds_serializer.h>
#define CDR_SAFETY_MARGIN 12
class uORB_Zenoh_Publisher : public Zenoh_Publisher
{
public:
uORB_Zenoh_Publisher(const orb_metadata *meta, const uint32_t *ops) :
Zenoh_Publisher(true),
_uorb_meta{meta},
_cdr_ops(ops)
{
_uorb_sub = orb_subscribe(meta);
};
~uORB_Zenoh_Publisher() override = default;
// Update the uORB Subscription and broadcast a Zenoh ROS2 message
virtual int8_t update() override
{
uint8_t data[_uorb_meta->o_size];
orb_copy(_uorb_meta, _uorb_sub, data);
uint8_t buf[_uorb_meta->o_size + 4 + CDR_SAFETY_MARGIN];
memcpy(buf, ros2_header, sizeof(ros2_header));
dds_ostream_t os;
os.m_buffer = buf;
os.m_index = (uint32_t)sizeof(ros2_header);
os.m_size = (uint32_t)sizeof(ros2_header) + _uorb_meta->o_size + CDR_SAFETY_MARGIN;
os.m_xcdr_version = DDSI_RTPS_CDR_ENC_VERSION_2;
if (dds_stream_write(&os,
&dds_allocator,
(const char *)&data,
_cdr_ops)) {
return publish((const uint8_t *)buf, os.m_size);
} else {
return _Z_ERR_MESSAGE_SERIALIZATION_FAILED;
}
};
void setPollFD(px4_pollfd_struct_t *pfd)
{
pfd->fd = _uorb_sub;
pfd->events = POLLIN;
}
void print()
{
printf("uORB %s -> ", _uorb_meta->o_name);
Zenoh_Publisher::print();
}
private:
const orb_metadata *_uorb_meta;
int _uorb_sub;
const uint32_t *_cdr_ops;
};

98
src/modules/zenoh/publishers/zenoh_publisher.cpp Normal file
View File

@ -0,0 +1,98 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file zenoh_publisher.cpp
*
* Zenoh publisher
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
#include "zenoh_publisher.hpp"
Zenoh_Publisher::Zenoh_Publisher(bool rostopic)
{
this->_rostopic = rostopic;
this->_topic[0] = 0x0;
}
Zenoh_Publisher::~Zenoh_Publisher()
{
undeclare_publisher();
}
int Zenoh_Publisher::undeclare_publisher()
{
z_undeclare_publisher(z_publisher_move(&_pub));
return 0;
}
int Zenoh_Publisher::declare_publisher(z_session_t s, const char *keyexpr)
{
if (_rostopic) {
strncpy(this->_topic, (char *)_rt_prefix, _rt_prefix_offset);
if (keyexpr[0] == '/') {
strncpy(this->_topic + _rt_prefix_offset, keyexpr + 1, sizeof(this->_topic) - _rt_prefix_offset);
} else {
strncpy(this->_topic + _rt_prefix_offset, keyexpr, sizeof(this->_topic) - _rt_prefix_offset);
}
} else {
strncpy(this->_topic, keyexpr, sizeof(this->_topic));
}
_pub = z_declare_publisher(s, z_keyexpr(this->_topic), NULL);
if (!z_publisher_check(&_pub)) {
printf("Unable to declare publisher for key expression!\n");
return -1;
}
return 0;
}
int8_t Zenoh_Publisher::publish(const uint8_t *buf, int size)
{
z_publisher_put_options_t options = z_publisher_put_options_default();
options.encoding = z_encoding(Z_ENCODING_PREFIX_APP_CUSTOM, NULL);
return z_publisher_put(z_publisher_loan(&_pub), buf, size, &options);
}
void Zenoh_Publisher::print()
{
printf("Topic: %s\n", this->_topic);
}

76
src/modules/zenoh/publishers/zenoh_publisher.hpp Normal file
View File

@ -0,0 +1,76 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file zenoh_publisher.hpp
*
* Defines basic functionality of Zenoh publisher class
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
#pragma once
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <lib/parameters/param.h>
#include <containers/List.hpp>
#include <zenoh-pico.h>
class Zenoh_Publisher : public ListNode<Zenoh_Publisher *>
{
public:
Zenoh_Publisher(bool rostopic = true);
virtual ~Zenoh_Publisher();
virtual int declare_publisher(z_session_t s, const char *keyexpr);
virtual int undeclare_publisher();
virtual int8_t update() = 0;
virtual void print();
protected:
int8_t publish(const uint8_t *, int size);
z_owned_publisher_t _pub;
char _topic[60]; // The Topic name is somewhere is the Zenoh stack as well but no good api to fetch it.
// Indicates ROS2 Topic namespace
bool _rostopic;
const char *_rt_prefix = "rt/";
const size_t _rt_prefix_offset = 3; // "rt/" are 3 chars
};

107
src/modules/zenoh/subscribers/uorb_subscriber.hpp Normal file
View File

@ -0,0 +1,107 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file uorb_subscriber.hpp
*
* Defines generic, templatized uORB over Zenoh / ROS2
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
#pragma once
#include "zenoh_subscriber.hpp"
#include <uORB/topics/input_rc.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/actuator_outputs.h>
class uORB_Zenoh_Subscriber : public Zenoh_Subscriber
{
public:
uORB_Zenoh_Subscriber(const orb_metadata *meta, const uint32_t *ops) :
Zenoh_Subscriber(true),
_uorb_meta{meta},
_cdr_ops(ops)
{
int instance = 0;
_uorb_pub_handle = orb_advertise_multi_queue(_uorb_meta, nullptr, &instance, 1); //FIXME template magic qsize
};
~uORB_Zenoh_Subscriber() override = default;
// Update the uORB Subscription and broadcast a Zenoh ROS2 message
void data_handler(const z_sample_t *sample)
{
char data[_uorb_meta->o_size];
dds_istream_t is = {.m_buffer = (unsigned char *)(sample->payload.start), .m_size = static_cast<int>(sample->payload.len),
.m_index = 4, .m_xcdr_version = DDSI_RTPS_CDR_ENC_VERSION_2
};
dds_stream_read(&is, data, &dds_allocator, _cdr_ops);
// As long as we don't have timesynchronization between Zenoh nodes
// we've to manually set the timestamp
fix_timestamp(data);
// ORB_ID::input_rc needs additional timestamp fixup
if (static_cast<ORB_ID>(_uorb_meta->o_id) == ORB_ID::input_rc) {
memcpy(&data[8], data, sizeof(hrt_abstime));
}
orb_publish(_uorb_meta, _uorb_pub_handle, &data);
};
void fix_timestamp(char *data)
{
hrt_abstime now = hrt_absolute_time();
memcpy(data, &now, sizeof(hrt_abstime));
}
void print()
{
Zenoh_Subscriber::print("uORB", _uorb_meta->o_name);
}
protected:
// Default payload-size function -- can specialize in derived class
size_t get_payload_size()
{
return _uorb_meta->o_size;
}
private:
const orb_metadata *_uorb_meta;
orb_advert_t _uorb_pub_handle;
const uint32_t *_cdr_ops;
};

111
src/modules/zenoh/subscribers/zenoh_subscriber.cpp Normal file
View File

@ -0,0 +1,111 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file zenoh_subscriber.cpp
*
* Zenoh subscriber
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
#include "zenoh_subscriber.hpp"
static void data_handler_cb(const z_sample_t *sample, void *arg)
{
static_cast<Zenoh_Subscriber *>(arg)->data_handler(sample);
}
void Zenoh_Subscriber::data_handler(const z_sample_t *sample)
{
z_owned_str_t keystr = z_keyexpr_to_string(sample->keyexpr);
printf(">> [Subscriber] Received ('%s' size '%d')\n", z_str_loan(&keystr), (int)sample->payload.len);
z_str_drop(z_str_move(&keystr));
}
Zenoh_Subscriber::Zenoh_Subscriber(bool rostopic)
{
this->_rostopic = rostopic;
this->_topic[0] = 0x0;
}
Zenoh_Subscriber::~Zenoh_Subscriber()
{
undeclare_subscriber();
}
int Zenoh_Subscriber::undeclare_subscriber()
{
z_undeclare_subscriber(z_subscriber_move(&_sub));
return 0;
}
int Zenoh_Subscriber::declare_subscriber(z_session_t s, const char *keyexpr)
{
z_owned_closure_sample_t callback = z_closure_sample(data_handler_cb, NULL, this);
if (_rostopic) {
strncpy(this->_topic, (char *)_rt_prefix, _rt_prefix_offset);
if (keyexpr[0] == '/') {
strncpy(this->_topic + _rt_prefix_offset, keyexpr + 1, sizeof(this->_topic) - _rt_prefix_offset);
} else {
strncpy(this->_topic + _rt_prefix_offset, keyexpr, sizeof(this->_topic) - _rt_prefix_offset);
}
} else {
strncpy(this->_topic, (char *)keyexpr, sizeof(this->_topic));
}
_sub = z_declare_subscriber(s, z_keyexpr(this->_topic), z_closure_sample_move(&callback), NULL);
if (!z_subscriber_check(&_sub)) {
printf("Unable to declare subscriber for key expression!\n %s\n", keyexpr);
return -1;
}
return 0;
}
void Zenoh_Subscriber::print()
{
printf("Topic: %s\n", this->_topic);
}
void Zenoh_Subscriber::print(const char *type_string, const char *topic_string)
{
printf("Topic: %s -> %s %s \n", this->_topic, type_string, topic_string);
}

80
src/modules/zenoh/subscribers/zenoh_subscriber.hpp Normal file
View File

@ -0,0 +1,80 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file zenoh_subscriber.hpp
*
* Defines basic functionality of Zenoh subscriber class
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
#pragma once
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <lib/parameters/param.h>
#include <containers/List.hpp>
#include <zenoh-pico.h>
// CycloneDDS CDR Deserializer
#include <dds/cdr/dds_cdrstream.h>
#include <dds_serializer.h>
class Zenoh_Subscriber : public ListNode<Zenoh_Subscriber *>
{
public:
Zenoh_Subscriber(bool rostopic = true);
virtual ~Zenoh_Subscriber();
virtual int declare_subscriber(z_session_t s, const char *keyexpr);
virtual int undeclare_subscriber();
virtual void data_handler(const z_sample_t *sample);
virtual void print();
protected:
virtual void print(const char *type_string, const char *topic_string);
z_owned_subscriber_t _sub;
char _topic[60]; // The Topic name is somewhere is the Zenoh stack as well but no good api to fetch it.
// Indicates ROS2 Topic namespace
bool _rostopic;
const char *_rt_prefix = "rt/";
const size_t _rt_prefix_offset = 3; // "rt/" are 3 chars
};

1
src/modules/zenoh/zenoh-pico Submodule

@ -0,0 +1 @@
Subproject commit 22bbfc215092a051341c234fdcf68f5baa267dec

290
src/modules/zenoh/zenoh.cpp Normal file
View File

@ -0,0 +1,290 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "zenoh.h"
#include <px4_platform_common/log.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/cli.h>
#include <errno.h>
#include <fcntl.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <ctype.h>
#include <string.h>
#include <zenoh-pico.h>
// CycloneDDS CDR Deserializer
#include <dds/cdr/dds_cdrstream.h>
// Auto-generated header to all uORB <-> CDR conversions
#include <uorb_pubsub_factory.hpp>
#define Z_PUBLISH
#define Z_SUBSCRIBE
extern "C" __EXPORT int zenoh_main(int argc, char *argv[]);
ZENOH::ZENOH():
ModuleParams(nullptr)
{
}
ZENOH::~ZENOH()
{
}
void ZENOH::run()
{
char mode[NET_MODE_SIZE];
char locator[NET_LOCATOR_SIZE];
int8_t ret;
int i;
Zenoh_Config z_config;
z_config.getNetworkConfig(mode, locator);
z_owned_config_t config = z_config_default();
zp_config_insert(z_config_loan(&config), Z_CONFIG_MODE_KEY, z_string_make(mode));
if (locator[0] != 0) {
zp_config_insert(z_config_loan(&config), Z_CONFIG_PEER_KEY, z_string_make(locator));
} else if (strcmp(Z_CONFIG_MODE_PEER, mode) == 0) {
zp_config_insert(z_config_loan(&config), Z_CONFIG_PEER_KEY, z_string_make(Z_CONFIG_MULTICAST_LOCATOR_DEFAULT));
}
PX4_INFO("Opening session...");
z_owned_session_t s = z_open(z_config_move(&config));
if (!z_session_check(&s)) {
PX4_ERR("Unable to open session!");
return;
}
// Start read and lease tasks for zenoh-pico
if (zp_start_read_task(z_session_loan(&s), NULL) < 0 || zp_start_lease_task(z_session_loan(&s), NULL) < 0) {
PX4_ERR("Unable to start read and lease tasks");
return;
}
#ifdef Z_SUBSCRIBE
_sub_count = z_config.getSubCount();
_zenoh_subscribers = (Zenoh_Subscriber **)malloc(sizeof(Zenoh_Subscriber *)*_sub_count);
{
char topic[TOPIC_INFO_SIZE];
char type[TOPIC_INFO_SIZE];
for (i = 0; i < _sub_count; i++) {
z_config.getSubscriberMapping(topic, type);
_zenoh_subscribers[i] = genSubscriber(type);
if (_zenoh_subscribers[i] != 0) {
_zenoh_subscribers[i]->declare_subscriber(z_session_loan(&s), topic);
}
}
if (z_config.getSubscriberMapping(topic, type) < 0) {
PX4_WARN("Subscriber mapping parsing error");
}
}
#endif
#ifdef Z_PUBLISH
_pub_count = z_config.getPubCount();
_zenoh_publishers = (uORB_Zenoh_Publisher **)malloc(_pub_count * sizeof(uORB_Zenoh_Publisher *));
px4_pollfd_struct_t pfds[_pub_count];
{
char topic[TOPIC_INFO_SIZE];
char type[TOPIC_INFO_SIZE];
for (i = 0; i < _pub_count; i++) {
z_config.getPublisherMapping(topic, type);
_zenoh_publishers[i] = genPublisher(type);
if (_zenoh_publishers[i] != 0) {
_zenoh_publishers[i]->declare_publisher(z_session_loan(&s), topic);
_zenoh_publishers[i]->setPollFD(&pfds[i]);
}
}
if (z_config.getSubscriberMapping(topic, type) < 0) {
PX4_WARN("Publisher mapping parsing error");
}
}
if (_pub_count == 0) {
// Nothing to publish but we don't want to stop this thread
while (!should_exit()) {
sleep(2);
}
}
while (!should_exit()) {
int pret = px4_poll(pfds, _pub_count, 100);
if (pret == 0) {
//PX4_INFO("Zenoh poll timeout\n");
} else {
for (i = 0; i < _pub_count; i++) {
if (pfds[i].revents & POLLIN) {
ret = _zenoh_publishers[i]->update();
if (ret < 0) {
PX4_WARN("Publisher error %i", ret);
}
}
}
}
}
#endif
// Exiting cleaning up publisher and subscribers
for (i = 0; i < _sub_count; i++) {
delete _zenoh_subscribers[i];
}
free(_zenoh_subscribers);
for (i = 0; i < _pub_count; i++) {
delete _zenoh_publishers[i];
}
free(_zenoh_publishers);
// Stop read and lease tasks for zenoh-pico
zp_stop_read_task(z_session_loan(&s));
zp_stop_lease_task(z_session_loan(&s));
z_close(z_session_move(&s));
exit_and_cleanup();
}
int ZENOH::custom_command(int argc, char *argv[])
{
if (argc > 0 && strcmp("config", argv[0]) == 0) {
Zenoh_Config z_config;
if (z_config.cli(argc, argv) == 0) {
return 0;
}
}
return print_usage("Unrecognized command.");
}
int ZENOH::print_usage(const char *reason)
{
if (reason) {
printf("%s\n\n", reason);
}
PRINT_MODULE_USAGE_NAME("zenoh", "driver");
PRINT_MODULE_DESCRIPTION(R"DESC_STR(
### Description
Zenoh demo bridge
)DESC_STR");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_COMMAND("stop");
PRINT_MODULE_USAGE_COMMAND("status");
PRINT_MODULE_USAGE_COMMAND("config");
PX4_INFO_RAW(" addpublisher <zenoh_topic> <uorb_topic> Publish uORB topic to Zenoh\n");
PX4_INFO_RAW(" addsubscriber <zenoh_topic> <uorb_topic> Publish Zenoh topic to uORB\n");
PX4_INFO_RAW(" net <mode> <locator> Zenoh network mode\n");
PX4_INFO_RAW(" <mode> values: client|peer \n");
PX4_INFO_RAW(" <locator> client: locator address for router\n");
PX4_INFO_RAW(" peer: multicast address e.g. udp/224.0.0.225:7447#iface=eth0\n");
return 0;
}
int ZENOH::print_status()
{
PX4_INFO("running");
PX4_INFO("Publishers");
for (int i = 0; i < _pub_count; i++) {
_zenoh_publishers[i]->print();
}
PX4_INFO("Subscribers");
for (int i = 0; i < _sub_count; i++) {
_zenoh_subscribers[i]->print();
}
return 0;
}
int ZENOH::task_spawn(int argc, char *argv[])
{
int task_id = px4_task_spawn_cmd(
"zenoh",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
4096,
&run_trampoline,
argv
);
if (task_id < 0) {
return -errno;
} else {
_task_id = task_id;
return 0;
}
}
ZENOH *ZENOH::instantiate(int argc, char *argv[])
{
return new ZENOH();
}
int zenoh_main(int argc, char *argv[])
{
return ZENOH::main(argc, argv);
}

95
src/modules/zenoh/zenoh.h Normal file
View File

@ -0,0 +1,95 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#ifndef ZENOH_MODULE_H
#define ZENOH_MODULE_H
#include <termios.h>
#include <poll.h>
#include <sys/select.h>
#include <sys/time.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/module.h>
#include <perf/perf_counter.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/parameter_update.h>
#include "zenoh_config.hpp"
#include "publishers/uorb_publisher.hpp"
#include "subscribers/uorb_subscriber.hpp"
class ZENOH : public ModuleBase<ZENOH>, public ModuleParams
{
public:
ZENOH();
~ZENOH();
/**
* @see ModuleBase::custom_command
*/
static int custom_command(int argc, char *argv[]);
/**
* @see ModuleBase::print_usage
*/
static int print_usage(const char *reason = nullptr);
/**
* @see ModuleBase::print_usage
*/
int print_status();
/**
* @see ModuleBase::task_spawn
*/
static int task_spawn(int argc, char *argv[]);
static ZENOH *instantiate(int argc, char *argv[]);
void run() override;
private:
Zenoh_Config _config;
int _pub_count;
uORB_Zenoh_Publisher **_zenoh_publishers;
int _sub_count;
Zenoh_Subscriber **_zenoh_subscribers;
};
#endif //ZENOH_MODULE_H

409
src/modules/zenoh/zenoh_config.cpp Normal file
View File

@ -0,0 +1,409 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file zenoh_config.cpp
*
* Zenoh configuration backend
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
#include "zenoh_config.hpp"
#include <dirent.h>
#include <sys/stat.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>
#include <uORB/topics/uORBTopics.hpp>
const char *default_net_config = Z_CONFIG_MODE_DEFAULT;
const char *default_pub_config = "";
const char *default_sub_config = ""; //TODO maybe use YAML
Zenoh_Config::Zenoh_Config()
{
bool correct_config = true;
DIR *dir = opendir(ZENOH_SD_ROOT_PATH);
fp_mapping = NULL;
if (dir) {
/* Directory exists. */
closedir(dir);
if (access(ZENOH_NET_CONFIG_PATH, F_OK) != 0) {
correct_config = false;
} else if (access(ZENOH_PUB_CONFIG_PATH, F_OK) != 0) {
correct_config = false;
} else if (access(ZENOH_SUB_CONFIG_PATH, F_OK) != 0) {
correct_config = false;
}
} else {
/* opendir() failed */
correct_config = false;
}
if (!correct_config) {
generate_clean_config();
}
}
Zenoh_Config::~Zenoh_Config()
{
if (fp_mapping != NULL) {
fclose(fp_mapping);
}
}
int Zenoh_Config::AddPubSub(char *topic, char *datatype, const char *filename)
{
{
char f_topic[TOPIC_INFO_SIZE];
char f_type[TOPIC_INFO_SIZE];
while (getPubSubMapping(f_topic, f_type, filename) > 0) {
if (strcmp(topic, f_topic) == 0
|| strcmp(datatype, f_type) == 0) {
printf("Already mapped to uORB %s -> %s\n", f_type, f_topic);
return 0;
}
}
}
for (size_t i = 0; i < orb_topics_count(); i++) {
const struct orb_metadata *meta = get_orb_meta((ORB_ID)i);
if (meta != NULL &&
strcmp(meta->o_name, datatype) == 0) {
FILE *fp = fopen(filename, "a");
if (fp) {
fprintf(fp, "%s;%s\n", topic, datatype);
} else {
return -1;
}
fclose(fp);
return 1;
}
}
printf("%s not found\n", datatype);
return 0;
}
int Zenoh_Config::SetNetworkConfig(char *mode, char *locator)
{
FILE *fp = fopen(ZENOH_NET_CONFIG_PATH, "w");
if (fp) {
if (locator == 0) {
fprintf(fp, "%s\n", mode);
} else {
fprintf(fp, "%s;%s\n", mode, locator);
}
} else {
return -1;
}
fclose(fp);
return 0;
}
int Zenoh_Config::cli(int argc, char *argv[])
{
if (argc == 1) {
dump_config();
} else if (argc == 3) {
if (strcmp(argv[1], "net") == 0) {
SetNetworkConfig(argv[2], 0);
}
} else if (argc == 4) {
if (strcmp(argv[1], "addpublisher") == 0) {
if (AddPubSub(argv[2], argv[3], ZENOH_PUB_CONFIG_PATH) > 0) {
printf("Added %s %s to publishers\n", argv[2], argv[3]);
} else {
printf("Could not add uORB %s -> %s to publishers\n", argv[3], argv[2]);
}
} else if (strcmp(argv[1], "addsubscriber") == 0) {
if (AddPubSub(argv[2], argv[3], ZENOH_SUB_CONFIG_PATH) > 0) {
printf("Added %s -> uORB %s to subscribers\n", argv[2], argv[3]);
} else {
printf("Could not add %s -> uORB %s to subscribers\n", argv[2], argv[3]);
}
} else if (strcmp(argv[1], "net") == 0) {
SetNetworkConfig(argv[2], argv[3]);
}
}
//TODO make CLI to modify configuration now you would have to manually modify the files
return 0;
}
const char *Zenoh_Config::get_csv_field(char *line, int num)
{
const char *tok;
for (
tok = strtok(line, ";");
tok && *tok;
tok = strtok(NULL, ";\n")) {
if (!--num) {
return tok;
}
}
return NULL;
}
void Zenoh_Config::getNetworkConfig(char *mode, char *locator)
{
FILE *fp;
char buffer[NET_CONFIG_LINE_SIZE];
fp = fopen(ZENOH_NET_CONFIG_PATH, "r");
// If file opened successfully, then read the file
if (fp) {
fgets(buffer, NET_CONFIG_LINE_SIZE, fp);
const char *config_locator = get_csv_field(buffer, 2);
char *config_mode = (char *)get_csv_field(buffer, 1);
if (config_mode) {
config_mode[strcspn(config_mode, "\n")] = 0;
strncpy(mode, config_mode, NET_MODE_SIZE);
} else {
mode[0] = 0;
}
if (config_locator) {
strncpy(locator, config_locator, NET_LOCATOR_SIZE);
} else {
locator[0] = 0;
}
} else {
printf("Failed to open the file\n");
}
//Close the file
fclose(fp);
}
int Zenoh_Config::getLineCount(const char *filename)
{
int lines = 0;
int ch;
// Open file in write mode
FILE *fp = fopen(filename, "r");
while ((ch = fgetc(fp)) != EOF) {
if (ch == '\n') {
lines++;
}
}
//Close the file
fclose(fp);
return lines;
}
// Very rudamentary here but we've to wait for a more advanced param system
int Zenoh_Config::getPubSubMapping(char *topic, char *type, const char *filename)
{
char buffer[MAX_LINE_SIZE];
if (fp_mapping == NULL) {
fp_mapping = fopen(filename, "r");
}
if (fp_mapping) {
while (fgets(buffer, MAX_LINE_SIZE, fp_mapping) != NULL) {
if (buffer[0] != '\n') {
const char *config_type = get_csv_field(buffer, 2);
const char *config_topic = get_csv_field(buffer, 1);
strncpy(type, config_type, TOPIC_INFO_SIZE);
strncpy(topic, config_topic, TOPIC_INFO_SIZE);
return 1;
}
}
} else {
printf("Failed to open the file\n");
return -1;
}
//Close the file
fclose(fp_mapping);
fp_mapping = NULL;
return 0;
}
void Zenoh_Config::dump_config()
{
printf("Network config:\n");
{
char mode[NET_MODE_SIZE];
char locator[NET_LOCATOR_SIZE];
getNetworkConfig(mode, locator);
printf("Mode: %s\n", mode);
if (locator[0] == 0) {
printf("Locator: scout\n");
} else {
printf("Locator: %s\n", locator);
}
printf("\n");
}
{
char topic[TOPIC_INFO_SIZE];
char type[TOPIC_INFO_SIZE];
printf("Publisher config:\n");
while (getPubSubMapping(topic, type, ZENOH_PUB_CONFIG_PATH) > 0) {
printf("Topic: %s\n", topic);
printf("Type: %s\n", type);
}
printf("\nSubscriber config:\n");
while (getPubSubMapping(topic, type, ZENOH_SUB_CONFIG_PATH) > 0) {
printf("Topic: %s\n", topic);
printf("Type: %s\n", type);
}
}
}
void Zenoh_Config::generate_clean_config()
{
printf("Generate clean\n");
FILE *fp;
DIR *dir = opendir(ZENOH_SD_ROOT_PATH);
if (dir) {
printf("Zenoh directory exists\n");
} else {
/* Create zenoh dir. */
if (mkdir(ZENOH_SD_ROOT_PATH, 0700) < 0) {
printf("Failed to create Zenoh directory\n");
return;
}
}
if (access(ZENOH_NET_CONFIG_PATH, F_OK) != 0) {
// Open file in write mode
fp = fopen(ZENOH_NET_CONFIG_PATH, "w");
// If file opened successfully, then write the string to file
if (fp) {
fputs(default_net_config, fp);
} else {
printf("Failed to open the file\n");
return;
}
//Close the file
fclose(fp);
}
if (access(ZENOH_PUB_CONFIG_PATH, F_OK) != 0) {
// Open file in write mode
fp = fopen(ZENOH_PUB_CONFIG_PATH, "w");
// If file opened successfully, then write the string to file
if (fp) {
fputs(default_pub_config, fp);
} else {
printf("Failed to open the file\n");
return;
}
//Close the file
fclose(fp);
}
if (access(ZENOH_SUB_CONFIG_PATH, F_OK) != 0) {
// Open file in write mode
fp = fopen(ZENOH_SUB_CONFIG_PATH, "w");
// If file opened successfully, then write the string to file
if (fp) {
fputs(default_sub_config, fp);
} else {
printf("Failed to open the file\n");
return;
}
//Close the file
fclose(fp);
}
}

103
src/modules/zenoh/zenoh_config.hpp Normal file
View File

@ -0,0 +1,103 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file zenoh_config.hpp
*
* Defines Zenoh configuration backend
*
* @author Peter van der Perk <peter.vanderperk@nxp.com>
*/
#pragma once
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <lib/parameters/param.h>
#include <containers/List.hpp>
#include <zenoh-pico.h>
#define ZENOH_MAX_PATH_LENGTH (128 + 40)
#define ZENOH_SD_ROOT_PATH CONFIG_ZENOH_CONFIG_PATH
#define ZENOH_PUB_CONFIG_PATH ZENOH_SD_ROOT_PATH"/pub.csv"
#define ZENOH_SUB_CONFIG_PATH ZENOH_SD_ROOT_PATH"/sub.csv"
#define ZENOH_NET_CONFIG_PATH ZENOH_SD_ROOT_PATH"/net.txt"
#define NET_MODE_SIZE sizeof("client")
#define NET_LOCATOR_SIZE 64
#define NET_CONFIG_LINE_SIZE NET_MODE_SIZE + NET_LOCATOR_SIZE
#define TOPIC_INFO_SIZE 64
#define MAX_LINE_SIZE 2*TOPIC_INFO_SIZE
class Zenoh_Config
{
public:
Zenoh_Config();
~Zenoh_Config();
int cli(int argc, char *argv[]);
void getNetworkConfig(char *mode, char *locator);
int getPubCount()
{
return getLineCount(ZENOH_PUB_CONFIG_PATH);
}
int getSubCount()
{
return getLineCount(ZENOH_SUB_CONFIG_PATH);
}
int getPublisherMapping(char *topic, char *type)
{
return getPubSubMapping(topic, type, ZENOH_PUB_CONFIG_PATH);
}
int getSubscriberMapping(char *topic, char *type)
{
return getPubSubMapping(topic, type, ZENOH_SUB_CONFIG_PATH);
}
private:
int getPubSubMapping(char *topic, char *type, const char *filename);
int AddPubSub(char *topic, char *datatype, const char *filename);
int SetNetworkConfig(char *mode, char *locator);
int getLineCount(const char *filename);
const char *get_csv_field(char *line, int num);
void generate_clean_config();
void dump_config();
FILE *fp_mapping;
};