msg ROS2 compatibility, microdds_client improvements (timesync, reduced code size, added topics, etc), fastrtps purge

- update all msgs to be directly compatible with ROS2
 - microdds_client improvements
   - timesync
   - reduced code size
   - add to most default builds if we can afford it
   - lots of other little changes
 - purge fastrtps (I tried to save this multiple times, but kept hitting roadblocks)

src/modules/microdds_client/CMakeLists.txt

@@ -31,89 +31,125 @@
 #
 ############################################################################
 
-px4_add_git_submodule(TARGET git_micro_xrce_dds_client PATH "${CMAKE_CURRENT_LIST_DIR}/Micro-XRCE-DDS-Client")
+if(${CMAKE_VERSION} VERSION_LESS "3.11")
+    message(WARNING "skipping microdds_client, Micro-XRCE-DDS-Client needs to be fixed to work with CMAKE_VERSION ${CMAKE_VERSION}")
 
-# If a toolchain file is given, explicitly define its path when building the Micro-XRCE-DDS-CLient
-if(CMAKE_TOOLCHAIN_FILE MATCHES "cmake$")
-	set(microxrceddsclient_toolchain ${CMAKE_TOOLCHAIN_FILE})
-elseif(CMAKE_TOOLCHAIN_FILE)
-	set(microxrceddsclient_toolchain ${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake/${CMAKE_TOOLCHAIN_FILE}.cmake)
-endif()
+else()
+	px4_add_git_submodule(TARGET git_micro_xrce_dds_client PATH "${CMAKE_CURRENT_LIST_DIR}/Micro-XRCE-DDS-Client")
 
-# Include NuttX headers
-get_property(include_dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES)
-set(c_flags_with_includes "${CMAKE_C_FLAGS}")
-set(cxx_flags_with_includes "${CMAKE_CXX_FLAGS}")
-foreach(dir ${include_dirs})
-	set(c_flags_with_includes "${c_flags_with_includes} -I${dir}")
-	set(cxx_flags_with_includes "${cxx_flags_with_includes} -I${dir}")
-endforeach()
+	# If a toolchain file is given, explicitly define its path when building the Micro-XRCE-DDS-CLient
+	if(CMAKE_TOOLCHAIN_FILE MATCHES "cmake$")
+		set(microxrceddsclient_toolchain ${CMAKE_TOOLCHAIN_FILE})
+	elseif(CMAKE_TOOLCHAIN_FILE)
+		set(microxrceddsclient_toolchain ${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake/${CMAKE_TOOLCHAIN_FILE}.cmake)
+	endif()
 
-set(lib_dir ${CMAKE_INSTALL_LIBDIR})
-if("${lib_dir}" STREQUAL "")
-	set(lib_dir "lib")
-endif()
+	# Include NuttX headers
+	get_property(include_dirs DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES)
+	set(c_flags_with_includes "${CMAKE_C_FLAGS}")
+	set(cxx_flags_with_includes "${CMAKE_CXX_FLAGS}")
+	foreach(dir ${include_dirs})
+		set(c_flags_with_includes "${c_flags_with_includes} -I${dir}")
+		set(cxx_flags_with_includes "${cxx_flags_with_includes} -I${dir}")
+	endforeach()
 
-set(microxrceddsclient_src_dir   ${CMAKE_CURRENT_SOURCE_DIR}/Micro-XRCE-DDS-Client)
-set(microxrceddsclient_build_dir ${CMAKE_CURRENT_BINARY_DIR})
+	set(lib_dir ${CMAKE_INSTALL_LIBDIR})
+	if("${lib_dir}" STREQUAL "")
+		set(lib_dir "lib")
+	endif()
 
-include(ExternalProject)
+	set(microxrceddsclient_src_dir   ${CMAKE_CURRENT_SOURCE_DIR}/Micro-XRCE-DDS-Client)
+	set(microxrceddsclient_build_dir ${CMAKE_CURRENT_BINARY_DIR})
 
-ExternalProject_Add(
-	libmicroxrceddsclient_project
-	PREFIX      ${microxrceddsclient_build_dir}
-	SOURCE_DIR  ${microxrceddsclient_src_dir}
-	INSTALL_DIR ${CMAKE_CURRENT_BINARY_DIR}
-	CMAKE_CACHE_ARGS
-		-DCMAKE_CXX_COMPILER:FILEPATH=${CMAKE_CXX_COMPILER}
-		-DCMAKE_C_COMPILER:FILEPATH=${CMAKE_C_COMPILER}
-		-DCMAKE_BUILD_TYPE:STRING=${CMAKE_BUILD_TYPE}
-		-DCMAKE_C_FLAGS:STRING=${c_flags_with_includes}
-		-DCMAKE_CXX_FLAGS:STRING=${cxx_flags_with_includes}
-		-DCMAKE_EXE_LINKER_FLAGS:STRING=${CMAKE_EXE_LINKER_FLAGS}
-		-DUCLIENT_PIC:BOOL=OFF
-		-DUCLIENT_PROFILE_TCP:BOOL=OFF
-		-DUCLIENT_PROFILE_UDP:BOOL=ON
-		-DUCLIENT_PROFILE_SERIAL:BOOL=ON
-		-DUCLIENT_PROFILE_DISCOVERY:BOOL=OFF
-		-DUCLIENT_PROFILE_CUSTOM_TRANSPORT:BOOL=ON
-		-DUCLIENT_PLATFORM_POSIX:BOOL=ON
-		-DUCLIENT_BUILD_MICROCDR:BOOL=ON
-		-DCMAKE_INSTALL_PREFIX:PATH=${CMAKE_CURRENT_BINARY_DIR}
-		-DCMAKE_PREFIX_PATH:PATH=${CMAKE_CURRENT_BINARY_DIR}
-		-DCMAKE_TOOLCHAIN_FILE:STRING=${microxrceddsclient_toolchain}
-	BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicroxrcedds_client.a
-	BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicrocdr.a
-	DEPENDS prebuild_targets git_micro_xrce_dds_client
-    )
+	include(ExternalProject)
 
-add_library(libmicroxrceddsclient STATIC IMPORTED GLOBAL)
-set_target_properties(libmicroxrceddsclient
-	PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicroxrcedds_client.a
-)
-
-add_library(libmicrocdr STATIC IMPORTED GLOBAL)
-set_target_properties(libmicrocdr
-	PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicrocdr.a
-)
-
-add_library(microxrceddsclient INTERFACE)
-add_dependencies(microxrceddsclient libmicroxrceddsclient)
-add_dependencies(microxrceddsclient libmicrocdr)
-add_dependencies(microxrceddsclient libmicroxrceddsclient_project)
-target_include_directories(microxrceddsclient INTERFACE ${microxrceddsclient_build_dir}/include)
-
-px4_add_module(
-	MODULE modules__microdds_client
-	MAIN microdds_client
-	STACK_MAIN 8000
-	SRCS
-		microdds_client.cpp
-	DEPENDS
-		microxrceddsclient
-		libmicroxrceddsclient
-		libmicrocdr
-		uorb_ucdr_headers
+	ExternalProject_Add(
+		libmicroxrceddsclient_project
+		PREFIX      ${microxrceddsclient_build_dir}
+		SOURCE_DIR  ${microxrceddsclient_src_dir}
+		INSTALL_DIR ${CMAKE_CURRENT_BINARY_DIR}
+		CMAKE_CACHE_ARGS
+			-DCMAKE_CXX_COMPILER:FILEPATH=${CMAKE_CXX_COMPILER}
+			-DCMAKE_C_COMPILER:FILEPATH=${CMAKE_C_COMPILER}
+			-DCMAKE_BUILD_TYPE:STRING=${CMAKE_BUILD_TYPE}
+			-DCMAKE_C_FLAGS:STRING=${c_flags_with_includes}
+			-DCMAKE_CXX_FLAGS:STRING=${cxx_flags_with_includes}
+			-DCMAKE_EXE_LINKER_FLAGS:STRING=${CMAKE_EXE_LINKER_FLAGS}
+			-DUCLIENT_PIC:BOOL=OFF
+			-DUCLIENT_PROFILE_TCP:BOOL=OFF
+			-DUCLIENT_PROFILE_UDP:BOOL=ON
+			-DUCLIENT_PROFILE_SERIAL:BOOL=ON
+			-DUCLIENT_PROFILE_DISCOVERY:BOOL=OFF
+			-DUCLIENT_PROFILE_CUSTOM_TRANSPORT:BOOL=OFF
+			-DUCLIENT_PROFILE_MULTITHREAD:BOOL=OFF
+			-DUCLIENT_PROFILE_SHARED_MEMORY:BOOL=OFF
+			-DUCLIENT_PLATFORM_POSIX:BOOL=ON
+			-DUCLIENT_BUILD_MICROCDR:BOOL=ON
+			-DCMAKE_INSTALL_PREFIX:PATH=${CMAKE_CURRENT_BINARY_DIR}
+			-DCMAKE_PREFIX_PATH:PATH=${CMAKE_CURRENT_BINARY_DIR}
+			-DCMAKE_TOOLCHAIN_FILE:STRING=${microxrceddsclient_toolchain}
+		BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicroxrcedds_client.a
+		BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicrocdr.a
+		DEPENDS prebuild_targets git_micro_xrce_dds_client
+		LOG_CONFIGURE true
+		LOG_BUILD true
+		LOG_INSTALL true
+		LOG_OUTPUT_ON_FAILURE true
 	)
 
-add_dependencies(modules__microdds_client topic_bridge_files)
+	add_library(libmicroxrceddsclient STATIC IMPORTED GLOBAL)
+	set_target_properties(libmicroxrceddsclient
+		PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicroxrcedds_client.a
+	)
+
+	add_library(libmicrocdr STATIC IMPORTED GLOBAL)
+	set_target_properties(libmicrocdr
+		PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${lib_dir}/libmicrocdr.a
+	)
+
+	add_library(microxrceddsclient INTERFACE)
+	add_dependencies(microxrceddsclient libmicroxrceddsclient)
+	add_dependencies(microxrceddsclient libmicrocdr)
+	add_dependencies(microxrceddsclient libmicroxrceddsclient_project)
+	target_include_directories(microxrceddsclient INTERFACE ${microxrceddsclient_build_dir}/include)
+
+
+
+	add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h
+		COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py
+			--topic-msg-dir ${PX4_SOURCE_DIR}/msg
+			--client-outdir ${CMAKE_CURRENT_BINARY_DIR}
+			--dds-topics-file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.yaml
+			--template_file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.h.em
+		DEPENDS
+			${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py
+			${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.yaml
+			${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.h.em
+		COMMENT "Generating XRCE-DDS topic bridge"
+	)
+	add_custom_target(topic_bridge_files DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h)
+
+	px4_add_module(
+		MODULE modules__microdds_client
+		MAIN microdds_client
+		STACK_MAIN 9000
+		INCLUDES
+			${CMAKE_CURRENT_SOURCE_DIR}
+		COMPILE_FLAGS
+			#-O0
+			${MAX_CUSTOM_OPT_LEVEL}
+		SRCS
+			${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h
+			microdds_client.cpp
+			microdds_client.h
+		DEPENDS
+			microxrceddsclient
+			libmicroxrceddsclient
+			libmicrocdr
+			timesync
+			topic_bridge_files
+			uorb_ucdr_headers
+		MODULE_CONFIG
+			module.yaml
+		)
+endif()

src/modules/microdds_client/dds_topics.h.em

@@ -0,0 +1,127 @@
+@###############################################
+@#
+@# EmPy template
+@#
+@###############################################
+@# Generates publications and subscriptions for XRCE
+@#
+@# Context:
+@#  - msgs (List) list of all RTPS messages
+@#  - topics (List) list of all topic names
+@#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
+@###############################################
+@{
+import os
+
+
+}@
+
+#include <utilities.hpp>
+
+#include <uxr/client/client.h>
+#include <ucdr/microcdr.h>
+
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+@[for include in type_includes]@
+#include <uORB/ucdr/@(include).h>
+@[end for]@
+
+// Subscribers for messages to send
+struct SendTopicsSubs {
+@[    for pub in publications]@
+	uORB::Subscription @(pub['topic_simple'])_sub{ORB_ID(@(pub['topic_simple']))};
+	uxrObjectId @(pub['topic_simple'])_data_writer{};
+@[    end for]@
+
+	uint32_t num_payload_sent{};
+
+	void update(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId best_effort_stream_id, uxrObjectId participant_id, const char *client_namespace);
+};
+
+void SendTopicsSubs::update(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId best_effort_stream_id, uxrObjectId participant_id, const char *client_namespace)
+{
+	int64_t time_offset_us = session->time_offset / 1000; // ns -> us
+@[    for idx, pub in enumerate(publications)]@
+
+	{
+		@(pub['simple_base_type'])_s data;
+
+		if (@(pub['topic_simple'])_sub.update(&data)) {
+
+			if (@(pub['topic_simple'])_data_writer.id == UXR_INVALID_ID) {
+				// data writer not created yet
+				create_data_writer(session, reliable_out_stream_id, participant_id, ORB_ID::@(pub['topic_simple']), client_namespace, "@(pub['topic_simple'])", "@(pub['dds_type'])", @(pub['topic_simple'])_data_writer);
+			}
+
+			if (@(pub['topic_simple'])_data_writer.id != UXR_INVALID_ID) {
+
+				ucdrBuffer ub;
+				uint32_t topic_size = ucdr_topic_size_@(pub['simple_base_type'])();
+				if (uxr_prepare_output_stream(session, best_effort_stream_id, @(pub['topic_simple'])_data_writer, &ub, topic_size) != UXR_INVALID_REQUEST_ID) {
+					ucdr_serialize_@(pub['simple_base_type'])(data, ub, time_offset_us);
+					// TODO: fill up the MTU and then flush, which reduces the packet overhead
+					uxr_flash_output_streams(session);
+					num_payload_sent += topic_size;
+
+				} else {
+					//PX4_ERR("Error uxr_prepare_output_stream UXR_INVALID_REQUEST_ID @(pub['topic_simple'])");
+				}
+
+			} else {
+				//PX4_ERR("Error UXR_INVALID_ID @(pub['topic_simple'])");
+			}
+
+		}
+	}
+@[    end for]@
+}
+
+// Publishers for received messages
+struct RcvTopicsPubs {
+@[    for sub in subscriptions]@
+	uORB::Publication<@(sub['simple_base_type'])_s> @(sub['topic_simple'])_pub{ORB_ID(@(sub['topic_simple']))};
+@[    end for]@
+
+	uint32_t num_payload_received{};
+
+	bool init(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId reliable_in_stream_id, uxrStreamId best_effort_in_stream_id, uxrObjectId participant_id, const char *client_namespace);
+};
+
+static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id,
+		     struct ucdrBuffer *ub, uint16_t length, void *args)
+{
+	RcvTopicsPubs *pubs = (RcvTopicsPubs *)args;
+	const int64_t time_offset_us = session->time_offset / 1000; // ns -> us
+	pubs->num_payload_received += length;
+
+	switch (object_id.id) {
+@[    for idx, sub in enumerate(subscriptions)]@
+	case @(idx)+1000: {
+			@(sub['simple_base_type'])_s data;
+
+			if (ucdr_deserialize_@(sub['simple_base_type'])(*ub, data, time_offset_us)) {
+				//print_message(ORB_ID(@(sub['simple_base_type'])), data);
+				pubs->@(sub['topic_simple'])_pub.publish(data);
+			}
+		}
+		break;
+
+@[    end for]@
+
+	default:
+		PX4_ERR("unknown object id: %i", object_id.id);
+		break;
+	}
+}
+
+bool RcvTopicsPubs::init(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId reliable_in_stream_id, uxrStreamId best_effort_in_stream_id, uxrObjectId participant_id, const char *client_namespace)
+{
+@[    for idx, sub in enumerate(subscriptions)]@
+	create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx), client_namespace, "@(sub['topic_simple'])", "@(sub['dds_type'])");
+@[    end for]@
+
+	uxr_set_topic_callback(session, on_topic_update, this);
+
+	return true;
+}

src/modules/microdds_client/dds_topics.yaml

@@ -0,0 +1,86 @@
+#####
+#
+# This file maps all the topics that are to be used on the micro DDS client.
+#
+#####
+publications:
+
+  - topic: /fmu/out/collision_constraints
+    type: px4_msgs::msg::CollisionConstraints
+
+  - topic: /fmu/out/failsafe_flags
+    type: px4_msgs::msg::FailsafeFlags
+
+  - topic: /fmu/out/sensor_combined
+    type: px4_msgs::msg::SensorCombined
+
+  - topic: /fmu/out/timesync_status
+    type: px4_msgs::msg::TimesyncStatus
+
+  # - topic: /fmu/out/vehicle_angular_velocity
+  #   type: px4_msgs::msg::VehicleAngularVelocity
+
+  - topic: /fmu/out/vehicle_attitude
+    type: px4_msgs::msg::VehicleAttitude
+
+  - topic: /fmu/out/vehicle_control_mode
+    type: px4_msgs::msg::VehicleControlMode
+
+  - topic: /fmu/out/vehicle_global_position
+    type: px4_msgs::msg::VehicleGlobalPosition
+
+  - topic: /fmu/out/vehicle_gps_position
+    type: px4_msgs::msg::SensorGps
+
+  - topic: /fmu/out/vehicle_local_position
+    type: px4_msgs::msg::VehicleLocalPosition
+
+  - topic: /fmu/out/vehicle_odometry
+    type: px4_msgs::msg::VehicleOdometry
+
+  - topic: /fmu/out/vehicle_status
+    type: px4_msgs::msg::VehicleStatus
+
+  - topic: /fmu/out/vehicle_trajectory_waypoint_desired
+    type: px4_msgs::msg::VehicleTrajectoryWaypoint
+
+subscriptions:
+
+  - topic: /fmu/in/offboard_control_mode
+    type: px4_msgs::msg::OffboardControlMode
+
+  - topic: /fmu/in/onboard_computer_status
+    type: px4_msgs::msg::OnboardComputerStatus
+
+  - topic: /fmu/in/obstacle_distance
+    type: px4_msgs::msg::ObstacleDistance
+
+  - topic: /fmu/in/sensor_optical_flow
+    type: px4_msgs::msg::SensorOpticalFlow
+
+  - topic: /fmu/in/telemetry_status
+    type: px4_msgs::msg::TelemetryStatus
+
+  - topic: /fmu/in/trajectory_setpoint
+    type: px4_msgs::msg::TrajectorySetpoint
+
+  - topic: /fmu/in/vehicle_attitude_setpoint
+    type: px4_msgs::msg::VehicleAttitudeSetpoint
+
+  - topic: /fmu/in/vehicle_mocap_odometry
+    type: px4_msgs::msg::VehicleOdometry
+
+  - topic: /fmu/in/vehicle_rates_setpoint
+    type: px4_msgs::msg::VehicleRatesSetpoint
+
+  - topic: /fmu/in/vehicle_visual_odometry
+    type: px4_msgs::msg::VehicleOdometry
+
+  - topic: /fmu/in/vehicle_command
+    type: px4_msgs::msg::VehicleCommand
+
+  - topic: /fmu/in/vehicle_trajectory_bezier
+    type: px4_msgs::msg::VehicleTrajectoryBezier
+
+  - topic: /fmu/in/vehicle_trajectory_waypoint
+    type: px4_msgs::msg::VehicleTrajectoryWaypoint

src/modules/microdds_client/generate_dds_topics.py

@@ -0,0 +1,143 @@
+#!/usr/bin/env python3
+################################################################################
+#
+# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+# Copyright (c) 2018-2021 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 of the copyright holder 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 HOLDER 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.
+#
+################################################################################
+
+# This script can generate the client code based on a set of topics
+# to sent and set to receive.
+
+import sys
+import os
+import argparse
+import re
+import em
+import yaml
+
+parser = argparse.ArgumentParser()
+parser.add_argument("-m", "--topic-msg-dir", dest='msgdir', type=str,
+                    help="Topics message, by default using relative path 'msg/'", default="msg")
+
+parser.add_argument("-y", "--dds-topics-file", dest='yaml_file', type=str,
+                    help="Setup topics file path, by default using 'dds_topics.yaml'")
+
+parser.add_argument("-t", "--template_file", dest='template_file', type=str,
+                    help="DDS topics template file")
+
+parser.add_argument("-u", "--client-outdir", dest='clientdir', type=str,
+                    help="Client output dir, by default using relative path 'src/modules/microdds_client'", default=None)
+
+if len(sys.argv) <= 1:
+    parser.print_usage()
+    exit(-1)
+
+# Parse arguments
+args = parser.parse_args()
+
+# Client files output path
+client_out_dir = os.path.abspath(args.clientdir)
+template_file = os.path.join(args.template_file)
+
+# Make sure output directory exists:
+if not os.path.isdir(client_out_dir):
+    os.makedirs(client_out_dir)
+
+output_file = os.path.join(client_out_dir, os.path.basename(template_file).replace(".em", ""))
+folder_name = os.path.dirname(output_file)
+
+if not os.path.exists(folder_name):
+    os.makedirs(folder_name)
+
+
+
+# open yaml file to load dictionary of publications and subscriptions
+with open(args.yaml_file, 'r') as file:
+    msg_map = yaml.safe_load(file)
+
+merged_em_globals = {}
+all_type_includes = []
+
+for p in msg_map['publications']:
+    # eg TrajectoryWaypoint from px4_msgs::msg::TrajectoryWaypoint
+    simple_base_type = p['type'].split('::')[-1]
+
+    # eg TrajectoryWaypoint -> trajectory_waypoint
+    base_type_name_snake_case = re.sub(r'(?<!^)(?=[A-Z])', '_', simple_base_type).lower()
+    all_type_includes.append(base_type_name_snake_case)
+
+    # simple_base_type: eg vehicle_status
+    p['simple_base_type'] = base_type_name_snake_case
+
+    # dds_type: eg px4_msgs::msg::dds_::VehicleStatus_
+    p['dds_type'] = p['type'].replace("::msg::", "::msg::dds_::") + "_"
+
+    # topic_simple: eg vehicle_status
+    p['topic_simple'] = p['topic'].split('/')[-1]
+
+
+merged_em_globals['publications'] = msg_map['publications']
+
+for s in msg_map['subscriptions']:
+    # eg TrajectoryWaypoint from px4_msgs::msg::TrajectoryWaypoint
+    simple_base_type = s['type'].split('::')[-1]
+
+    # eg TrajectoryWaypoint -> trajectory_waypoint
+    base_type_name_snake_case = re.sub(r'(?<!^)(?=[A-Z])', '_', simple_base_type).lower()
+    all_type_includes.append(base_type_name_snake_case)
+
+    # simple_base_type: eg vehicle_status
+    s['simple_base_type'] = base_type_name_snake_case
+
+    # dds_type: eg px4_msgs::msg::dds_::VehicleStatus_
+    s['dds_type'] = s['type'].replace("::msg::", "::msg::dds_::") + "_"
+
+    # topic_simple: eg vehicle_status
+    s['topic_simple'] = s['topic'].split('/')[-1]
+
+
+merged_em_globals['subscriptions'] = msg_map['subscriptions']
+
+merged_em_globals['type_includes'] = sorted(set(all_type_includes))
+
+
+# run interpreter
+ofile = open(output_file, 'w')
+interpreter = em.Interpreter(output=ofile, globals=merged_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()

src/modules/microdds_client/microdds_client.cpp

@@ -33,7 +33,6 @@
 
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/cli.h>
-#include <uORB/topics/vehicle_imu.h>
 
 #include "microdds_client.h"
 
@@ -46,27 +45,68 @@
 #include <stdlib.h>
 #include <unistd.h>
 
+#if defined(CONFIG_NET) || defined(__PX4_POSIX)
+# define MICRODDS_CLIENT_UDP 1
+#endif
+
 #define STREAM_HISTORY  4
 #define BUFFER_SIZE (UXR_CONFIG_SERIAL_TRANSPORT_MTU * STREAM_HISTORY) // MTU==512 by default
 
 using namespace time_literals;
 
+void on_time(uxrSession *session, int64_t current_time, int64_t received_timestamp, int64_t transmit_timestamp,
+	     int64_t originate_timestamp, void *args)
+{
+	// latest round trip time (RTT)
+	int64_t rtt = current_time - originate_timestamp;
+
+	// HRT to AGENT
+	int64_t offset_1 = (received_timestamp - originate_timestamp) - (rtt / 2);
+	int64_t offset_2 = (transmit_timestamp - current_time) - (rtt / 2);
+
+	session->time_offset = (offset_1 + offset_2) / 2;
+
+	if (args) {
+		Timesync *timesync = static_cast<Timesync *>(args);
+		timesync->update(current_time / 1000, transmit_timestamp, originate_timestamp);
+
+		//fprintf(stderr, "time_offset: %ld, timesync: %ld, diff: %ld\n", session->time_offset/1000, timesync->offset(), session->time_offset/1000 + timesync->offset());
+
+		session->time_offset = -timesync->offset() * 1000; // us -> ns
+	}
+}
+
 MicroddsClient::MicroddsClient(Transport transport, const char *device, int baudrate, const char *host,
-			       const char *port, bool localhost_only)
-	: _localhost_only(localhost_only)
+			       const char *port, bool localhost_only, const char *client_namespace) :
+	ModuleParams(nullptr),
+	_localhost_only(localhost_only),
+	_client_namespace(client_namespace)
 {
 	if (transport == Transport::Serial) {
 
-		int fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK);
+		int fd = -1;
 
-		if (fd < 0) {
-			PX4_ERR("open %s failed (%i)", device, errno);
+		for (int attempt = 0; attempt < 3; attempt++) {
+			fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK);
+
+			if (fd < 0) {
+				PX4_ERR("open %s failed (%i)", device, errno);
+				// sleep before trying again
+				usleep(1'000'000);
+
+			} else {
+				break;
+			}
 		}
 
 		_transport_serial = new uxrSerialTransport();
 
 		if (fd >= 0 && setBaudrate(fd, baudrate) == 0 && _transport_serial) {
-			if (uxr_init_serial_transport(_transport_serial, fd, 0, 1)) {
+			// TODO:
+			uint8_t remote_addr = 0; // Identifier of the Agent in the connection
+			uint8_t local_addr = 1; // Identifier of the Client in the serial connection
+
+			if (uxr_init_serial_transport(_transport_serial, fd, remote_addr, local_addr)) {
 				_comm = &_transport_serial->comm;
 				_fd = fd;
 
@@ -77,7 +117,7 @@ MicroddsClient::MicroddsClient(Transport transport, const char *device, int baud
 
 	} else if (transport == Transport::Udp) {
 
-#if defined(CONFIG_NET) || defined(__PX4_POSIX)
+#if defined(MICRODDS_CLIENT_UDP)
 		_transport_udp = new uxrUDPTransport();
 
 		if (_transport_udp) {
@@ -127,8 +167,6 @@ void MicroddsClient::run()
 		return;
 	}
 
-	int polling_topic_sub = orb_subscribe(ORB_ID(vehicle_imu));
-
 	while (!should_exit()) {
 		bool got_response = false;
 
@@ -142,29 +180,48 @@ void MicroddsClient::run()
 		}
 
 		// Session
+		// The key identifier of the Client. All Clients connected to an Agent must have a different key.
+		const uint32_t key = 0xAAAABBBB;
 		uxrSession session;
-		uxr_init_session(&session, _comm, 0xAAAABBBB);
+		uxr_init_session(&session, _comm, key);
 
+		// void uxr_create_session_retries(uxrSession* session, size_t retries);
 		if (!uxr_create_session(&session)) {
 			PX4_ERR("uxr_create_session failed");
 			return;
 		}
 
+		// TODO: uxr_set_status_callback
+
 		// Streams
 		// Reliable for setup, afterwards best-effort to send the data (important: need to create all 4 streams)
 		uint8_t output_reliable_stream_buffer[BUFFER_SIZE] {};
-		uxrStreamId reliable_out = uxr_create_output_reliable_stream(&session, output_reliable_stream_buffer, BUFFER_SIZE,
-					   STREAM_HISTORY);
-		uint8_t output_data_stream_buffer[1024] {};
-		uxrStreamId data_out = uxr_create_output_best_effort_stream(&session, output_data_stream_buffer,
-				       sizeof(output_data_stream_buffer));
+		uxrStreamId reliable_out = uxr_create_output_reliable_stream(&session, output_reliable_stream_buffer,
+					   sizeof(output_reliable_stream_buffer), STREAM_HISTORY);
+
+		uint8_t output_data_stream_buffer[2048] {};
+		uxrStreamId best_effort_out = uxr_create_output_best_effort_stream(&session, output_data_stream_buffer,
+					      sizeof(output_data_stream_buffer));
 
 		uint8_t input_reliable_stream_buffer[BUFFER_SIZE] {};
-		uxr_create_input_reliable_stream(&session, input_reliable_stream_buffer, BUFFER_SIZE, STREAM_HISTORY);
-		uxrStreamId input_stream = uxr_create_input_best_effort_stream(&session);
+		uxrStreamId reliable_in = uxr_create_input_reliable_stream(&session, input_reliable_stream_buffer,
+					  sizeof(input_reliable_stream_buffer),
+					  STREAM_HISTORY);
+		(void)reliable_in;
+
+		uxrStreamId best_effort_in = uxr_create_input_best_effort_stream(&session);
+		(void)best_effort_in;
 
 		// Create entities
 		uxrObjectId participant_id = uxr_object_id(0x01, UXR_PARTICIPANT_ID);
+
+		uint16_t domain_id = _param_xrce_dds_dom_id.get();
+
+		// const char *participant_name = "px4_micro_xrce_dds";
+		// uint16_t participant_req = uxr_buffer_create_participant_bin(&session, reliable_out, participant_id, domain_id,
+		// 			   participant_name, UXR_REPLACE);
+
+		// TODO: configurable participant name with client namespace?
 		const char *participant_xml = _localhost_only ?
 					      "<dds>"
 					      "<profiles>"
@@ -178,7 +235,7 @@ void MicroddsClient::run()
 					      "</profiles>"
 					      "<participant>"
 					      "<rtps>"
-					      "<name>default_xrce_participant</name>"
+					      "<name>px4_micro_xrce_dds</name>"
 					      "<useBuiltinTransports>false</useBuiltinTransports>"
 					      "<userTransports><transport_id>udp_localhost</transport_id></userTransports>"
 					      "</rtps>"
@@ -188,11 +245,11 @@ void MicroddsClient::run()
 					      "<dds>"
 					      "<participant>"
 					      "<rtps>"
-					      "<name>default_xrce_participant</name>"
+					      "<name>px4_micro_xrce_dds</name>"
 					      "</rtps>"
 					      "</participant>"
 					      "</dds>" ;
-		uint16_t participant_req = uxr_buffer_create_participant_xml(&session, reliable_out, participant_id, 0,
+		uint16_t participant_req = uxr_buffer_create_participant_xml(&session, reliable_out, participant_id, domain_id,
 					   participant_xml, UXR_REPLACE);
 
 		uint8_t request_status;
@@ -202,69 +259,60 @@ void MicroddsClient::run()
 			return;
 		}
 
-		if (!_subs->init(&session, reliable_out, participant_id)) {
-			PX4_ERR("subs init failed");
-			return;
-		}
-
-		if (!_pubs->init(&session, reliable_out, input_stream, participant_id)) {
+		if (!_pubs->init(&session, reliable_out, reliable_in, best_effort_in, participant_id, _client_namespace)) {
 			PX4_ERR("pubs init failed");
 			return;
 		}
 
 		_connected = true;
 
+		// Set time-callback.
+		uxr_set_time_callback(&session, on_time, &_timesync);
+
+		// Synchronize with the Agent
+		bool synchronized = false;
+
+		while (!synchronized) {
+			synchronized = uxr_sync_session(&session, 1000);
+
+			if (synchronized) {
+				PX4_INFO("synchronized with time offset %-5" PRId64 "us", session.time_offset / 1000);
+				//sleep(1);
+
+			} else {
+				usleep(10000);
+			}
+		}
+
+		hrt_abstime last_sync_session = 0;
 		hrt_abstime last_status_update = hrt_absolute_time();
 		hrt_abstime last_ping = hrt_absolute_time();
 		int num_pings_missed = 0;
 		bool had_ping_reply = false;
 		uint32_t last_num_payload_sent{};
 		uint32_t last_num_payload_received{};
-		bool error_printed = false;
-		hrt_abstime last_read = hrt_absolute_time();
 
 		while (!should_exit() && _connected) {
-			px4_pollfd_struct_t fds[1];
-			fds[0].fd = polling_topic_sub;
-			fds[0].events = POLLIN;
-			// we could poll on the uart/udp fd as well (on nuttx)
-			int pret = px4_poll(fds, 1, 20);
 
-			if (pret < 0) {
-				if (!error_printed) {
-					PX4_ERR("poll failed (%i)", pret);
-					error_printed = true;
-				}
+			_subs->update(&session, reliable_out, best_effort_out, participant_id, _client_namespace);
 
-			} else if (pret != 0) {
-				if (fds[0].revents & POLLIN) {
-					vehicle_imu_s data;
-					orb_copy(ORB_ID(vehicle_imu), polling_topic_sub, &data);
+			uxr_run_session_timeout(&session, 0);
+
+			// time sync session
+			if (hrt_elapsed_time(&last_sync_session) > 1_s) {
+				if (uxr_sync_session(&session, 100)) {
+					//PX4_INFO("synchronized with time offset %-5" PRId64 "ns", session.time_offset);
+					last_sync_session = hrt_absolute_time();
 				}
 			}
 
-			_subs->update(data_out);
-
-			hrt_abstime read_start = hrt_absolute_time();
-
-			if (read_start - last_read > 5_ms) {
-				last_read = read_start;
-
-				// Read as long as there's data or until a timeout
-				pollfd fd_read;
-				fd_read.fd = _fd;
-				fd_read.events = POLLIN;
-
-				do {
-					uxr_run_session_timeout(&session, 0);
-
-					if (session.on_pong_flag == 1 /* PONG_IN_SESSION_STATUS */) { // Check for a ping response
-						had_ping_reply = true;
-					}
-				} while (poll(&fd_read, 1, 0) > 0 && hrt_absolute_time() - read_start < 2_ms);
+			// Check for a ping response
+			/* PONG_IN_SESSION_STATUS */
+			if (session.on_pong_flag == 1) {
+				had_ping_reply = true;
 			}
 
-			hrt_abstime now = hrt_absolute_time();
+			const hrt_abstime now = hrt_absolute_time();
 
 			if (now - last_status_update > 1_s) {
 				float dt = (now - last_status_update) / 1e6f;
@@ -287,6 +335,7 @@ void MicroddsClient::run()
 				}
 
 				uxr_ping_agent_session(&session, 0, 1);
+
 				had_ping_reply = false;
 			}
 
@@ -294,14 +343,14 @@ void MicroddsClient::run()
 				PX4_INFO("No ping response, disconnecting");
 				_connected = false;
 			}
+
+			px4_usleep(1000);
 		}
 
 		uxr_delete_session_retries(&session, _connected ? 1 : 0);
 		_last_payload_tx_rate = 0;
 		_last_payload_tx_rate = 0;
 	}
-
-	orb_unsubscribe(polling_topic_sub);
 }
 
 int MicroddsClient::setBaudrate(int fd, unsigned baud)
@@ -333,6 +382,48 @@ int MicroddsClient::setBaudrate(int fd, unsigned baud)
 
 	case 921600: speed = B921600; break;
 
+#ifndef B1000000
+#define B1000000 1000000
+#endif
+
+	case 1000000: speed = B1000000; break;
+
+#ifndef B1500000
+#define B1500000 1500000
+#endif
+
+	case 1500000: speed = B1500000; break;
+
+#ifndef B2000000
+#define B2000000 2000000
+#endif
+
+	case 2000000: speed = B2000000; break;
+
+#ifndef B2500000
+#define B2500000 2500000
+#endif
+
+	case 2500000: speed = B2500000; break;
+
+#ifndef B3000000
+#define B3000000 3000000
+#endif
+
+	case 3000000: speed = B3000000; break;
+
+#ifndef B3500000
+#define B3500000 3500000
+#endif
+
+	case 3500000: speed = B3500000; break;
+
+#ifndef B4000000
+#define B4000000 4000000
+#endif
+
+	case 4000000: speed = B4000000; break;
+
 	default:
 		PX4_ERR("ERR: unknown baudrate: %d", baud);
 		return -EINVAL;
@@ -399,12 +490,6 @@ int MicroddsClient::setBaudrate(int fd, unsigned baud)
 	return 0;
 }
 
-
-int microdds_client_main(int argc, char *argv[])
-{
-	return MicroddsClient::main(argc, argv);
-}
-
 int MicroddsClient::custom_command(int argc, char *argv[])
 {
 	return print_usage("unknown command");
@@ -414,8 +499,8 @@ int MicroddsClient::task_spawn(int argc, char *argv[])
 {
 	_task_id = px4_task_spawn_cmd("microdds_client",
 				      SCHED_DEFAULT,
-				      SCHED_PRIORITY_DEFAULT - 4,
-				      PX4_STACK_ADJUSTED(8000),
+				      SCHED_PRIORITY_DEFAULT,
+				      PX4_STACK_ADJUSTED(10000),
 				      (px4_main_t)&run_trampoline,
 				      (char *const *)argv);
 
@@ -442,14 +527,22 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[])
 	int ch;
 	const char *myoptarg = nullptr;
 
+#if defined(MICRODDS_CLIENT_UDP)
 	Transport transport = Transport::Udp;
-	const char *device = nullptr;
-	const char *ip = "127.0.0.1";
-	int baudrate = 921600;
-	const char *port = "15555";
-	bool localhost_only = false;
 
-	while ((ch = px4_getopt(argc, argv, "t:d:b:h:p:l", &myoptind, &myoptarg)) != EOF) {
+#else
+	Transport transport = Transport::Serial;
+#endif
+	const char *device = nullptr;
+	int baudrate = 921600;
+
+	const char *port = "8888";
+	bool localhost_only = false;
+	const char *ip = "127.0.0.1";
+
+	const char *client_namespace = nullptr;//"px4";
+
+	while ((ch = px4_getopt(argc, argv, "t:d:b:h:p:l:n:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 't':
 			if (!strcmp(myoptarg, "serial")) {
@@ -477,6 +570,8 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[])
 
 			break;
 
+#if defined(MICRODDS_CLIENT_UDP)
+
 		case 'h':
 			ip = myoptarg;
 			break;
@@ -488,6 +583,11 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[])
 		case 'l':
 			localhost_only = true;
 			break;
+#endif // MICRODDS_CLIENT_UDP
+
+		case 'n':
+			client_namespace = myoptarg;
+			break;
 
 		case '?':
 			error_flag = true;
@@ -511,7 +611,7 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[])
 		}
 	}
 
-	return new MicroddsClient(transport, device, baudrate, ip, port, localhost_only);
+	return new MicroddsClient(transport, device, baudrate, ip, port, localhost_only, client_namespace);
 }
 
 int MicroddsClient::print_usage(const char *reason)
@@ -536,9 +636,15 @@ $ microdds_client start -t udp -h 127.0.0.1 -p 15555
 	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "serial device", true);
 	PRINT_MODULE_USAGE_PARAM_INT('b', 0, 0, 3000000, "Baudrate (can also be p:<param_name>)", true);
 	PRINT_MODULE_USAGE_PARAM_STRING('h', "127.0.0.1", "<IP>", "Host IP", true);
-	PRINT_MODULE_USAGE_PARAM_INT('p', 15555, 0, 3000000, "Remote Port", true);
+	PRINT_MODULE_USAGE_PARAM_INT('p', 8888, 0, 65535, "Remote Port", true);
 	PRINT_MODULE_USAGE_PARAM_FLAG('l', "Restrict to localhost (use in combination with ROS_LOCALHOST_ONLY=1)", true);
+	PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Client DDS namespace", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
 	return 0;
 }
+
+extern "C" __EXPORT int microdds_client_main(int argc, char *argv[])
+{
+	return MicroddsClient::main(argc, argv);
+}

src/modules/microdds_client/microdds_client.h

@@ -34,13 +34,13 @@
 #pragma once
 
 #include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
 
-#include <src/modules/micrortps_bridge/micrortps_client/dds_topics.h>
+#include <src/modules/microdds_client/dds_topics.h>
 
-extern "C" __EXPORT int microdds_client_main(int argc, char *argv[]);
+#include <lib/timesync/Timesync.hpp>
 
-
-class MicroddsClient : public ModuleBase<MicroddsClient>
+class MicroddsClient : public ModuleBase<MicroddsClient>, public ModuleParams
 {
 public:
 	enum class Transport {
@@ -49,7 +49,7 @@ public:
 	};
 
 	MicroddsClient(Transport transport, const char *device, int baudrate, const char *host, const char *port,
-		       bool localhost_only);
+		       bool localhost_only, const char *client_namespace);
 
 	~MicroddsClient();
 
@@ -75,6 +75,7 @@ private:
 	int setBaudrate(int fd, unsigned baud);
 
 	const bool _localhost_only;
+	const char *_client_namespace;
 
 	SendTopicsSubs *_subs{nullptr};
 	RcvTopicsPubs *_pubs{nullptr};
@@ -87,5 +88,11 @@ private:
 	int _last_payload_tx_rate{}; ///< in B/s
 	int _last_payload_rx_rate{}; ///< in B/s
 	bool _connected{false};
+
+	Timesync _timesync{};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::XRCE_DDS_DOM_ID>) _param_xrce_dds_dom_id
+	)
 };
 

src/modules/microdds_client/module.yaml

@@ -0,0 +1,56 @@
+
+
+# parameters to auto start
+#  mode (normal, minimal)
+#  UDP port
+#  max rate
+#  DDS DOMAIN ID
+#
+
+
+# multiple instances?
+
+
+module_name: Micro XRCE-DDS
+serial_config:
+    - command: |
+        if [ $SERIAL_DEV != ethernet ]; then
+            set XRCE_DDS_ARGS "-t serial -d ${SERIAL_DEV} -b p:${BAUD_PARAM}"
+        else
+            set XRCE_DDS_ARGS "-t udp"
+        fi
+        microdds_client start ${XRCE_DDS_ARGS}
+
+      port_config_param:
+        name: XRCE_DDS_${i}_CFG
+        group: Micro XRCE-DDS
+        # MAVLink instances:
+        # 0: Telem1 Port (Telemetry Link)
+        # 1: Telem2 Port (Companion Link). Disabled by default to reduce RAM usage
+        # 2: Board-specific / no fixed function or port
+        #default: [TEL1, "", ""]
+      supports_networking: true
+
+parameters:
+    - group: Micro XRCE-DDS
+      definitions:
+
+        XRCE_DDS_DOM_ID:
+            description:
+                short: XRCE DDS domain ID
+                long: XRCE DDS domain ID
+            category: System
+            type: int32
+            reboot_required: true
+            default: 0
+
+        XRCE_DDS_UDP_PRT:
+            description:
+                short: Micro DDS UDP Port
+                long: |
+                    If ethernet enabled and selected as configuration for micro DDS,
+                    selected udp port will be set and used.
+            type: int32
+            reboot_required: true
+            default: 8888
+            requires_ethernet: true

src/modules/microdds_client/utilities.hpp

@@ -0,0 +1,138 @@
+
+#pragma once
+
+#include <uxr/client/client.h>
+#include <ucdr/microcdr.h>
+
+#include <uORB/topics/uORBTopics.hpp>
+
+#define TOPIC_NAME_SIZE 128
+
+uxrObjectId topic_id_from_orb(ORB_ID orb_id, uint8_t instance = 0)
+{
+	if (orb_id != ORB_ID::INVALID) {
+		uint16_t id = static_cast<uint8_t>(orb_id) + (instance * UINT8_MAX);
+		uxrObjectId topic_id = uxr_object_id(id, UXR_TOPIC_ID);
+		return topic_id;
+	}
+
+	return uxrObjectId{};
+}
+
+static bool generate_topic_name(char *topic, const char *client_namespace, const char *direction, const char *name)
+{
+	if (client_namespace != nullptr) {
+		int ret = snprintf(topic, TOPIC_NAME_SIZE, "rt/%s/fmu/%s/%s", client_namespace, direction, name);
+		return (ret > 0 && ret < TOPIC_NAME_SIZE);
+	}
+
+	int ret = snprintf(topic, TOPIC_NAME_SIZE, "rt/fmu/%s/%s", direction, name);
+	return (ret > 0 && ret < TOPIC_NAME_SIZE);
+}
+
+static bool create_data_writer(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrObjectId participant_id,
+			       ORB_ID orb_id, const char *client_namespace, const char *topic_name_simple, const char *type_name,
+			       uxrObjectId &datawriter_id)
+{
+	// topic
+	char topic_name[TOPIC_NAME_SIZE];
+
+	if (!generate_topic_name(topic_name, client_namespace, "out", topic_name_simple)) {
+		PX4_ERR("topic path too long");
+		return false;
+	}
+
+	uxrObjectId topic_id = topic_id_from_orb(orb_id);
+	uint16_t topic_req = uxr_buffer_create_topic_bin(session, reliable_out_stream_id, topic_id, participant_id, topic_name,
+			     type_name, UXR_REPLACE);
+
+
+	// publisher
+	uxrObjectId publisher_id = uxr_object_id(topic_id.id, UXR_PUBLISHER_ID);
+	uint16_t publisher_req = uxr_buffer_create_publisher_bin(session, reliable_out_stream_id, publisher_id, participant_id,
+				 UXR_REPLACE);
+
+
+	// data writer
+	datawriter_id = uxr_object_id(topic_id.id, UXR_DATAWRITER_ID);
+
+	uxrQoS_t qos = {
+		.durability = UXR_DURABILITY_TRANSIENT_LOCAL,
+		.reliability = UXR_RELIABILITY_BEST_EFFORT,
+		.history = UXR_HISTORY_KEEP_LAST,
+		.depth = 0,
+	};
+
+	uint16_t datawriter_req = uxr_buffer_create_datawriter_bin(session, reliable_out_stream_id, datawriter_id, publisher_id,
+				  topic_id, qos, UXR_REPLACE);
+
+	// Send create entities message and wait its status
+	uint16_t requests[3] {topic_req, publisher_req, datawriter_req};
+	uint8_t status[3];
+
+	if (!uxr_run_session_until_all_status(session, 1000, requests, status, 3)) {
+		PX4_ERR("create entities failed: %s, topic: %i publisher: %i datawriter: %i",
+			topic_name, status[0], status[1], status[2]);
+		return false;
+
+	} else {
+		PX4_INFO("successfully created %s data writer, topic id: %d", topic_name, topic_id.id);
+	}
+
+	return true;
+}
+
+static bool create_data_reader(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId input_stream_id,
+			       uxrObjectId participant_id, uint16_t index, const char *client_namespace, const char *topic_name_simple,
+			       const char *type_name)
+{
+	// topic
+	char topic_name[TOPIC_NAME_SIZE];
+
+	if (!generate_topic_name(topic_name, client_namespace, "in", topic_name_simple)) {
+		PX4_ERR("topic path too long");
+		return false;
+	}
+
+	uint16_t id = index + 1000;
+
+
+	uxrObjectId topic_id = uxr_object_id(id, UXR_TOPIC_ID);
+	uint16_t topic_req = uxr_buffer_create_topic_bin(session, reliable_out_stream_id, topic_id, participant_id, topic_name,
+			     type_name, UXR_REPLACE);
+
+
+	// subscriber
+	uxrObjectId subscriber_id = uxr_object_id(id, UXR_SUBSCRIBER_ID);
+	uint16_t subscriber_req = uxr_buffer_create_subscriber_bin(session, reliable_out_stream_id, subscriber_id,
+				  participant_id, UXR_REPLACE);
+
+
+	// data reader
+	uxrObjectId datareader_id = uxr_object_id(id, UXR_DATAREADER_ID);
+
+	uxrQoS_t qos = {
+		.durability = UXR_DURABILITY_TRANSIENT_LOCAL,
+		.reliability = UXR_RELIABILITY_BEST_EFFORT,
+		.history = UXR_HISTORY_KEEP_LAST,
+		.depth = 0,
+	};
+
+	uint16_t datareader_req = uxr_buffer_create_datareader_bin(session, reliable_out_stream_id, datareader_id,
+				  subscriber_id, topic_id, qos, UXR_REPLACE);
+
+	uint16_t requests[3] {topic_req, subscriber_req, datareader_req};
+	uint8_t status[3];
+
+	if (!uxr_run_session_until_all_status(session, 1000, requests, status, 3)) {
+		PX4_ERR("create entities failed: %s %i %i %i", topic_name,
+			status[0], status[1], status[2]);
+		return false;
+	}
+
+	uxrDeliveryControl delivery_control{};
+	delivery_control.max_samples = UXR_MAX_SAMPLES_UNLIMITED;
+	uxr_buffer_request_data(session, reliable_out_stream_id, datareader_id, input_stream_id, &delivery_control);
+
+	return true;
+}