diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/CMakeLists.txt b/src/subjugator/gnc/subjugator_sensor_monitoring/CMakeLists.txt
index deebd363..ac40e9d9 100644
--- a/src/subjugator/gnc/subjugator_sensor_monitoring/CMakeLists.txt
+++ b/src/subjugator/gnc/subjugator_sensor_monitoring/CMakeLists.txt
@@ -20,6 +20,8 @@ if(BUILD_TESTING)
# to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
+ find_package(subjugator_msgs REQUIRED)
+ install(PROGRAMS src/monitoring_subs.py DESTINATION lib/${PROJECT_NAME})
endif()
ament_package()
diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/package.xml b/src/subjugator/gnc/subjugator_sensor_monitoring/package.xml
index fc93455b..6274ce97 100644
--- a/src/subjugator/gnc/subjugator_sensor_monitoring/package.xml
+++ b/src/subjugator/gnc/subjugator_sensor_monitoring/package.xml
@@ -12,6 +12,8 @@
ament_lint_auto
ament_lint_common
+ subjugator_msgs
+
ament_cmake
diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/src/monitoring_subs.py b/src/subjugator/gnc/subjugator_sensor_monitoring/src/monitoring_subs.py
new file mode 100755
index 00000000..92c56cfe
--- /dev/null
+++ b/src/subjugator/gnc/subjugator_sensor_monitoring/src/monitoring_subs.py
@@ -0,0 +1,154 @@
+#!/usr/bin/env python3
+
+import rclpy
+from nav_msgs.msg import Odometry
+from rclpy.node import Node
+from sensor_msgs.msg import Imu
+from subjugator_msgs.msg import SensorSpike, SensorSpikeArray
+
+
+class MonitoringNode(Node):
+ def __init__(self):
+ super().__init__("monitoring_node")
+ self.data = ""
+ self.pub_spike = self.create_publisher(SensorSpikeArray, "spike_monitoring", 10)
+ self.array_msg = SensorSpikeArray()
+
+ self.create_subscription(Odometry, "dvl/odom", self.dvl_odom_callback, 10)
+ self.create_subscription(Imu, "imu/data", self.imu_data_callback, 10)
+
+ self.imu_accelerationx_array = []
+ self.imu_accelerationy_array = []
+ self.imu_accelerationz_array = []
+
+ self.dvl_accelerationx_array = []
+ self.dvl_accelerationy_array = []
+ self.dvl_accelerationz_array = []
+
+ self.in_spike_state_imu_x = False
+ self.in_spike_state_imu_y = False
+ self.in_spike_state_imu_z = False
+
+ self.in_spike_state_dvl_x = False
+ self.in_spike_state_dvl_y = False
+ self.in_spike_state_dvl_z = False
+
+ def process_imu(self, value, array, spike_state_attr, axis_name):
+
+ if len(array) <= 20:
+ array.append(value)
+ else:
+ # Calculate average using sum and abs
+ avg = sum(abs(val) for val in array) / len(array)
+
+ # Calculate percent difference
+ percent_diff = ((abs(value) - avg) / avg) * 100
+
+ # Check if within threshold
+ if -3000 < percent_diff < 3000:
+ array.append(value)
+ del array[0]
+ setattr(self, spike_state_attr, False)
+ else:
+ if not getattr(self, spike_state_attr):
+ # publish a SensorSpike message with details about the detected spike
+ msg = SensorSpike()
+ msg.header.stamp = self.get_clock().now().to_msg()
+ msg.header.frame_id = "imu"
+ msg.spike_detected = True
+ msg.sensor_type = axis_name
+ msg.measured_value = float(value)
+ self.array_msg.sensors_status.append(msg)
+ setattr(self, spike_state_attr, True)
+
+ def process_dvl(self, value, array, spike_state_attr, axis_name):
+ if len(array) <= 20:
+ array.append(value)
+ else:
+ # Calculate average using sum and abs
+ avg = sum(abs(val) for val in array) / len(array)
+
+ # Calculate percent difference
+ percent_diff = ((abs(value) - avg) / avg) * 100
+
+ # Check if within threshold
+ if -3000 < percent_diff < 3000:
+ array.append(value)
+ del array[0]
+ setattr(self, spike_state_attr, False)
+ else:
+ if not getattr(self, spike_state_attr):
+ # publish a SensorSpike message with details about the detected spike
+ msg = SensorSpike()
+ msg.header.stamp = self.get_clock().now().to_msg()
+ msg.header.frame_id = "dvl"
+ msg.spike_detected = True
+ msg.sensor_type = axis_name
+ msg.measured_value = float(value)
+ self.array_msg.sensors_status.append(msg)
+ setattr(self, spike_state_attr, True)
+
+ def dvl_odom_callback(self, dmsg: Odometry):
+ # Process x-velocity
+ self.process_dvl(
+ dmsg.twist.twist.linear.x,
+ self.dvl_accelerationx_array,
+ "in_spike_state_dvl_x",
+ "dvl_vel_x",
+ )
+
+ # Process y-velocity
+ self.process_dvl(
+ dmsg.twist.twist.linear.y,
+ self.dvl_accelerationy_array,
+ "in_spike_state_dvl_y",
+ "dvl_vel_y",
+ )
+
+ # Process z-velocity
+ self.process_dvl(
+ dmsg.twist.twist.linear.z,
+ self.dvl_accelerationz_array,
+ "in_spike_state_dvl_z",
+ "dvl_vel_z",
+ )
+
+ self.pub_spike.publish(self.array_msg)
+
+ def imu_data_callback(self, imsg: Imu):
+ # Process x-axis
+ self.process_imu(
+ imsg.linear_acceleration.x,
+ self.imu_accelerationx_array,
+ "in_spike_state_imu_x",
+ "lin_accel_x",
+ )
+
+ # Process y-axis
+ self.process_imu(
+ imsg.linear_acceleration.y,
+ self.imu_accelerationy_array,
+ "in_spike_state_imu_y",
+ "lin_accel_y",
+ )
+
+ # Process z-axis
+ self.process_imu(
+ imsg.linear_acceleration.z,
+ self.imu_accelerationz_array,
+ "in_spike_state_imu_z",
+ "lin_accel_z",
+ )
+
+ self.pub_spike.publish(self.array_msg)
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = MonitoringNode()
+ rclpy.spin(node)
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/subjugator/subjugator_msgs/CMakeLists.txt b/src/subjugator/subjugator_msgs/CMakeLists.txt
index e4d34f50..477dfecc 100644
--- a/src/subjugator/subjugator_msgs/CMakeLists.txt
+++ b/src/subjugator/subjugator_msgs/CMakeLists.txt
@@ -22,6 +22,8 @@ rosidl_generate_interfaces(
"msg/Trajectory.msg"
"msg/Waypoint.msg"
"msg/Centroid.msg"
+ "msg/SensorSpike.msg"
+ "msg/SensorSpikeArray.msg"
"srv/BMatrix.srv"
"srv/OcrRequest.srv"
"srv/TBDetectionSwitch.srv"
diff --git a/src/subjugator/subjugator_msgs/msg/SensorSpikeArray.msg b/src/subjugator/subjugator_msgs/msg/SensorSpikeArray.msg
index 26eff8c7..b585c851 100644
--- a/src/subjugator/subjugator_msgs/msg/SensorSpikeArray.msg
+++ b/src/subjugator/subjugator_msgs/msg/SensorSpikeArray.msg
@@ -2,4 +2,4 @@
std_msgs/Header header
-SensorSpike[] sensorsStatus
+SensorSpike[] sensors_status