Working off the shared I2C bus implementation I completed last week I started implementing sensor nodes. Deciding to begin with the IMU I thought, given its complexity, it would be better to find a library rather than write it myself. Thankfully Bosch, the company that makes the sensor provides an extremely over-engineered C library. Given that it hadn’t been updated in 7 years, I decided that vendoring the library would be the simplest solution.

As such, my folder structure became:

src
 -> sensors
     -> vendor
        -> BNO055_SensorAPI
           -> bno055.c
           -> bno055.h

Because the library should just be bundled with the sensor package I made sure to add it as static:

add_library(BNO055_SensorAPI STATIC vendor/BNO055_SensorAPI/bno055.c)

The one problem I ran into in doing this was that it was compiling with a different C version than the library required, and so failed to compile. This was as simple as mandating at least C standard 99:

set_target_properties(BNO055_SensorAPI PROPERTIES
  C_STANDARD 99
  C_STANDARD_REQUIRED ON
)

As for the node itself, I first started by writing a quick driver to interface with the library. This allowed me to do more robust error handling using std::error_code and std::expected to return errors by value.

The library is platform agnostic by providing a few functions which you need to provide implementations for that allow it to write to the I2C bus, read from the bus, and delay for a number of milliseconds. In writing these functions I realized that I somehow needed to smuggle in a pointer to the bus. It couldn’t be a static variable because in i2c_bus.hpp get_instance() I provided as shared pointer to a bus at runtime so that multiple nodes could access the bus simultanously. I looked at a couple possible solutions, but the easiest seemed to be just modifying the signatures of the bus write and read functions to include a userdata pointer argument. This was really easy—only a couple of lines—and simplified the driver code significantly;

I just added a userdata field to the bno055_t struct so that I could store the userdata pointer rather than passing it to every function call. This meant none of the other internal library code needed to change.

struct bno055_t
{
    u8 chip_id; /**< chip_id of bno055 */
    u16 sw_rev_id; /**< software revision id of bno055 */
    u8 page_id; /**< page_id of bno055 */
    u8 accel_rev_id; /**< accel revision id of bno055 */
    u8 mag_rev_id; /**< mag revision id of bno055 */
    u8 gyro_rev_id; /**< gyro revision id of bno055 */
    u8 bl_rev_id; /**< boot loader revision id of bno055 */
    u8 dev_addr; /**< i2c device address of bno055 */
    void *userdata; // ✅✅ new field here
    BNO055_WR_FUNC_PTR; /**< bus write function pointer */
    BNO055_RD_FUNC_PTR; /**<bus read function pointer */
    void (*delay_msec)(BNO055_MDELAY_DATA_TYPE); /**< delay function pointer */
};

Then I just modified the function signatures:

#define BNO055_WR_FUNC_PTR       s8 (*bus_write) \
        (u8, u8, u8 *, u8, void *)

#define BNO055_BUS_WRITE_FUNC(dev_addr, reg_addr, reg_data, wr_len) \
    bus_write(dev_addr, reg_addr, reg_data, wr_len, p_bno055->userdata)

#define BNO055_RD_FUNC_PTR       s8 \
    (*bus_read)(u8, u8, u8 *, u8, void *)

#define BNO055_BUS_READ_FUNC(dev_addr, reg_addr, reg_data, r_len) \
    bus_read(dev_addr, reg_addr, reg_data, r_len, p_bno055->userdata)

Then the code to actually setup the library was extremely simple:

  auto driver = std::unique_ptr<Bno055Driver>(new Bno055Driver(std::move(bus)));

  // hook up library functions
  driver->dev_.dev_addr = use_alternate_addr ? BNO055_I2C_ADDR2 : BNO055_I2C_ADDR1;
  driver->dev_.bus_write = &Bno055Driver::bus_write_callback;
  driver->dev_.bus_read = &Bno055Driver::bus_read_callback;
  driver->dev_.delay_msec = &Bno055Driver::delay_millis_callback;
  driver->dev_.userdata = driver->bus_.get();

The way the library was set up it expected bus_write and bus_read to take in a register address and a data buffer seperately. For the read function this was really easy, we just use the transfer function in I2cBus that I made last week to first send a register and then read into the buffer:

static s8 bus_read_callback(u8 dev_addr, u8 reg_addr, u8 *data, u8 len, void *userdata) noexcept
{
  auto *bus = static_cast<I2cBus *>(userdata);

  // write register and then read in data
  auto result = bus->transfer(dev_addr, {&reg_addr, 1}, {data, len});
  return result ? BNO055_SUCCESS : BNO055_ERROR;
}

