Twist with covariance stamped Compass heading

edit

My team and I are fairly new to ROS and we are trying to make an autonomous robot and we are using a compass to tell our heading.

The problem was we were using a borrowed IMU and acted like a compass using gyros and we got our robot to work decent with that IMU but created a lot of drift.

So we are using now a Ada Fruit LSM303DLHC compass and accelerator which has very little drift the only thing I'm struggling with is trying to get ros my arduino code to spit out a geometry message twist with covariance stamped, because that is what we had our old IMU spitting out and it worked well.

I have the arduino code that reads the heading and I have already calibrated the compass offsets. I just need to get my code to output that type of message. if anyone can help that would be greatly appreciated.

Attached you should find a picture of what the IMU was publishing.

The code is publishing integers to the Serial monitor a the moment.

Here is the heading code:

#include <Wire.h>
#include <LSM303.h>

LSM303 compass;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  compass.init();
  compass.enableDefault();

  /*
  Calibration values; the default values of +/-32767 for each axis
  lead to an assumed magnetometer bias of 0. Use the Calibrate example
  program to determine appropriate values for your particular unit.
  */
  compass.m_min = (LSM303::vector<int16_t>){ -4096,  -4096,  -4096};
  compass.m_max = (LSM303::vector<int16_t>){ +1497,  +1969,  +1482};
}

void loop() {
  compass.read();

  /*
  When given no arguments, the heading() function returns the angular
  difference in the horizontal plane between a default vector and
  north, in degrees.

  The default vector is chosen by the library to point along the
  surface of the PCB, in the direction of the top of the text on the
  silkscreen. This is the +X axis on the Pololu LSM303D carrier and
  the -Y axis on the Pololu LSM303DLHC, LSM303DLM, and LSM303DLH
  carriers.

  To use a different vector as a reference, use the version of heading()
  that takes a vector argument; for example, use

    compass.heading((LSM303::vector<int>){0, 0, 1});

  to use the +Z axis as a reference.
  */
  float heading = compass.heading();

  Serial.println(heading);
  delay(100);
}