2018-11-06 使用Arduino mega2560 将I

本文记录如何使用Arduino 么GA60将 IMU6050 的数据发送至ROS话题中

准备条件：
hardware：Arduino 2560 （或者Uno）、IMU6050 、串口线
software：Ubuntu （16.04 或其他）、 Arduino IDE、其他编辑工具

硬件连接(用Uno板代替了)：

image.png

Arduino 的5V和GND分别连接IMU模块的VCC和GND，Arduino另一侧的2号、20号（SDA）、21号（SCL）管脚分别连接IMU模块的INTA、SDA、SCL引脚。
在Linux 中的Arduino IDE中安装ros_lib ，安装教程参考：http://wiki.ros.org/rosserial_arduino/Tutorials/Arduino%20IDE%20Setup
正确安装后会有： Arduino IDE->File->Example->ros_lib

image.png

代码烧写：

// These are the Arduino headers to read IMU values using I2C protocoal. 
//It also include Arduino - MPU 9150 headers for performing special functions.

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

//Creating a MPU6050 handle which can be used for MPU 9250
MPU6050 mpu;

// These are the ROS headers for getting ROS Client API's.
#include <ros.h>

//Header for Vector3 ROS message
#include <geometry_msgs/Vector3.h>

//Header for TF broadcast
#include <tf/transform_broadcaster.h>

//These are the object handlers of TF message and broadcaster 
geometry_msgs::TransformStamped t;
tf::TransformBroadcaster broadcaster;


//Creating handlers of Node, IMU message, quaternion and ROS publisher.
ros::NodeHandle nh;

//Creating orientation message header 
geometry_msgs::Vector3 orient;

//Creating ROS publisher object for IMU orientation
ros::Publisher imu_pub("imu_data", &orient);

//The frame_id helps to visulize the Transform data of IMU w.r.t this link
char frameid[] = "/base_link";
char child[] = "/imu_frame";


//Defining an LED pin to show the status of IMU data read
#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
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector


// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

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



// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================
// This is the setup function of Arduino
// This will initialize I2C communication, ROS node handle, TF publisher, ROS publisher and DMP of IMU 
void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    Wire.begin();
    
    nh.initNode();
    broadcaster.init(nh);

    nh.advertise(imu_pub);

    mpu.initialize();

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

        // enable Arduino interrupt detection
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
      
          ;
          }

    // configure LED for output
    pinMode(LED_PIN, OUTPUT);
}



// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
    // if programming failed, don't try to do anything
    
    
    
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
          ;
    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();


    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus & 0x01) {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

        
            // display quaternion values in easy matrix form: w x y z
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

            //Assigning YAW,PITCH,ROLL to vector message and publishing the values
            
            orient.x = ypr[0] * 180/M_PI;
            orient.y = ypr[1] * 180/M_PI;
            orient.z = ypr[2] * 180/M_PI;
            imu_pub.publish(&orient);

            //Assigning values to TF header and publish the transform
            t.header.frame_id = frameid;
            t.child_frame_id = child;
            t.transform.translation.x = 1.0; 
            t.transform.rotation.x = q.x;
            t.transform.rotation.y = q.y; 
            t.transform.rotation.z = q.z; 
            t.transform.rotation.w = q.w;  
            t.header.stamp = nh.now();
            broadcaster.sendTransform(t);



            nh.spinOnce();
   

            delay(200);
          
            // blink LED to indicate activity
            blinkState = !blinkState;
            digitalWrite(LED_PIN, blinkState);
            delay(200);
    }
}

这样Arduino就可以作为一个节点向 imu_data 这个话题发布姿态数据了。

下面需要在Linux 系统中跑主节点以及读取串口的节点来获取Arduino 发布的姿态数据（Arduino通过串口与Linux系统通信）
先把主节点跑起来：

roscore

重新开启一个终端，将rosserial节点跑起来：

rosrun rosserial_python serial_ne.py _port:=/dev/ttyACM0 _baud:=230400

注意上面的_port:=/dev/ttyACM0 _baud:=230400 需要根据自己的实际设置情况进行配置，如果不匹配的话无法成功发布。
至此我们可以获取到IMU的数据了：

 rosrun rosserial_python serial_ne.py _port:=/dev/ttyACM0 _baud:=230400
[INFO] [1541515772.944421]: ROS Serial Python Node
[INFO] [1541515772.948151]: Connecting to /dev/ttyACM0 at 230400 baud
[INFO] [1541515776.648450]: Note: publish buffer size is 512 bytes
[INFO] [1541515776.648876]: Setup publisher on /tf [tf/tfMessage]
[INFO] [1541515776.654233]: Setup publisher on imu_data [geometry_msgs/Vector3]

使用命令查看话题数据：
rostopic echo /tf

transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1541515927
        nsecs: 513689098
      frame_id: "/base_link"
    child_frame_id: "/imu_frame"
    transform: 
      translation: 
        x: 1.0
        y: 0.0
        z: 0.0
      rotation: 
        x: -0.335266113281
        y: 0.061767578125
        z: 0.086181640625
        w: 0.936096191406

查看IMU数据：
rostopic echo imu_data

x: -14.7708902359
y: -10.0628929138
z: -37.8008956909
---
x: -14.7714719772
y: -10.0682411194
z: -37.7910270691
---
x: -14.7731533051
y: -10.0694589615
z: -37.7995758057
---
x: -14.7645750046
y: -10.0606603622
z: -37.801990509