From 03c11d3c7f3a29de9575c52a4b152b16ff80f069 Mon Sep 17 00:00:00 2001 From: Mohana Date: Mon, 10 Nov 2025 22:21:42 -0500 Subject: [PATCH 1/8] intial framework-just imu/dvl subs --- .../gnc/subjugator_sensor_monitoring/src/redundancy_check.py | 0 src/subjugator/gnc/subjugator_sensor_monitoring/src/test.py | 1 - 2 files changed, 1 deletion(-) create mode 100644 src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py delete mode 100644 src/subjugator/gnc/subjugator_sensor_monitoring/src/test.py diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py b/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/src/test.py b/src/subjugator/gnc/subjugator_sensor_monitoring/src/test.py deleted file mode 100644 index b9ba30461..000000000 --- a/src/subjugator/gnc/subjugator_sensor_monitoring/src/test.py +++ /dev/null @@ -1 +0,0 @@ -# changes to get CI to run again after PR. From caeb0c818e3fb26b3353c392e69d23559391fe57 Mon Sep 17 00:00:00 2001 From: Mohana Date: Wed, 12 Nov 2025 23:57:40 -0500 Subject: [PATCH 2/8] TF transforms times are not synchronized --- .../CMakeLists.txt | 6 +- .../launch/redundancy_check.launch.py | 15 ++ .../subjugator_sensor_monitoring/package.xml | 2 + .../src/redundancy_check.py | 144 ++++++++++++++++++ 4 files changed, 165 insertions(+), 2 deletions(-) create mode 100644 src/subjugator/gnc/subjugator_sensor_monitoring/launch/redundancy_check.launch.py mode change 100644 => 100755 src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/CMakeLists.txt b/src/subjugator/gnc/subjugator_sensor_monitoring/CMakeLists.txt index deebd363d..f1606aea8 100644 --- a/src/subjugator/gnc/subjugator_sensor_monitoring/CMakeLists.txt +++ b/src/subjugator/gnc/subjugator_sensor_monitoring/CMakeLists.txt @@ -7,8 +7,10 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in further dependencies -# manually. find_package( REQUIRED) +find_package(rclpy 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) diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/launch/redundancy_check.launch.py b/src/subjugator/gnc/subjugator_sensor_monitoring/launch/redundancy_check.launch.py new file mode 100644 index 000000000..decaf65e8 --- /dev/null +++ b/src/subjugator/gnc/subjugator_sensor_monitoring/launch/redundancy_check.launch.py @@ -0,0 +1,15 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription( + [ + Node( + package="subjugator_sensor_monitoring", + executable="redundancy_check.py", + name="redundancy_check_node", + output="screen", + ), + ], + ) diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/package.xml b/src/subjugator/gnc/subjugator_sensor_monitoring/package.xml index fc93455bf..0985b0069 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 + rclpy + ament_cmake diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py b/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py old mode 100644 new mode 100755 index e69de29bb..609506233 --- a/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py +++ b/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py @@ -0,0 +1,144 @@ +#!/usr/bin/env python3 + +import rclpy +import rclpy.node +import tf2_ros +from geometry_msgs.msg import Vector3Stamped +from nav_msgs.msg import Odometry +from sensor_msgs.msg import Imu + + +class RedundancyCheckNode(rclpy.node.Node): + def __init__(self): + 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 = tf2_ros.Buffer( + cache_time=rclpy.duration.Duration(seconds=10.0), + ) + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + self.imu_data = None + self.dvl_data = None + self.previous_dvl_time = None + + self.diff_threshold = 0.5 # threshold val for redundancy check + + self.get_logger().info("Redundancy Check Node has been started.") + + def imu_callback(self, msg): + self.imu_data = msg + self.check_redundancy() + + def dvl_callback(self, msg): + self.dvl_data = msg + + def check_redundancy(self): + + # imu_transform frame id: imu_link + # dvl transfor frame id: odom + # No direct redundancy check. + # DVL = Linear x,y,z velocities + # IMU - Roll, Pitch, Yaw rates, Angular Velocity, Liner Accel x,y,z + # Differentiate DVL velocity --> compare against IMU linear acceleration + + # Main redundancy check logic: + if self.imu_data is None or self.dvl_data is None: + return + + try: + + # do transforms now + + imu_lin_acc = Vector3Stamped() + imu_lin_acc.header = self.imu_data.header + imu_lin_acc.vector = self.imu_data.linear_acceleration + + imu_lin_acc_transformed = self.tf_buffer.transform( + imu_lin_acc, + "base_link", + rclpy.time.Time(), + rclpy.duration.Duration(seconds=0.1), + ).vector + + dvl_linear_vel = Vector3Stamped() + dvl_linear_vel.header = self.dvl_data.header + dvl_linear_vel.vector = self.dvl_data.twist.twist.linear + + dvl_transformed = self.tf_buffer.transform( + dvl_linear_vel, + "base_link", + rclpy.time.Time(), + rclpy.duration.Duration(seconds=0.1), + ).vector + + except tf2_ros.TransformException as ex: + self.get_logger().error(f"Transform error: {ex}") + return + + dvl_linear_vel = dvl_transformed + imu_linear_acc = imu_lin_acc_transformed + + # Calculate dt from actual timestamp of the message. + 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 = dvl_linear_vel + return + + dt_duration = current_time - self.previous_dvl_time + dt = dt_duration.nanoseconds * 1e-9 # Convert to seconds + + if dt <= 0: + self.get_logger().warn("dt is neg. or zero, skipping redundancy check.") + return + + # 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 - 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): + rclpy.init(args=args) + node = RedundancyCheckNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() From f821be73ee934a146227a001c0ede925e7a2a740 Mon Sep 17 00:00:00 2001 From: Mohana Date: Thu, 13 Nov 2025 11:54:23 -0500 Subject: [PATCH 3/8] Redundancy check works, may need to tune the threshold --- .../src/redundancy_check.py | 75 ++++++++++--------- 1 file changed, 39 insertions(+), 36 deletions(-) diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py b/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py index 609506233..e05c85489 100755 --- a/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py +++ b/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py @@ -2,10 +2,13 @@ import rclpy import rclpy.node -import tf2_ros 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): @@ -26,25 +29,25 @@ def __init__(self): 10, ) - self.tf_buffer = tf2_ros.Buffer( - cache_time=rclpy.duration.Duration(seconds=10.0), - ) - self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + 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 = 0.5 # threshold val for redundancy check + self.diff_threshold = 10 # threshold val for redundancy check self.get_logger().info("Redundancy Check Node has been started.") def imu_callback(self, msg): self.imu_data = msg - self.check_redundancy() def dvl_callback(self, msg): self.dvl_data = msg + # Only checking when DVL received new data + self.check_redundancy() def check_redundancy(self): @@ -59,53 +62,53 @@ def check_redundancy(self): if self.imu_data is None or self.dvl_data is None: return + # Calculate currenbt 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 = self.imu_data.header + 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", - rclpy.time.Time(), - rclpy.duration.Duration(seconds=0.1), - ).vector + timeout=Duration(seconds=1.0), + ) + # Transform DVL linear velocity to base_link dvl_linear_vel = Vector3Stamped() - dvl_linear_vel.header = self.dvl_data.header + 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_transformed = self.tf_buffer.transform( + dvl_vel_transformed = self.tf_buffer.transform( dvl_linear_vel, "base_link", - rclpy.time.Time(), - rclpy.duration.Duration(seconds=0.1), - ).vector + timeout=Duration(seconds=1.0), + ) - except tf2_ros.TransformException as ex: + except TransformException as ex: self.get_logger().error(f"Transform error: {ex}") return - dvl_linear_vel = dvl_transformed - imu_linear_acc = imu_lin_acc_transformed - - # Calculate dt from actual timestamp of the message. - 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 = dvl_linear_vel - return - - dt_duration = current_time - self.previous_dvl_time - dt = dt_duration.nanoseconds * 1e-9 # Convert to seconds - - if dt <= 0: - self.get_logger().warn("dt is neg. or zero, skipping redundancy check.") - 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 = [ @@ -125,7 +128,7 @@ def check_redundancy(self): or diff_z > self.diff_threshold ): self.get_logger().warn( - f"Redundancy check failed! Differences - X: {diff_x}, Y: {diff_y}, Z: {diff_z}", + f"Redundancy check failed: Differences - X: {diff_x}, Y: {diff_y}, Z: {diff_z}", ) self.previous_dvl_time = current_time From 079869b0937ace636da49be8359a0ae54430a049 Mon Sep 17 00:00:00 2001 From: Mohana Date: Sun, 16 Nov 2025 14:32:34 -0500 Subject: [PATCH 4/8] added to launch file --- .../launch/redundancy_check.launch.py | 39 +++++++++++++++++++ 1 file changed, 39 insertions(+) diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/launch/redundancy_check.launch.py b/src/subjugator/gnc/subjugator_sensor_monitoring/launch/redundancy_check.launch.py index decaf65e8..216892ba9 100644 --- a/src/subjugator/gnc/subjugator_sensor_monitoring/launch/redundancy_check.launch.py +++ b/src/subjugator/gnc/subjugator_sensor_monitoring/launch/redundancy_check.launch.py @@ -1,8 +1,45 @@ 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(): + + 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( @@ -11,5 +48,7 @@ def generate_launch_description(): name="redundancy_check_node", output="screen", ), + localization_launch, + enable_localization, ], ) From 0b475b53227582529a8a212b94e234463f89512e Mon Sep 17 00:00:00 2001 From: Mohana Date: Sun, 16 Nov 2025 15:25:49 -0500 Subject: [PATCH 5/8] added formatting changes for CI --- .../launch/redundancy_check.launch.py | 16 +++++++++----- .../src/redundancy_check.py | 21 +++++++++++++------ 2 files changed, 26 insertions(+), 11 deletions(-) diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/launch/redundancy_check.launch.py b/src/subjugator/gnc/subjugator_sensor_monitoring/launch/redundancy_check.launch.py index 216892ba9..dc153f80c 100644 --- a/src/subjugator/gnc/subjugator_sensor_monitoring/launch/redundancy_check.launch.py +++ b/src/subjugator/gnc/subjugator_sensor_monitoring/launch/redundancy_check.launch.py @@ -1,13 +1,21 @@ +"""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.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( [ @@ -21,7 +29,6 @@ def generate_launch_description(): ], ), ) - # Service call to enable localization enable_localization = TimerAction( period=5.0, # Wait 2 seconds @@ -39,7 +46,6 @@ def generate_launch_description(): ), ], ) - return LaunchDescription( [ Node( diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py b/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py index e05c85489..d93fc1b73 100755 --- a/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py +++ b/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 - +"""Redundancy check node for comparing DVL and IMU sensor data.""" import rclpy import rclpy.node from geometry_msgs.msg import Vector3Stamped @@ -12,7 +12,10 @@ 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( @@ -42,27 +45,29 @@ def __init__(self): 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 # Only checking when DVL received new data self.check_redundancy() def check_redundancy(self): - + """Check redundancy between DVL and IMU data.""" # imu_transform frame id: imu_link # dvl transfor frame id: odom # No direct redundancy check. # DVL = Linear x,y,z velocities # IMU - Roll, Pitch, Yaw rates, Angular Velocity, Liner Accel x,y,z - # Differentiate DVL velocity --> compare against IMU linear acceleration + # Differentiate DVL velocity --> compare against IMU linear accel # Main redundancy check logic: if self.imu_data is None or self.dvl_data is None: return - # Calculate currenbt time from DVL data timestamp + # 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: @@ -73,7 +78,9 @@ def check_redundancy(self): dt = (current_time - self.previous_dvl_time).nanoseconds / 1e9 if dt <= 0: - self.get_logger().warn("dt is neg. or zero, skipping redundancy check.") + self.get_logger().warn( + "dt is neg. or zero, skipping redundancy check.", + ) return try: @@ -128,7 +135,8 @@ def check_redundancy(self): or diff_z > self.diff_threshold ): self.get_logger().warn( - f"Redundancy check failed: Differences - X: {diff_x}, Y: {diff_y}, Z: {diff_z}", + f"Redundancy check failed: Differences - " + f"X: {diff_x}, Y: {diff_y}, Z: {diff_z}", ) self.previous_dvl_time = current_time @@ -136,6 +144,7 @@ def check_redundancy(self): def main(args=None): + """Run the redundancy check node.""" rclpy.init(args=args) node = RedundancyCheckNode() rclpy.spin(node) From f03a65c41241dcf7d7c8fc97d73784d8847faa57 Mon Sep 17 00:00:00 2001 From: Mohana Date: Sun, 23 Nov 2025 16:03:13 -0500 Subject: [PATCH 6/8] fixed formatting --- .../scripts/monitoring_subs.py | 113 ++++++++++++++++++ .../src/redundancy_check.py | 8 +- 2 files changed, 114 insertions(+), 7 deletions(-) create mode 100644 src/subjugator/gnc/subjugator_localization/scripts/monitoring_subs.py diff --git a/src/subjugator/gnc/subjugator_localization/scripts/monitoring_subs.py b/src/subjugator/gnc/subjugator_localization/scripts/monitoring_subs.py new file mode 100644 index 000000000..40cd6b682 --- /dev/null +++ b/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() diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py b/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py index d93fc1b73..7d508fc76 100755 --- a/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py +++ b/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py @@ -51,17 +51,11 @@ def imu_callback(self, msg): def dvl_callback(self, msg): """Handle incoming DVL messages.""" self.dvl_data = msg - # Only checking when DVL received new data + self.check_redundancy() def check_redundancy(self): """Check redundancy between DVL and IMU data.""" - # imu_transform frame id: imu_link - # dvl transfor frame id: odom - # No direct redundancy check. - # DVL = Linear x,y,z velocities - # IMU - Roll, Pitch, Yaw rates, Angular Velocity, Liner Accel x,y,z - # Differentiate DVL velocity --> compare against IMU linear accel # Main redundancy check logic: if self.imu_data is None or self.dvl_data is None: From 5d3ca223217cd46adffca82301b054ee66e843c4 Mon Sep 17 00:00:00 2001 From: Mohana Date: Sun, 23 Nov 2025 16:43:04 -0500 Subject: [PATCH 7/8] Fixed all format for CI --- src/subjugator/gnc/subjugator_sensor_monitoring/package.xml | 1 - .../gnc/subjugator_sensor_monitoring/src/redundancy_check.py | 1 - 2 files changed, 2 deletions(-) diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/package.xml b/src/subjugator/gnc/subjugator_sensor_monitoring/package.xml index 0985b0069..c381c3ad1 100644 --- a/src/subjugator/gnc/subjugator_sensor_monitoring/package.xml +++ b/src/subjugator/gnc/subjugator_sensor_monitoring/package.xml @@ -1,5 +1,4 @@ - subjugator_sensor_monitoring 0.0.0 diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py b/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py index 7d508fc76..2eef422d6 100755 --- a/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py +++ b/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py @@ -56,7 +56,6 @@ def dvl_callback(self, msg): 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 From d4bb85e3be1400713fbc235c67bba75c13c387c4 Mon Sep 17 00:00:00 2001 From: Mohana Date: Sun, 23 Nov 2025 20:34:29 -0500 Subject: [PATCH 8/8] Added missing dependencies --- src/subjugator/gnc/subjugator_sensor_monitoring/CMakeLists.txt | 2 ++ src/subjugator/gnc/subjugator_sensor_monitoring/package.xml | 3 +++ .../gnc/subjugator_sensor_monitoring/src/redundancy_check.py | 1 + 3 files changed, 6 insertions(+) diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/CMakeLists.txt b/src/subjugator/gnc/subjugator_sensor_monitoring/CMakeLists.txt index f1606aea8..3e212a97f 100644 --- a/src/subjugator/gnc/subjugator_sensor_monitoring/CMakeLists.txt +++ b/src/subjugator/gnc/subjugator_sensor_monitoring/CMakeLists.txt @@ -8,6 +8,8 @@ endif() # find dependencies find_package(ament_cmake 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}) diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/package.xml b/src/subjugator/gnc/subjugator_sensor_monitoring/package.xml index c381c3ad1..bc91ec58b 100644 --- a/src/subjugator/gnc/subjugator_sensor_monitoring/package.xml +++ b/src/subjugator/gnc/subjugator_sensor_monitoring/package.xml @@ -12,6 +12,9 @@ ament_lint_common rclpy + geometry_msgs + tf2_geometry_msgs + tf2_ros ament_cmake diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py b/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py index 2eef422d6..3bcddc0fc 100755 --- a/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py +++ b/src/subjugator/gnc/subjugator_sensor_monitoring/src/redundancy_check.py @@ -2,6 +2,7 @@ """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