For the write function however, I would have needed to copy the entire buffer just so I could provide a continous section of memory for the write function to act upon. The I2cBus::write() function has that weird API because it mirrors that of the system ioctl call. Anyways, I originally did that with a memcopy, but in doing so I needed to specify a maximum message length, which was the length of the buffer I was copying into. In doing so I grepped through the library and looked at how long each write actually was. It turned out fairly trivially to see that the library only every wrote one byte at a time. This meant that implementation was actually trivial. Given that this was going 500 meters under the ocean, however, I still put in a check:

static s8 bus_write_callback(u8 dev_addr, u8 reg_addr, u8 *data, u8 len, void *userdata) noexcept
{
  // library appears to only write length of one; enables avoiding large buffer allocation.
  // guarded against just in case.
  if (len != 1)
    return BNO055_ERROR;
  std::array<uint8_t, 2> buf = {reg_addr, data[0]};

  auto *bus = static_cast<I2cBus *>(userdata);

  auto result = bus->write(dev_addr, buf);
  return result ? BNO055_SUCCESS : BNO055_ERROR;
}

The only weirdness was the constructor where I needed to move the passed pointer into the class because copying a shared pointer is undefined behavior:

private:
  explicit Bno055Driver(std::shared_ptr<I2cBus> bus) : bus_(std::move(bus)) {}

  std::shared_ptr<I2cBus> bus_;
  bno055_t dev_{};

As for the node itself, I first wanted to make as much of node as possible configurable. As such a lot of the sheer volume of code is declaring and getting parameters. Some of these are fetched into local variables if they’re needed in the publish callback. The others are just const variables. Checking the ROS2 wiki I learned that you can specify allowed parameter ranges. Here’s a quick example for the publication rate:

rcl_interfaces::msg::ParameterDescriptor pub_rate_desc;
rcl_interfaces::msg::IntegerRange pub_rate_range;
pub_rate_range.from_value = 2;
pub_rate_range.to_value = 401;
pub_rate_range.step = 2;
pub_rate_desc.integer_range = {pub_rate_range};
this->declare_parameter("pub_rate_hz", kDefaultPubRateHz, pub_rate_desc);

This, combined with the ROS2 launch system, allows me to load clean config files:

sensors:
  imu_node:
    ros__parameters:
      pub_rate_hz: 100

      i2c_bus: 1
      use_alternate_i2c_addr: false

      enable_orientation: false
      enable_accel: true
      enable_gyro: true
      enable_mag: true

      imu_data_topic: imu/imu
      mag_data_topic: imu/mag

      link: imu_sensor_link

These config files, along with the launch files that load them are within the bringup package, structued as:

src
 -> bringup
     -> config
         -> common
         -> sim
         -> hardware
             -> imu.yaml
             -> temp_humid.yaml
             -> webrtc.yaml
     -> launch
         -> common
         -> sim
         -> hardware
             -> hardware.launch.py
             -> imu.launch.py
             -> temp_humid.launch.py
             -> webrtc.launch.py

This system allows each node to be launched together in a single container using hardware.launch.py, which avoids the overhead of intra-process communication. It also allows the launching of individual nodes for testing purposes.

The central hardware.launch.py file creates a container and loads each other launch file into it using the hardware_launch() function I wrote:

def hardware_launch(filename, args={}):
    return IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([FindPackageShare("bringup"), "launch", "hardware", filename])
        ]),
        launch_arguments=args.items()
    )

def generate_launch_description():
    container = ComposableNodeContainer(
        name='rov_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=[],
        output='screen',
    )

    return LaunchDescription([
        container, 
        hardware_launch("imu.launch.py", {'container': 'rov_container'}),
        hardware_launch("temp_humid.launch.py", {'container': 'rov_container'}),
        hardware_launch("webrtc.launch.py", {'container': 'rov_container'})
    ])

Each individual node has its own launch file, where it loads in its associated config file and then launches the node, into a container provided via a launch argument, defaulting to rov_container. This argument can be provided either, by hardware.launch.py, or, if launching individual nodes is desirable for testing purposes, in the command line itself.

After loading parameters, the IMU node itself is fairly simple, setting up topics to publish its data to, getting an I2C bys handle, loading the BNO055 driver using it, and then setting up a timer to run a callback that intermittently reads sensor data and publishes it to these topics.

The one interesting thing here that I needed to do was select the correct mode for the combination of enabled sensors. I ended up creating a quick sensor mode array:

