From c0f26113297a0450419f8048003735f2672b00f9 Mon Sep 17 00:00:00 2001 From: Sergei Grichine Date: Fri, 7 Nov 2025 10:07:02 -0600 Subject: [PATCH 01/11] Using smbus2, detecting and ignoring large jumps in orientation data --- bno055/bno055.py | 2 ++ bno055/connectors/i2c.py | 2 +- bno055/sensor/SensorService.py | 62 +++++++++++++++++++++++----------- package.xml | 2 +- 4 files changed, 46 insertions(+), 22 deletions(-) diff --git a/bno055/bno055.py b/bno055/bno055.py index 950a863..970998d 100755 --- a/bno055/bno055.py +++ b/bno055/bno055.py @@ -111,9 +111,11 @@ def read_data(): node.sensor.get_sensor_data() except BusOverRunException: # data not available yet, wait for next cycle | see #5 + node.get_logger().warn('Receiving sensor data failed: BusOverRunException') return except ZeroDivisionError: # division by zero in get_sensor_data, return + node.get_logger().warn('Receiving sensor data failed: ZeroDivisionError') return except Exception as e: # noqa: B902 node.get_logger().warn('Receiving sensor data failed with %s:"%s"' diff --git a/bno055/connectors/i2c.py b/bno055/connectors/i2c.py index d71e5c2..0d5be08 100644 --- a/bno055/connectors/i2c.py +++ b/bno055/connectors/i2c.py @@ -27,7 +27,7 @@ # POSSIBILITY OF SUCH DAMAGE. -from smbus import SMBus +from smbus2 import SMBus from rclpy.node import Node diff --git a/bno055/sensor/SensorService.py b/bno055/sensor/SensorService.py index 10f1cf3..f76e23f 100644 --- a/bno055/sensor/SensorService.py +++ b/bno055/sensor/SensorService.py @@ -35,7 +35,8 @@ from bno055.connectors.Connector import Connector from bno055.params.NodeParameters import NodeParameters -from geometry_msgs.msg import Quaternion, Vector3 +from geometry_msgs.msg import Vector3 +from tf_transformations import unit_vector from rclpy.node import Node from rclpy.qos import QoSProfile from sensor_msgs.msg import Imu, MagneticField, Temperature @@ -63,6 +64,8 @@ def __init__(self, node: Node, connector: Connector, param: NodeParameters): self.pub_calib_status = node.create_publisher(String, prefix + 'calib_status', QoSProf) self.srv = self.node.create_service(Trigger, prefix + 'calibration_request', self.calibration_request_callback) + self.prev_w = -1000.0 # store last valid reading for jump detection + def configure(self): """Configure the IMU sensor hardware.""" self.node.get_logger().info('Configuring device...') @@ -183,27 +186,37 @@ def get_sensor_data(self): 0.0, self.param.variance_angular_vel.value[1], 0.0, 0.0, 0.0, self.param.variance_angular_vel.value[2] ] - # node.get_logger().info('Publishing imu message') - self.pub_imu_raw.publish(imu_raw_msg) # TODO: make this an option to publish? # Publish filtered data imu_msg.header.stamp = self.node.get_clock().now().to_msg() imu_msg.header.frame_id = self.param.frame_id.value - q = Quaternion() - # imu_msg.header.seq = seq - q.w = self.unpackBytesToFloat(buf[24], buf[25]) - q.x = self.unpackBytesToFloat(buf[26], buf[27]) - q.y = self.unpackBytesToFloat(buf[28], buf[29]) - q.z = self.unpackBytesToFloat(buf[30], buf[31]) - # TODO(flynneva): replace with standard normalize() function - # normalize - norm = sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w) - imu_msg.orientation.x = q.x / norm - imu_msg.orientation.y = q.y / norm - imu_msg.orientation.z = q.z / norm - imu_msg.orientation.w = q.w / norm + # q = Quaternion() + # # imu_msg.header.seq = seq + # q.w = self.unpackBytesToFloat(buf[24], buf[25]) + # q.x = self.unpackBytesToFloat(buf[26], buf[27]) + # q.y = self.unpackBytesToFloat(buf[28], buf[29]) + # q.z = self.unpackBytesToFloat(buf[30], buf[31]) + # # TODO(flynneva): replace with standard normalize() function + # # normalize + # norm = sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w) + # imu_msg.orientation.x = q.x / norm + # imu_msg.orientation.y = q.y / norm + # imu_msg.orientation.z = q.z / norm + # w = imu_msg.orientation.w = q.w / norm + + q = [ + self.unpackBytesToFloat(buf[26], buf[27]), + self.unpackBytesToFloat(buf[28], buf[29]), + self.unpackBytesToFloat(buf[30], buf[31]), + self.unpackBytesToFloat(buf[24], buf[25]) + ] + q = unit_vector(q) + + imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z, imu_msg.orientation.w = q + w = imu_msg.orientation.w + w_jump = w - self.prev_w imu_msg.orientation_covariance = imu_raw_msg.orientation_covariance @@ -221,7 +234,6 @@ def get_sensor_data(self): imu_msg.angular_velocity.z = \ self.unpackBytesToFloat(buf[16], buf[17]) / self.param.gyr_factor.value imu_msg.angular_velocity_covariance = imu_raw_msg.angular_velocity_covariance - self.pub_imu.publish(imu_msg) # Publish magnetometer data mag_msg.header.stamp = self.node.get_clock().now().to_msg() @@ -238,7 +250,6 @@ def get_sensor_data(self): 0.0, self.param.variance_mag.value[1], 0.0, 0.0, 0.0, self.param.variance_mag.value[2] ] - self.pub_mag.publish(mag_msg) grav_msg.x = \ self.unpackBytesToFloat(buf[38], buf[39]) / self.param.grav_factor.value @@ -246,14 +257,25 @@ def get_sensor_data(self): self.unpackBytesToFloat(buf[40], buf[41]) / self.param.grav_factor.value grav_msg.z = \ self.unpackBytesToFloat(buf[42], buf[43]) / self.param.grav_factor.value - self.pub_grav.publish(grav_msg) # Publish temperature temp_msg.header.stamp = self.node.get_clock().now().to_msg() temp_msg.header.frame_id = self.param.frame_id.value # temp_msg.header.seq = seq temp_msg.temperature = float(buf[44]) - self.pub_temp.publish(temp_msg) + + # ignore large jumps in orientation data - skip the cycle if detected: + if abs(w_jump) < 0.1: + # self.node.get_logger().info('Publishing imu messages') + self.pub_imu_raw.publish(imu_raw_msg) + self.pub_imu.publish(imu_msg) + self.pub_mag.publish(mag_msg) + self.pub_grav.publish(grav_msg) + self.pub_temp.publish(temp_msg) + #else: + # self.node.get_logger().warn(f"Skipped publishing due to large jump: {w_jump:+.3f}") + + self.prev_w = w def get_calib_status(self): """ diff --git a/package.xml b/package.xml index 453898a..c30c36e 100644 --- a/package.xml +++ b/package.xml @@ -11,7 +11,7 @@ python3-serial - python3-smbus + python3-smbus2 rclpy From c8009b7ac5c1f0567dd1d248fad91820f8355db1 Mon Sep 17 00:00:00 2001 From: Sergei Grichine Date: Fri, 7 Nov 2025 15:34:32 -0600 Subject: [PATCH 02/11] publish_if_valid() as a separate method. Better peak detection --- bno055/sensor/SensorService.py | 97 +++++++++++++++++++++++++++++----- 1 file changed, 83 insertions(+), 14 deletions(-) diff --git a/bno055/sensor/SensorService.py b/bno055/sensor/SensorService.py index f76e23f..1939fbf 100644 --- a/bno055/sensor/SensorService.py +++ b/bno055/sensor/SensorService.py @@ -64,7 +64,13 @@ def __init__(self, node: Node, connector: Connector, param: NodeParameters): self.pub_calib_status = node.create_publisher(String, prefix + 'calib_status', QoSProf) self.srv = self.node.create_service(Trigger, prefix + 'calibration_request', self.calibration_request_callback) - self.prev_w = -1000.0 # store last valid reading for jump detection + # Jump detection state variables: + self.prev_w = None # last accepted (published) value + self.candidate_w = None # tentative new value + self.candidate_count = 0 # consecutive readings matching candidate + self.THRESHOLD = 0.01 # jump threshold + self.PERSIST_COUNT = 3 # readings needed to accept candidate + self.CANDIDATE_TOL = 0.02 # tolerance for candidate stability (optional) def configure(self): """Configure the IMU sensor hardware.""" @@ -140,6 +146,81 @@ def configure(self): self.node.get_logger().info('Bosch BNO055 IMU configuration complete.') + # + # The sensor reading often contains invalid large jumps of the orientation w component. + # To mitigate this, we implement a simple jump detection and filtering logic. + # Publish only if the reading is valid according to jump detection logic + # + def publish_if_valid(self, w, imu_raw_msg, imu_msg, mag_msg, grav_msg, temp_msg): + # first-time case: accept and publish immediately + if self.prev_w is None: + self.prev_w = w + self.publish_all(imu_raw_msg, imu_msg, mag_msg, grav_msg, temp_msg) + # reset any candidate state + self.candidate_w = None + self.candidate_count = 0 + return + + # difference from last published value + w_jump = w - self.prev_w + + # If within threshold of last published value -> normal reading, publish + if abs(w_jump) <= self.THRESHOLD: + self.publish_all(imu_raw_msg, imu_msg, mag_msg, grav_msg, temp_msg) + # accept this as the new published value (keep publishing incremental changes) + self.prev_w = w + # reset candidate monitoring + self.candidate_w = None + self.candidate_count = 0 + return + + # At this point: large jump relative to prev_w — candidate handling + # If we have no candidate yet, start one + if self.candidate_w is None: + self.candidate_w = w + self.candidate_count = 1 + self.node.get_logger().warn( + f"Large jump detected: Δw={w_jump:+.3f}. Starting candidate monitoring." + ) + # DO NOT update prev_w; do NOT publish + return + + # We already have a candidate: check whether the new reading matches it (within tolerance) + if abs(w - self.candidate_w) <= self.CANDIDATE_TOL: + self.candidate_count += 1 + self.node.get_logger().debug( + f"Candidate stable count={self.candidate_count} (candidate={self.candidate_w:+.4f})" + ) + # If candidate persisted long enough, accept it + if self.candidate_count >= self.PERSIST_COUNT: + self.node.get_logger().info( + f"Accepted new stable value after {self.candidate_count} reads: {self.candidate_w:+.3f}" + ) + # Accept candidate: publish and update prev_w + # Note: you may want to set prev_w to candidate_w or to the current w (they are close) + self.prev_w = self.candidate_w + self.publish_all(imu_raw_msg, imu_msg, mag_msg, grav_msg, temp_msg) + # reset candidate state + self.candidate_w = None + self.candidate_count = 0 + else: + # Candidate didn't hold — restart candidate with the newest reading + self.node.get_logger().warn( + f"Candidate lost (new reading {w:+.4f} deviates from candidate {self.candidate_w:+.4f}). Resetting candidate." + ) + self.candidate_w = w + self.candidate_count = 1 + # still do not publish + + # helper to centralize publishing + def publish_all(self, imu_raw_msg, imu_msg, mag_msg, grav_msg, temp_msg): + self.pub_imu_raw.publish(imu_raw_msg) + self.pub_imu.publish(imu_msg) + self.pub_mag.publish(mag_msg) + self.pub_grav.publish(grav_msg) + self.pub_temp.publish(temp_msg) + + def get_sensor_data(self): """Read IMU data from the sensor, parse and publish.""" # Initialize ROS msgs @@ -216,7 +297,6 @@ def get_sensor_data(self): imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z, imu_msg.orientation.w = q w = imu_msg.orientation.w - w_jump = w - self.prev_w imu_msg.orientation_covariance = imu_raw_msg.orientation_covariance @@ -264,18 +344,7 @@ def get_sensor_data(self): # temp_msg.header.seq = seq temp_msg.temperature = float(buf[44]) - # ignore large jumps in orientation data - skip the cycle if detected: - if abs(w_jump) < 0.1: - # self.node.get_logger().info('Publishing imu messages') - self.pub_imu_raw.publish(imu_raw_msg) - self.pub_imu.publish(imu_msg) - self.pub_mag.publish(mag_msg) - self.pub_grav.publish(grav_msg) - self.pub_temp.publish(temp_msg) - #else: - # self.node.get_logger().warn(f"Skipped publishing due to large jump: {w_jump:+.3f}") - - self.prev_w = w + self.publish_if_valid(w, imu_raw_msg, imu_msg, mag_msg, grav_msg, temp_msg) def get_calib_status(self): """ From 73e627894ecfdcedc38720406eb93e5f21fc06b5 Mon Sep 17 00:00:00 2001 From: Sergei Grichine Date: Fri, 7 Nov 2025 16:51:02 -0600 Subject: [PATCH 03/11] Update SensorService.py --- bno055/sensor/SensorService.py | 39 +++++++++++++++++++++++++++------- 1 file changed, 31 insertions(+), 8 deletions(-) diff --git a/bno055/sensor/SensorService.py b/bno055/sensor/SensorService.py index 1939fbf..4a07237 100644 --- a/bno055/sensor/SensorService.py +++ b/bno055/sensor/SensorService.py @@ -36,7 +36,6 @@ from bno055.params.NodeParameters import NodeParameters from geometry_msgs.msg import Vector3 -from tf_transformations import unit_vector from rclpy.node import Node from rclpy.qos import QoSProfile from sensor_msgs.msg import Imu, MagneticField, Temperature @@ -65,10 +64,11 @@ def __init__(self, node: Node, connector: Connector, param: NodeParameters): self.srv = self.node.create_service(Trigger, prefix + 'calibration_request', self.calibration_request_callback) # Jump detection state variables: + self.prev_norm = None # last computed raw quaternion norm self.prev_w = None # last accepted (published) value self.candidate_w = None # tentative new value self.candidate_count = 0 # consecutive readings matching candidate - self.THRESHOLD = 0.01 # jump threshold + self.THRESHOLD = 0.02 # jump threshold self.PERSIST_COUNT = 3 # readings needed to accept candidate self.CANDIDATE_TOL = 0.02 # tolerance for candidate stability (optional) @@ -287,13 +287,34 @@ def get_sensor_data(self): # imu_msg.orientation.z = q.z / norm # w = imu_msg.orientation.w = q.w / norm + # Quaternion: q = [ - self.unpackBytesToFloat(buf[26], buf[27]), - self.unpackBytesToFloat(buf[28], buf[29]), - self.unpackBytesToFloat(buf[30], buf[31]), - self.unpackBytesToFloat(buf[24], buf[25]) + self.unpackBytesToFloat(buf[26], buf[27]), # x + self.unpackBytesToFloat(buf[28], buf[29]), # y + self.unpackBytesToFloat(buf[30], buf[31]), # z + self.unpackBytesToFloat(buf[24], buf[25]) # w ] - q = unit_vector(q) + + can_publish = False + + # Compute norm safely + norm = sqrt(q[0]**2 + q[1]**2 + q[2]**2 + q[3]**2) + if norm < 1e-6: + self.node.get_logger().warn("Invalid quaternion (zero norm) — skipping normalization.") + # Set default neutral quaternion (no rotation) + q = [0.0, 0.0, 0.0, 1.0] + else: + q = [x / norm for x in q] + if self.prev_norm is None: # first good value + self.prev_norm = norm + + if self.prev_norm is not None: + norm_jump = norm - self.prev_norm + if abs(norm_jump) > self.prev_norm * 0.05: # 5% jump + self.node.get_logger().warn("Large jump in quaternion norm detected: norm: {} prev: {} jump: {}".format(norm, self.prev_norm, norm_jump)) + else: + can_publish = True + self.prev_norm = norm imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z, imu_msg.orientation.w = q w = imu_msg.orientation.w @@ -344,7 +365,9 @@ def get_sensor_data(self): # temp_msg.header.seq = seq temp_msg.temperature = float(buf[44]) - self.publish_if_valid(w, imu_raw_msg, imu_msg, mag_msg, grav_msg, temp_msg) + if can_publish: + self.publish_all(imu_raw_msg, imu_msg, mag_msg, grav_msg, temp_msg) + #self.publish_if_valid(w, imu_raw_msg, imu_msg, mag_msg, grav_msg, temp_msg) def get_calib_status(self): """ From d17acb8cdd612bed9b6263f4e7c08f598f5b8c8a Mon Sep 17 00:00:00 2001 From: Sergei Grichine Date: Fri, 7 Nov 2025 17:27:31 -0600 Subject: [PATCH 04/11] Sanity check - jump detection based solely on 5% quaternion norm change --- bno055/sensor/SensorService.py | 173 +++++++++------------------------ 1 file changed, 46 insertions(+), 127 deletions(-) diff --git a/bno055/sensor/SensorService.py b/bno055/sensor/SensorService.py index 4a07237..ca05120 100644 --- a/bno055/sensor/SensorService.py +++ b/bno055/sensor/SensorService.py @@ -63,14 +63,14 @@ def __init__(self, node: Node, connector: Connector, param: NodeParameters): self.pub_calib_status = node.create_publisher(String, prefix + 'calib_status', QoSProf) self.srv = self.node.create_service(Trigger, prefix + 'calibration_request', self.calibration_request_callback) - # Jump detection state variables: + # + # The sensor reading often contains invalid large jumps of the orientation w component. + # To mitigate this, we implement a simple jump detection and filtering logic. + # Publish only if the reading is valid according to jump detection logic + # + + # Jump detection state variable: self.prev_norm = None # last computed raw quaternion norm - self.prev_w = None # last accepted (published) value - self.candidate_w = None # tentative new value - self.candidate_count = 0 # consecutive readings matching candidate - self.THRESHOLD = 0.02 # jump threshold - self.PERSIST_COUNT = 3 # readings needed to accept candidate - self.CANDIDATE_TOL = 0.02 # tolerance for candidate stability (optional) def configure(self): """Configure the IMU sensor hardware.""" @@ -146,80 +146,6 @@ def configure(self): self.node.get_logger().info('Bosch BNO055 IMU configuration complete.') - # - # The sensor reading often contains invalid large jumps of the orientation w component. - # To mitigate this, we implement a simple jump detection and filtering logic. - # Publish only if the reading is valid according to jump detection logic - # - def publish_if_valid(self, w, imu_raw_msg, imu_msg, mag_msg, grav_msg, temp_msg): - # first-time case: accept and publish immediately - if self.prev_w is None: - self.prev_w = w - self.publish_all(imu_raw_msg, imu_msg, mag_msg, grav_msg, temp_msg) - # reset any candidate state - self.candidate_w = None - self.candidate_count = 0 - return - - # difference from last published value - w_jump = w - self.prev_w - - # If within threshold of last published value -> normal reading, publish - if abs(w_jump) <= self.THRESHOLD: - self.publish_all(imu_raw_msg, imu_msg, mag_msg, grav_msg, temp_msg) - # accept this as the new published value (keep publishing incremental changes) - self.prev_w = w - # reset candidate monitoring - self.candidate_w = None - self.candidate_count = 0 - return - - # At this point: large jump relative to prev_w — candidate handling - # If we have no candidate yet, start one - if self.candidate_w is None: - self.candidate_w = w - self.candidate_count = 1 - self.node.get_logger().warn( - f"Large jump detected: Δw={w_jump:+.3f}. Starting candidate monitoring." - ) - # DO NOT update prev_w; do NOT publish - return - - # We already have a candidate: check whether the new reading matches it (within tolerance) - if abs(w - self.candidate_w) <= self.CANDIDATE_TOL: - self.candidate_count += 1 - self.node.get_logger().debug( - f"Candidate stable count={self.candidate_count} (candidate={self.candidate_w:+.4f})" - ) - # If candidate persisted long enough, accept it - if self.candidate_count >= self.PERSIST_COUNT: - self.node.get_logger().info( - f"Accepted new stable value after {self.candidate_count} reads: {self.candidate_w:+.3f}" - ) - # Accept candidate: publish and update prev_w - # Note: you may want to set prev_w to candidate_w or to the current w (they are close) - self.prev_w = self.candidate_w - self.publish_all(imu_raw_msg, imu_msg, mag_msg, grav_msg, temp_msg) - # reset candidate state - self.candidate_w = None - self.candidate_count = 0 - else: - # Candidate didn't hold — restart candidate with the newest reading - self.node.get_logger().warn( - f"Candidate lost (new reading {w:+.4f} deviates from candidate {self.candidate_w:+.4f}). Resetting candidate." - ) - self.candidate_w = w - self.candidate_count = 1 - # still do not publish - - # helper to centralize publishing - def publish_all(self, imu_raw_msg, imu_msg, mag_msg, grav_msg, temp_msg): - self.pub_imu_raw.publish(imu_raw_msg) - self.pub_imu.publish(imu_msg) - self.pub_mag.publish(mag_msg) - self.pub_grav.publish(grav_msg) - self.pub_temp.publish(temp_msg) - def get_sensor_data(self): """Read IMU data from the sensor, parse and publish.""" @@ -238,6 +164,39 @@ def get_sensor_data(self): # TODO: do headers need sequence counters now? # imu_raw_msg.header.seq = seq + # + # Sanity check - compute quaternion norm and see if it is valid + # + + # Quaternion: + q = [ + self.unpackBytesToFloat(buf[26], buf[27]), # x + self.unpackBytesToFloat(buf[28], buf[29]), # y + self.unpackBytesToFloat(buf[30], buf[31]), # z + self.unpackBytesToFloat(buf[24], buf[25]) # w + ] + + # Compute norm safely, return if anything wrong: + norm = sqrt(q[0]**2 + q[1]**2 + q[2]**2 + q[3]**2) + if abs(norm - 16000.0) > 1000.0: + # small norm - invalid quaternion. It should be usually ~16000, as values are large. + self.node.get_logger().warn("Invalid quaternion norm: {} — sensor reading ignored".format(norm)) + return + else: + q = [x / norm for x in q] + if self.prev_norm is None: # first good value + self.prev_norm = norm + + if self.prev_norm is not None: + norm_jump = norm - self.prev_norm + if abs(norm_jump) > self.prev_norm * 0.05: # 5% jump + self.node.get_logger().warn("Large jump in quaternion norm detected: norm: {} prev: {} jump: {}".format(norm, self.prev_norm, norm_jump)) + return + else: + self.prev_norm = norm # first good reading + + # OK, sanity check passed, we are good to publish the data + # TODO: make this an option to publish? imu_raw_msg.orientation_covariance = [ self.param.variance_orientation.value[0], 0.0, 0.0, @@ -273,49 +232,6 @@ def get_sensor_data(self): imu_msg.header.stamp = self.node.get_clock().now().to_msg() imu_msg.header.frame_id = self.param.frame_id.value - # q = Quaternion() - # # imu_msg.header.seq = seq - # q.w = self.unpackBytesToFloat(buf[24], buf[25]) - # q.x = self.unpackBytesToFloat(buf[26], buf[27]) - # q.y = self.unpackBytesToFloat(buf[28], buf[29]) - # q.z = self.unpackBytesToFloat(buf[30], buf[31]) - # # TODO(flynneva): replace with standard normalize() function - # # normalize - # norm = sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w) - # imu_msg.orientation.x = q.x / norm - # imu_msg.orientation.y = q.y / norm - # imu_msg.orientation.z = q.z / norm - # w = imu_msg.orientation.w = q.w / norm - - # Quaternion: - q = [ - self.unpackBytesToFloat(buf[26], buf[27]), # x - self.unpackBytesToFloat(buf[28], buf[29]), # y - self.unpackBytesToFloat(buf[30], buf[31]), # z - self.unpackBytesToFloat(buf[24], buf[25]) # w - ] - - can_publish = False - - # Compute norm safely - norm = sqrt(q[0]**2 + q[1]**2 + q[2]**2 + q[3]**2) - if norm < 1e-6: - self.node.get_logger().warn("Invalid quaternion (zero norm) — skipping normalization.") - # Set default neutral quaternion (no rotation) - q = [0.0, 0.0, 0.0, 1.0] - else: - q = [x / norm for x in q] - if self.prev_norm is None: # first good value - self.prev_norm = norm - - if self.prev_norm is not None: - norm_jump = norm - self.prev_norm - if abs(norm_jump) > self.prev_norm * 0.05: # 5% jump - self.node.get_logger().warn("Large jump in quaternion norm detected: norm: {} prev: {} jump: {}".format(norm, self.prev_norm, norm_jump)) - else: - can_publish = True - self.prev_norm = norm - imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z, imu_msg.orientation.w = q w = imu_msg.orientation.w @@ -365,9 +281,12 @@ def get_sensor_data(self): # temp_msg.header.seq = seq temp_msg.temperature = float(buf[44]) - if can_publish: - self.publish_all(imu_raw_msg, imu_msg, mag_msg, grav_msg, temp_msg) - #self.publish_if_valid(w, imu_raw_msg, imu_msg, mag_msg, grav_msg, temp_msg) + self.pub_imu_raw.publish(imu_raw_msg) + self.pub_imu.publish(imu_msg) + self.pub_mag.publish(mag_msg) + self.pub_grav.publish(grav_msg) + self.pub_temp.publish(temp_msg) + def get_calib_status(self): """ From 1ee0d64df63eab47d49f760f664e3725afac6615 Mon Sep 17 00:00:00 2001 From: Sergei Grichine Date: Fri, 7 Nov 2025 17:32:24 -0600 Subject: [PATCH 05/11] Update SensorService.py --- bno055/sensor/SensorService.py | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/bno055/sensor/SensorService.py b/bno055/sensor/SensorService.py index ca05120..eacfb10 100644 --- a/bno055/sensor/SensorService.py +++ b/bno055/sensor/SensorService.py @@ -149,20 +149,9 @@ def configure(self): def get_sensor_data(self): """Read IMU data from the sensor, parse and publish.""" - # Initialize ROS msgs - imu_raw_msg = Imu() - imu_msg = Imu() - mag_msg = MagneticField() - grav_msg = Vector3() - temp_msg = Temperature() # read from sensor buf = self.con.receive(registers.BNO055_ACCEL_DATA_X_LSB_ADDR, 45) - # Publish raw data - imu_raw_msg.header.stamp = self.node.get_clock().now().to_msg() - imu_raw_msg.header.frame_id = self.param.frame_id.value - # TODO: do headers need sequence counters now? - # imu_raw_msg.header.seq = seq # # Sanity check - compute quaternion norm and see if it is valid @@ -179,7 +168,7 @@ def get_sensor_data(self): # Compute norm safely, return if anything wrong: norm = sqrt(q[0]**2 + q[1]**2 + q[2]**2 + q[3]**2) if abs(norm - 16000.0) > 1000.0: - # small norm - invalid quaternion. It should be usually ~16000, as values are large. + # abnormal norm - invalid quaternion. It should be usually ~16000, as values are large. self.node.get_logger().warn("Invalid quaternion norm: {} — sensor reading ignored".format(norm)) return else: @@ -197,6 +186,19 @@ def get_sensor_data(self): # OK, sanity check passed, we are good to publish the data + # Initialize ROS msgs + imu_raw_msg = Imu() + imu_msg = Imu() + mag_msg = MagneticField() + grav_msg = Vector3() + temp_msg = Temperature() + + # Publish raw data + imu_raw_msg.header.stamp = self.node.get_clock().now().to_msg() + imu_raw_msg.header.frame_id = self.param.frame_id.value + # TODO: do headers need sequence counters now? + # imu_raw_msg.header.seq = seq + # TODO: make this an option to publish? imu_raw_msg.orientation_covariance = [ self.param.variance_orientation.value[0], 0.0, 0.0, From 4c1ea069b53eea8c4917e778131e4a5023b2eee9 Mon Sep 17 00:00:00 2001 From: Sergei Grichine Date: Fri, 7 Nov 2025 22:15:31 -0600 Subject: [PATCH 06/11] Update README.md --- README.md | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/README.md b/README.md index 7d2148e..b8b9bf6 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,13 @@ +------------------------ + +**Note:** this fork fixes the "*data spiking*" issue and uses *smbus2* instead of *smbus* for I2C communication. + +Please see [this guide](https://github.com/slgrobotics/robots_bringup/blob/main/Docs/Sensors/BNO055%20IMU.md#issues-when-using-the-standard-ros-2-driver) for details. + +Original README follows + +------------------------ + # A BNO05 ROS2 Package ## Description From d48d186ecc5c5bfb33acfca951f43f57b93e3f80 Mon Sep 17 00:00:00 2001 From: Sergei Grichine Date: Fri, 13 Feb 2026 12:32:38 -0600 Subject: [PATCH 07/11] Dev => Main merge after many optimizations and refactoring (#1) * Minor changes after AI review * WIP: launch files * WIP: launch files * Update bno055_params_i2c.yaml * Update bno055_I2C.launch.py * WIP: launch files * Update bno055.launch.py * Comments added to clarify operation mode * Update bno055.launch.py * WIP: mag publishes zeroes * Update SensorService.py * Update SensorService.py * Clarified "with FMC" vs NDOF_FMC_OFF in comments * Update SensorService.py * Update SensorService.py * Cleanup and comments * Added launch/bno055_raw.launch.py * Update bno055_raw.launch.py * Bumped version to 0.5.1 * Update CHANGELOG.rst * Update SensorService.py * Update SensorService.py * Refactored bno055.py - moved code from main() to class body * non-fusing mode: orientation covarience unknown, calib and grav data not published * Update SensorService.py * parameter 'i2c_addr' changed to INTEGER_ARRAY, e.g. [0x28, 0x29] --- AUTHORS | 1 + CHANGELOG.rst | 6 + README.md | 2 +- bno055/bno055.py | 222 ++++++++++--------- bno055/connectors/i2c.py | 29 ++- bno055/params/NodeParameters.py | 12 +- bno055/params/bno055_params.yaml | 10 +- bno055/params/bno055_params_i2c.yaml | 16 +- bno055/params/bno055_params_test.yaml | 10 +- bno055/registers.py | 20 +- bno055/sensor/SensorService.py | 308 ++++++++++++++------------ launch/bno055.launch.py | 83 ++++++- launch/bno055_I2C.launch.py | 77 +++++++ launch/bno055_UART.launch.py | 47 ++++ launch/bno055_raw.launch.py | 137 ++++++++++++ package.xml | 2 +- setup.py | 2 +- 17 files changed, 689 insertions(+), 295 deletions(-) create mode 100644 launch/bno055_I2C.launch.py create mode 100644 launch/bno055_UART.launch.py create mode 100644 launch/bno055_raw.launch.py diff --git a/AUTHORS b/AUTHORS index 085a4f9..17e45df 100644 --- a/AUTHORS +++ b/AUTHORS @@ -2,3 +2,4 @@ Michal Drweiga Evan Flynn Manfred Novotny Pascal Voser +Sergei Grichine diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 268c325..3d0adb3 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,12 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package bno055 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.5.1 (2026-02-12) +------------------ +* Bump to 0.5.1 to prep for release +* Optimized get_sensor_data() for speed, only publish necessary topics based on operation mode +* Added launch files for raw data and imu_tools madgwick filter, and one to avoid config file inclusion. +* Contributors: Sergei Grichine 0.5.0 (2024-02-17) ------------------ diff --git a/README.md b/README.md index b8b9bf6..c47d3f6 100644 --- a/README.md +++ b/README.md @@ -57,7 +57,7 @@ ros2 run bno055 bno055 --ros-args --params-file ./src/bno055/bno055/params/bno05 - **connection_type=i2c**: Defines I2C as sensor connection type; default='uart' - **i2c_bus**: The integer I2C bus number to use; default=0 -- **i2c_address**: The hexadecimal I2C address to use; default=0x28 +- **i2c_address**: The hexadecimal I2C addresses to use; default=[0x28,0x29] ### Sensor Configuration diff --git a/bno055/bno055.py b/bno055/bno055.py index 970998d..fc5af63 100755 --- a/bno055/bno055.py +++ b/bno055/bno055.py @@ -26,52 +26,56 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. - import sys import threading +import rclpy +from rclpy.node import Node + from bno055.connectors.i2c import I2C from bno055.connectors.uart import UART from bno055.error_handling.exceptions import BusOverRunException from bno055.params.NodeParameters import NodeParameters from bno055.sensor.SensorService import SensorService -import rclpy -from rclpy.node import Node class Bno055Node(Node): - """ - ROS2 Node for interfacing Bosch Bno055 IMU sensor. - - :param Node: ROS2 Node Class to initialize from - :type Node: ROS2 Node - :raises NotImplementedError: Indicates feature/function is not implemented yet. - """ - - sensor = None - param = None + """ROS2 Node for interfacing Bosch BNO055 IMU sensor.""" def __init__(self): - # Initialize parent (ROS Node) super().__init__('bno055') - def setup(self): - # Initialize ROS2 Node Parameters: + # Instance attributes + self.param = None + self.sensor = None + + # Timers + lock on the node + self._lock = threading.Lock() + self._data_timer = None + self._status_timer = None + + def setup(self) -> None: + """Configure params, connector, sensor service, and start timers.""" self.param = NodeParameters(self) - # Get connector according to configured sensor connection type: + # Connector selection if self.param.connection_type.value == UART.CONNECTIONTYPE_UART: - connector = UART(self, - self.param.uart_baudrate.value, - self.param.uart_port.value, - self.param.uart_timeout.value) + connector = UART( + self, + self.param.uart_baudrate.value, + self.param.uart_port.value, + self.param.uart_timeout.value, + ) elif self.param.connection_type.value == I2C.CONNECTIONTYPE_I2C: - connector = I2C(self, - self.param.i2c_bus.value, - self.param.i2c_addr.value) + connector = I2C( + self, + self.param.i2c_bus.value, # INTEGER, e.g. 1 for Raspberry Pi + self.param.i2c_addr.value, # INTEGER_ARRAY, e.g. [0x28, 0x29] for Adafruit BNO055 and GY-85 clone with both jumpers closed + ) else: - raise NotImplementedError('Unsupported connection type: ' - + str(self.param.connection_type.value)) + raise NotImplementedError( + f"Unsupported connection type: {self.param.connection_type.value}" + ) # Connect to BNO055 device: connector.connect() @@ -82,92 +86,102 @@ def setup(self): # configure imu self.sensor.configure() + self._start_timers() -def main(args=None): - try: - """Main entry method for this ROS2 node.""" - # Initialize ROS Client Libraries (RCL) for Python: - rclpy.init() + def _start_timers(self) -> None: + """Create timers that periodically query IMU + calibration status.""" + data_period = 1.0 / float(self.param.data_query_frequency.value) + status_period = 1.0 / float(self.param.calib_status_frequency.value) - # Create & initialize ROS2 node: - node = Bno055Node() - node.setup() + self._data_timer = self.create_timer(data_period, self._read_data_cb) - # Create lock object to prevent overlapping data queries - lock = threading.Lock() - - def read_data(): - """Periodic data_query_timer executions to retrieve sensor IMU data.""" - if lock.locked(): - # critical area still locked - # that means that the previous data query is still being processed - node.get_logger().warn('Message communication in progress - skipping query cycle') - return - - # Acquire lock before entering critical area to prevent overlapping data queries - lock.acquire() - try: - # perform synchronized block: - node.sensor.get_sensor_data() - except BusOverRunException: - # data not available yet, wait for next cycle | see #5 - node.get_logger().warn('Receiving sensor data failed: BusOverRunException') - return - except ZeroDivisionError: - # division by zero in get_sensor_data, return - node.get_logger().warn('Receiving sensor data failed: ZeroDivisionError') - return - except Exception as e: # noqa: B902 - node.get_logger().warn('Receiving sensor data failed with %s:"%s"' - % (type(e).__name__, e)) - finally: - lock.release() - - def log_calibration_status(): - """Periodic logging of calibration data (quality indicators).""" - if lock.locked(): - # critical area still locked - # that means that the previous data query is still being processed - node.get_logger().warn('Message communication in progress - skipping query cycle') - # traceback.print_exc() - return - - # Acquire lock before entering critical area to prevent overlapping data queries - lock.acquire() - try: - # perform synchronized block: - node.sensor.get_calib_status() - except Exception as e: # noqa: B902 - node.get_logger().warn('Receiving calibration status failed with %s:"%s"' - % (type(e).__name__, e)) - # traceback.print_exc() - finally: - lock.release() - - # start regular sensor transmissions: - # please be aware that frequencies around 30Hz and above might cause performance impacts: - # https://github.com/ros2/rclpy/issues/520 - f = 1.0 / float(node.param.data_query_frequency.value) - data_query_timer = node.create_timer(f, read_data) - - # start regular calibration status logging - f = 1.0 / float(node.param.calib_status_frequency.value) - status_timer = node.create_timer(f, log_calibration_status) + if self.sensor.is_fusing_mode: + # Only start calib status timer if we are in a fusing mode that provides calibration status data. + self._status_timer = self.create_timer(status_period, self._calib_status_cb) - rclpy.spin(node) + self.get_logger().info( + f"Timers started: data={1.0/data_period:.2f} Hz, status={1.0/status_period:.2f} Hz" + ) + + def _is_shutting_down(self) -> bool: + """ + True if ROS is shutting down / context is not OK. + This is safe to call from timer callbacks. + """ + return (not rclpy.ok()) or (not self.context.ok()) + + def _read_data_cb(self) -> None: + """Timer callback: read and publish IMU data.""" + if self._is_shutting_down(): + return + if not self._lock.acquire(blocking=False): + # Consider throttling this to avoid spam + self.get_logger().warn("I/O busy - skipping IMU read cycle") + return + + try: + self.sensor.get_sensor_data() + except BusOverRunException: + # Usually means "data not ready"; warning every cycle can spam logs + self.get_logger().debug("BusOverRunException (data not ready)") + except ZeroDivisionError: + self.get_logger().warn("ZeroDivisionError in get_sensor_data()") + except Exception as e: # noqa: B902 + self.get_logger().warn(f"Receiving sensor data failed with {type(e).__name__}: {e}") + finally: + self._lock.release() + + def _calib_status_cb(self) -> None: + """Timer callback: read and publish calibration status.""" + if self._is_shutting_down(): + return + + if not self._lock.acquire(blocking=False): + self.get_logger().warn("I/O busy - skipping calib status cycle") + return + + try: + self.sensor.get_calib_status() + except Exception as e: # noqa: B902 + self.get_logger().warn(f"Receiving calib status failed with {type(e).__name__}: {e}") + finally: + self._lock.release() + + def destroy_node(self): + """ + Ensure timers are stopped before node teardown. + (rclpy usually handles this, but explicit is safer.) + """ + try: + if self._data_timer is not None: + self._data_timer.cancel() + self.destroy_timer(self._data_timer) + self._data_timer = None + + if self._status_timer is not None: + self._status_timer.cancel() + self.destroy_timer(self._status_timer) + self._status_timer = None + except Exception: + pass + + return super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = Bno055Node() + + try: + node.setup() + rclpy.spin(node) except KeyboardInterrupt: - node.get_logger().info('Ctrl+C received - exiting...') - sys.exit(0) + node.get_logger().info("Ctrl+C received - exiting...") finally: - node.get_logger().info('ROS node shutdown') - try: - node.destroy_timer(data_query_timer) - node.destroy_timer(status_timer) - except UnboundLocalError: - node.get_logger().info('No timers to shutdown') node.destroy_node() - rclpy.shutdown() + if rclpy.ok(): + rclpy.shutdown() if __name__ == '__main__': diff --git a/bno055/connectors/i2c.py b/bno055/connectors/i2c.py index 0d5be08..256f7d3 100644 --- a/bno055/connectors/i2c.py +++ b/bno055/connectors/i2c.py @@ -41,26 +41,42 @@ class I2C(Connector): CONNECTIONTYPE_I2C = 'i2c' - def __init__(self, node: Node, i2c_bus=0, i2c_addr=registers.BNO055_ADDRESS_A): + def __init__(self, node: Node, i2c_bus=0, i2c_addresses=[registers.BNO055_ADDRESS_A, registers.BNO055_ADDRESS_B]): """Initialize the I2C class. :param node: a ROS node :param i2c_bus: I2C bus to use - :param i2c_addr: I2C address to connect to + :param i2c_addresses: candidate I2C addresses to connect to :return: """ super().__init__(node) + + self.i2c_addresses = i2c_addresses + self.bus = SMBus(i2c_bus) - self.address = i2c_addr + self.address = None + def connect(self): """Connect to the sensor :return: """ - returned_id = self.bus.read_byte_data(self.address, registers.BNO055_CHIP_ID_ADDR) - if returned_id != registers.BNO055_ID: - raise TransmissionException('Could not get BNO055 chip ID via I2C') + for addr in self.i2c_addresses: + try: + self.node._logger.info(f"IP: trying I2C address: 0x{addr: 02x}") + returned_id = self.bus.read_byte_data(addr, registers.BNO055_CHIP_ID_ADDR) + if returned_id == registers.BNO055_ID: + self.address = addr + self.node._logger.info(f" address: 0x{self.address: 02x} chip ID: 0x{returned_id: 02x} ✓ (connected)") + break + except Exception as e: + self.node._logger.info(f" i2c address 0x{addr: 02x} failed to connect") + continue + + if self.address is None: + self.node._logger.error(f"Could not connect to BNO055 via I2C. Tried addresses: {[f'0x{addr: 02x}' for addr in self.i2c_addresses]}") + def read(self, reg_addr, length): """Read data from sensor via I2C. @@ -80,6 +96,7 @@ def read(self, reg_addr, length): bytes_left_to_read -= read_len return buffer + def write(self, reg_addr, length, data: bytes): """Write data to sensor via I2C. diff --git a/bno055/params/NodeParameters.py b/bno055/params/NodeParameters.py index 947fdaf..4e490c6 100644 --- a/bno055/params/NodeParameters.py +++ b/bno055/params/NodeParameters.py @@ -58,7 +58,7 @@ def __init__(self, node: Node): # I2C bus number node.declare_parameter('i2c_bus', value=0) # I2C address - node.declare_parameter('i2c_addr', value=0x28) + node.declare_parameter('i2c_addr', value=[0x28, 0x29]) # Adafruit - 0x28, GY Clone - 0x29 (with both jumpers closed) # UART port node.declare_parameter('uart_port', value='/dev/ttyUSB0') # UART Baud Rate @@ -72,13 +72,13 @@ def __init__(self, node: Node): # Node timer frequency in Hz, defining how often calibration status data is requested node.declare_parameter('calib_status_frequency', value=0.1) # sensor operation mode - node.declare_parameter('operation_mode', value=0x0C) + node.declare_parameter('operation_mode', value=0x0C) # default: OPERATION_MODE_NDOF (with FMC) # placement_axis_remap defines the position and orientation of the sensor mount node.declare_parameter('placement_axis_remap', value='P1') # scaling factor for acceleration node.declare_parameter('acc_factor', value=100.0) # scaling factor for magnetometer - node.declare_parameter('mag_factor', value=16000000.0) + node.declare_parameter('mag_factor', value=16000000.0) # 16 million LSB per Tesla, as per datasheet # scaling factor for gyroscope node.declare_parameter('gyr_factor', value=900.0) # scaling factor for gyroscope @@ -116,8 +116,8 @@ def __init__(self, node: Node): self.i2c_bus = node.get_parameter('i2c_bus') node.get_logger().info('\ti2c_bus:\t\t"%s"' % self.i2c_bus.value) - self.i2c_addr = node.get_parameter('i2c_addr') - node.get_logger().info('\ti2c_addr:\t\t"%s"' % self.i2c_addr.value) + self.i2c_addr = node.get_parameter('i2c_addr') # INTEGER_ARRAY, e.g. [0x28, 0x29] for Adafruit BNO055 and GY-85 clone with both jumpers closed + node.get_logger().info('\ti2c_addr:\t\t"%s"' % ['0x%02x' % addr for addr in self.i2c_addr.value]) elif self.connection_type.value == UART.CONNECTIONTYPE_UART: @@ -152,7 +152,7 @@ def __init__(self, node: Node): node.get_logger().info('\tacc_factor:\t\t"%s"' % self.acc_factor.value) self.mag_factor = node.get_parameter('mag_factor') - node.get_logger().info('\tmag_factor:\t\t"%s"' % self.mag_factor.value) + node.get_logger().info('\tmag_factor:\t\t"%s"' % self.mag_factor.value) # 16 million LSB per Tesla, as per datasheet self.gyr_factor = node.get_parameter('gyr_factor') node.get_logger().info('\tgyr_factor:\t\t"%s"' % self.gyr_factor.value) diff --git a/bno055/params/bno055_params.yaml b/bno055/params/bno055_params.yaml index 3add21f..760f89a 100644 --- a/bno055/params/bno055_params.yaml +++ b/bno055/params/bno055_params.yaml @@ -37,13 +37,13 @@ bno055: data_query_frequency: 100 calib_status_frequency: 0.1 frame_id: "bno055" - operation_mode: 0x0C + operation_mode: 0x0C # OPERATION_MODE_NDOF (with FMC) placement_axis_remap: "P2" acc_factor: 100.0 - mag_factor: 16000000.0 - gyr_factor: 900.0 - grav_factor: 100.0 - set_offsets: false # set to true to use offsets below + mag_factor: 16000000.0 # 16 million LSB per Tesla, as per datasheet + gyr_factor: 900.0 # scaling factor for gyroscope + grav_factor: 100.0 # scaling factor for gravity vector + set_offsets: false # set to true to use offsets below offset_acc: [0xFFEC, 0x00A5, 0xFFE8] offset_mag: [0xFFB4, 0xFE9E, 0x027D] offset_gyr: [0x0002, 0xFFFF, 0xFFFF] diff --git a/bno055/params/bno055_params_i2c.yaml b/bno055/params/bno055_params_i2c.yaml index 745425b..b9bd5cb 100644 --- a/bno055/params/bno055_params_i2c.yaml +++ b/bno055/params/bno055_params_i2c.yaml @@ -31,18 +31,18 @@ bno055: ros__parameters: ros_topic_prefix: "bno055/" connection_type: "i2c" - i2c_bus: 0 - i2c_addr: 0x28 + i2c_bus: 1 + i2c_addr: [0x29,0x28] # GY-85 clone with both jumpers closed - 0x29, Adafruit BNO055 - 0x28 data_query_frequency: 100 calib_status_frequency: 0.1 - frame_id: "bno055" - operation_mode: 0x0C + frame_id: "imu_link" + operation_mode: 0x0C # OPERATION_MODE_NDOF (with FMC) placement_axis_remap: "P2" acc_factor: 100.0 - mag_factor: 16000000.0 - gyr_factor: 900.0 - grav_factor: 100.0 - set_offsets: false # set to true to use offsets below + mag_factor: 16000000.0 # 16 million LSB per Tesla, as per datasheet + gyr_factor: 900.0 # scaling factor for gyroscope + grav_factor: 100.0 # scaling factor for gravity vector + set_offsets: false # set to true to use offsets below offset_acc: [0xFFEC, 0x00A5, 0xFFE8] offset_mag: [0xFFB4, 0xFE9E, 0x027D] offset_gyr: [0x0002, 0xFFFF, 0xFFFF] diff --git a/bno055/params/bno055_params_test.yaml b/bno055/params/bno055_params_test.yaml index 353bb31..c9aa184 100644 --- a/bno055/params/bno055_params_test.yaml +++ b/bno055/params/bno055_params_test.yaml @@ -36,13 +36,13 @@ bno055: data_query_frequency: 10 calib_status_frequency: 0.1 frame_id: "bno055" - operation_mode: 0x08 + operation_mode: 0x08 # OPERATION_MODE_IMUPLUS placement_axis_remap: "P1" acc_factor: 100.0 - mag_factor: 16000000.0 - gyr_factor: 900.0 - grav_factor: 100.0 - set_offsets: false # set to true to use offsets below + mag_factor: 16000000.0 # 16 million LSB per Tesla, as per datasheet + gyr_factor: 900.0 # scaling factor for gyroscope + grav_factor: 100.0 # scaling factor for gravity vector + set_offsets: false # set to true to use offsets below offset_acc: [0xFFEC, 0x00A5, 0xFFE8] offset_mag: [0xFFB4, 0xFE9E, 0x027D] offset_gyr: [0x0002, 0xFFFF, 0xFFFF] diff --git a/bno055/registers.py b/bno055/registers.py index 5efbfaa..f0898fb 100644 --- a/bno055/registers.py +++ b/bno055/registers.py @@ -222,12 +222,13 @@ OPERATION_MODE_ACCMAG = 0X04 OPERATION_MODE_ACCGYRO = 0X05 OPERATION_MODE_MAGGYRO = 0X06 -OPERATION_MODE_AMG = 0X07 -OPERATION_MODE_IMUPLUS = 0X08 -OPERATION_MODE_COMPASS = 0X09 -OPERATION_MODE_M4G = 0X0A -OPERATION_MODE_NDOF_FMC_OFF = 0X0B -OPERATION_MODE_NDOF = 0X0C +OPERATION_MODE_AMG = 0X07 # All three sensors (Accelerometer, Magnetometer, Gyroscope) are active +# Fusion modes: +OPERATION_MODE_IMUPLUS = 0X08 # Fuses accelerometer and gyroscope, will drift +OPERATION_MODE_COMPASS = 0X09 # Fuses accelerometer and magnetometer. It acts as a tilt-compensated compass +OPERATION_MODE_M4G = 0X0A # "Mag-for-Gyro" Similar to IMU mode, but uses the mag instead of the gyro to detect rotation +OPERATION_MODE_NDOF_FMC_OFF = 0X0B # 9-DOF fusion (all sensors) with Fast Mag Calibration (FMC) disabled. It requires a full "figure-eight" motion for calibration but is more stable. +OPERATION_MODE_NDOF = 0X0C # 9-DOF fusion with FMC enabled. This is the standard "Absolute Orientation" mode, allowing quick auto-calibration with minimal movement. #: Communication constants COM_START_BYTE_WR = 0xAA @@ -246,8 +247,8 @@ #: +/- 2000 units up to 32000 (dps range dependent) (1 unit = 1/16 dps) DEFAULT_OFFSET_GYR = [0x0002, 0xFFFF, 0xFFFF] -DEFAULT_RADIUS_MAG = 0x0 -DEFAULT_RADIUS_ACC = 0x3E8 +DEFAULT_RADIUS_MAG = 0x0 # Dynamic baseline; requires user movement to calibrate +DEFAULT_RADIUS_ACC = 0x3E8 # 1000 decimal, means 1G = 1000 units LSB #: Sensor standard deviation squared (^2) defaults [x, y, z] #: Used to get covariance matrices (stddev^2 = variance) #: values taken from this ROS1 driver from octanis: @@ -256,6 +257,5 @@ DEFAULT_VARIANCE_ANGULAR_VEL = [0.04, 0.04, 0.04] DEFAULT_VARIANCE_ORIENTATION = [0.0159, 0.0159, 0.0159] # TODO(flynneva) calculate default magnetic variance matrice -DEFAULT_VARIANCE_MAG = [0.0, 0.0, 0.0] - +DEFAULT_VARIANCE_MAG = [-1.0, 0.0, 0.0] # -1 in covariance matrix indicates that the covariance is unknown, see REP 117 diff --git a/bno055/sensor/SensorService.py b/bno055/sensor/SensorService.py index eacfb10..9c0d976 100644 --- a/bno055/sensor/SensorService.py +++ b/bno055/sensor/SensorService.py @@ -26,16 +26,17 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. import json -from math import sqrt import struct import sys from time import sleep +import numpy as np from bno055 import registers from bno055.connectors.Connector import Connector from bno055.params.NodeParameters import NodeParameters -from geometry_msgs.msg import Vector3 +from geometry_msgs.msg import Vector3 # or Vector3Stamped +import rclpy from rclpy.node import Node from rclpy.qos import QoSProfile from sensor_msgs.msg import Imu, MagneticField, Temperature @@ -58,11 +59,57 @@ def __init__(self, node: Node, connector: Connector, param: NodeParameters): self.pub_imu_raw = node.create_publisher(Imu, prefix + 'imu_raw', QoSProf) self.pub_imu = node.create_publisher(Imu, prefix + 'imu', QoSProf) self.pub_mag = node.create_publisher(MagneticField, prefix + 'mag', QoSProf) - self.pub_grav = node.create_publisher(Vector3, prefix + 'grav', QoSProf) + self.pub_grav = node.create_publisher(Vector3, prefix + 'grav', QoSProf) # or Vector3Stamped self.pub_temp = node.create_publisher(Temperature, prefix + 'temp', QoSProf) self.pub_calib_status = node.create_publisher(String, prefix + 'calib_status', QoSProf) self.srv = self.node.create_service(Trigger, prefix + 'calibration_request', self.calibration_request_callback) + # initialize message objects to reuse for publishing (avoid creating new objects every time): + self._imu_raw_msg = Imu() + self._imu_msg = Imu() + self._mag_msg = MagneticField() + self._grav_msg = Vector3() # or Vector3Stamped + self._temp_msg = Temperature() + + # precompute covariance matrices from parameters (avoid recomputing every time): + self._cov_ori = [ + self.param.variance_orientation.value[0], 0.0, 0.0, + 0.0, self.param.variance_orientation.value[1], 0.0, + 0.0, 0.0, self.param.variance_orientation.value[2], + ] + self._cov_acc = [ + self.param.variance_acc.value[0], 0.0, 0.0, + 0.0, self.param.variance_acc.value[1], 0.0, + 0.0, 0.0, self.param.variance_acc.value[2], + ] + self._cov_gyr = [ + self.param.variance_angular_vel.value[0], 0.0, 0.0, + 0.0, self.param.variance_angular_vel.value[1], 0.0, + 0.0, 0.0, self.param.variance_angular_vel.value[2], + ] + self._cov_mag = [ + self.param.variance_mag.value[0], 0.0, 0.0, + 0.0, self.param.variance_mag.value[1], 0.0, + 0.0, 0.0, self.param.variance_mag.value[2], + ] + + self._cov_unknown = [ + -1.0, 0.0, 0.0, + 0.0, -1.0, 0.0, + 0.0, 0.0, -1.0, + ] + + # Set covariances once on reused messages + self._imu_raw_msg.orientation_covariance = self._cov_ori + self._imu_raw_msg.linear_acceleration_covariance = self._cov_acc + self._imu_raw_msg.angular_velocity_covariance = self._cov_gyr + + self._imu_msg.orientation_covariance = self._cov_ori + self._imu_msg.linear_acceleration_covariance = self._cov_acc + self._imu_msg.angular_velocity_covariance = self._cov_gyr + + self._mag_msg.magnetic_field_covariance = self._cov_mag + # # The sensor reading often contains invalid large jumps of the orientation w component. # To mitigate this, we implement a simple jump detection and filtering logic. @@ -70,7 +117,11 @@ def __init__(self, node: Node, connector: Connector, param: NodeParameters): # # Jump detection state variable: - self.prev_norm = None # last computed raw quaternion norm + self._prev_q = None # for sign continuity / jump checks + + # Temperature publish throttling: + self._last_temp_publish_time = None # Track last temperature publish time + def configure(self): """Configure the IMU sensor hardware.""" @@ -139,7 +190,13 @@ def configure(self): # Set Device mode device_mode = self.param.operation_mode.value - self.node.get_logger().info(f"Setting device_mode to {device_mode}") + self.is_fusing_mode = self.param.operation_mode.value in [0x0B, 0x0C] + # only NDOF_FMC_OFF or NDOF (with FMC) modes provide fused orientation data (but mag data reads zeroes) + self.node.get_logger().info(f"Setting device_mode to {device_mode} is_fusing_mode={self.is_fusing_mode}") + + if not self.is_fusing_mode: + # In non-fusing modes, the orientation data is not valid, so set covariance to -1 to indicate unknown. + self._imu_raw_msg.orientation_covariance = self._cov_unknown if not (self.con.transmit(registers.BNO055_OPR_MODE_ADDR, 1, bytes([device_mode]))): self.node.get_logger().warn('Unable to set IMU operation mode into operation mode.') @@ -150,144 +207,124 @@ def configure(self): def get_sensor_data(self): """Read IMU data from the sensor, parse and publish.""" - # read from sensor + # avoid publishing during shutdown + if not rclpy.ok(): + return + + # read from sensor: bytearray, 45 bytes starting from BNO055_ACCEL_DATA_X_LSB_ADDR buf = self.con.receive(registers.BNO055_ACCEL_DATA_X_LSB_ADDR, 45) - # - # Sanity check - compute quaternion norm and see if it is valid - # + if not buf or len(buf) != 45: + self.node.get_logger().warn(f"Short read: got {len(buf) if buf else 0} bytes") + return - # Quaternion: - q = [ - self.unpackBytesToFloat(buf[26], buf[27]), # x - self.unpackBytesToFloat(buf[28], buf[29]), # y - self.unpackBytesToFloat(buf[30], buf[31]), # z - self.unpackBytesToFloat(buf[24], buf[25]) # w - ] + # if unsure about buffer type, copy the buffer to a bytes object + #b = buf if isinstance(buf, (bytes, bytearray, memoryview)) else bytes(buf) + b = buf - # Compute norm safely, return if anything wrong: - norm = sqrt(q[0]**2 + q[1]**2 + q[2]**2 + q[3]**2) - if abs(norm - 16000.0) > 1000.0: - # abnormal norm - invalid quaternion. It should be usually ~16000, as values are large. - self.node.get_logger().warn("Invalid quaternion norm: {} — sensor reading ignored".format(norm)) - return - else: - q = [x / norm for x in q] - if self.prev_norm is None: # first good value - self.prev_norm = norm - - if self.prev_norm is not None: - norm_jump = norm - self.prev_norm - if abs(norm_jump) > self.prev_norm * 0.05: # 5% jump - self.node.get_logger().warn("Large jump in quaternion norm detected: norm: {} prev: {} jump: {}".format(norm, self.prev_norm, norm_jump)) - return + # Unpack blocks (fewer Python calls) + ax_raw, ay_raw, az_raw, mx_raw, my_raw, mz_raw, gx_raw, gy_raw, gz_raw = struct.unpack_from(" 2000.0: + self.node.get_logger().warn(f"Invalid quaternion norm: {norm} — publishing identity quaternion") + qn = np.array([0.0, 0.0, 0.0, 1.0], dtype=float) else: - self.prev_norm = norm # first good reading + qn = q / norm + # sign continuity + if self._prev_q is not None and float(np.dot(self._prev_q, qn)) < 0.0: + qn = -qn + self._prev_q = qn - # OK, sanity check passed, we are good to publish the data + # Header (single timestamp) + self._imu_msg.header.stamp = now_msg + self._imu_msg.header.frame_id = frame_id - # Initialize ROS msgs - imu_raw_msg = Imu() - imu_msg = Imu() - mag_msg = MagneticField() - grav_msg = Vector3() - temp_msg = Temperature() + qx, qy, qz, qw = map(float, qn) # explicit conversion + self._imu_msg.orientation.x = qx + self._imu_msg.orientation.y = qy + self._imu_msg.orientation.z = qz + self._imu_msg.orientation.w = qw - # Publish raw data - imu_raw_msg.header.stamp = self.node.get_clock().now().to_msg() - imu_raw_msg.header.frame_id = self.param.frame_id.value - # TODO: do headers need sequence counters now? - # imu_raw_msg.header.seq = seq + # raw gyroscope data + self._imu_msg.angular_velocity.x = gx_raw / gyr_div + self._imu_msg.angular_velocity.y = gy_raw / gyr_div + self._imu_msg.angular_velocity.z = gz_raw / gyr_div - # TODO: make this an option to publish? - imu_raw_msg.orientation_covariance = [ - self.param.variance_orientation.value[0], 0.0, 0.0, - 0.0, self.param.variance_orientation.value[1], 0.0, - 0.0, 0.0, self.param.variance_orientation.value[2] - ] + # “filtered/linear accel” block + self._imu_msg.linear_acceleration.x = lax_raw / acc_div + self._imu_msg.linear_acceleration.y = lay_raw / acc_div + self._imu_msg.linear_acceleration.z = laz_raw / acc_div - imu_raw_msg.linear_acceleration.x = \ - self.unpackBytesToFloat(buf[0], buf[1]) / self.param.acc_factor.value - imu_raw_msg.linear_acceleration.y = \ - self.unpackBytesToFloat(buf[2], buf[3]) / self.param.acc_factor.value - imu_raw_msg.linear_acceleration.z = \ - self.unpackBytesToFloat(buf[4], buf[5]) / self.param.acc_factor.value - imu_raw_msg.linear_acceleration_covariance = [ - self.param.variance_acc.value[0], 0.0, 0.0, - 0.0, self.param.variance_acc.value[1], 0.0, - 0.0, 0.0, self.param.variance_acc.value[2] - ] - imu_raw_msg.angular_velocity.x = \ - self.unpackBytesToFloat(buf[12], buf[13]) / self.param.gyr_factor.value - imu_raw_msg.angular_velocity.y = \ - self.unpackBytesToFloat(buf[14], buf[15]) / self.param.gyr_factor.value - imu_raw_msg.angular_velocity.z = \ - self.unpackBytesToFloat(buf[16], buf[17]) / self.param.gyr_factor.value - imu_raw_msg.angular_velocity_covariance = [ - self.param.variance_angular_vel.value[0], 0.0, 0.0, - 0.0, self.param.variance_angular_vel.value[1], 0.0, - 0.0, 0.0, self.param.variance_angular_vel.value[2] - ] + self.pub_imu.publish(self._imu_msg) - # TODO: make this an option to publish? - # Publish filtered data - imu_msg.header.stamp = self.node.get_clock().now().to_msg() - imu_msg.header.frame_id = self.param.frame_id.value - - imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z, imu_msg.orientation.w = q - w = imu_msg.orientation.w - - imu_msg.orientation_covariance = imu_raw_msg.orientation_covariance - - imu_msg.linear_acceleration.x = \ - self.unpackBytesToFloat(buf[32], buf[33]) / self.param.acc_factor.value - imu_msg.linear_acceleration.y = \ - self.unpackBytesToFloat(buf[34], buf[35]) / self.param.acc_factor.value - imu_msg.linear_acceleration.z = \ - self.unpackBytesToFloat(buf[36], buf[37]) / self.param.acc_factor.value - imu_msg.linear_acceleration_covariance = imu_raw_msg.linear_acceleration_covariance - imu_msg.angular_velocity.x = \ - self.unpackBytesToFloat(buf[12], buf[13]) / self.param.gyr_factor.value - imu_msg.angular_velocity.y = \ - self.unpackBytesToFloat(buf[14], buf[15]) / self.param.gyr_factor.value - imu_msg.angular_velocity.z = \ - self.unpackBytesToFloat(buf[16], buf[17]) / self.param.gyr_factor.value - imu_msg.angular_velocity_covariance = imu_raw_msg.angular_velocity_covariance - - # Publish magnetometer data - mag_msg.header.stamp = self.node.get_clock().now().to_msg() - mag_msg.header.frame_id = self.param.frame_id.value - # mag_msg.header.seq = seq - mag_msg.magnetic_field.x = \ - self.unpackBytesToFloat(buf[6], buf[7]) / self.param.mag_factor.value - mag_msg.magnetic_field.y = \ - self.unpackBytesToFloat(buf[8], buf[9]) / self.param.mag_factor.value - mag_msg.magnetic_field.z = \ - self.unpackBytesToFloat(buf[10], buf[11]) / self.param.mag_factor.value - mag_msg.magnetic_field_covariance = [ - self.param.variance_mag.value[0], 0.0, 0.0, - 0.0, self.param.variance_mag.value[1], 0.0, - 0.0, 0.0, self.param.variance_mag.value[2] - ] + # gravity block + self._grav_msg.x = grx_raw / grav_div + self._grav_msg.y = gry_raw / grav_div + self._grav_msg.z = grz_raw / grav_div + + self.pub_grav.publish(self._grav_msg) # Vector3 does not need header. Use Vector3Stamped if you need timestamp and frame_id + + else: + # In other modes, we do not have fused orientation data, but have raw magnetometer data - grav_msg.x = \ - self.unpackBytesToFloat(buf[38], buf[39]) / self.param.grav_factor.value - grav_msg.y = \ - self.unpackBytesToFloat(buf[40], buf[41]) / self.param.grav_factor.value - grav_msg.z = \ - self.unpackBytesToFloat(buf[42], buf[43]) / self.param.grav_factor.value + # Headers (same timestamp) + self._imu_raw_msg.header.stamp = now_msg + self._imu_raw_msg.header.frame_id = frame_id + self._mag_msg.header.stamp = now_msg + self._mag_msg.header.frame_id = frame_id - # Publish temperature - temp_msg.header.stamp = self.node.get_clock().now().to_msg() - temp_msg.header.frame_id = self.param.frame_id.value - # temp_msg.header.seq = seq - temp_msg.temperature = float(buf[44]) + # raw accelerometer data + self._imu_raw_msg.linear_acceleration.x = ax_raw / acc_div + self._imu_raw_msg.linear_acceleration.y = ay_raw / acc_div + self._imu_raw_msg.linear_acceleration.z = az_raw / acc_div - self.pub_imu_raw.publish(imu_raw_msg) - self.pub_imu.publish(imu_msg) - self.pub_mag.publish(mag_msg) - self.pub_grav.publish(grav_msg) - self.pub_temp.publish(temp_msg) + # raw gyroscope data + self._imu_raw_msg.angular_velocity.x = gx_raw / gyr_div + self._imu_raw_msg.angular_velocity.y = gy_raw / gyr_div + self._imu_raw_msg.angular_velocity.z = gz_raw / gyr_div + + self.pub_imu_raw.publish(self._imu_raw_msg) + + # raw magnetometer data + mag_div = self.param.mag_factor.value + self._mag_msg.magnetic_field.x = mx_raw / mag_div # 16 million LSB per Tesla, as per datasheet + self._mag_msg.magnetic_field.y = my_raw / mag_div + self._mag_msg.magnetic_field.z = mz_raw / mag_div + + self.pub_mag.publish(self._mag_msg) + + # Temperature - publish at most once per second + current_time = self.node.get_clock().now() + if self._last_temp_publish_time is None or \ + (current_time - self._last_temp_publish_time).nanoseconds >= 1_000_000_000: + # Header + self._temp_msg.header.stamp = now_msg + self._temp_msg.header.frame_id = frame_id + + # temperature - one byte: + self._temp_msg.temperature = float(temp_raw) # a signed byte with unit 1 degree Celsius + + self.pub_temp.publish(self._temp_msg) + self._last_temp_publish_time = current_time def get_calib_status(self): @@ -297,13 +334,13 @@ def get_calib_status(self): Quality scale: 0 = bad, 3 = best """ calib_status = self.con.receive(registers.BNO055_CALIB_STAT_ADDR, 1) - sys = (calib_status[0] >> 6) & 0x03 - gyro = (calib_status[0] >> 4) & 0x03 - accel = (calib_status[0] >> 2) & 0x03 - mag = calib_status[0] & 0x03 + c_sys = (calib_status[0] >> 6) & 0x03 + c_gyro = (calib_status[0] >> 4) & 0x03 + c_accel = (calib_status[0] >> 2) & 0x03 + c_mag = calib_status[0] & 0x03 # Create dictionary (map) and convert it to JSON string: - calib_status_dict = {'sys': sys, 'gyro': gyro, 'accel': accel, 'mag': mag} + calib_status_dict = {'sys': c_sys, 'gyro': c_gyro, 'accel': c_accel, 'mag': c_mag} calib_status_str = String() calib_status_str.data = json.dumps(calib_status_dict) @@ -440,6 +477,3 @@ def calibration_request_callback(self, request, response): response.success = True response.message = str(calib_data) return response - - def unpackBytesToFloat(self, start, end): - return float(struct.unpack('h', struct.pack('BB', start, end))[0]) diff --git a/launch/bno055.launch.py b/launch/bno055.launch.py index 3c5bad4..8aff276 100644 --- a/launch/bno055.launch.py +++ b/launch/bno055.launch.py @@ -26,22 +26,83 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +# colcon build; source install/setup.bash; ros2 launch bno055 bno055.launch.py + +# see https://github.com/slgrobotics/articubot_one/blob/main/robots/turtle/launch/turtle.sensors.launch.py +# https://github.com/slgrobotics/robots_bringup/blob/main/Docs/Sensors/BNO055%20IMU.md + import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node def generate_launch_description(): + + namespace = "" + ld = LaunchDescription() - config = os.path.join( - get_package_share_directory('bno055'), - 'config', - 'bno055_params.yaml' - ) - node=Node( - package = 'bno055', - executable = 'bno055', - parameters = [config] + bno055_driver_node = Node( + package='bno055', + namespace=namespace, + executable='bno055', + name='bno055', + output='screen', + respawn=True, + respawn_delay=4, + parameters=[{ + # see https://github.com/slgrobotics/bno055 + # https://github.com/slgrobotics/robots_bringup/blob/main/Docs/Sensors/BNO055%20IMU.md + 'ros_topic_prefix': 'imu/', + 'connection_type': 'i2c', + 'i2c_bus': 1, + 'i2c_addr': [0x29,0x28], # Adafruit - 0x28, GY Clone - 0x29 (with both jumpers closed) + 'data_query_frequency': 30, + 'calib_status_frequency': 0.1, + 'frame_id': 'imu_link', + # Fast Magnetometer Calibration mode (FMC) provides faster magnetometer calibration + # at the cost of slightly higher noise. NDOF_FMC_OFF is the default mode with slower calibration but lower noise. + # ACCGYRO and MAGGYRO modes provide raw accelerometer and gyroscope data without sensor fusion, + # which can be useful for certain applications but may require additional processing to obtain orientation data. + 'operation_mode': 0x0C, # 0x0C = NDOF (with FMC), 0x0B - NDOF_FMC_OFF, 0x07 - ACC+GYRO+MAG (AMG) + 'placement_axis_remap': 'P1', # P1 - default, ENU. See Bosch BNO055 datasheet section "Axis Remap" + 'acc_factor': 100.0, + 'mag_factor': 16000000.0, + 'gyr_factor': 900.0, + 'grav_factor': 100.0, + 'set_offsets': False, # set to true to use offsets below + 'offset_acc': [0xFFEC, 0x00A5, 0xFFE8], + 'offset_gyr': [0x0002, 0xFFFF, 0xFFFF], + 'offset_mag': [0xFFB4, 0xFE9E, 0x027D], + 'radius_mag': 800, # means 800 microtesla per LSB + 'radius_acc': 1000, # means 1G = 1000 units LSB + # Sensor standard deviation [x,y,z] + # Used to calculate covariance matrices + # driver defaults are used if parameters below are not provided - bno055/src/bno055/bno055/registers.py:255 + # see https://chatgpt.com/s/t_691b60f38e1c8191a0a309cbcf99e478 + 'variance_acc': [0.017, 0.017, 0.017], # [m/s^2] defaults: [0.017, 0.017, 0.017] + 'variance_angular_vel': [0.04, 0.04, 0.04], # [rad/s] defaults: [0.04, 0.04, 0.04] + 'variance_orientation': [0.0159, 0.0159, 0.0159], # [rad] - (roll, pitch, yaw) defaults: [0.0159, 0.0159, 0.0159] + 'variance_mag': [-1.0, 0.0, 0.0], # [Tesla] defaults: [-1.0, 0.0, 0.0] - "unknown" covariance, see REP 117 + }], + remappings=[("imu/imu", "imu/data"), ("imu/imu_raw", "imu/data_raw")] ) - ld.add_action(node) - return ld \ No newline at end of file + + # for experiments: RViz starts with "map" as Global Fixed Frame, provide a TF to see axes etc. + tf = Node( + package = "tf2_ros", + executable = "static_transform_publisher", + arguments=[ + '--x', '0.0', # X translation in meters + '--y', '0.0', # Y translation in meters + '--z', '0.1', # Z translation in meters + '--roll', '0.0', # Roll in radians + '--pitch', '0.0', # Pitch in radians + '--yaw', '0.0', # Yaw in radians (e.g., 90 degrees) + '--frame-id', 'map', # Parent frame ID + '--child-frame-id', 'imu_link' # Child frame ID + ] + ) + ld.add_action(bno055_driver_node) + ld.add_action(tf) + return ld + diff --git a/launch/bno055_I2C.launch.py b/launch/bno055_I2C.launch.py new file mode 100644 index 0000000..8587ef7 --- /dev/null +++ b/launch/bno055_I2C.launch.py @@ -0,0 +1,77 @@ +# Copyright 2021 AUTHORS +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the AUTHORS nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# colcon build; source install/setup.bash; ros2 launch bno055 bno055_I2C.launch.py + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +def generate_launch_description(): + + ld = LaunchDescription() + + config = os.path.join( + get_package_share_directory('bno055'), + 'config', + 'bno055_params_i2c.yaml' + ) + + node = Node( + package = 'bno055', + executable = 'bno055', + namespace = '', + parameters = [config], + remappings=[("bno055/imu", "imu/data"), + ("bno055/imu_raw", "imu/data_raw"), + ("bno055/mag","imu/mag"), + ("bno055/temp", "imu/temp"), + ("bno055/grav", "imu/grav"), + ("bno055/calib_status", "imu/calib_status") + ] + ) + + # for experiments: RViz starts with "map" as Global Fixed Frame, provide a TF to see axes etc. + tf = Node( + package = "tf2_ros", + executable = "static_transform_publisher", + arguments=[ + '--x', '0.0', # X translation in meters + '--y', '0.0', # Y translation in meters + '--z', '0.1', # Z translation in meters + '--roll', '0.0', # Roll in radians + '--pitch', '0.0', # Pitch in radians + '--yaw', '0.0', # Yaw in radians (e.g., 90 degrees) + '--frame-id', 'map', # Parent frame ID + '--child-frame-id', 'imu_link' # Child frame ID + ] + ) + + ld.add_action(node) + ld.add_action(tf) + return ld \ No newline at end of file diff --git a/launch/bno055_UART.launch.py b/launch/bno055_UART.launch.py new file mode 100644 index 0000000..3c5bad4 --- /dev/null +++ b/launch/bno055_UART.launch.py @@ -0,0 +1,47 @@ +# Copyright 2021 AUTHORS +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the AUTHORS nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +def generate_launch_description(): + ld = LaunchDescription() + config = os.path.join( + get_package_share_directory('bno055'), + 'config', + 'bno055_params.yaml' + ) + + node=Node( + package = 'bno055', + executable = 'bno055', + parameters = [config] + ) + ld.add_action(node) + return ld \ No newline at end of file diff --git a/launch/bno055_raw.launch.py b/launch/bno055_raw.launch.py new file mode 100644 index 0000000..5877c9d --- /dev/null +++ b/launch/bno055_raw.launch.py @@ -0,0 +1,137 @@ +# Copyright 2021 AUTHORS +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the AUTHORS nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# colcon build; source install/setup.bash; ros2 launch bno055 bno055_raw.launch.py + +# see https://github.com/slgrobotics/articubot_one/blob/main/robots/turtle/launch/turtle.sensors.launch.py +# https://github.com/slgrobotics/robots_bringup/blob/main/Docs/Sensors/BNO055%20IMU.md + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +def generate_launch_description(): + + namespace = "" + + ld = LaunchDescription() + + bno055_driver_raw_node = Node( + package='bno055', + namespace=namespace, + executable='bno055', + name='bno055', + output='screen', + respawn=True, + respawn_delay=4, + parameters=[{ + # see https://github.com/slgrobotics/bno055 + # https://github.com/slgrobotics/robots_bringup/blob/main/Docs/Sensors/BNO055%20IMU.md + 'ros_topic_prefix': 'imu/', + 'connection_type': 'i2c', + 'i2c_bus': 1, + 'i2c_addr': [0x29,0x28], # Adafruit - 0x28, GY Clone - 0x29 (with both jumpers closed) + 'data_query_frequency': 30, + 'calib_status_frequency': 0.1, + 'frame_id': 'imu_link', + # Fast Magnetometer Calibration mode (FMC) provides faster magnetometer calibration + # at the cost of slightly higher noise. NDOF_FMC_OFF is the default mode with slower calibration but lower noise. + # ACCGYRO and MAGGYRO modes provide raw accelerometer and gyroscope data without sensor fusion, + # which can be useful for certain applications but may require additional processing to obtain orientation data. + 'operation_mode': 0x07, # 0x0C = NDOF (with FMC), 0x0B - NDOF_FMC_OFF, 0x07 - ACC+GYRO+MAG (AMG) + 'placement_axis_remap': 'P1', # P1 - default, ENU. See Bosch BNO055 datasheet section "Axis Remap" + 'acc_factor': 100.0, + 'mag_factor': 16000000.0, + 'gyr_factor': 900.0, + 'grav_factor': 100.0, + 'set_offsets': False, # set to true to use offsets below + 'offset_acc': [0xFFEC, 0x00A5, 0xFFE8], + 'offset_gyr': [0x0002, 0xFFFF, 0xFFFF], + 'offset_mag': [0xFFB4, 0xFE9E, 0x027D], + 'radius_mag': 800, # means 800 microtesla per LSB + 'radius_acc': 1000, # means 1G = 1000 units LSB + # Sensor standard deviation [x,y,z] + # Used to calculate covariance matrices + # driver defaults are used if parameters below are not provided - bno055/src/bno055/bno055/registers.py:255 + # see https://chatgpt.com/s/t_691b60f38e1c8191a0a309cbcf99e478 + 'variance_acc': [0.017, 0.017, 0.017], # [m/s^2] defaults: [0.017, 0.017, 0.017] + 'variance_angular_vel': [0.04, 0.04, 0.04], # [rad/s] defaults: [0.04, 0.04, 0.04] + 'variance_orientation': [0.0159, 0.0159, 0.0159], # [rad] - (roll, pitch, yaw) defaults: [0.0159, 0.0159, 0.0159] + 'variance_mag': [-1.0, 0.0, 0.0], # [Tesla] defaults: [-1.0, 0.0, 0.0] - "unknown" covariance, see REP 117 + }], + remappings=[("imu/imu", "imu/data"), ("imu/imu_raw", "imu/data_raw")] + ) + + # Madgwick filter node to compute orientation quaternion from raw IMU data + # publishes to "imu/data" topic + # https://github.com/CCNYRoboticsLab/imu_tools + # sudo apt install ros-${ROS_DISTRO}-imu-tools + madgwick_node =Node( + package='imu_filter_madgwick', + executable='imu_filter_madgwick_node', + name='imu_filter', + output='screen', + parameters=[{ + "stateless": False, + "use_mag": True, + "publish_tf": True, + "reverse_tf": False, + "fixed_frame": "odom", + "constant_dt": 0.0, + "publish_debug_topics": False, + "world_frame": "enu", + "gain": 0.1, + "zeta": 0.0, + "mag_bias_x": 0.0, + "mag_bias_y": 0.0, + "mag_bias_z": 0.0, + "orientation_stddev": 0.0 + }], + #remappings=[("imu/mag", "imu/mag"), ("imu/data_raw", "imu/data_raw"), ("imu/data", "imu/data")], + ) + + # for experiments: RViz starts with "map" as Global Fixed Frame, provide a TF to see axes etc. + tf = Node( + package = "tf2_ros", + executable = "static_transform_publisher", + arguments=[ + '--x', '0.0', # X translation in meters + '--y', '0.0', # Y translation in meters + '--z', '0.1', # Z translation in meters + '--roll', '0.0', # Roll in radians + '--pitch', '0.0', # Pitch in radians + '--yaw', '0.0', # Yaw in radians (e.g., 90 degrees) + '--frame-id', 'map', # Parent frame ID + '--child-frame-id', 'odom' # Child frame ID + ] + ) + ld.add_action(bno055_driver_raw_node) + ld.add_action(madgwick_node) + ld.add_action(tf) + return ld + diff --git a/package.xml b/package.xml index c30c36e..549a7dc 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ bno055 - 0.5.0 + 0.5.1 Bosch BNO055 IMU driver for ROS2 flynneva BSD diff --git a/setup.py b/setup.py index eb36166..abc97e7 100644 --- a/setup.py +++ b/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version='0.5.0', + version='0.5.1', # find sub-packages automatically in order to allow sub-modules, etc. to be imported: # packages=[package_name], packages=find_packages(exclude=['test']), From 17b6d12b1c6e930ee612d162cd0d5c721ad526b8 Mon Sep 17 00:00:00 2001 From: Sergei Grichine Date: Fri, 13 Feb 2026 12:41:16 -0600 Subject: [PATCH 08/11] Update README.md --- README.md | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index c47d3f6..be7b52f 100644 --- a/README.md +++ b/README.md @@ -1,10 +1,18 @@ ------------------------ -**Note:** this fork fixes the "*data spiking*" issue and uses *smbus2* instead of *smbus* for I2C communication. +**Note:** this fork fixes the "*data spiking*" issue and uses *smbus2* instead of *smbus* for I2C communication. There's some code optimization and refactoring. Please see [this guide](https://github.com/slgrobotics/robots_bringup/blob/main/Docs/Sensors/BNO055%20IMU.md#issues-when-using-the-standard-ros-2-driver) for details. -Original README follows +**Note changes in parameters:** +- `i2c_bus`: The integer I2C bus number to use; default=0 - Raspberry Pi mostly uses bus=1 +- `i2c_address`: The hexadecimal I2C addresses to use; default=[0x28,0x29] - now will try addresses from a list + +**Look for examples** in [launch](https://github.com/slgrobotics/bno055/tree/main/launch) folder + +------------------------ + +Original README follows: ------------------------ From a04be67fd2d7f77538eb779674c999cbed15320d Mon Sep 17 00:00:00 2001 From: Sergei Grichine Date: Fri, 13 Feb 2026 13:10:51 -0600 Subject: [PATCH 09/11] Update i2c.py --- bno055/connectors/i2c.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/bno055/connectors/i2c.py b/bno055/connectors/i2c.py index 256f7d3..45d45fb 100644 --- a/bno055/connectors/i2c.py +++ b/bno055/connectors/i2c.py @@ -64,18 +64,18 @@ def connect(self): """ for addr in self.i2c_addresses: try: - self.node._logger.info(f"IP: trying I2C address: 0x{addr: 02x}") + self.node.get_logger().info(f"IP: trying I2C address: 0x{addr: 02x}") returned_id = self.bus.read_byte_data(addr, registers.BNO055_CHIP_ID_ADDR) if returned_id == registers.BNO055_ID: self.address = addr - self.node._logger.info(f" address: 0x{self.address: 02x} chip ID: 0x{returned_id: 02x} ✓ (connected)") + self.node.get_logger().info(f" address: 0x{self.address: 02x} chip ID: 0x{returned_id: 02x} ✓ (connected)") break except Exception as e: - self.node._logger.info(f" i2c address 0x{addr: 02x} failed to connect") + self.node.get_logger().info(f" i2c address 0x{addr: 02x} failed to connect") continue if self.address is None: - self.node._logger.error(f"Could not connect to BNO055 via I2C. Tried addresses: {[f'0x{addr: 02x}' for addr in self.i2c_addresses]}") + self.node.get_logger().error(f"Could not connect to BNO055 via I2C. Tried addresses: {[f'0x{addr: 02x}' for addr in self.i2c_addresses]}") def read(self, reg_addr, length): From 063779e2dfefe433ea1965da8fd349f2266d5b6e Mon Sep 17 00:00:00 2001 From: Sergei Grichine Date: Fri, 13 Feb 2026 13:15:10 -0600 Subject: [PATCH 10/11] Update CHANGELOG.rst --- CHANGELOG.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 3d0adb3..24cbd11 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -5,6 +5,7 @@ Changelog for package bno055 ------------------ * Bump to 0.5.1 to prep for release * Optimized get_sensor_data() for speed, only publish necessary topics based on operation mode +* Parameter i2c_address: The hexadecimal I2C addresses to use; default=[0x28,0x29] - now will try addresses from a list * Added launch files for raw data and imu_tools madgwick filter, and one to avoid config file inclusion. * Contributors: Sergei Grichine From cddd4ce0e58befef4026a2738895271b4474f505 Mon Sep 17 00:00:00 2001 From: Sergei Grichine Date: Sat, 14 Feb 2026 11:15:06 -0600 Subject: [PATCH 11/11] Changes: calib_status_frequency: 1.0, publishing old quaternion on spikes --- README.md | 2 +- bno055/params/NodeParameters.py | 2 +- bno055/params/bno055_params.yaml | 2 +- bno055/params/bno055_params_i2c.yaml | 2 +- bno055/params/bno055_params_test.yaml | 2 +- bno055/sensor/SensorService.py | 28 +++++++++++++++------------ launch/bno055.launch.py | 2 +- launch/bno055_raw.launch.py | 2 +- 8 files changed, 23 insertions(+), 19 deletions(-) diff --git a/README.md b/README.md index be7b52f..20f5322 100644 --- a/README.md +++ b/README.md @@ -73,7 +73,7 @@ ros2 run bno055 bno055 --ros-args --params-file ./src/bno055/bno055/params/bno05 - **frame_id**: coordinate frame id of sensor default='bno055' - **baudrate**: baudrate of sensor default=115200 - **data_query_frequency**: frequency (HZ) to read and publish data from sensor; default=100 Hz -- **calib_status_frequency**: frequency (HZ) to read and publish calibration status data from sensor; default=0.1 Hz +- **calib_status_frequency**: frequency (HZ) to read and publish calibration status data from sensor; default=1.0 Hz - **placement_axis_remap**: The sensor placement configuration (Axis remapping) defines the position and orientation of the sensor mount. See Bosch BNO055 datasheet section "Axis Remap" for valid positions: "P0", "P1" (default), "P2", "P3", "P4", "P5", "P6", "P7". diff --git a/bno055/params/NodeParameters.py b/bno055/params/NodeParameters.py index 4e490c6..a7f191b 100644 --- a/bno055/params/NodeParameters.py +++ b/bno055/params/NodeParameters.py @@ -70,7 +70,7 @@ def __init__(self, node: Node): # Node timer frequency in Hz, defining how often sensor data is requested node.declare_parameter('data_query_frequency', value=10) # Node timer frequency in Hz, defining how often calibration status data is requested - node.declare_parameter('calib_status_frequency', value=0.1) + node.declare_parameter('calib_status_frequency', value=1.0) # sensor operation mode node.declare_parameter('operation_mode', value=0x0C) # default: OPERATION_MODE_NDOF (with FMC) # placement_axis_remap defines the position and orientation of the sensor mount diff --git a/bno055/params/bno055_params.yaml b/bno055/params/bno055_params.yaml index 760f89a..8bbe4fd 100644 --- a/bno055/params/bno055_params.yaml +++ b/bno055/params/bno055_params.yaml @@ -35,7 +35,7 @@ bno055: uart_baudrate: 115200 uart_timeout: 0.1 data_query_frequency: 100 - calib_status_frequency: 0.1 + calib_status_frequency: 1.0 frame_id: "bno055" operation_mode: 0x0C # OPERATION_MODE_NDOF (with FMC) placement_axis_remap: "P2" diff --git a/bno055/params/bno055_params_i2c.yaml b/bno055/params/bno055_params_i2c.yaml index b9bd5cb..c7ee699 100644 --- a/bno055/params/bno055_params_i2c.yaml +++ b/bno055/params/bno055_params_i2c.yaml @@ -34,7 +34,7 @@ bno055: i2c_bus: 1 i2c_addr: [0x29,0x28] # GY-85 clone with both jumpers closed - 0x29, Adafruit BNO055 - 0x28 data_query_frequency: 100 - calib_status_frequency: 0.1 + calib_status_frequency: 1.0 frame_id: "imu_link" operation_mode: 0x0C # OPERATION_MODE_NDOF (with FMC) placement_axis_remap: "P2" diff --git a/bno055/params/bno055_params_test.yaml b/bno055/params/bno055_params_test.yaml index c9aa184..3bcf1ca 100644 --- a/bno055/params/bno055_params_test.yaml +++ b/bno055/params/bno055_params_test.yaml @@ -34,7 +34,7 @@ bno055: uart_baudrate: 115200 uart_timeout: 0.1 data_query_frequency: 10 - calib_status_frequency: 0.1 + calib_status_frequency: 1.0 frame_id: "bno055" operation_mode: 0x08 # OPERATION_MODE_IMUPLUS placement_axis_remap: "P1" diff --git a/bno055/sensor/SensorService.py b/bno055/sensor/SensorService.py index 9c0d976..6318d64 100644 --- a/bno055/sensor/SensorService.py +++ b/bno055/sensor/SensorService.py @@ -53,15 +53,15 @@ def __init__(self, node: Node, connector: Connector, param: NodeParameters): self.param = param prefix = self.param.ros_topic_prefix.value - QoSProf = QoSProfile(depth=10) + qos_profile = QoSProfile(depth=10) # create topic publishers: - self.pub_imu_raw = node.create_publisher(Imu, prefix + 'imu_raw', QoSProf) - self.pub_imu = node.create_publisher(Imu, prefix + 'imu', QoSProf) - self.pub_mag = node.create_publisher(MagneticField, prefix + 'mag', QoSProf) - self.pub_grav = node.create_publisher(Vector3, prefix + 'grav', QoSProf) # or Vector3Stamped - 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_raw = node.create_publisher(Imu, prefix + 'imu_raw', qos_profile) + self.pub_imu = node.create_publisher(Imu, prefix + 'imu', qos_profile) + self.pub_mag = node.create_publisher(MagneticField, prefix + 'mag', qos_profile) + self.pub_grav = node.create_publisher(Vector3, prefix + 'grav', qos_profile) # or Vector3Stamped + self.pub_temp = node.create_publisher(Temperature, prefix + 'temp', qos_profile) + self.pub_calib_status = node.create_publisher(String, prefix + 'calib_status', qos_profile) self.srv = self.node.create_service(Trigger, prefix + 'calibration_request', self.calibration_request_callback) # initialize message objects to reuse for publishing (avoid creating new objects every time): @@ -190,7 +190,7 @@ def configure(self): # Set Device mode device_mode = self.param.operation_mode.value - self.is_fusing_mode = self.param.operation_mode.value in [0x0B, 0x0C] + self.is_fusing_mode = self.param.operation_mode.value in (0x0B, 0x0C) # only NDOF_FMC_OFF or NDOF (with FMC) modes provide fused orientation data (but mag data reads zeroes) self.node.get_logger().info(f"Setting device_mode to {device_mode} is_fusing_mode={self.is_fusing_mode}") @@ -207,8 +207,8 @@ def configure(self): def get_sensor_data(self): """Read IMU data from the sensor, parse and publish.""" - # avoid publishing during shutdown - if not rclpy.ok(): + # avoid publishing/logging during shutdown + if not rclpy.ok() or not self.node.context.ok(): return # read from sensor: bytearray, 45 bytes starting from BNO055_ACCEL_DATA_X_LSB_ADDR @@ -245,8 +245,12 @@ def get_sensor_data(self): q = np.array([qx_raw, qy_raw, qz_raw, qw_raw], dtype=float) norm = float(np.linalg.norm(q)) if norm < 1e-6 or abs(norm - 16384.0) > 2000.0: - self.node.get_logger().warn(f"Invalid quaternion norm: {norm} — publishing identity quaternion") - qn = np.array([0.0, 0.0, 0.0, 1.0], dtype=float) + if self._prev_q is None: + self.node.get_logger().warn(f"Invalid quaternion norm: {norm} — skipped publishing imu message to avoid invalid data") + return # no valid reading and no previous reading - skip publishing to avoid invalid data + else: + self.node.get_logger().warn(f"Invalid quaternion norm: {norm} — publishing previous quaternion") + qn = self._prev_q # hold last valid reading to mitigate jumps, if we have one else: qn = q / norm # sign continuity diff --git a/launch/bno055.launch.py b/launch/bno055.launch.py index 8aff276..7d34a50 100644 --- a/launch/bno055.launch.py +++ b/launch/bno055.launch.py @@ -57,7 +57,7 @@ def generate_launch_description(): 'i2c_bus': 1, 'i2c_addr': [0x29,0x28], # Adafruit - 0x28, GY Clone - 0x29 (with both jumpers closed) 'data_query_frequency': 30, - 'calib_status_frequency': 0.1, + 'calib_status_frequency': 1.0, 'frame_id': 'imu_link', # Fast Magnetometer Calibration mode (FMC) provides faster magnetometer calibration # at the cost of slightly higher noise. NDOF_FMC_OFF is the default mode with slower calibration but lower noise. diff --git a/launch/bno055_raw.launch.py b/launch/bno055_raw.launch.py index 5877c9d..b68b547 100644 --- a/launch/bno055_raw.launch.py +++ b/launch/bno055_raw.launch.py @@ -57,7 +57,7 @@ def generate_launch_description(): 'i2c_bus': 1, 'i2c_addr': [0x29,0x28], # Adafruit - 0x28, GY Clone - 0x29 (with both jumpers closed) 'data_query_frequency': 30, - 'calib_status_frequency': 0.1, + 'calib_status_frequency': 1.0, 'frame_id': 'imu_link', # Fast Magnetometer Calibration mode (FMC) provides faster magnetometer calibration # at the cost of slightly higher noise. NDOF_FMC_OFF is the default mode with slower calibration but lower noise.