microdds_client: rename to uxrce_dds_client

| | old version | new version (second proposal) |
|-|-|-|
| module name | `microdds_client` | **`uxrce_dds_client`** |
| strings / comments about the module | non consistent | **UXRCE-DDS Client** |
| menuconfig option | `MODULES_MICRODDS_CLIENT` | **`MODULES_UXRCE_DDS_CLIENT`** |
| module parameters group name | `Micro XRCE-DDS` | **UXRCE-DDS Client** |
| module parameters name prefix | `XRCE_DDS_` | `UXRCE_DDS_` |
| module class name | `MicroddsClient` | **`UxrceddsClient`** |
|`init.d/rcS` whenever the module is mentioned | `microdds` | **`uxrce_dds`** |
| main doc page name | XRCE-DDS (PX4-FastDDS Bridge) | **uXRCE-DDS (PX4-micro XRCE-DDS Bridge)**|
| environment variable to have custom namespace in simulation | PX4_MICRODDS_NS | **PX4_UXRCE_DDS_NS** |

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>

src/modules/uxrce_dds_client/CMakeLists.txt

@@ -0,0 +1,155 @@
+############################################################################
+#
+# Copyright (c) 2022 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(${CMAKE_VERSION} VERSION_LESS "3.11")
+    message(WARNING "skipping uxrce_dds_client, Micro-XRCE-DDS-Client needs to be fixed to work with CMAKE_VERSION ${CMAKE_VERSION}")
+
+else()
+	px4_add_git_submodule(TARGET git_micro_xrce_dds_client PATH "${CMAKE_CURRENT_LIST_DIR}/Micro-XRCE-DDS-Client")
+
+	# 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()
+
+	# 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(lib_dir ${CMAKE_INSTALL_LIBDIR})
+	if("${lib_dir}" STREQUAL "")
+		set(lib_dir "lib")
+	endif()
+
+	set(microxrceddsclient_src_dir   ${CMAKE_CURRENT_SOURCE_DIR}/Micro-XRCE-DDS-Client)
+	set(microxrceddsclient_build_dir ${CMAKE_CURRENT_BINARY_DIR})
+
+	include(ExternalProject)
+
+	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_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__uxrce_dds_client
+		MAIN uxrce_dds_client
+		STACK_MAIN 9000
+		INCLUDES
+			${CMAKE_CURRENT_SOURCE_DIR}
+		COMPILE_FLAGS
+			#-O0
+			${MAX_CUSTOM_OPT_LEVEL}
+		SRCS
+			${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h
+			uxrce_dds_client.cpp
+			uxrce_dds_client.h
+		DEPENDS
+			microxrceddsclient
+			libmicroxrceddsclient
+			libmicrocdr
+			timesync
+			topic_bridge_files
+			uorb_ucdr_headers
+		MODULE_CONFIG
+			module.yaml
+		)
+endif()

src/modules/uxrce_dds_client/Kconfig

@@ -0,0 +1,5 @@
+menuconfig MODULES_UXRCE_DDS_CLIENT
+	bool "uxrce_dds_client"
+	default n
+	---help---
+		Enable support for the UXRCE-DDS Client

src/modules/uxrce_dds_client/dds_topics.h.em

@@ -0,0 +1,138 @@
+@###############################################
+@#
+@# 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 reset();
+};
+
+void SendTopicsSubs::reset() {
+	num_payload_sent = 0;
+@[    for idx, pub in enumerate(publications)]@
+	@(pub['topic_simple'])_data_writer = uxr_object_id(0, UXR_INVALID_ID);
+@[    end for]@
+};
+
+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)]@
+	{
+			uint16_t queue_depth = uORB::DefaultQueueSize<@(sub['simple_base_type'])_s>::value * 2; // use a bit larger queue size than internal
+			create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx), client_namespace, "@(sub['topic_simple'])", "@(sub['dds_type'])", queue_depth);
+	}
+@[    end for]@
+
+	uxr_set_topic_callback(session, on_topic_update, this);
+
+	return true;
+}

src/modules/uxrce_dds_client/dds_topics.yaml

