microdds: add xrce client

.gitmodules

@@ -64,3 +64,6 @@
 	path = src/lib/crypto/libtommath
 	url = https://github.com/PX4/libtommath.git
 	branch = px4
+[submodule "src/modules/microdds_client/Micro-XRCE-DDS-Client"]
+	path = src/modules/microdds_client/Micro-XRCE-DDS-Client
+	url = https://github.com/eProsima/Micro-XRCE-DDS-Client.git

Tools/astyle/files_to_check_code_style.sh

@@ -27,4 +27,5 @@ exec find boards msg src platforms test \
     -path src/lib/crypto/monocypher -prune -o \
     -path src/lib/crypto/libtomcrypt -prune -o \
     -path src/lib/crypto/libtommath -prune -o \
+    -path src/modules/microdds_client/Micro-XRCE-DDS-Client -prune -o \
     -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN

Tools/kconfig/cmake_kconfig_lut.txt

@@ -139,6 +139,7 @@ mc_hover_thrust_estimator,CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 mc_pos_control,CONFIG_MODULES_MC_POS_CONTROL=y
 mc_rate_control,CONFIG_MODULES_MC_RATE_CONTROL=y
 micrortps_bridge,CONFIG_MODULES_MICRORTPS_BRIDGE=y
+microdds_client,CONFIG_MODULES_MICRODDS_CLIENT=y
 navigator,CONFIG_MODULES_NAVIGATOR=y
 px4iofirmware,CONFIG_MODULES_PX4IOFIRMWARE=y
 rc_update,CONFIG_MODULES_RC_UPDATE=y

boards/px4/fmu-v5/rtps.px4board

@@ -1,3 +1,4 @@
 CONFIG_DRIVERS_HEATER=n
 CONFIG_DRIVERS_OSD=n
 CONFIG_MODULES_MICRORTPS_BRIDGE=y
+CONFIG_MODULES_MICRODDS_CLIENT=y

boards/px4/fmu-v5x/rtps.px4board

@@ -1 +1,2 @@
 CONFIG_MODULES_MICRORTPS_BRIDGE=y
+CONFIG_MODULES_MICRODDS_CLIENT=y

boards/px4/sitl/rtps.px4board

@@ -1 +1,2 @@
 CONFIG_MODULES_MICRORTPS_BRIDGE=y
+CONFIG_MODULES_MICRODDS_CLIENT=y

msg/CMakeLists.txt

@@ -260,14 +260,17 @@ endif()
 
 # headers
 set(msg_out_path ${PX4_BINARY_DIR}/uORB/topics)
+set(ucdr_out_path ${PX4_BINARY_DIR}/uORB/ucdr)
 set(msg_source_out_path	${CMAKE_CURRENT_BINARY_DIR}/topics_sources)
 
 set(uorb_headers ${msg_out_path}/uORBTopics.hpp)
 set(uorb_sources ${msg_source_out_path}/uORBTopics.cpp)
+set(uorb_ucdr_headers)
 foreach(msg_file ${msg_files})
 	get_filename_component(msg ${msg_file} NAME_WE)
 	list(APPEND uorb_headers ${msg_out_path}/${msg}.h)
 	list(APPEND uorb_sources ${msg_source_out_path}/${msg}.cpp)
+	list(APPEND uorb_ucdr_headers ${ucdr_out_path}/${msg}.h)
 endforeach()
 
 if (px4_constrained_flash_build)
@@ -300,6 +303,27 @@ add_custom_command(OUTPUT ${uorb_headers}
 	)
 add_custom_target(uorb_headers DEPENDS ${uorb_headers})
 
