Ask Your Question
1

Ardunio IMU (mpu6050) interfacing with ROS

asked 2015-12-20 11:07:24 -0500

ganesh gravatar image

updated 2015-12-20 16:45:12 -0500

I tried to interface IMU (mpu6050) which is controlled by Ardunio and it should send ROS messages (Publishing the coordinates) I tried the following code

#include <ros.h>
#include <std_msgs/Int8.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
std_msgs::Int8 ul_msg;
ros::Publisher pub_ul("temperature", &ul_msg);


MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;
// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}
// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {


        Wire.begin();
       // TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Comment this line if having compilation difficulties with TWBR.
        Serial.begin(57600);
        while (!Serial);
        Serial.println("Initializing I2C devices...");
        mpu.initialize();
        // verify connection
        Serial.println(F("Testing device connections..."));
        Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
        // wait for ready
        Serial.println(F("\nSend any character to begin DMP programming and demo: "));
        while (Serial.available() && Serial.read()); // empty buffer
        while (!Serial.available());                 // wait for data
        while (Serial.available() && Serial.read()); // empty buffer again
        // load and configure the DMP
        Serial.println(F("Initializing DMP..."));
       devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F ...
(more)
edit retag flag offensive close merge delete

Comments

did you try the code without the ROS part? so you can confirm that the arduino is in fact communicating with the IMU.

Gary Servin gravatar imageGary Servin ( 2015-12-20 16:47:12 -0500 )edit

does it show any output in serial monitor at all ?

nickw gravatar imagenickw ( 2015-12-21 03:32:13 -0500 )edit

3 Answers

Sort by ยป oldest newest most voted
1

answered 2015-12-21 17:27:55 -0500

fsteinhardt gravatar image

updated 2016-10-06 12:50:31 -0500

I also played around with the mpu6050 connected to an arduino and wrote myself a small ROS node which connects to an example script from i2cdevlib. You can find this node here:

https://github.com/fsteinhardt/mpu6050_serial_to_imu

Updated: now it reads the orientation, the accelerations and the rotational velocities .

edit flag offensive delete link more
1

answered 2015-12-21 03:46:34 -0500

updated 2015-12-21 04:55:17 -0500

when I compile that on my computer for an arduino uno I get

Sketch uses 22,598 bytes (70%) of program storage space. Maximum is 32,256 bytes.
Global variables use 1,746 bytes (85%) of dynamic memory, leaving 302 bytes for local variables. Maximum is 2,048 bytes.
Low memory available, stability problems may occur.

I would suspect that is your issue. If your code works without the ROS code in you could try removing all references to Serial to free up memory as the code runs, or try using a board with more memory on it.

When I compile it for a mega board I get

Sketch uses 23,604 bytes (9%) of program storage space. Maximum is 253,952 bytes.
Global variables use 2,222 bytes (27%) of dynamic memory, leaving 5,970 bytes for local variables. Maximum is 8,192 bytes.

I haven't got you IMU so can't test whether that is the case.

Running it on an arduino uno without the imu I get

Initializing I2C devices...
Testing device connections...
MPU6050 connection failed

Send any character to begin DMP programming and demo:

I just had a face palm moment too. You need to remove all the Serial prints you have anyway, as they will interfere with the node trying to make sense of the messages that are being sent to it by the ROS library over the serial port

You also have

ros::NodeHandle nh;

twice - inside setup, and outside between setup and loop.

You need to have it once, with global scope so put that up near the top like the examples they provide

edit flag offensive delete link more

Comments

I tried to putting it in the global scope .But it still does not show the output in the serial monitor. If I remove all the

ros::NodeHandle nh; and nh from the code it shows output in serial monitor but the ros does publich the data.

ganesh gravatar imageganesh ( 2015-12-21 22:02:55 -0500 )edit

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

ganesh gravatar imageganesh ( 2015-12-21 22:12:43 -0500 )edit

when I tried to remove all the serial print and tried to run this:"rosrun rosserial_python serial_node.py /dev/ttyUSB0" then it gave the above error

ganesh gravatar imageganesh ( 2015-12-21 22:15:37 -0500 )edit
1

answered 2015-12-21 22:02:27 -0500

ganesh gravatar image

updated 2015-12-21 22:12:21 -0500

I tried to putting it in the global scope .But it still does not show the output in the serial monitor. If I remove all the

ros::NodeHandle nh; and nh from the code it shows output in serial monitor but the ros does publich the data.

I also tried removing all serial prints it shows this error: [ERROR] [WallTime: 1450757476.810495] Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

Stats

Asked: 2015-12-20 11:07:24 -0500

Seen: 3,962 times

Last updated: Oct 06 '16