三、ROS小车闭环控制：树莓派 3B 接MPU6050方法 以及ROS应用（2）

上一篇我们已经实现了硬件层面的连接并使用python脚本验证确实是通的。

那么这一次没什么说的，上ROS。

本人使用树莓派3B参考这个教程有效：

https://github.com/chrisspen/ros_mpu6050_node

里面用到的比较出名的库“MPU6050_6Axis_MotionApps20.h”在这：

https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/MPU6050_6Axis_MotionApps20.h

注意：源程序自身少输出一些东西，而且有一些来源不明的变量。简要来说就是只要在源程序(mpu6050.cpp)开头的部分保持当前状态这两句不要注释掉，其他的不要好奇而解开注释即可：

#define OUTPUT_READABLE_YAWPITCHROLL
#define OUTPUT_READABLE_REALACCEL

而其他的不要解开，不然会报错(不细说了)。

另外，源程序输出的angular_velocity是错误的，目前其实输出的是三个欧拉角。

angular_velocity应该是两个相邻时刻角度偏差的除以时间，这个需要你自己去编写，基本思路就是保存上一时刻的角度与当前角度求差然后除以真实时间（s）。

IMU的完整信息是参见：http://docs.ros.org/jade/api/sensor_msgs/html/msg/Imu.html

sensor_msgs/Imu Message 信息需要以下信息

Header header

geometry_msgs/Quaternion orientation
float64[9] orientation_covariance # Row major about x, y, z axes

geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance # Row major about x, y, z axes

geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance # Row major x, y z 

这样就可以完整的输出以上信息。

下面进入正题：

一、安装I2Cdevlib：

树莓派中装上I2C驱动：

sudo mkdir -p /usr/share/arduino/libraries
cd /usr/share/arduino/libraries
sudo git clone https://github.com/chrisspen/i2cdevlib.git

原作者说明：这个I2Cdevlib有bug且不再维护了，因此要用他这个fork。

二、安装Bcm2835:

放心，跟上一篇不冲突！直接安装就行。

cd /tmp
wget http://www.airspayce.com/mikem/bcm2835/bcm2835-1.50.tar.gz
tar zxvf bcm2835-1.50.tar.gz
cd bcm2835-1.50
./configure
make
make check
sudo make install

三、ROS实现

然后把这位作者的ROS工程下载到你的工作空间的catkin_ws/src目录里面（常规操作）

git clone https://github.com/chrisspen/ros_mpu6050_node.git

然后切换到工作空间主目录catkin_ws，运行以下指令，只编译了这个包。有可能你需要source你的setup.bash在运行这句，也有可能需要将你的build清理一下，全都重新catkin_make，这是ROS编译的一些问题，各位遇到的问题可能不同，不过到此就只剩ROS的基本操作，而非MPU的用法了。

catkin_make --pkg ros_mpu6050_node

四、测试

先source再roslaunch

source /catkin_ws(你的工作空间)/devel/setup.bash

然后：

roslaunch ros_mpu6050_node mpu6050.launch

正常的话，应该最后会提示“DMP ready!”成功，我遇到过不成功还要再launch一遍的，挺奇怪。此launch自带清零效果。

五、观察或使用

rostopic echo /imu/data

可以看出，发布的topic是/imu/data，类型是 sensor_msgs::Imu 那么你的ros工程订阅这个topic就好了。

目前默认是10Hz，可以按照自己的需求修改launch，我还没测试是否也要改源文件才行。

这个launch文件也可以很轻松的跟你已有工程结合。

六、进一步的思考

便宜的东西自然不能追求质量，那么这个MPU6050的稳定性和准确度想要保证，可能要滤波了。这是下一步的工作。

而且MPU6050也有参数设置，可以参考这个手册：https://www.olimex.com/Products/Modules/Sensors/MOD-MPU6050/resources/RM-MPU-60xxA_rev_4.pdf

加速度的信息与坐标系的关系也是需要考虑的问题。

继续研究。