美文网首页
2018-11-06 使用Arduino mega2560 将I

2018-11-06 使用Arduino mega2560 将I

作者: 我爱做笔记 | 来源:发表于2018-11-06 22:57 被阅读0次

    本文记录如何使用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
    

    相关文章

      网友评论

          本文标题:2018-11-06 使用Arduino mega2560 将I

          本文链接:https://www.haomeiwen.com/subject/mjmpxqtx.html