uxrce_dds: support multi-instance uORB topics (#26305)

uxrce_dds: improve DDS to uORB multi-instance routing and docs * Route single DDS topic to multiple uORB instances via message field * Document route_field → instance mapping * Allow subscriptions_demux without route_field * Rename subscriptions_demux → subscriptions_multi * Update docs (ROS 2 wording, version tip, minor fixes)
2026-10-21 04:43:34 +08:00 · 2026-03-03 15:57:24 +01:00
parent 43aa8de22b
commit 7f5de5d141
5 changed files with 104 additions and 37 deletions
@@ -163,7 +163,20 @@ struct RcvTopicsPubs {
@[    end for]@

@[    for sub in subscriptions_multi]@
+@[        if sub.get('route_field')]@
+	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]@
+	};
+	// Maps route_field values (arbitrary, not bounded to [0, max_instances)) to uORB instance indices
+	struct {
+		uint32_t assigned_ids[@(sub['max_instances'])] {};
+		uint8_t num_assigned {0};
+	} @(sub['topic_simple'])_demux;
+@[        else]@
 	uORB::PublicationMulti<@(sub['simple_base_type'])_s> @(sub['topic_simple'])_pub{ORB_ID(@(sub['topic_simple']))};
+@[        end if]@
@[    end for]@

 	uint32_t num_payload_received{};
@@ -179,7 +192,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 +203,38 @@ static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t
 		}
 		break;

+@[    end for]@
+@[    for idx, sub in enumerate(subscriptions_multi)]@
+	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);
+@[        if sub.get('route_field')]@
+				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);
+				}
+@[        else]@
+				pubs->@(sub['topic_simple'])_pub.publish(data);
+@[        end if]@
+			}
+		}
+		break;
+
@[    end for]@

 	default:
@@ -200,13 +245,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_multi)]@
+	{
+			uint16_t queue_depth = orb_get_queue_size(ORB_ID(@(sub['topic_simple']))) * @(sub.get('max_instances', 2)); // 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);

@@ -133,8 +133,8 @@ subscriptions:
  - topic: /fmu/in/config_control_setpoints
    type: px4_msgs::msg::VehicleControlMode

-  - topic: /fmu/in/distance_sensor
-    type: px4_msgs::msg::DistanceSensor
+  # - topic: /fmu/in/distance_sensor
+  #   type: px4_msgs::msg::DistanceSensor

  - topic: /fmu/in/manual_control_input
    type: px4_msgs::msg::ManualControlSetpoint
@@ -190,9 +190,6 @@ subscriptions:
  - topic: /fmu/in/actuator_servos
    type: px4_msgs::msg::ActuatorServos

-  - topic: /fmu/in/aux_global_position
-    type: px4_msgs::msg::AuxGlobalPosition
-
  - topic: /fmu/in/fixed_wing_longitudinal_setpoint
    type: px4_msgs::msg::FixedWingLongitudinalSetpoint

@@ -226,5 +223,11 @@ subscriptions:
  - topic: /fmu/in/landing_gear
    type: px4_msgs::msg::LandingGear

-# Create uORB::PublicationMulti
+# Subscriptions multiplexed to uORB multi-instances (optionally demultiplexed based on a message field)
 subscriptions_multi:
+  - topic: /fmu/in/aux_global_position
+    type: px4_msgs::msg::AuxGlobalPosition
+    route_field: id
+    max_instances: 4
+  - topic: /fmu/in/distance_sensor
+    type: px4_msgs::msg::DistanceSensor
@@ -130,12 +130,11 @@ if subs_not_empty:

 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_multi = msg_map.get('subscriptions_multi') or []
+for sd in subs_multi:
+    process_message_type(sd)

-merged_em_globals['subscriptions_multi'] = msg_map['subscriptions_multi'] if subs_multi_not_empty else []
+merged_em_globals['subscriptions_multi'] = subs_multi

 merged_em_globals['type_includes'] = sorted(set(all_type_includes))