+add_custom_command(OUTPUT ${uorb_ucdr_headers}
+	COMMAND ${PYTHON_EXECUTABLE} tools/px_generate_uorb_topic_files.py
+		--headers
+		-f ${msg_files}
+		-i ${CMAKE_CURRENT_SOURCE_DIR}
+		-o ${ucdr_out_path}
+		-e templates/ucdr
+		-t ${CMAKE_CURRENT_BINARY_DIR}/tmp/ucdr_headers
+		-q
+		${added_arguments}
+	DEPENDS
+		${msg_files}
+		templates/ucdr/msg.h.em
+		tools/px_generate_uorb_topic_files.py
+		tools/px_generate_uorb_topic_helper.py
+	COMMENT "Generating uORB topic ucdr headers"
+	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+	VERBATIM
+	)
+add_custom_target(uorb_ucdr_headers DEPENDS ${uorb_ucdr_headers})
+
 # Generate uORB sources
 add_custom_command(OUTPUT ${uorb_sources}
 	COMMAND ${PYTHON_EXECUTABLE} tools/px_generate_uorb_topic_files.py

msg/templates/ucdr/msg.h.em

@@ -0,0 +1,128 @@
+@###############################################
+@#
+@# EmPy template
+@#
+@###############################################
+@# generates CDR serialization & deserialization methods
+@#
+@# Context:
+@#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
+@#  - search_path (str) Path to .msg files
+@###############################################
+@{
+import genmsg.msgs
+from px_generate_uorb_topic_helper import * # this is in Tools/
+
+topic = spec.short_name
+uorb_struct = '%s_s'%spec.short_name
+
+# get fields, struct size and paddings
+def add_fields(msg_fields, name_prefix='', offset=0):
+	fields = []
+	for field in msg_fields:
+		if not field.is_header:
+			field_size = sizeof_field_type(field)
+
+			type_name = field.type
+			# detect embedded types
+			sl_pos = type_name.find('/')
+			if (sl_pos >= 0):
+				package = type_name[:sl_pos]
+				type_name = type_name[sl_pos + 1:]
+
+			# detect arrays
+			a_pos = type_name.find('[')
+			array_size = 1
+			if (a_pos >= 0):
+				# field is array
+				array_size = int(type_name[a_pos+1:-1])
+				type_name = type_name[:a_pos]
+
+			if sl_pos >= 0: # nested type
+
+				children_fields = get_children_fields(field.base_type, search_path)
+
+				for i in range(array_size):
+					sub_name_prefix = name_prefix+field.name
+					if array_size > 1:
+						sub_name_prefix += '['+str(i)+']'
+					sub_fields, offset = add_fields(children_fields, sub_name_prefix+'.', offset)
+					fields.extend(sub_fields)
+			else:
+				assert field_size > 0
+
+				# note: the maximum alignment for XCDR is 8 and for XCDR2 it is 4
+				padding = (field_size - (offset % field_size)) & (field_size - 1)
+
+				fields.append((type_name, name_prefix+field.name, field_size * array_size, padding))
+				offset += array_size * field_size + padding
+	return fields, offset
+
+fields, struct_size = add_fields(spec.parsed_fields())
+
+}@
+
+// auto-generated file
+
+#pragma once
+
+#include <ucdr/microcdr.h>
+#include <string.h>
+#include <uORB/topics/@(topic).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 <uORB/ucdr/%s.h>'%(name))
+}@
+
+static inline constexpr int ucdr_topic_size_@(topic)()
+{
+	return @(struct_size);
+}
+
+bool ucdr_serialize_@(topic)(const @(uorb_struct)& topic, ucdrBuffer& buf)
+{
+	if (ucdr_buffer_remaining(&buf) < @(struct_size)) {
+		return false;
+	}
+@{
+for field_type, field_name, field_size, padding in fields:
+	if padding > 0:
+		print('\tbuf.iterator += {:}; // padding'.format(padding))
+		print('\tbuf.offset += {:}; // padding'.format(padding))
+
+	print('\tstatic_assert(sizeof(topic.{0}) == {1}, "size mismatch");'.format(field_name, field_size))
+	print('\tmemcpy(buf.iterator, &topic.{0}, sizeof(topic.{0}));'.format(field_name))
+	print('\tbuf.iterator += sizeof(topic.{:});'.format(field_name))
+	print('\tbuf.offset += sizeof(topic.{:});'.format(field_name))
+
+}@
+	return true;
+}
+
+bool ucdr_deserialize_@(topic)(ucdrBuffer& buf, @(uorb_struct)& topic)
+{
+	if (ucdr_buffer_remaining(&buf) < @(struct_size)) {
+		return false;
+	}
+@{
+for field_type, field_name, field_size, padding in fields:
+	if padding > 0:
+		print('\tbuf.iterator += {:}; // padding'.format(padding))
+		print('\tbuf.offset += {:}; // padding'.format(padding))
+
+	print('\tstatic_assert(sizeof(topic.{0}) == {1}, "size mismatch");'.format(field_name, field_size))
+	print('\tmemcpy(&topic.{0}, buf.iterator, sizeof(topic.{0}));'.format(field_name))
+	print('\tbuf.iterator += sizeof(topic.{:});'.format(field_name))
+	print('\tbuf.offset += sizeof(topic.{:});'.format(field_name))
+
+}@
+	return true;
+}