Redundancy check against other sensors

src/subjugator/gnc/subjugator_localization/scripts/monitoring_subs.py

@@ -0,0 +1,113 @@
+import rclpy
+from nav_msgs.msg import Odometry
+from rclpy.node import Node
+from sensor_msgs.msg import Imu
+
+# from subjugator_msgs.msg import SpikeStatus
+
+
+class MonitoringNode(Node):
+    def __init__(self):
+        super().__init__("monitoring_node")
+        self.data = ""
+
+        self.pub_dvl_ = self.create_publisher(Odometry, "monitoring_dvl", 10)
+        self.pub_imu_ = self.create_publisher(Imu, "monitoring_imu", 10)
+        # self.pub_spike_ = self.create_publisher(SpikeStatus, "spike_detection", 10)
+
+        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.moving_average_window = 3
+
+    def dvl_odom_callback(self, dmsg: Odometry):
+        self.pub_dvl_.publish(dmsg)
+
+    def imu_data_callback(self, imsg: Imu):
+        # create running average for relevant measurements:
+        # acceleration x
+        if len(self.imu_accelerationx_array) < self.moving_average_window:
+
+            self.imu_accelerationx_array.append(imsg.linear_acceleration.x)
+
+        else:
+            imu_accelerationx_avg = (
+                self.imu_accelerationx_array[0]
+                + self.imu_accelerationx_array[1]
+                + self.imu_accelerationx_array[2]
+            ) / len(self.imu_accelerationx_array)
+
+            percent_diff = (
+                (imsg.linear_acceleration.x - imu_accelerationx_avg)
+                / imu_accelerationx_avg
+            ) * 100
+
+            if -500 < percent_diff < 500:  # arbitrary large percentage difference
+
+                self.imu_accelerationx_array.append(imsg.linear_acceleration.x)
+
+                del self.imu_accelerationx_array[0]
+
+            else:
+
+                self.get_logger().info("Spike Detected.")
+
+        # acceleration y
+        if len(self.imu_accelerationy_array) < self.moving_average_window:
+
+            self.imu_accelerationy_array.append(imsg.linear_acceleration.y)
+
+        else:
+            imu_accelerationy_avg = (
+                self.imu_accelerationy_array[0]
+                + self.imu_accelerationy_array[1]
+                + self.imu_accelerationy_array[2]
+            ) / len(self.imu_accelerationy_array)
+
+            percent_diff = (
+                (imsg.linear_acceleration.y - imu_accelerationy_avg)
+                / imu_accelerationy_avg
+            ) * 100
+
+            if -500 < percent_diff < 500:  # arbitrary large percentage difference
+                self.imu_accelerationy_array.append(imsg.linear_acceleration.y)
+                del self.imu_accelerationy_array[0]
+
+            else:
+                self.get_logger().info("Spike Detected.")
+
+        # acceleration z
+        if self.imu_accelerationz_array.size() < self.moving_average_window:
+            self.imu_accelerationz_array.append(imsg.linear_acceleration.z)
+        else:
+            imu_accelerationz_avg = (
+                self.imu_accelerationz_array[0]
+                + self.imu_accelerationz_array[1]
+                + self.imu_accelerationz_array[2]
+            ) / len(self.imu_accelerationz_array)
+            percent_diff = (
+                (imsg.linear_acceleration.z - imu_accelerationz_avg)
+                / imu_accelerationz_avg
+            ) * 100
+            if -500 < percent_diff < 500:  # arbitrary large percentage difference
+                self.imu_accelerationz_array.append(imsg.linear_acceleration.z)
+                del self.imu_accelerationz_array[0]
+            else:
+                self.get_logger().info("Spike Detected.")
+
+        self.pub_imu_.publish(imsg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = MonitoringNode()
+    rclpy.spin(node)
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/subjugator/gnc/subjugator_sensor_monitoring/CMakeLists.txt

@@ -7,8 +7,12 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in further dependencies
-# manually. find_package(<dependency> REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/)
+install(PROGRAMS src/redundancy_check.py DESTINATION lib/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

src/subjugator/gnc/subjugator_sensor_monitoring/launch/redundancy_check.launch.py

@@ -0,0 +1,60 @@
+"""Launch file for redundancy check node."""
+
+from launch import LaunchDescription
+from launch.actions import (
+    ExecuteProcess,
+    IncludeLaunchDescription,
+    TimerAction,
+)
+from launch.launch_description_sources import (
+    PythonLaunchDescriptionSource,
+)
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    """Generate launch description for redundancy check."""
+    localization_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("subjugator_localization"),
+                        "launch",
+                        "subjugator_localization.launch.py",
+                    ],
+                ),
+            ],
+        ),
+    )
+    # Service call to enable localization
+    enable_localization = TimerAction(
+        period=5.0,  # Wait 2 seconds
+        actions=[
+            ExecuteProcess(
+                cmd=[
+                    "ros2",
+                    "service",
+                    "call",
+                    "/subjugator_localization/enable",
+                    "std_srvs/srv/Empty",
+                    "{}",
+                ],
+                output="screen",
+            ),
+        ],
+    )
+    return LaunchDescription(
+        [
+            Node(
+                package="subjugator_sensor_monitoring",
+                executable="redundancy_check.py",
+                name="redundancy_check_node",
+                output="screen",
+            ),
+            localization_launch,
+            enable_localization,
+        ],
+    )

