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..24cbd11 100644 --- a/CHANGELOG.rst +++ b/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) ------------------ diff --git a/README.md b/README.md index 7d2148e..20f5322 100644 --- a/README.md +++ b/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,7 +65,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 @@ -55,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/bno055.py b/bno055/bno055.py index 950a863..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,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__': diff --git a/bno055/connectors/i2c.py b/bno055/connectors/i2c.py index d71e5c2..45d45fb 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 @@ -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. diff --git a/bno055/params/NodeParameters.py b/bno055/params/NodeParameters.py index 947fdaf..a7f191b 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 @@ -70,15 +70,15 @@ 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) + 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..8bbe4fd 100644 --- a/bno055/params/bno055_params.yaml +++ b/bno055/params/bno055_params.yaml @@ -35,15 +35,15 @@ 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: 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..c7ee699 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 + calib_status_frequency: 1.0 + 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..3bcf1ca 100644 --- a/bno055/params/bno055_params_test.yaml +++ b/bno055/params/bno055_params_test.yaml @@ -34,15 +34,15 @@ 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: 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 10f1cf3..6318d64 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 Quaternion, 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 @@ -52,17 +53,76 @@ 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) - 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): + 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. + # Publish only if the reading is valid according to jump detection logic + # + + # Jump detection state variable: + 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.""" self.node.get_logger().info('Configuring device...') @@ -130,130 +190,146 @@ 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.') self.node.get_logger().info('Bosch BNO055 IMU configuration complete.') + 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 + + # 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 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 - - # 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] - ] - 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] - ] - # 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 - - 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 - self.pub_imu.publish(imu_msg) - - # 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] - ] - self.pub_mag.publish(mag_msg) - - 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 - 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) + if not buf or len(buf) != 45: + self.node.get_logger().warn(f"Short read: got {len(buf) if buf else 0} bytes") + return + + # 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 + + # 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: + 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 + if self._prev_q is not None and float(np.dot(self._prev_q, qn)) < 0.0: + qn = -qn + self._prev_q = qn + + # Header (single timestamp) + self._imu_msg.header.stamp = now_msg + self._imu_msg.header.frame_id = frame_id + + 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 + + # 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 + + # “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 + + self.pub_imu.publish(self._imu_msg) + + # 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 + + # 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 + + # 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 + + # 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): """ @@ -262,13 +338,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) @@ -405,6 +481,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..7d34a50 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': 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. + # 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..b68b547 --- /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': 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. + # 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 453898a..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 @@ -11,7 +11,7 @@ python3-serial - python3-smbus + python3-smbus2 rclpy 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']),