uxrce_dds: demux single DDS topic to multi-instance uORB by message field

Author: haumarco

src/modules/uxrce_dds_client/dds_topics.h.em

@@ -162,8 +162,16 @@ struct RcvTopicsPubs {
 	uORB::Publication<@(sub['simple_base_type'])_s> @(sub['topic_simple'])_pub{ORB_ID(@(sub['topic_simple']))};
 @[    end for]@
 
-@[    for sub in subscriptions_multi]@
-	uORB::PublicationMulti<@(sub['simple_base_type'])_s> @(sub['topic_simple'])_pub{ORB_ID(@(sub['topic_simple']))};
+@[    for sub in subscriptions_demux]@
+	uORB::PublicationMulti<@(sub['simple_base_type'])_s> @(sub['topic_simple'])_pubs[@(sub['max_instances'])] {
+@[        for idx in range(sub['max_instances'])]@
+		{ORB_ID(@(sub['topic_simple']))}@('' if idx == sub['max_instances']-1 else ',')
+@[        end for]@
+	};
+	struct {
+		uint8_t assigned_ids[@(sub['max_instances'])] {};
+		uint8_t num_assigned {0};
+	} @(sub['topic_simple'])_demux;
 @[    end for]@
 
 	uint32_t num_payload_received{};
@@ -179,7 +187,7 @@ static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t
 	pubs->num_payload_received += length;
 
 	switch (object_id.id) {
-@[    for idx, sub in enumerate(subscriptions + subscriptions_multi)]@
+@[    for idx, sub in enumerate(subscriptions)]@
 	case @(idx)+ (65535U / 32U) + 1: {
 			@(sub['simple_base_type'])_s data;
 
@@ -190,6 +198,34 @@ static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t
 		}
 		break;
 
+@[    end for]@
+@[    for idx, sub in enumerate(subscriptions_demux)]@
+	case @(idx + len(subscriptions))+ (65535U / 32U) + 1: {
+			@(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);
+				int instance = -1;
+
+				for (uint8_t i = 0; i < pubs->@(sub['topic_simple'])_demux.num_assigned; i++) {
+					if (pubs->@(sub['topic_simple'])_demux.assigned_ids[i] == data.@(sub['route_field'])) {
+						instance = i;
+						break;
+					}
+				}
+
+				if (instance < 0 && pubs->@(sub['topic_simple'])_demux.num_assigned < @(sub['max_instances'])) {
+					instance = pubs->@(sub['topic_simple'])_demux.num_assigned++;
+					pubs->@(sub['topic_simple'])_demux.assigned_ids[instance] = data.@(sub['route_field']);
+				}
+
+				if (instance >= 0) {
+					pubs->@(sub['topic_simple'])_pubs[instance].publish(data);
+				}
+			}
+		}
+		break;
+
 @[    end for]@
 
 	default:
@@ -200,13 +236,20 @@ static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t
 
 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 + subscriptions_multi)]@
+@[    for idx, sub in enumerate(subscriptions)]@
 	{
 			uint16_t queue_depth = orb_get_queue_size(ORB_ID(@(sub['simple_base_type']))) * 2; // use a bit larger queue size than internal
 			uint32_t message_version = get_message_version<@(sub['simple_base_type'])_s>();
 			create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx), client_namespace, "@(sub['topic'])", message_version, "@(sub['dds_type'])", queue_depth);
 	}
 @[    end for]@
+@[    for idx, sub in enumerate(subscriptions_demux)]@
+	{
+			uint16_t queue_depth = orb_get_queue_size(ORB_ID(@(sub['topic_simple']))) * @(sub['max_instances']); // scale queue for multiple sources
+			uint32_t message_version = get_message_version<@(sub['simple_base_type'])_s>();
+			create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx + len(subscriptions)), client_namespace, "@(sub['topic'])", message_version, "@(sub['dds_type'])", queue_depth);
+	}
+@[    end for]@
 
 	uxr_set_topic_callback(session, on_topic_update, this);
 

src/modules/uxrce_dds_client/dds_topics.yaml

@@ -52,7 +52,7 @@ publications:
 
   - topic: /fmu/out/transponder_report
     type: px4_msgs::msg::TransponderReport
- 
+
   # - topic: /fmu/out/vehicle_angular_velocity
   #   type: px4_msgs::msg::VehicleAngularVelocity
 
@@ -110,6 +110,10 @@ publications:
     type: px4_msgs::msg::GimbalDeviceAttitudeStatus
     rate_limit: 20.
 
+  - topic: /fmu/out/vehicle_global_position_groundtruth
+    type: px4_msgs::msg::VehicleGlobalPosition
+    rate_limit: 20.
+
 # Create uORB::Publication
 subscriptions:
   - topic: /fmu/in/register_ext_component_request
@@ -190,9 +194,6 @@ subscriptions:
   - topic: /fmu/in/actuator_servos
     type: px4_msgs::msg::ActuatorServos
 
-  - topic: /fmu/in/aux_global_position
-    type: px4_msgs::msg::VehicleGlobalPosition
-
   - topic: /fmu/in/fixed_wing_longitudinal_setpoint
     type: px4_msgs::msg::FixedWingLongitudinalSetpoint
 
@@ -226,5 +227,9 @@ subscriptions:
   - topic: /fmu/in/landing_gear
     type: px4_msgs::msg::LandingGear
 
-# Create uORB::PublicationMulti
-subscriptions_multi:
+# Subscriptions demultiplexed to uORB multi-instances based on a message field
+subscriptions_demux:
+  - topic: /fmu/in/aux_global_position
+    type: px4_msgs::msg::AuxGlobalPosition
+    route_field: id
+    max_instances: 4

src/modules/uxrce_dds_client/generate_dds_topics.py

@@ -130,12 +130,11 @@ def process_message_instance(msg_type):
 
 merged_em_globals['subscriptions'] = msg_map['subscriptions'] if subs_not_empty else []
 
-subs_multi_not_empty = msg_map['subscriptions_multi'] is not None
-if subs_multi_not_empty:
-    for sm in msg_map['subscriptions_multi']:
-        process_message_type(sm)
+subs_demux = msg_map.get('subscriptions_demux') or []
+for sd in subs_demux:
+    process_message_type(sd)
 
-merged_em_globals['subscriptions_multi'] = msg_map['subscriptions_multi'] if subs_multi_not_empty else []
+merged_em_globals['subscriptions_demux'] = subs_demux
 
 merged_em_globals['type_includes'] = sorted(set(all_type_includes))