src/subjugator/gnc/subjugator_sensor_monitoring/package.xml

@@ -1,5 +1,4 @@
 <?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>subjugator_sensor_monitoring</name>
   <version>0.0.0</version>
@@ -12,6 +11,11 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
+  <exec_depend>rclpy</exec_depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py

@@ -0,0 +1,150 @@
+#!/usr/bin/env python3
+"""Redundancy check node for comparing DVL and IMU sensor data."""
+import rclpy
+import rclpy.node
+import tf2_geometry_msgs  # noqa: F401
+from geometry_msgs.msg import Vector3Stamped
+from nav_msgs.msg import Odometry
+from rclpy.duration import Duration
+from sensor_msgs.msg import Imu
+from tf2_ros import TransformException
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+
+
+class RedundancyCheckNode(rclpy.node.Node):
+    """Node to check redundancy between DVL and IMU sensors."""
+
+    def __init__(self):
+        """Initialize the redundancy check node."""
+        super().__init__("redundancy_check_node")
+
+        self.imu_subscriber = self.create_subscription(
+            Imu,
+            "/imu/data",
+            self.imu_callback,
+            10,
+        )
+
+        self.dvl_subscriber = self.create_subscription(
+            Odometry,
+            "/dvl/odom",
+            self.dvl_callback,
+            10,
+        )
+
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+        self.imu_data = None
+        self.dvl_data = None
+        self.previous_dvl_time = None
+        self.previous_dvl_vel = None
+
+        self.diff_threshold = 10  # threshold val for redundancy check
+
+        self.get_logger().info("Redundancy Check Node has been started.")
+
+    def imu_callback(self, msg):
+        """Handle incoming IMU messages."""
+        self.imu_data = msg
+
+    def dvl_callback(self, msg):
+        """Handle incoming DVL messages."""
+        self.dvl_data = msg
+
+        self.check_redundancy()
+
+    def check_redundancy(self):
+        """Check redundancy between DVL and IMU data."""
+        # Main redundancy check logic:
+        if self.imu_data is None or self.dvl_data is None:
+            return
+
+        # Calculate current time from DVL data timestamp
+        current_time = rclpy.time.Time.from_msg(self.dvl_data.header.stamp)
+
+        if self.previous_dvl_time is None:
+            self.previous_dvl_time = current_time
+            self.previous_dvl_vel = self.dvl_data.twist.twist.linear
+            return
+
+        dt = (current_time - self.previous_dvl_time).nanoseconds / 1e9
+
+        if dt <= 0:
+            self.get_logger().warn(
+                "dt is neg. or zero, skipping redundancy check.",
+            )
+            return
+
+        try:
+
+            # do transforms now
+            # Transform IMU linear acceleration to base_link
+            imu_lin_acc = Vector3Stamped()
+            imu_lin_acc.header.frame_id = "imu_link"
+            imu_lin_acc.header.stamp = rclpy.time.Time().to_msg()
+            imu_lin_acc.vector = self.imu_data.linear_acceleration
+
+            imu_lin_acc_transformed = self.tf_buffer.transform(
+                imu_lin_acc,
+                "base_link",
+                timeout=Duration(seconds=1.0),
+            )
+
+            # Transform DVL linear velocity to base_link
+            dvl_linear_vel = Vector3Stamped()
+            dvl_linear_vel.header.frame_id = "odom"
+            dvl_linear_vel.header.stamp = rclpy.time.Time().to_msg()
+            dvl_linear_vel.vector = self.dvl_data.twist.twist.linear
+
+            dvl_vel_transformed = self.tf_buffer.transform(
+                dvl_linear_vel,
+                "base_link",
+                timeout=Duration(seconds=1.0),
+            )
+
+        except TransformException as ex:
+            self.get_logger().error(f"Transform error: {ex}")
+            return
+
+        imu_linear_acc = imu_lin_acc_transformed.vector
+        dvl_linear_vel = dvl_vel_transformed.vector
+
+        # Calculate the derivate of the DVL linear x,y,z velocities
+        dvl_acceleration = [
+            (dvl_linear_vel.x - self.previous_dvl_vel.x) / dt,
+            (dvl_linear_vel.y - self.previous_dvl_vel.y) / dt,
+            (dvl_linear_vel.z - self.previous_dvl_vel.z) / dt,
+        ]
+
+        # Compare DVL acceleration with IMU linear acceleration
+        diff_x = abs(dvl_acceleration[0] - imu_linear_acc.x)
+        diff_y = abs(dvl_acceleration[1] - imu_linear_acc.y)
+        diff_z = abs(dvl_acceleration[2] - imu_linear_acc.z)
+
+        if (
+            diff_x > self.diff_threshold
+            or diff_y > self.diff_threshold
+            or diff_z > self.diff_threshold
+        ):
+            self.get_logger().warn(
+                f"Redundancy check failed: Differences - "
+                f"X: {diff_x}, Y: {diff_y}, Z: {diff_z}",
+            )
+
+        self.previous_dvl_time = current_time
+        self.previous_dvl_vel = dvl_linear_vel
+
+
+def main(args=None):
+    """Run the redundancy check node."""
+    rclpy.init(args=args)
+    node = RedundancyCheckNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()