diff --git a/docs/en/middleware/uxrce_dds.md b/docs/en/middleware/uxrce_dds.md index e931cda1a9..e50b3131e6 100644 --- a/docs/en/middleware/uxrce_dds.md +++ b/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. diff --git a/src/modules/uxrce_dds_client/dds_topics.h.em b/src/modules/uxrce_dds_client/dds_topics.h.em index 7ff7605ccd..30ba6053ea 100644 --- a/src/modules/uxrce_dds_client/dds_topics.h.em +++ b/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((@(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(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; } diff --git a/src/modules/uxrce_dds_client/generate_dds_topics.py b/src/modules/uxrce_dds_client/generate_dds_topics.py index 3698c71e9e..957ff4b578 100644 --- a/src/modules/uxrce_dds_client/generate_dds_topics.py +++ b/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 [] diff --git a/src/modules/uxrce_dds_client/utilities.hpp b/src/modules/uxrce_dds_client/utilities.hpp index c2d038b3ed..346dc28455 100644 --- a/src/modules/uxrce_dds_client/utilities.hpp +++ b/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);