rosserial problem displaying linear_acceleration values from IMU

asked 2018-07-08 12:33:20 -0500

hemanthm713 gravatar image

I have written a code for Arduino to connect with rosserial and publish on ros. However, it displays all the values correctly except linear_acceleration. I have changed the baud rate, call function from "vector.x()" to "vector[0]" and rechecked my sensor_msg/Imu.h file too. Everything looks correct. My Arduino code is as follows

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <ros.h>
#include <sensor_msgs/Imu.h>
#define BNO055_SAMPLERATE_DELAY_MS (10)
sensor_msgs::Imu odom_msg;
ros::Publisher pub_odom("imu0", &odom_msg);
ros::NodeHandle nh;
Adafruit_BNO055 bno = Adafruit_BNO055(10);
void setup(void)
{ 
  nh.initNode(); 
  nh.advertise(pub_odom);
  Serial.begin(115200);
  Serial.println(F("Orientation Sensor Test")); Serial.println(F(""));
  /* Initialise the sensor */
  if(!bno.begin())
  {
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.print(F("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"));
    while(1);
  }
  delay(10);
}
void loop(void)
{
  imu::Quaternion quat = bno.getQuat();
  odom_msg.orientation.x = quat.x();
  odom_msg.orientation.y = quat.y();
  odom_msg.orientation.z = quat.z();
  odom_msg.orientation.w = quat.w();
  //odom_msg.orientation_covariance[0] = -1;
  imu::Vector<3> angaccel = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
  odom_msg.angular_velocity.x = angaccel[0];
  odom_msg.angular_velocity.y = angaccel[1];
  odom_msg.angular_velocity.z = angaccel[2];
  imu::Vector<3> euler1 = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
  odom_msg.linear_acceleration.x = euler1[0];
  odom_msg.linear_acceleration.y = euler1[1];
  odom_msg.linear_acceleration.z = euler1[2];
  //odom_msg.linear_acceleration.x = euler1.x();
  //odom_msg.linear_acceleration.y = euler1.y();
  //odom_msg.linear_acceleration.z = euler1.z();
  odom_msg.angular_velocity_covariance[0] = 0;
  odom_msg.linear_acceleration_covariance[0] = 0;
  odom_msg.header.frame_id = "imu";
  odom_msg.header.stamp = nh.now();
  pub_odom.publish(&odom_msg);
  nh.spinOnce();
  delay(BNO055_SAMPLERATE_DELAY_MS);
}

On Arduino serial monitor with a similar code, i am able to get all values correctly. But not on rosserial. The ourput on rosserial is the following

---
header: 
  seq: 311
  stamp: 
    secs: 1531067114
    nsecs:  53815114
  frame_id: imu
orientation: 
  x: -0.0120849609375
  y: -0.0156860351562
  z: 0.0182495117188
  w: 0.999633789062
orientation_covariance: [0.0, 0.0, 0.0, 605297.1875, 9.805493222936335e-12, 6.358072525405582e-30, -8.836671222230488e-18, 6.8332662107650395e-34, 7.80629698470661e-36]
angular_velocity: 
  x: -0.00111111113802
  y: -0.00111111113802
  z: 0.00222222227603
angular_velocity_covariance: [0.0, 2.375406719749005e-36, -37636.5, -601862893993984.0, -37759.015625, 3.8772519442846755e-34, 2.6677259486100874e-23, 3.56086808692405e-310, 4.425906002008659e-23]
linear_acceleration: 
  x: 1.26991024075e-33
  y: 6.96170055628e-309
  z: 8.84134401092e-36
linear_acceleration_covariance: [9.091709527866322e-34, 4.673124563523994e+16, 1.9121903536199017e-308, 1.956152820753018e-38, -0.0004948825226165354, -1.010046607868369e+22, -5.629338932247528e+25, -4.8146399716934285e-28, 0.019999999552965164]
---

I am not worried about the covariance values, as i dont require them, but the linear_acceleration values don't change with time, even under a lot of moment of imu. In the code, I even tried to set the value of published linear_acceleration to 0 but still shows that constant value. Please help me, if anyone is able to figure out my mistake.

edit retag flag offensive close merge delete