Optimizations and refactoring

AUTHORS

@@ -2,3 +2,4 @@ Michal Drweiga
 Evan Flynn
 Manfred Novotny
 Pascal Voser
+Sergei Grichine

CHANGELOG.rst

@@ -1,6 +1,13 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 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
+* 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
 
 0.5.0 (2024-02-17)
 ------------------

README.md

@@ -1,3 +1,21 @@
+------------------------
+
+**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.
+
+**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:
+
+------------------------
+
 # A BNO05 ROS2 Package 
 
 ## Description
@@ -47,15 +65,15 @@ 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
 
 - **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".   
 

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,90 +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
-                return
-            except ZeroDivisionError:
-                # division by zero in get_sensor_data, return
-                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__':

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
 
@@ -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.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.get_logger().info(f"   address: 0x{self.address: 02x}  chip ID: 0x{returned_id: 02x} ✓ (connected)")
+                    break
+            except Exception as e:
+                self.node.get_logger().info(f"   i2c address 0x{addr: 02x} failed to connect")
+                continue
+
+        if self.address is None:
+            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):
         """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.