本文记录如何使用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
//Header for Vector3 ROS message
#include
//Header for TF broadcast
#include
//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