From 89015ee7a59555b62ff92844bb72c64286ee6e37 Mon Sep 17 00:00:00 2001
From: David <ds0196@uah.edu>
Date: Tue, 14 Oct 2025 15:21:23 -0500
Subject: [PATCH] feat: add VicCAN message validity checking and core feedback
 data length checking

---
 src/anchor_pkg/anchor_pkg/anchor_node.py | 30 +++++++++++++++++++++++-
 src/core_pkg/core_pkg/core_node.py       | 23 ++++++++++++++++--
 2 files changed, 50 insertions(+), 3 deletions(-)

diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py
index aa46fdb..1f1d78f 100644
--- a/src/anchor_pkg/anchor_pkg/anchor_node.py
+++ b/src/anchor_pkg/anchor_pkg/anchor_node.py
@@ -171,9 +171,37 @@ class SerialRelay(Node):
         """ Relay a string message from the MCU to the appropriate VicCAN topic """
         self.fromvic_debug_pub_.publish(String(data=msg))
         parts = msg.strip().split(",")
-        if len(parts) < 3 or parts[0] != "can_relay_fromvic":
+        if len(parts) > 0 and parts[0] != "can_relay_fromvic":
+            self.get_logger().debug(f"Ignoring non-VicCAN message: '{msg.strip()}'")
             return
 
+        # String validation
+        malformed: bool = False
+        malformed_reason: str = ""
+        if len(parts) < 3 or len(parts) > 7:
+            malformed = True
+            malformed_reason = f"invalid argument count (expected [3,7], got {len(parts)})"
+        elif parts[1] not in ["core", "arm", "digit", "citadel", "broadcast"]:
+            malformed = True
+            malformed_reason = f"invalid mcu_name '{parts[1]}'"
+        elif parts[2].isnumeric() is False or int(parts[2]) < 0:
+            malformed = True
+            malformed_reason = f"command_id '{parts[2]}' is not a non-negative integer"
+        else:
+            for x in parts[3:]:
+                try:
+                    float(x)
+                except ValueError:
+                    malformed = True
+                    malformed_reason = f"data '{x}' is not a float"
+                    break
+
+        if malformed:
+            self.get_logger().warning(f"Ignoring malformed from_vic message: '{msg.strip()}'; reason: {malformed_reason}")
+            return
+
+        # Have valid VicCAN message
+
         output = VicCAN()
         output.mcu_name = parts[1]
         output.command_id = int(parts[2])
diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py
index 65252a2..67caa51 100644
--- a/src/core_pkg/core_pkg/core_node.py
+++ b/src/core_pkg/core_pkg/core_node.py
@@ -41,6 +41,20 @@ control_qos = qos.QoSProfile(
     liveliness_lease_duration=Duration(seconds=5)
 )
 
+# Used to verify the length of an incoming VicCAN feedback message
+# Key is VicCAN command_id, value is expected length of data list
+viccan_msg_len_dict = {
+    48: 1,
+    49: 1,
+    50: 2,
+    51: 4,
+    52: 4,
+    53: 4,
+    54: 4,
+    56: 3,
+    58: 3
+}
+
 
 class SerialRelay(Node):
     def __init__(self):
@@ -357,8 +371,13 @@ class SerialRelay(Node):
     def relay_fromvic(self, msg: VicCAN):
         # Assume that the message is coming from Core
         # skill diff if not
-        
-        # TODO: add len(msg.data) checks to each feedback message
+
+        # Check message len to prevent crashing on bad data
+        if msg.command_id in viccan_msg_len_dict:
+            expected_len = viccan_msg_len_dict[msg.command_id]
+            if len(msg.data) != expected_len:
+                self.get_logger().warning(f"Ignoring VicCAN message with id {msg.command_id} due to unexpected data length (expected {expected_len}, got {len(msg.data)})")
+                return
 
         match msg.command_id:
             # GNSS