[uxrce_dds_client] Allow for arbitrary topic instances to be bridged (#22350)

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@gmail.com>
Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>

docs/en/middleware/uxrce_dds.md

@@ -503,6 +503,11 @@ publications:
   - topic: /fmu/out/vehicle_trajectory_waypoint_desired
     type: px4_msgs::msg::VehicleTrajectoryWaypoint
 
+  - topic: /fmu/out/vehicle_imu
+    type: px4_msgs::msg::VehicleImu
+    rate_limit: 50.
+    instance: 1 # OPTIONAL
+
 subscriptions:
 
   - topic: /fmu/in/offboard_control_mode
@@ -535,6 +540,9 @@ Each (`topic`,`type`) pairs defines:
 4. The message type (`VehicleOdometry`, `VehicleStatus`, `OffboardControlMode`, etc.) and the ROS 2 package (`px4_msgs`) that is expected to provide the message definition.
 5. **(Optional)**: An additional `rate_limit` field (only for publication entries), which specifies the maximum rate (Hz) at which messages will be published on this topic by PX4 to ROS 2.
    If left unspecified, the maximum publication rate limit is set to 100 Hz.
+6. **(Optional)**: An additional `instance` field (only for publication entries), which lets you select which instance of a [multi-instance topic](./uorb.md#multi-instance) you want to be published to ROS 2.
+   If provided, this option changes the ROS 2 topic name of the advertised uORB topic appending the instance number: `fmu/out/[uorb topic name][instance]` (plus eventual namespace and message version).
+   In the example above the final topic name would be `/fmu/out/vehicle_imu1`.
 
 `subscriptions` and `subscriptions_multi` allow us to choose the uORB topic instance that ROS 2 topics are routed to: either a shared instance that may also be getting updates from internal PX4 uORB publishers, or a separate instance that is reserved for ROS2 publications, respectively.
 Without this mechanism all ROS 2 messages would be routed to the _same_ uORB topic instance (because ROS 2 does not have the concept of [multiple topic instances](../middleware/uorb.md#multi-instance)), and it would not be possible for PX4 subscribers to differentiate between streams from ROS 2 or PX4 publishers.

src/modules/uxrce_dds_client/dds_topics.h.em

@@ -67,6 +67,7 @@ struct SendSubscription {
 	uint32_t topic_size;
 	UcdrSerializeMethod ucdr_serialize_method;
 	uint64_t publish_interval_ms;
+	uint8_t orb_instance;
 };
 
 // Subscribers for messages to send
@@ -81,6 +82,7 @@ struct SendTopicsSubs {
 			  ucdr_topic_size_@(pub['simple_base_type'])(),
 			  &ucdr_serialize_@(pub['simple_base_type']),
 			  static_cast<uint64_t>((@(pub.get('rate_limit', 0)) > 0) ? (1e3 / @(pub.get('rate_limit', 1e3))) : UXRCE_DEFAULT_POLL_INTERVAL_MS),
+			  @(pub['instance'])
 			},
 @[    end for]@
 	};
@@ -98,13 +100,13 @@ bool SendTopicsSubs::init(uxrSession *session, uxrStreamId reliable_out_stream_i
 	bool ret = true;
 	for (unsigned idx = 0; idx < sizeof(send_subscriptions)/sizeof(send_subscriptions[0]); ++idx) {
 		if (fds[idx].events == 0) {
-			fds[idx].fd = orb_subscribe(send_subscriptions[idx].orb_meta);
+			fds[idx].fd = orb_subscribe_multi(send_subscriptions[idx].orb_meta, send_subscriptions[idx].orb_instance);
 			fds[idx].events = POLLIN;
 			orb_set_interval(fds[idx].fd, send_subscriptions[idx].publish_interval_ms);
 		}
 
 		if (!create_data_writer(session, reliable_out_stream_id, participant_id, static_cast<ORB_ID>(send_subscriptions[idx].orb_meta->o_id), client_namespace, send_subscriptions[idx].topic,
-								   send_subscriptions[idx].message_version,
+								   send_subscriptions[idx].message_version, send_subscriptions[idx].orb_instance,
 								   send_subscriptions[idx].dds_type_name, send_subscriptions[idx].data_writer)) {
 			ret = false;
 		}

src/modules/uxrce_dds_client/generate_dds_topics.py

@@ -102,12 +102,24 @@ def process_message_type(msg_type):
     # topic_simple: eg vehicle_status
     msg_type['topic_simple'] = msg_type['topic'].split('/')[-1]
 
+def process_message_instance(msg_type):
+    if 'instance' in msg_type:
+        # if instance is given, check if it is a non negative integer
+        if not (type(msg_type['instance']) is int and msg_type['instance'] >= 0) :
+            raise TypeError("`instance` must be a non negative integer")
+        # add trailing instance to topic name
+        msg_type['topic'] = f"{msg_type['topic']}{msg_type['instance']}"
+    else:
+        # if instance is not given,
+        msg_type['instance'] = 0
+
 merged_em_globals['namespace'] = namespace
 
 pubs_not_empty = msg_map['publications'] is not None
 if pubs_not_empty:
     for p in msg_map['publications']:
         process_message_type(p)
+        process_message_instance(p)
 
 merged_em_globals['publications'] = msg_map['publications'] if pubs_not_empty else []
 

src/modules/uxrce_dds_client/utilities.hpp

@@ -50,7 +50,7 @@ static bool generate_topic_name(char *topic_name, const char *client_namespace,
 }
 
 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, uint32_t message_version, const char *type_name,
+			       ORB_ID orb_id, const char *client_namespace, const char *topic, uint32_t message_version, uint8_t instance, const char *type_name,
 			       uxrObjectId &datawriter_id)
 {
 	// topic
@@ -61,7 +61,7 @@ static bool create_data_writer(uxrSession *session, uxrStreamId reliable_out_str
 		return false;
 	}
 
-	uxrObjectId topic_id = topic_id_from_orb(orb_id);
+	uxrObjectId topic_id = topic_id_from_orb(orb_id, instance);
 	uint16_t topic_req = uxr_buffer_create_topic_bin(session, reliable_out_stream_id, topic_id, participant_id, topic_name,
 			     type_name, UXR_REPLACE);