feat: add whitelisting

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

@@ -193,16 +193,20 @@ static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t
 	const int64_t time_offset_us = session->time_offset / 1000; // ns -> us
 	pubs->num_payload_received += length;
 
-	if(!pubs->_allow_publishing) return;
-
-	switch (object_id.id) {
+switch (object_id.id) {
 @[    for idx, sub in enumerate(subscriptions)]@
 	case @(idx)+ (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['topic_simple'] not in whitelist_topics]@
+				if (pubs->_allow_publishing) {
+@[        end if]@
 				pubs->@(sub['topic_simple'])_pub.publish(data);
+@[        if sub['topic_simple'] not in whitelist_topics]@
+				}
+@[        end if]@
 			}
 		}
 		break;
@@ -230,10 +234,22 @@ static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t
 				}
 
 				if (instance >= 0) {
+@[            if sub['topic_simple'] not in whitelist_topics]@
+					if (pubs->_allow_publishing) {
+@[            end if]@
 					pubs->@(sub['topic_simple'])_pubs[instance].publish(data);
+@[            if sub['topic_simple'] not in whitelist_topics]@
+					}
+@[            end if]@
 				}
 @[        else]@
+@[            if sub['topic_simple'] not in whitelist_topics]@
+				if (pubs->_allow_publishing) {
+@[            end if]@
 				pubs->@(sub['topic_simple'])_pub.publish(data);
+@[            if sub['topic_simple'] not in whitelist_topics]@
+				}
+@[            end if]@
 @[        end if]@
 			}
 		}

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/safety_whitelist.yaml

@@ -0,0 +1,2 @@
+whitelist:
+  - vehicle_command