@@ -0,0 +1,89 @@
+#####
+#
+# This file maps all the topics that are to be used on the uXRCE-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/position_setpoint_triplet
+    type: px4_msgs::msg::PositionSetpointTriplet
+
+  - 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/uxrce_dds_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/uxrce_dds_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/uxrce_dds_client/module.yaml

@@ -0,0 +1,66 @@
+module_name: UXRCE-DDS Client
+serial_config:
+    - command: |
+        if [ $SERIAL_DEV != ethernet ]; then
+            set UXRCE_DDS_ARGS "-t serial -d ${SERIAL_DEV} -b p:${BAUD_PARAM}"
+        else
+            set UXRCE_DDS_ARGS "-t udp"
+        fi
+        uxrce_dds_client start ${UXRCE_DDS_ARGS}
+
+      port_config_param:
+        name: UXRCE_DDS_CFG
+        group: UXRCE-DDS Client
+      supports_networking: true
+
+parameters:
+    - group: UXRCE-DDS Client
+      definitions:
+
+        UXRCE_DDS_DOM_ID:
+            description:
+                short: uXRCE-DDS domain ID
+                long: uXRCE-DDS domain ID
+            category: System
+            type: int32
+            reboot_required: true
+            default: 0
+
+        UXRCE_DDS_KEY:
+            description:
+                short: uXRCE-DDS Session key
+                long: |
+                    uXRCE-DDS key, must be different from zero.
+                    In a single agent - multi client configuration, each client
+                    must have a unique session key.
+            category: System
+            type: int32
+            reboot_required: true
+            default: 1
+
+        UXRCE_DDS_PRT:
+            description:
+                short: uXRCE-DDS UDP Port
+                long: |
+                    If ethernet enabled and selected as configuration for uXRCE-DDS,
+                    selected udp port will be set and used.
+            type: int32
+            min: 0
+            max: 65535
+            reboot_required: true
+            default: 8888
+            requires_ethernet: true
+
+        UXRCE_DDS_AG_IP:
+            description:
+                short: uXRCE-DDS Agent IP address
+                long: |
+                    If ethernet enabled and selected as configuration for uXRCE-DDS,
+                    selected Agent IP address will be set and used.
+                    Decimal dot notation is not supported. IP address must be provided
+                    in int32 format. For example, 192.168.1.2 is mapped to -1062731518;
+                    127.0.0.1 is mapped to 2130706433.
+            type: int32
+            reboot_required: true
+            default: 2130706433
+            requires_ethernet: true

src/modules/uxrce_dds_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, uint16_t queue_depth)
+{
+	// 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_VOLATILE,
+		.reliability = UXR_RELIABILITY_BEST_EFFORT,
+		.history = UXR_HISTORY_KEEP_LAST,
+		.depth = queue_depth,
+	};
+
+	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;
+}

src/modules/uxrce_dds_client/uxrce_dds_client.cpp

