From a92897fb582b45aa9194bef832e99ce2d9b85f4d Mon Sep 17 00:00:00 2001
From: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>
Date: Fri, 21 Oct 2022 16:23:55 -0700
Subject: [PATCH] microdds_client: add namespace to partecipant name

The partecipant name is modified into
"client_namespace"/px4_micro_xrce_dds

For sitl builds the microdds_client is automatically started
with namespace
px4_"instance_number"
and udp port
8888+"intance_number"

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>
---
 ROMFS/px4fmu_common/init.d-posix/rcS          |  2 +-
 .../microdds_client/microdds_client.cpp       | 70 +++++++++++--------
 2 files changed, 43 insertions(+), 29 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS
index 4e6acb9c56..daccb7bbab 100644
--- a/ROMFS/px4fmu_common/init.d-posix/rcS
+++ b/ROMFS/px4fmu_common/init.d-posix/rcS
@@ -260,7 +260,7 @@ fi
 navigator start
 
 # Try to start the microdds_client with UDP transport if module exists
-microdds_client start -t udp -p 8888
+microdds_client start -t udp -p "$((px4_instance+8888))" -n "px4_${px4_instance}"
 
 if param greater -s MNT_MODE_IN -1
 then
diff --git a/src/modules/microdds_client/microdds_client.cpp b/src/modules/microdds_client/microdds_client.cpp
index 2e22d9c304..490d84af11 100644
--- a/src/modules/microdds_client/microdds_client.cpp
+++ b/src/modules/microdds_client/microdds_client.cpp
@@ -53,6 +53,8 @@
 #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,
@@ -222,34 +224,46 @@ void MicroddsClient::run()
 		// 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>"
-					      "<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>"
-					      "<name>px4_micro_xrce_dds</name>"
-					      "<useBuiltinTransports>false</useBuiltinTransports>"
-					      "<userTransports><transport_id>udp_localhost</transport_id></userTransports>"
-					      "</rtps>"
-					      "</participant>"
-					      "</dds>"
-					      :
-					      "<dds>"
-					      "<participant>"
-					      "<rtps>"
-					      "<name>px4_micro_xrce_dds</name>"
-					      "</rtps>"
-					      "</participant>"
-					      "</dds>" ;
+		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 >= TOPIC_NAME_SIZE) {
+			PX4_ERR("create entities failed: namespace too long");
+			return;
+		}
+
 		uint16_t participant_req = uxr_buffer_create_participant_xml(&session, reliable_out, participant_id, domain_id,
 					   participant_xml, UXR_REPLACE);