static constexpr std::array<u8, 8> kSensorModeTable{
    0,                             // 000 - none (invalid)
    BNO055_OPERATION_MODE_GYRONLY, // 001
    BNO055_OPERATION_MODE_MAGONLY, // 010
    BNO055_OPERATION_MODE_MAGGYRO, // 011
    BNO055_OPERATION_MODE_ACCONLY, // 100
    BNO055_OPERATION_MODE_ACCGYRO, // 101
    BNO055_OPERATION_MODE_ACCMAG,  // 110
    BNO055_OPERATION_MODE_AMG,     // 111
};

That can be easily indexed as to derive the operation mode as such:

u8 op_mode;
if (enable_orientation_) {
  op_mode = BNO055_OPERATION_MODE_NDOF;
} else {
  // index into constant array to retrieve correct mode value for enabled sensors
  uint8_t idx = (enable_accel_ << 2) | (enable_mag_ << 1) | enable_gyro_;
  op_mode = kSensorModeTable[idx];
}

The actual callback is not really very interesting, except that a large portion of it is again dedicated to error handling.

When going to write the temperature/humidity sensor driver I originally went looking for libraries, but then realized when looking at the datasheet that it would be drastically easier to simply implement a driver myself than attempting to integrate something else as I did for the IMU. There was again not much interesting going on, except that I had to calculate the crc to ensure that the data was transmitted correctly, and continously poll the data if the measurement had not allready completed. I’m going to include the function in its totality because I think it is mildly interesting:

[[nodiscard]] std::expected<RawAht20Measurement, std::error_code> read_raw() noexcept
{
  auto result = bus_->write(kI2cAddr, kTriggerMeasurementMessage);
  if (!result)
    return std::unexpected(result.error());

  rclcpp::sleep_for(std::chrono::milliseconds(80));

  std::array<uint8_t, 7> measurement_data;
  uint16_t elapsed_ms = 80;
  bool timed_out = true;
  while (true) {
    auto result = bus_->read(kI2cAddr, measurement_data);
    if (!result)
      return std::unexpected(result.error());

    // if measurement completed
    if ((measurement_data[0] & kMeasurementBusyMask) == 0) {
      timed_out = false;
      break;
    }

    rclcpp::sleep_for(std::chrono::milliseconds(measure_wait_period_ms_));
    elapsed_ms += measure_wait_period_ms_;
    if (elapsed_ms >= measure_timeout_ms_)
      break;
  }

  if (timed_out) {
    return std::unexpected(Aht20ErrorCode::measurement_timed_out);
  }

  // loop over all bytes before the crc byte
  uint8_t crc = kCrcInitVal;
  for (uint8_t byte_idx = 0; byte_idx < measurement_data.size() - 1; ++byte_idx) {
    crc ^= measurement_data[byte_idx];
    for (uint8_t bit_idx = 0; bit_idx < 8; ++bit_idx) {
      if (crc & 0x80)
        crc = (crc << 1) ^ kCrcPolynomial;
      else
        crc <<= 1;
    }
  }

  if (crc != measurement_data[6])
    return std::unexpected(make_error_code(Aht20ErrorCode::crc_mismatch));

  // promote to uint32_t in one go to avoid a billion static casts
  const uint32_t b1 = measurement_data[1], b2 = measurement_data[2], b3 = measurement_data[3],
                  b4 = measurement_data[4], b5 = measurement_data[5];

  // Temperature and humidity are both 20 bits so we pack it into two uint32_ts
  return RawAht20Measurement{
      .temperature = (b1 << 12) | (b2 << 4) | (b3 >> 4),
      .relative_humidity = ((b3 & 0x0F) << 16) | (b4 << 8) | b5};
}

Lastly the actual node itself is very simple. The only real notable thing about it is that, for configuration it actually runs some runtime checks to ensure that the measurement timeout and poll period lie within the allowed limits. This is because these limits themselves depend on the values of other configuration parameters. This is done through a quick check and before throwing the std::invalid_argument errors. For example:

// limit maximum timeout to ensure that it doesn't exceed the update rate
const auto max_timeout = 1'000'000 / pub_rate_hz;
const auto measure_timeout = this->get_parameter("measurement_timeout_us").as_int();
if (measure_timeout > max_timeout) {
  auto msg = std::format(
      "measurement_timeout_us ({}) must be <= 1000000 / pub_rate_hz ({})",
      measure_timeout,
      max_timeout);
  RCLCPP_FATAL(get_logger(), "%s", msg.c_str());
  throw std::invalid_argument(msg);
}