Update cdrstream code generator including typehash

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
 }

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

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

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"

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)

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)