Ask Your Question
0

BNO05 IMU + ROS

asked 2017-05-01 14:07:01 -0500

MattMalt gravatar image

I've recently purchased the BNO05 IMU and wish to start publishing sensor_msgs/Imu data on my ROS network. The sensor is connected to an Arduino Uno board which is then connected to my computer. I've found sample code for the IMU which obtains quaternion data from the sensor and displays it on the serial monitor.

I've tried to modify the code so that a node is created which publishes the data in the sensor_msgs/IMU format. The problem is that once this extra code is added, the serial monitor starts outputting rubbish. I believe that this might be due to delays.

Does anyone have experience with this particular IMU?

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <ros.h>
#include <sensor_msgs/Imu.h>

/* 

   Connections

   ===========
   Connect SCL to analog 5
   Connect SDA to analog 4
   Connect VDD to 3-5V DC
   Connect GROUND to common ground

   History
   =======
   2015/MAR/03  - First release (KTOWN)
   2015/AUG/27  - Added calibration and system status helpers
*/

/* Set the delay between fresh samples */
#define BNO055_SAMPLERATE_DELAY_MS (1000)

sensor_msgs::Imu odom_msg;
ros::Publisher pub_odom("imuOdom", &odom_msg);
ros::NodeHandle nh;

Adafruit_BNO055 bno = Adafruit_BNO055(55);

/**************************************************************************/
/*
    Displays some basic information on this sensor from the unified
    sensor API sensor_t type (see Adafruit_Sensor for more information)
*
/
/**************************************************************************/
void displaySensorDetails(void)
{
  sensor_t sensor;
  bno.getSensor(&sensor);
  Serial.println("------------------------------------");
  Serial.print  ("Sensor:       "); Serial.println(sensor.name);
  Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);
  Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);
  Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" xxx");
  Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" xxx");
  Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" xxx");



Serial.println("------------------------------------");

  Serial.println("");
  delay(500);
}

/**************************************************************************/
/*
    Display some basic info about the sensor status
*/
/**************************************************************************/
void displaySensorStatus(void)
{
  /* Get the system status values (mostly for debugging purposes) */

  uint8_t system_status, self_test_results, system_error;
  system_status = self_test_results = system_error = 0;
  bno.getSystemStatus(&system_status, &self_test_results, &system_error);

  /* Display the results in the Serial Monitor */
  Serial.println("");
  Serial.print("System Status: 0x");
  Serial.println(system_status, HEX);
  Serial.print("Self Test:     0x");
  Serial.println(self_test_results, HEX);
  Serial.print("System Error:  0x");

  Serial.println(system_error, HEX);
  Serial.println("");
  delay(500);
}

/**************************************************************************/
/*
    Display sensor calibration status
*/
/**************************************************************************/

    void displayCalStatus(void)
{
  /* Get the four calibration values (0..3) */
  /* Any sensor data reporting 0 should be ignored, */
  /* 3 means 'fully calibrated" */
  uint8_t system, gyro, accel, mag;
  system = gyro = accel = mag = 0;
  bno.getCalibration(&system, &gyro, &accel, &mag);

  /* The data should be ignored until the system calibration is > 0 */
  Serial.print("\t");
  if (!system)
  {

    Serial.print("! ");
  }

  /* Display the individual values */
  Serial.print("Sys:");
  Serial.print(system, DEC);
  Serial.print(" G:");
  Serial.print(gyro, DEC);
  Serial.print(" A:");
  Serial.print(accel, DEC);
  Serial.print(" M:");

  Serial.print(mag, DEC);
}

/**************************************************************************/
/*
    Arduino setup function (automatically called at startup)
*/
/**************************************************************************/
void setup(void)
{ 
  nh.initNode(); 

  nh.advertise(pub_odom);

  Serial.begin(9600);
  Serial.println("Orientation Sensor Test"); Serial.println("");

  /* Initialise the sensor */
  if(!bno.begin())
  {
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2017-05-01 14:17:29 -0500

gvdhoorn gravatar image

updated 2017-05-01 14:18:40 -0500

The problem is that once this extra code is added, the serial monitor starts outputting rubbish. I believe that this might be due to delays.

No. You cannot use the same serial port for both rosserial and regular output (ie: Serial.begin(..), etc).

If you have more serial ports, use another one for the serial monitor. If you don't, don't do anything with it yourself (not even Serial.begin(..)), but let rosserial use it.

edit flag offensive delete link more

Comments

I've removed the serial outputs to the monitor, but I'm now getting this error message :

[ERROR] [WallTime: 1493716237.303846] Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino

MattMalt gravatar imageMattMalt ( 2017-05-02 04:12:49 -0500 )edit

There are multiple Q&A's about that here on ROS Answers. A first check: make sure you don't do anything with the serial port directly, anywhere in your code.

gvdhoorn gravatar imagegvdhoorn ( 2017-05-02 04:14:22 -0500 )edit

I found that the reason that the board wasn't connecting with ROS was because of the different baud rates. Entering the line nh.getHardware()->setBaud(57600); before nh.initNode() solved the problem, and I am seeing the topics published on ROS.

MattMalt gravatar imageMattMalt ( 2017-05-02 04:27:15 -0500 )edit

Good find. 'anything' was a bit strong.

9k6 is a bit on the slow side, so increasing it is indeed a good idea. On the ROS side, setting the baud rate is done using the baud parameter (wiki/rosserial_python - Parameters).

gvdhoorn gravatar imagegvdhoorn ( 2017-05-02 04:52:13 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2017-05-01 14:07:01 -0500

Seen: 658 times

Last updated: May 01 '17