diff --git a/Tools/msg/templates/cdrstream/uorb_idl_header.h.em b/Tools/msg/templates/cdrstream/uorb_idl_header.h.em index 4ad097a54c..435dcc39e4 100644 --- a/Tools/msg/templates/cdrstream/uorb_idl_header.h.em +++ b/Tools/msg/templates/cdrstream/uorb_idl_header.h.em @@ -49,14 +49,14 @@ for field in spec.parsed_fields(): (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)) + print('typedef px4_msgs_msg_%s px4_msgs_msg_px4_msgs__msg__%s;' % (name,name)) }@ -typedef struct @uorb_struct px4_msg_@(file_base_name); +typedef struct @uorb_struct px4_msgs_msg_@(file_base_name); -extern const struct dds_cdrstream_desc px4_msg_@(file_base_name)_cdrstream_desc; +extern const struct dds_cdrstream_desc px4_msgs_msg_@(file_base_name)_cdrstream_desc; #ifdef __cplusplus } diff --git a/Tools/zenoh/px_generate_zenoh_topic_files.py b/Tools/zenoh/px_generate_zenoh_topic_files.py index b9110dbea8..a6f5d12c4b 100755 --- a/Tools/zenoh/px_generate_zenoh_topic_files.py +++ b/Tools/zenoh/px_generate_zenoh_topic_files.py @@ -42,6 +42,7 @@ import os import argparse import re import sys +import json try: import em @@ -124,7 +125,7 @@ def generate_by_template(output_file, template_file, em_globals): return True -def generate_topics_list_file_from_files(files, outputdir, template_filename, templatedir): +def generate_topics_list_file_from_files(files, outputdir, template_filename, templatedir, rihs_path): # 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")]: @@ -138,11 +139,23 @@ def generate_topics_list_file_from_files(files, outputdir, template_filename, te 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","")) + rihs01_hashes = dict() + if rihs_path != '': + for topic in full_base_names: + with open(rihs_path + "/msg/" + topic + ".json") as f: + d = json.load(f) + + rihs01_hash = d['type_hashes'][0]['hash_string'][7:] + + byte_array = [f"0x{rihs01_hash[i:i+2]}" for i in range(0, len(rihs01_hash), 2)] + c_code = f"{{ {', '.join(byte_array)} }};" + rihs01_hashes[topic] = c_code + 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_globals = {"msgs": filenames, "topics": topics, "datatypes": datatypes, "full_base_names": full_base_names, "rihs01_hashes": rihs01_hashes} tl_template_file = os.path.join(templatedir, template_filename) tl_out_file = os.path.join(outputdir, template_filename.replace(".em", "")) @@ -162,13 +175,15 @@ if __name__ == "__main__": parser.add_argument('-p', dest='prefix', default='', help='string added as prefix to the output file ' ' name when converting directories') + parser.add_argument('-rihs', dest='rihs', default='', + help='path where rihs01 json files located') args = parser.parse_args() if args.zenoh_config: - generate_topics_list_file_from_files(args.file, args.outputdir, ZENOH_TEMPLATE_FILE[0], args.templatedir) + generate_topics_list_file_from_files(args.file, args.outputdir, ZENOH_TEMPLATE_FILE[0], args.templatedir, args.rihs) exit(0) elif args.zenoh_pub_sub: - generate_topics_list_file_from_files(args.file, args.outputdir, ZENOH_TEMPLATE_FILE[1], args.templatedir) + generate_topics_list_file_from_files(args.file, args.outputdir, ZENOH_TEMPLATE_FILE[1], args.templatedir, args.rihs) exit(0) else: print('Error: either --headers or --sources must be specified') diff --git a/Tools/zenoh/templates/zenoh/uorb_pubsub_factory.hpp.em b/Tools/zenoh/templates/zenoh/uorb_pubsub_factory.hpp.em index d7bdc2f16b..41eb34f947 100644 --- a/Tools/zenoh/templates/zenoh/uorb_pubsub_factory.hpp.em +++ b/Tools/zenoh/templates/zenoh/uorb_pubsub_factory.hpp.em @@ -88,9 +88,14 @@ type_topic_count = len([e for e in topic_names_all if e.startswith(topic_name)]) CONFIG_ZENOH_PUBSUB_@(topic_name.upper())_COUNT + \ @[end for] 0 +@[for topic_name, rihs01_hash in rihs01_hashes.items()]@ +const uint8_t @(topic_name)_hash[32] = @(rihs01_hash) +@[end for] + typedef struct { const uint32_t *ops; const orb_metadata* orb_meta; + const uint8_t *hash; } UorbPubSubTopicBinder; const UorbPubSubTopicBinder _topics[ZENOH_PUBSUB_COUNT] { @@ -104,8 +109,9 @@ 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)) + px4_msgs_msg_@(topic_dict[topic_name])_cdrstream_desc.ops.ops, + ORB_ID(@(topic_name_inst)), + @(topic_dict[topic_name])_hash }, @{ uorb_id_idx += 1 @@ -152,3 +158,21 @@ Zenoh_Subscriber* genSubscriber(const char *name) { } return NULL; } + +const uint8_t* getRIHS01_Hash(const orb_metadata *meta) { + for (auto &sub : _topics) { + if(sub.orb_meta->o_id == meta->o_id) { + return sub.hash; + } + } + return NULL; +} + +const uint8_t* getRIHS01_Hash(const char *name) { + for (auto &sub : _topics) { + if(strcmp(sub.orb_meta->o_name, name) == 0) { + return sub.hash; + } + } + return NULL; +} diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index cf1a150dea..cbe2085d7b 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -424,9 +424,11 @@ add_dependencies(uorb_msgs prebuild_targets uorb_headers) if(CONFIG_LIB_CDRSTREAM) set(uorb_cdr_idl) set(uorb_cdr_msg) + set(uorb_cdr_hash) 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_rihs01_out_path ${idl_include_path}/px4) set(idl_uorb_path ${PX4_BINARY_DIR}/msg/px4/msg) # Make sure that CycloneDDS has been checkout out @@ -457,6 +459,7 @@ if(CONFIG_LIB_CDRSTREAM) configure_file(${msg_file} ${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_hash ${idl_out_path}/${msg}.json) list(APPEND uorb_cdr_idl_uorb ${idl_uorb_path}/${msg}.h) endforeach() @@ -476,6 +479,25 @@ if(CONFIG_LIB_CDRSTREAM) VERBATIM ) + file(CREATE_LINK ${idl_rihs01_out_path} ${idl_include_path}/px4_msgs SYMBOLIC) + + # Generate IDL from .msg using rosidl_adapter + # Note this a submodule inside PX4 hence no ROS2 installation required + add_custom_command( + OUTPUT ${uorb_cdr_hash} + COMMAND ${CMAKE_COMMAND} + -E env "PYTHONPATH=${PX4_SOURCE_DIR}/src/lib/cdrstream/rosidl/rosidl_adapter:${PX4_SOURCE_DIR}/src/lib/cdrstream/rosidl/rosidl_cli:${PX4_SOURCE_DIR}/src/lib/cdrstream/rosidl/rosidl_parser:${PX4_SOURCE_DIR}/src/lib/cdrstream/rosidl/rosidl_generator_type_description" + ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/cdrstream/idl2rihs01.py + --output-dir ${idl_rihs01_out_path} + ${uorb_cdr_idl} + DEPENDS + ${uorb_cdr_idl} + git_cyclonedds + COMMENT "Generating RIHS01 hashes from IDL" + 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") @@ -505,6 +527,7 @@ if(CONFIG_LIB_CDRSTREAM) DEPENDS uorb_cdrstream ${msg_files} + ${uorb_cdr_hash} ${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 @@ -534,8 +557,10 @@ if(CONFIG_MODULES_ZENOH) -f ${msg_files} -o ${PX4_BINARY_DIR}/src/modules/zenoh/ -e ${PX4_SOURCE_DIR}/Tools/zenoh/templates/zenoh + -rihs ${idl_rihs01_out_path} DEPENDS ${msg_files} + ${uorb_cdr_hash} ${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" diff --git a/src/lib/cdrstream/idl2rihs01.py b/src/lib/cdrstream/idl2rihs01.py new file mode 100644 index 0000000000..f535d73ac2 --- /dev/null +++ b/src/lib/cdrstream/idl2rihs01.py @@ -0,0 +1,92 @@ +#!/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 +import os +import tempfile +import json + +try: + from rosidl_generator_type_description import generate_type_hash +except ImportError: + # modifying sys.path and importing the Python package with the same + # name as this script does not work on Windows + rosidl_generator_type_description_root = os.path.dirname(os.path.dirname(__file__)) + rosidl_generator_type_description_module = os.path.join( + rosidl_generator_type_description_root, 'rosidl_generator_type_description', '__init__.py') + if not os.path.exists(rosidl_generator_type_description_module): + raise + from importlib.machinery import SourceFileLoader + + loader = SourceFileLoader('rosidl_generator_type_description', rosidl_generator_type_description_module) + rosidl_generator_type_description = loader.load_module() + generate_type_hash = rosidl_generator_type_description.generate_type_hash + +if __name__ == '__main__': + parser = argparse.ArgumentParser( + description=f'Convert px4 .idl files to rihs01') + parser.add_argument( + 'interface_files', nargs='+', + help='The interface files to convert') + parser.add_argument( + '--output-dir', '-o', + help='The directory to save converted files (default: current directory)') + args = parser.parse_args(sys.argv[1:]) + + # So for some odd reason rosidl doesn't do proper cli arguments but believes + # that some magically crafted json is better + + idl_files = [] + + type_hash_arguments = {} + type_hash_arguments['package_name'] = "px4_msgs" + type_hash_arguments['output_dir'] = args.output_dir + type_hash_arguments["idl_tuples"] = idl_files + + for interface_file in args.interface_files: + # So file path need to be magically encoded with a : to let the parser do its thing + + interface_file = str(pathlib.Path(interface_file)).replace("px4/msg", "px4:msg") + idl_files.append(interface_file) + + json_file = tempfile.NamedTemporaryFile(mode="w+",delete=False) + json.dump(type_hash_arguments, json_file) + json_file.flush() + + print(json_file.name) + + generate_type_hash(json_file.name) diff --git a/src/lib/cdrstream/msg2idl.py b/src/lib/cdrstream/msg2idl.py index 74a464ab3d..2b02739907 100644 --- a/src/lib/cdrstream/msg2idl.py +++ b/src/lib/cdrstream/msg2idl.py @@ -52,6 +52,6 @@ if __name__ == '__main__': package_dir = interface_file.parent.absolute() convert_msg_to_idl( - package_dir, 'px4', + package_dir, 'px4_msgs', interface_file.absolute().relative_to(package_dir), interface_file.parent)