fix: add whitelist on dds em

src/modules/uxrce_dds_client/CMakeLists.txt

@@ -118,6 +118,7 @@ else()
 		COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py
 			--client-outdir ${CMAKE_CURRENT_BINARY_DIR}
 			--dds-topics-file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.yaml
+			--whitelist-file ${CMAKE_CURRENT_SOURCE_DIR}/safety_whitelist.yaml
 			--template_file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.h.em
 		DEPENDS
 			${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py

src/modules/uxrce_dds_client/dds_topics.h.em

@@ -180,8 +180,10 @@ struct RcvTopicsPubs {
 @[    end for]@
 
 	uint32_t num_payload_received{};
+	bool _allow_publishing{false};
 
 	bool 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);
+	void allow_publishing(bool enabled) { _allow_publishing = enabled; }
 };
 
 static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id,
@@ -196,10 +198,17 @@ static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t
 	case @(idx)+ (65535U / 32U) + 1: {
 			@(sub['simple_base_type'])_s data;
 
+@[        if sub['topic_simple'] in whitelist_topics]@
 			if (ucdr_deserialize_@(sub['simple_base_type'])(*ub, data, time_offset_us)) {
 				//print_message(ORB_ID(@(sub['simple_base_type'])), data);
 				pubs->@(sub['topic_simple'])_pub.publish(data);
 			}
+@[        else]@
+			if (pubs->_allow_publishing && ucdr_deserialize_@(sub['simple_base_type'])(*ub, data, time_offset_us)) {
+				//print_message(ORB_ID(@(sub['simple_base_type'])), data);
+				pubs->@(sub['topic_simple'])_pub.publish(data);
+			}
+@[        end if]@
 		}
 		break;
 
@@ -208,9 +217,10 @@ static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t
 	case @(idx + len(subscriptions))+ (65535U / 32U) + 1: {
 			@(sub['simple_base_type'])_s data;
 
+@[        if sub.get('route_field')]@
+@[            if sub['topic_simple'] in whitelist_topics]@
 			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++) {
@@ -228,10 +238,42 @@ static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t
 				if (instance >= 0) {
 					pubs->@(sub['topic_simple'])_pubs[instance].publish(data);
 				}
-@[        else]@
-				pubs->@(sub['topic_simple'])_pub.publish(data);
-@[        end if]@
 			}
+@[            else]@
+			if (pubs->_allow_publishing && 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);
+				}
+			}
+@[            end if]@
+@[        else]@
+@[            if sub['topic_simple'] in whitelist_topics]@
+			if (ucdr_deserialize_@(sub['simple_base_type'])(*ub, data, time_offset_us)) {
+				//print_message(ORB_ID(@(sub['simple_base_type'])), data);
+				pubs->@(sub['topic_simple'])_pub.publish(data);
+			}
+@[            else]@
+			if (pubs->_allow_publishing && ucdr_deserialize_@(sub['simple_base_type'])(*ub, data, time_offset_us)) {
+				//print_message(ORB_ID(@(sub['simple_base_type'])), data);
+				pubs->@(sub['topic_simple'])_pub.publish(data);
+			}
+@[            end if]@
+@[        end if]@
 		}
 		break;
 

src/modules/uxrce_dds_client/generate_dds_topics.py

