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 00000000..40cd6b68 --- /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/CMakeLists.txt b/src/subjugator/gnc/subjugator_sensor_monitoring/CMakeLists.txt index deebd363..3e212a97 100644 --- a/src/subjugator/gnc/subjugator_sensor_monitoring/CMakeLists.txt +++ b/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( 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) 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 00000000..dc153f80 --- /dev/null +++ b/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, + ], + ) diff --git a/src/subjugator/gnc/subjugator_sensor_monitoring/package.xml b/src/subjugator/gnc/subjugator_sensor_monitoring/package.xml index fc93455b..bc91ec58 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 @@ -12,6 +11,11 @@ ament_lint_auto 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 new file mode 100755 index 00000000..3bcddc0f --- /dev/null +++ b/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() 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 b9ba3046..00000000 --- a/src/subjugator/gnc/subjugator_sensor_monitoring/src/test.py +++ /dev/null @@ -1 +0,0 @@ -# changes to get CI to run again after PR.