how to improve 9-axis-motion-shield update rate working with rosserial

Hi guys,

I am working on the project which need at least 100hz IMU sensor update rate translated to ROS using rosserial_arduinno. So, I got a arduino Mega2560 attached a 9-axis-motion-shield which integrated a BNO055 IMU. In the user guide of BNO099, they said they can get 100hz in the imu fusion mode. But I tried many methods, like changing the operation mode or update mode, I still can only get most 18hz update rate.
The following is my arduino code:

#include <ros.h>
#include <ros/time.h>
#include <sensor_msgs/Imu.h>
#include "NAxisMotion.h"
#include <Wire.h>

ros::NodeHandle nh;

sensor_msgs::Imu imu_msg;
ros::Publisher imu_pub("/imu_data", &imu_msg);

NAxisMotion myImu;

unsigned long previousMillis = 0;
unsigned long counter = 0;
const long interval = 10;

//unsigned long lastStreamTime = 0;
//const int streamPeriod = 10;

char odom[] = "/imu0";

void setup() {
  // put your setup code here, to run once:
  Wire.begin();

  nh.initNode();
  nh.advertise(imu_pub);

  myImu.initSensor();
  myImu.setOperationMode(OPERATION_MODE_NDOF);
  myImu.setUpdateMode(MANUAL);
  myImu.setPowerMode(POWER_MODE_NORMAL);
  //myImu.writeAccelConfig(ACCEL_RANGE_2G, ACCEL_BW_250HZ, ACCEL_NORMAL);
  //myImu.updateAccelConfig();

}

void loop() {
  // put your main code here, to run repeatedly:
    myImu.updateLinearAccel();
    myImu.updateGyro();
    myImu.updateQuat();
    //unsigned long currentMillis = millis();
    //if (currentMillis - previousMillis >= interval){

    imu_msg.header.frame_id = odom;
    imu_msg.header.seq = counter;
    imu_msg.header.stamp = nh.now();
    imu_msg.orientation.x = myImu.readQuatX();
    imu_msg.orientation.y = myImu.readQuatY();
    imu_msg.orientation.z = myImu.readQuatZ();
    imu_msg.orientation.w = myImu.readQuatW();
    
    imu_msg.angular_velocity.x = myImu.readGyroX();
    imu_msg.angular_velocity.y = myImu.readGyroY();
    imu_msg.angular_velocity.z = myImu.readGyroZ();
    
    imu_msg.linear_acceleration.x = myImu.readAccelX();
    imu_msg.linear_acceleration.y = myImu.readAccelY();
    imu_msg.linear_acceleration.z = myImu.readAccelZ();
    
    counter++;

    imu_pub.publish(&imu_msg); 
    nh.spinOnce();
    //}

}

I use rostopic to check the imu data rate

rostopic hz /imu_data

And this is the output

subscribed to [/imu_data]
average rate: 18.125
	min: 0.053s max: 0.058s std dev: 0.00207s window: 18
average rate: 18.066
	min: 0.053s max: 0.058s std dev: 0.00207s window: 36
average rate: 18.072
	min: 0.053s max: 0.058s std dev: 0.00207s window: 54
average rate: 18.057
	min: 0.053s max: 0.058s std dev: 0.00206s window: 72
average rate: 18.062
	min: 0.053s max: 0.058s std dev: 0.00205s window: 90

Does anyone knows where is the problem? I really need your help. Thank you for any reply!

If in fusion mode, why are you reading extraneous variables like raw gyro and acc outputs? All those single variable reads have tremendous overhead.

You can get three Euler angles or the quaternion in one single read operation.

How long does this operation take?

imu_pub.publish(&imu_msg);

Why do you do this?

    myImu.updateLinearAccel();
    myImu.updateGyro();
    myImu.updateQuat();

Hi jremington,

First, thank you for reply. According to the API that the arduino 9 axes motion shield document offers, I cannot use a single line to read the Euler angles or Quaternions.

As to the Publish(), the time it takes depends on how large is the imu_msg. In the official document, the publish() should be very fast.

Thank you for help!
cheers.

Don’t use that idiotic API, then. Use I2C directly, like this:

    // get current orientation angles
    I2C_Read3Vector(BNO055_A0, BNO055_FUSED_EULER, (int16_t *)v);

    for (i = 0; i < 3; i++) v[i] >>= 4; //divide by 16 to get integer degrees
    
    sprintf(buf, "Euler %4d %4d %4d", v[0], v[1], v[2]);
    Serial.println(buf);

The function:

// read 3 int16 values from successive BNO055 registers (using autoincrement)

void I2C_Read3Vector(unsigned char busAddr, unsigned char deviceRegister, int* vector) {

  unsigned int data = 0;
  unsigned char l;
  I2C_Start(busAddr); // send device address
  I2C_Write(deviceRegister); // set register pointer
  I2C_Start(busAddr + READBIT); // restart as a read operation
  l = I2C_ReadACK(); // read the register data
  data = I2C_ReadACK(); //read high byte
  *vector++ = ((data << 8) | l); //store X
  l = I2C_ReadACK();
  data = I2C_ReadACK();
  *vector++ = ((data << 8) | l); //store Y
  l = I2C_ReadACK();
  data = I2C_ReadNACK();
  *vector = ((data << 8) | l); //store Z
  I2C_Stop();
}

the publish() should be very fast.

“very fast” is meaningless, especially if the data go out the serial port.