@@ -0,0 +1,735 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 <px4_platform_common/getopt.h>
+#include <px4_platform_common/cli.h>
+#include <px4_platform_common/posix.h>
+
+#include "uxrce_dds_client.h"
+
+#include <uxr/client/client.h>
+#include <uxr/client/util/ping.h>
+#include <ucdr/microcdr.h>
+
+#include <termios.h>
+#include <fcntl.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#if defined(CONFIG_NET) || defined(__PX4_POSIX)
+# define UXRCE_DDS_CLIENT_UDP 1
+#endif
+
+#define STREAM_HISTORY  4
+#define BUFFER_SIZE (UXR_CONFIG_SERIAL_TRANSPORT_MTU * STREAM_HISTORY) // MTU==512 by default
+
+#define PARTICIPANT_XML_SIZE 512
+
+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
+	}
+}
+
+UxrceddsClient::UxrceddsClient(Transport transport, const char *device, int baudrate, const char *agent_ip,
+			       const char *port, bool localhost_only, bool custom_participant, const char *client_namespace) :
+	ModuleParams(nullptr),
+	_localhost_only(localhost_only), _custom_participant(custom_participant),
+	_client_namespace(client_namespace)
+{
+	if (transport == Transport::Serial) {
+
+		int fd = -1;
+
+		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) {
+			// 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;
+
+			} else {
+				PX4_ERR("uxr_init_serial_transport failed");
+			}
+		}
+
+	} else if (transport == Transport::Udp) {
+
+#if defined(UXRCE_DDS_CLIENT_UDP)
+		_transport_udp = new uxrUDPTransport();
+		strncpy(_port, port, PORT_MAX_LENGTH - 1);
+		strncpy(_agent_ip, agent_ip, AGENT_IP_MAX_LENGTH - 1);
+
+		if (_transport_udp) {
+			if (uxr_init_udp_transport(_transport_udp, UXR_IPv4, _agent_ip, _port)) {
+				_comm = &_transport_udp->comm;
+				_fd = _transport_udp->platform.poll_fd.fd;
+
+			} else {
+				PX4_ERR("uxr_init_udp_transport failed");
+			}
+		}
+
+#else
+		PX4_ERR("UDP not supported");
+#endif
+	}
+}
+
+UxrceddsClient::~UxrceddsClient()
+{
+	delete _subs;
+	delete _pubs;
+
+	if (_transport_serial) {
+		uxr_close_serial_transport(_transport_serial);
+		delete _transport_serial;
+	}
+
+	if (_transport_udp) {
+		uxr_close_udp_transport(_transport_udp);
+		delete _transport_udp;
+	}
+}
+
+void UxrceddsClient::run()
+{
+	if (!_comm) {
+		PX4_ERR("init failed");
+		return;
+	}
+
+	_subs = new SendTopicsSubs();
+	_pubs = new RcvTopicsPubs();
+
+	if (!_subs || !_pubs) {
+		PX4_ERR("alloc failed");
+		return;
+	}
+
+	while (!should_exit()) {
+		bool got_response = false;
+
+		while (!should_exit() && !got_response) {
+			// Sending ping without initing a XRCE session
+			got_response = uxr_ping_agent_attempts(_comm, 1000, 1);
+		}
+
+		if (!got_response) {
+			break;
+		}
+
+		// Session
+		// The key identifier of the Client. All Clients connected to an Agent must have a different key.
+		const uint32_t key = (uint32_t)_param_xrce_key.get();
+
+		if (key == 0) {
+			PX4_ERR("session key must be different from zero");
+			return;
+		}
+
+		uxrSession session;
+		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,
+					   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] {};
+		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);
+
+		char participant_xml[PARTICIPANT_XML_SIZE];
+		int ret = snprintf(participant_xml, PARTICIPANT_XML_SIZE, "%s<name>%s/px4_micro_xrce_dds</name>%s",
+				   _localhost_only ?
+				   "<dds>"
+				   "<profiles>"
+				   "<transport_descriptors>"
+				   "<transport_descriptor>"
+				   "<transport_id>udp_localhost</transport_id>"
+				   "<type>UDPv4</type>"
+				   "<interfaceWhiteList><address>127.0.0.1</address></interfaceWhiteList>"
+				   "</transport_descriptor>"
+				   "</transport_descriptors>"
+				   "</profiles>"
+				   "<participant>"
+				   "<rtps>"
+				   :
+				   "<dds>"
+				   "<participant>"
+				   "<rtps>",
+				   _client_namespace != nullptr ?
+				   _client_namespace
+				   :
+				   "",
+				   _localhost_only ?
+				   "<useBuiltinTransports>false</useBuiltinTransports>"
+				   "<userTransports><transport_id>udp_localhost</transport_id></userTransports>"
+				   "</rtps>"
+				   "</participant>"
+				   "</dds>"
+				   :
+				   "</rtps>"
+				   "</participant>"
+				   "</dds>"
+				  );
+
+		if (ret < 0 || ret >= PARTICIPANT_XML_SIZE) {
+			PX4_ERR("create entities failed: namespace too long");
+			return;
+		}
+
+
+		uint16_t participant_req{};
+
+		if (_custom_participant) {
+			participant_req = uxr_buffer_create_participant_ref(&session, reliable_out, participant_id, domain_id,
+					  "px4_participant", UXR_REPLACE);
+
+		} else {
+			participant_req = uxr_buffer_create_participant_xml(&session, reliable_out, participant_id, domain_id,
+					  participant_xml, UXR_REPLACE);
+		}
+
+		uint8_t request_status;
+
+		if (!uxr_run_session_until_all_status(&session, 1000, &participant_req, &request_status, 1)) {
+			PX4_ERR("create entities failed: participant: %i", request_status);
+			return;
+		}
+
+		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{};
+
+		while (!should_exit() && _connected) {
+
+			_subs->update(&session, reliable_out, best_effort_out, participant_id, _client_namespace);
+
+			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();
+				}
+			}
+
+			// Check for a ping response
+			/* PONG_IN_SESSION_STATUS */
+			if (session.on_pong_flag == 1) {
+				had_ping_reply = true;
+			}
+
+			const hrt_abstime now = hrt_absolute_time();
+
+			if (now - last_status_update > 1_s) {
+				float dt = (now - last_status_update) / 1e6f;
+				_last_payload_tx_rate = (_subs->num_payload_sent - last_num_payload_sent) / dt;
+				_last_payload_rx_rate = (_pubs->num_payload_received - last_num_payload_received) / dt;
+				last_num_payload_sent = _subs->num_payload_sent;
+				last_num_payload_received = _pubs->num_payload_received;
+				last_status_update = now;
+			}
+
+			// Handle ping
+			if (now - last_ping > 500_ms) {
+				last_ping = now;
+
+				if (had_ping_reply) {
+					num_pings_missed = 0;
+
+				} else {
+					++num_pings_missed;
+				}
+
+				uxr_ping_agent_session(&session, 0, 1);
+
+				had_ping_reply = false;
+			}
+
+			if (num_pings_missed > 2) {
+				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;
+		_subs->reset();
+	}
+}
+
+int UxrceddsClient::setBaudrate(int fd, unsigned baud)
+{
+	int speed;
+
+	switch (baud) {
+	case 9600:   speed = B9600;   break;
+
+	case 19200:  speed = B19200;  break;
+
+	case 38400:  speed = B38400;  break;
+
+	case 57600:  speed = B57600;  break;
+
+	case 115200: speed = B115200; break;
+
+	case 230400: speed = B230400; break;
+
+#ifndef B460800
+#define B460800 460800
+#endif
+
+	case 460800: speed = B460800; break;
+
+#ifndef B921600
+#define B921600 921600
+#endif
+
+	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;
+	}
+
+	struct termios uart_config;
+
+	int termios_state;
+
+	/* fill the struct for the new configuration */
+	tcgetattr(fd, &uart_config);
+
+	/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
+
+	//
+	// Input flags - Turn off input processing
+	//
+	// convert break to null byte, no CR to NL translation,
+	// no NL to CR translation, don't mark parity errors or breaks
+	// no input parity check, don't strip high bit off,
+	// no XON/XOFF software flow control
+	//
+	uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
+				 INLCR | PARMRK | INPCK | ISTRIP | IXON);
+	//
+	// Output flags - Turn off output processing
+	//
+	// no CR to NL translation, no NL to CR-NL translation,
+	// no NL to CR translation, no column 0 CR suppression,
+	// no Ctrl-D suppression, no fill characters, no case mapping,
+	// no local output processing
+	//
+	// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
+	//                     ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
+	uart_config.c_oflag = 0;
+
+	//
+	// No line processing
+	//
+	// echo off, echo newline off, canonical mode off,
+	// extended input processing off, signal chars off
+	//
+	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+
+	/* no parity, one stop bit, disable flow control */
+	uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
+
+	/* set baud rate */
+	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+		PX4_ERR("ERR: %d (cfsetispeed)", termios_state);
+		return -1;
+	}
+
+	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+		PX4_ERR("ERR: %d (cfsetospeed)", termios_state);
+		return -1;
+	}
+
+	if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
+		PX4_ERR("ERR: %d (tcsetattr)", termios_state);
+		return -1;
+	}
+
+	return 0;
+}
+
+int UxrceddsClient::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int UxrceddsClient::task_spawn(int argc, char *argv[])
+{
+	_task_id = px4_task_spawn_cmd("uxrce_dds_client",
+				      SCHED_DEFAULT,
+				      SCHED_PRIORITY_DEFAULT,
+				      PX4_STACK_ADJUSTED(10000),
+				      (px4_main_t)&run_trampoline,
+				      (char *const *)argv);
+
+	if (_task_id < 0) {
+		_task_id = -1;
+		return -errno;
+	}
+
+	return 0;
+}
+
+int UxrceddsClient::print_status()
+{
+	PX4_INFO("Running, %s", _connected ? "connected" : "disconnected");
+#if defined(UXRCE_DDS_CLIENT_UDP)
+
+	if (_transport_udp != nullptr) {
+		PX4_INFO("Using transport: udp");
+		PX4_INFO("Agent IP: %s", _agent_ip);
+		PX4_INFO("Agent port: %s", _port);
+
+	}
+
+#endif
+
+	if (_transport_serial != nullptr) {
+		PX4_INFO("Using transport: serial");
+	}
+
+	if (_connected) {
+		PX4_INFO("Payload tx: %i B/s", _last_payload_tx_rate);
+		PX4_INFO("Payload rx: %i B/s", _last_payload_rx_rate);
+	}
+
+	return 0;
+}
+
+UxrceddsClient *UxrceddsClient::instantiate(int argc, char *argv[])
+{
+	bool error_flag = false;
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+
+	char port[PORT_MAX_LENGTH] = {0};
+	char agent_ip[AGENT_IP_MAX_LENGTH] = {0};
+
+#if defined(UXRCE_DDS_CLIENT_UDP)
+	Transport transport = Transport::Udp;
+#else
+	Transport transport = Transport::Serial;
+#endif
+	const char *device = nullptr;
+	int baudrate = 921600;
+
+	bool localhost_only = false;
+	bool custom_participant = false;
+
+	const char *client_namespace = nullptr;//"px4";
+
+	while ((ch = px4_getopt(argc, argv, "t:d:b:h:p:lcn:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 't':
+			if (!strcmp(myoptarg, "serial")) {
+				transport = Transport::Serial;
+
+			} else if (!strcmp(myoptarg, "udp")) {
+				transport = Transport::Udp;
+
+			} else {
+				PX4_ERR("unknown transport: %s", myoptarg);
+				error_flag = true;
+			}
+
+			break;
+
+		case 'd':
+			device = myoptarg;
+			break;
+
+		case 'b':
+			if (px4_get_parameter_value(myoptarg, baudrate) != 0) {
+				PX4_ERR("baudrate parsing failed");
+				error_flag = true;
+			}
+
+			break;
+
+#if defined(UXRCE_DDS_CLIENT_UDP)
+
+		case 'h':
+			snprintf(agent_ip, AGENT_IP_MAX_LENGTH, "%s", myoptarg);
+			break;
+
+		case 'p':
+			snprintf(port, PORT_MAX_LENGTH, "%s", myoptarg);
+			break;
+
+		case 'l':
+			localhost_only = true;
+			break;
+
+		case 'c':
+			custom_participant = true;
+			break;
+#endif // UXRCE_DDS_CLIENT_UDP
+
+		case 'n':
+			client_namespace = myoptarg;
+			break;
+
+		case '?':
+			error_flag = true;
+			break;
+
+		default:
+			PX4_WARN("unrecognized flag");
+			error_flag = true;
+			break;
+		}
+	}
+
+#if defined(UXRCE_DDS_CLIENT_UDP)
+
+	if (port[0] == '\0') {
+		// no port specified, use UXRCE_DDS_PRT
+		int32_t port_i = 0;
+		param_get(param_find("UXRCE_DDS_PRT"), &port_i);
+
+		if (port_i < 0 || port_i > 65535) {
+			PX4_ERR("port must be between 0 and 65535");
+			return nullptr;
+		}
+
+		snprintf(port, PORT_MAX_LENGTH, "%u", (uint16_t)port_i);
+	}
+
+	if (agent_ip[0] == '\0') {
+		// no agent ip specified, use UXRCE_DDS_AG_IP
+		int32_t ip_i = 0;
+		param_get(param_find("UXRCE_DDS_AG_IP"), &ip_i);
+		snprintf(agent_ip, AGENT_IP_MAX_LENGTH, "%u.%u.%u.%u", static_cast<uint8_t>(((ip_i) >> 24) & 0xff),
+			 static_cast<uint8_t>(((ip_i) >> 16) & 0xff),
+			 static_cast<uint8_t>(((ip_i) >> 8) & 0xff),
+			 static_cast<uint8_t>(ip_i & 0xff));
+	}
+
+#endif // UXRCE_DDS_CLIENT_UDP
+
+	if (error_flag) {
+		return nullptr;
+	}
+
+	if (transport == Transport::Serial) {
+		if (!device) {
+			PX4_ERR("Missing device");
+			return nullptr;
+		}
+	}
+
+	return new UxrceddsClient(transport, device, baudrate, agent_ip, port, localhost_only, custom_participant,
+				  client_namespace);
+}
+
+int UxrceddsClient::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+UXRCE-DDS Client used to communicate uORB topics with an Agent over serial or UDP.
+
+### Examples
+$ uxrce_dds_client start -t serial -d /dev/ttyS3 -b 921600
+$ uxrce_dds_client start -t udp -h 127.0.0.1 -p 15555
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("uxrce_dds_client", "system");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAM_STRING('t', "udp", "serial|udp", "Transport protocol", true);
+	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', nullptr, "<IP>", "Agent IP. If not provided, defaults to UXRCE_DDS_AG_IP", true);
+	PRINT_MODULE_USAGE_PARAM_INT('p', -1, 0, 65535, "Agent listening port. If not provided, defaults to UXRCE_DDS_PRT", true);
+	PRINT_MODULE_USAGE_PARAM_FLAG('l', "Restrict to localhost (use in combination with ROS_LOCALHOST_ONLY=1)", true);
+	PRINT_MODULE_USAGE_PARAM_FLAG('c', "Use custom participant config (profile_name=\"px4_participant\")", true);
+	PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Client DDS namespace", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int uxrce_dds_client_main(int argc, char *argv[])
+{
+	return UxrceddsClient::main(argc, argv);
+}

src/modules/uxrce_dds_client/uxrce_dds_client.h

@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+
+#include <src/modules/uxrce_dds_client/dds_topics.h>
+
+#include <lib/timesync/Timesync.hpp>
+
+class UxrceddsClient : public ModuleBase<UxrceddsClient>, public ModuleParams
+{
+public:
+	enum class Transport {
+		Serial,
+		Udp
+	};
+
+	UxrceddsClient(Transport transport, const char *device, int baudrate, const char *host, const char *port,
+		       bool localhost_only, bool custom_participant, const char *client_namespace);
+
+	~UxrceddsClient();
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static UxrceddsClient *instantiate(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::run() */
+	void run() override;
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+private:
+	int setBaudrate(int fd, unsigned baud);
+
+	const bool _localhost_only;
+	const bool _custom_participant;
+	const char *_client_namespace;
+
+
+	// max port characters (5+'\0')
+	static const uint8_t PORT_MAX_LENGTH = 6;
+	// max agent ip characters (15+'\0')
+	static const uint8_t AGENT_IP_MAX_LENGTH = 16;
+
+#if defined(CONFIG_NET) || defined(__PX4_POSIX)
+	char _port[PORT_MAX_LENGTH];
+	char _agent_ip[AGENT_IP_MAX_LENGTH];
+#endif
+
+	SendTopicsSubs *_subs{nullptr};
+	RcvTopicsPubs *_pubs{nullptr};
+
+	uxrSerialTransport *_transport_serial{nullptr};
+	uxrUDPTransport *_transport_udp{nullptr};
+	uxrCommunication *_comm{nullptr};
+	int _fd{-1};
+
+	int _last_payload_tx_rate{}; ///< in B/s
+	int _last_payload_rx_rate{}; ///< in B/s
+	bool _connected{false};
+
+	Timesync _timesync{timesync_status_s::SOURCE_PROTOCOL_DDS};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::UXRCE_DDS_DOM_ID>) _param_xrce_dds_dom_id,
+		(ParamInt<px4::params::UXRCE_DDS_KEY>) _param_xrce_key
+	)
+};