@@ -51,6 +51,9 @@ parser.add_argument("-t", "--template_file", dest='template_file', type=str,
 
 parser.add_argument("-u", "--client-outdir", dest='clientdir', type=str,
                     help="Client output dir, by default using relative path 'src/modules/uxrce_dds_client'", default=None)
+parser.add_argument("-w", "--whitelist-file", dest='whitelist_file', type=str,
+                    help="Whitelist topics file path for topics that publish regardless of _allow_publishing flag",
+                    default=None)
 
 if len(sys.argv) <= 1:
     parser.print_usage()
@@ -138,6 +141,15 @@ merged_em_globals['subscriptions_multi'] = subs_multi
 
 merged_em_globals['type_includes'] = sorted(set(all_type_includes))
 
+# Load whitelist topics that should not be fail-safed
+whitelist_topics = set()
+if args.whitelist_file and os.path.exists(args.whitelist_file):
+    with open(args.whitelist_file, 'r') as f:
+        whitelist_data = yaml.safe_load(f)
+        if whitelist_data and '__whitelist' in whitelist_data:
+            whitelist_topics = set(whitelist_data['__whitelist'])
+
+merged_em_globals['whitelist_topics'] = whitelist_topics
 
 # run interpreter
 ofile = open(output_file, 'w')

src/modules/uxrce_dds_client/module.yaml

@@ -153,3 +153,14 @@ parameters:
             category: System
             reboot_required: true
             default: 0
+
+        UXRCE_DDS_SAFE:
+            description:
+                short: Enables offboard safety protection
+                long: |
+                    If disable, allows offboard passthrough
+                    even in non-offboard modes.
+            type: boolean
+            category: System
+            reboot_required: true
+            default: 1

src/modules/uxrce_dds_client/safety_whitelist.yaml

@@ -0,0 +1,4 @@
+module_name: uxrce_dds_client
+
+__whitelist:
+  - vehicle_command

src/modules/uxrce_dds_client/uxrce_dds_client.cpp

@@ -374,6 +374,8 @@ bool UxrceddsClient::setupSession(uxrSession *session)
 	}
 
 	_connected = true;
+
+	_safe_dds_mode = _param_uxrce_dds_safe.get();
 	return true;
 }
 
@@ -651,6 +653,16 @@ void UxrceddsClient::run()
 
 			int bytes_available = 0;
 
+			// Update vehicle status to check for offboard mode
+			vehicle_status_s vehicle_status{};
+			_vehicle_status_sub.copy(&vehicle_status);
+			_offboard_mode_enabled = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD);
+
+			// Allow publish from DDS to uORB if:
+			// - _param_uxrce_dds_safe is false , regardless of offboard mode
+			// - _param_uxrce_dds_safe is true AND offboard mode is enabled
+			_pubs->allow_publishing(!_safe_dds_mode || (_safe_dds_mode && _offboard_mode_enabled));
+
 			if (ioctl(_fd, FIONREAD, (unsigned long)&bytes_available) == OK) {
 				if (bytes_available > 10) {
 					orb_poll_timeout_ms = 0;

src/modules/uxrce_dds_client/uxrce_dds_client.h

@@ -41,6 +41,7 @@
 #include <uORB/topics/message_format_request.h>
 #include <uORB/topics/message_format_response.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_status.h>
 
 #include <lib/timesync/Timesync.hpp>
 
@@ -138,6 +139,7 @@ private:
 
 	uORB::Publication<message_format_response_s> _message_format_response_pub{ORB_ID(message_format_response)};
 	uORB::Subscription _message_format_request_sub{ORB_ID(message_format_request)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	/** Synchronizes the system clock if the time is off by more than 5 seconds */
 	void syncSystemClock(uxrSession *session);
@@ -202,6 +204,8 @@ private:
 	bool _connected{false};
 	bool _session_created{false};
 	bool _timesync_converged{false};
+	bool _offboard_mode_enabled{false};
+	bool _safe_dds_mode{true};
 
 	Timesync _timesync{timesync_status_s::SOURCE_PROTOCOL_DDS};
 
@@ -216,6 +220,7 @@ private:
 		(ParamInt<px4::params::UXRCE_DDS_SYNCT>) _param_uxrce_dds_synct,
 		(ParamInt<px4::params::UXRCE_DDS_TX_TO>) _param_uxrce_dds_tx_to,
 		(ParamInt<px4::params::UXRCE_DDS_RX_TO>) _param_uxrce_dds_rx_to,
-		(ParamInt<px4::params::UXRCE_DDS_FLCTRL>) _param_uxrce_dds_flctrl
+		(ParamInt<px4::params::UXRCE_DDS_FLCTRL>) _param_uxrce_dds_flctrl,
+		(ParamInt<px4::params::UXRCE_DDS_SAFE>) _param_uxrce_dds_safe
 	)
 };