From a9abe370e792cb964a3359c0d45ca5ec469b9eee Mon Sep 17 00:00:00 2001 From: Zarqu0n Date: Wed, 7 Jan 2026 13:54:29 +0300 Subject: [PATCH] Add IMU anomaly detection parameters and functionality --- bno055/params/NodeParameters.py | 15 ++++ bno055/sensor/SensorService.py | 143 +++++++++++++++++++++++++++++++- 2 files changed, 155 insertions(+), 3 deletions(-) diff --git a/bno055/params/NodeParameters.py b/bno055/params/NodeParameters.py index 947fdaf..eeff00f 100644 --- a/bno055/params/NodeParameters.py +++ b/bno055/params/NodeParameters.py @@ -101,6 +101,12 @@ def __init__(self, node: Node): node.declare_parameter('variance_orientation', value=registers.DEFAULT_VARIANCE_ORIENTATION) node.declare_parameter('variance_mag', value=registers.DEFAULT_VARIANCE_MAG) + # IMU anomaly detection parameters + node.declare_parameter('imu_change_epsilon', value=0.0001) + node.declare_parameter('imu_constant_threshold', value=100) + node.declare_parameter('imu_history_size', value=20) + node.declare_parameter('max_std_dev_threshold', value=1.5) + # get the parameters - requires CLI arguments '--ros-args --params-file ' node.get_logger().info('Parameters set to:') @@ -186,6 +192,15 @@ def __init__(self, node: Node): node.get_logger().info('\tvariance_orientation:\t"%s"' % self.variance_orientation.value) self.variance_mag = node.get_parameter('variance_mag') node.get_logger().info('\tvariance_mag:\t\t"%s"' % self.variance_mag.value) + + self.imu_change_epsilon = node.get_parameter('imu_change_epsilon') + node.get_logger().info('\timu_change_epsilon:\t"%s"' % self.imu_change_epsilon.value) + self.imu_constant_threshold = node.get_parameter('imu_constant_threshold') + node.get_logger().info('\timu_constant_threshold:\t"%s"' % self.imu_constant_threshold.value) + self.imu_history_size = node.get_parameter('imu_history_size') + node.get_logger().info('\timu_history_size:\t\t"%s"' % self.imu_history_size.value) + self.max_std_dev_threshold = node.get_parameter('max_std_dev_threshold') + node.get_logger().info('\tmax_std_dev_threshold:\t"%s"' % self.max_std_dev_threshold.value) except Exception as e: # noqa: B902 node.get_logger().warn('Could not get parameters...setting variables to default') node.get_logger().warn('Error: "%s"' % e) diff --git a/bno055/sensor/SensorService.py b/bno055/sensor/SensorService.py index 10f1cf3..07da1cf 100644 --- a/bno055/sensor/SensorService.py +++ b/bno055/sensor/SensorService.py @@ -29,7 +29,7 @@ from math import sqrt import struct import sys -from time import sleep +from time import sleep, time from bno055 import registers from bno055.connectors.Connector import Connector @@ -37,10 +37,11 @@ from geometry_msgs.msg import Quaternion, Vector3 from rclpy.node import Node -from rclpy.qos import QoSProfile +from rclpy.qos import QoSProfile, DurabilityPolicy, HistoryPolicy from sensor_msgs.msg import Imu, MagneticField, Temperature -from std_msgs.msg import String +from std_msgs.msg import String, Bool from example_interfaces.srv import Trigger +from robot_msgs.msg import Log, RosTopicsConfig class SensorService: @@ -53,6 +54,11 @@ def __init__(self, node: Node, connector: Connector, param: NodeParameters): prefix = self.param.ros_topic_prefix.value QoSProf = QoSProfile(depth=10) + LatchQoSProf = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST + ) # create topic publishers: self.pub_imu_raw = node.create_publisher(Imu, prefix + 'imu_raw', QoSProf) @@ -61,8 +67,30 @@ def __init__(self, node: Node, connector: Connector, param: NodeParameters): self.pub_grav = node.create_publisher(Vector3, prefix + 'grav', QoSProf) self.pub_temp = node.create_publisher(Temperature, prefix + 'temp', QoSProf) self.pub_calib_status = node.create_publisher(String, prefix + 'calib_status', QoSProf) + self.pub_imu_ok = node.create_publisher(Bool, prefix + 'imu_ok', LatchQoSProf) + self.pub_global_log = node.create_publisher(Log, RosTopicsConfig.LOGGING, 10) self.srv = self.node.create_service(Trigger, prefix + 'calibration_request', self.calibration_request_callback) + # IMU anomaly detection state variables + self.prev_accel_x = 0.0 + self.prev_accel_y = 0.0 + self.prev_accel_z = 0.0 + self.prev_gyro_x = 0.0 + self.prev_gyro_y = 0.0 + self.prev_gyro_z = 0.0 + self.prev_imu_time = None + self.imu_constant_count = 0 + self.imu_ok = True + self.prev_imu_ok = True + + # History buffers for std dev calculation + self.accel_x_history = [] + self.accel_y_history = [] + self.accel_z_history = [] + + # Log throttle map + self.log_throttle_map = {} + def configure(self): """Configure the IMU sensor hardware.""" self.node.get_logger().info('Configuring device...') @@ -255,6 +283,115 @@ def get_sensor_data(self): temp_msg.temperature = float(buf[44]) self.pub_temp.publish(temp_msg) + # IMU anomaly detection + self._check_imu_anomalies(imu_msg) + + def publish_log_throttled(self, log_key, log_message, log_type, timeout_sec): + current_time = time() + if log_key not in self.log_throttle_map or (current_time - self.log_throttle_map[log_key]) >= timeout_sec: + # Console logging + if log_type == Log.WARN: + self.node.get_logger().warn(log_message) + elif log_type == Log.ERROR: + self.node.get_logger().error(log_message) + else: + self.node.get_logger().info(log_message) + + # Topic logging + log_msg = Log() + log_msg.node = "bno055" # or self.node.get_name() but self.node.get_name() might be fully qualified + log_msg.type = log_type + log_msg.log = log_message + self.pub_global_log.publish(log_msg) + + self.log_throttle_map[log_key] = current_time + + def _check_imu_anomalies(self, imu_msg): + """Check for IMU anomalies and publish imu_ok status.""" + current_time = time() + + if self.prev_imu_time is not None: + # Check if IMU data is constant + is_constant = ( + abs(imu_msg.linear_acceleration.x - self.prev_accel_x) < self.param.imu_change_epsilon.value and + abs(imu_msg.linear_acceleration.y - self.prev_accel_y) < self.param.imu_change_epsilon.value and + abs(imu_msg.linear_acceleration.z - self.prev_accel_z) < self.param.imu_change_epsilon.value and + abs(imu_msg.angular_velocity.x - self.prev_gyro_x) < self.param.imu_change_epsilon.value and + abs(imu_msg.angular_velocity.y - self.prev_gyro_y) < self.param.imu_change_epsilon.value and + abs(imu_msg.angular_velocity.z - self.prev_gyro_z) < self.param.imu_change_epsilon.value + ) + + if is_constant: + self.imu_constant_count += 1 + if self.imu_constant_count >= self.param.imu_constant_threshold.value: + freq = self.param.data_query_frequency.value if hasattr(self.param, 'data_query_frequency') else 100.0 + msg = ( + f"IMU anomaly detected: Data constant for {self.imu_constant_count / freq:.2f} seconds " + f"(accel: {imu_msg.linear_acceleration.x:.3f}, {imu_msg.linear_acceleration.y:.3f}, {imu_msg.linear_acceleration.z:.3f} | " + f"gyro: {imu_msg.angular_velocity.x:.3f}, {imu_msg.angular_velocity.y:.3f}, {imu_msg.angular_velocity.z:.3f})" + ) + self.publish_log_throttled("imu_constant", msg, Log.WARN, 5.0) + + self.imu_ok = False + else: + self.imu_constant_count = 0 + self.imu_ok = True + + # Check for abnormal acceleration values using standard deviation + self.accel_x_history.append(imu_msg.linear_acceleration.x) + self.accel_y_history.append(imu_msg.linear_acceleration.y) + self.accel_z_history.append(imu_msg.linear_acceleration.z) + + if len(self.accel_x_history) > self.param.imu_history_size.value: + self.accel_x_history.pop(0) + self.accel_y_history.pop(0) + self.accel_z_history.pop(0) + + if len(self.accel_x_history) >= self.param.imu_history_size.value // 2: + # Calculate mean + mean_x = sum(self.accel_x_history) / len(self.accel_x_history) + mean_y = sum(self.accel_y_history) / len(self.accel_y_history) + mean_z = sum(self.accel_z_history) / len(self.accel_z_history) + + # Calculate variance + variance_x = sum((x - mean_x) ** 2 for x in self.accel_x_history) / len(self.accel_x_history) + variance_y = sum((y - mean_y) ** 2 for y in self.accel_y_history) / len(self.accel_y_history) + variance_z = sum((z - mean_z) ** 2 for z in self.accel_z_history) / len(self.accel_z_history) + + # Calculate standard deviation + std_dev_x = sqrt(variance_x) + std_dev_y = sqrt(variance_y) + std_dev_z = sqrt(variance_z) + + if (std_dev_x > self.param.max_std_dev_threshold.value or + std_dev_y > self.param.max_std_dev_threshold.value or + std_dev_z > self.param.max_std_dev_threshold.value): + + msg = ( + f"IMU anomaly detected: Standard deviation too high (noisy/broken data). " + f"Std Dev: ({std_dev_x:.4f}, {std_dev_y:.4f}, {std_dev_z:.4f}) | " + f"Threshold: {self.param.max_std_dev_threshold.value:.2f} m/s² | " + f"Mean: ({mean_x:.3f}, {mean_y:.3f}, {mean_z:.3f})" + ) + self.publish_log_throttled("imu_std_dev", msg, Log.WARN, 5.0) + self.imu_ok = False + + # Update previous values + self.prev_accel_x = imu_msg.linear_acceleration.x + self.prev_accel_y = imu_msg.linear_acceleration.y + self.prev_accel_z = imu_msg.linear_acceleration.z + self.prev_gyro_x = imu_msg.angular_velocity.x + self.prev_gyro_y = imu_msg.angular_velocity.y + self.prev_gyro_z = imu_msg.angular_velocity.z + self.prev_imu_time = current_time + + # Publish imu_ok status only when it changes + if self.imu_ok != self.prev_imu_ok: + imu_ok_msg = Bool() + imu_ok_msg.data = self.imu_ok + self.pub_imu_ok.publish(imu_ok_msg) + self.prev_imu_ok = self.imu_ok + def get_calib_status(self): """ Read calibration status for sys/gyro/acc/mag.