测试环境：
Hardware : BCM2835 -- I2C -- MPU6050
Model : Raspberry Pi 4 Model B Rev 1.4
CPU : 4x Cortex A72
Linux version 5.4.0-1052-raspi (buildd@bos02-arm64-043) (gcc version 9.3.0 (Ubuntu 9.3.0-17ubuntu1~20.04)) #58-Ubuntu SMP PREEMPT Mon Feb 7 16:52:35 UTC 2022
Orb-SLAM3: V1.0, December 22th, 2021
ROS : noetic
IMU : MPU6050
测试过程：
1、硬件连接：参照 ros树梅派4bmpu6050 - miaorn - 博客园
MPU6050模块连接树莓派4B：P1(PWR3.3V) ,P3(SDA), P5(SCL), P6(GND)
2、测试MPU6050 I2C通信，用 i2cdetect 确认通信正常：参照如上链接
3、下载安装 bcm2835 的通信端口驱动（包含I2C的驱动）： http://www.airspayce.com/mikem/bcm2835/bcm2835-1.71.tar.gz
参考 bcm2835: C library for Broadcom BCM 2835 as used in Raspberry Pi
4、写一个简单的测试代码，通过安装的I2C驱动读写MPU6050：
(1)测试代码如下：
#include#include #include "bcm2835.h" int main() { uint8_t temp = 0; printf("Hello BCM2835!rn"); bcm2835_set_debug(0); if(!bcm2835_init()) return 1; bcm2835_i2c_begin(); bcm2835_i2c_setSlaveAddress(0x68); bcm2835_i2c_set_baudrate(100000); // 100KB int cnt =1; while(cnt>0) { temp = 0x75; if(bcm2835_i2c_write((char*)&temp, 1) == BCM2835_I2C_REASON_OK) { printf("write = 0x%x, ", temp); bcm2835_i2c_read((char*)&temp, 1); printf("read = 0x%xrn", temp); } bcm2835_delay(1000); cnt--; } bcm2835_i2c_end(); bcm2835_close(); return 0; }
gland@gland-desktop:~/Downloads/Mpu6050/bcm2835-1.71/examples/i2c$ gcc -o i2ctest i2ctest.c -l bcm2835
gland@gland-desktop:~/Downloads/Mpu6050/bcm2835-1.71/examples/i2c$ sudo ./i2ctest
Hello BCM2835! enter bcm2835_i2c_set_baudrate:100000 enter bcm2835_i2c_setClockDivider:2500 bcm2835_i2c_setClockDivider paddr:0x8e4a3014 0x8dea4000,0x8e4a3000 write = 0x75, read = 0x68
注意：需要在root权限下运行，否则会出现： Segmentation fault.
5、创建IMU的ROS node ：
（1）创建ros node工程：
gland@gland-desktop:~/Downloads/Mpu6050$ mkdir -p mpu6050_ws/src gland@gland-desktop:~/Downloads/Mpu6050$ cd mpu6050_ws/src/ gland@gland-desktop:~/Downloads/Mpu6050/mpu6050_ws/src$ catkin_init_workspace gland@gland-desktop:~/Downloads/Mpu6050/mpu6050_ws/src$ cd ../ gland@gland-desktop:~/Downloads/Mpu6050/mpu6050_ws$ catkin_make
在ubuntu20.04中存在多个版本python的情况下，会出现如下错误：
-- Found Python: /usr/bin/python3.8 (found version "3.8.10") found components: Interpreter -- Using PYTHON_EXECUTABLE: -- Using Debian Python package layout -- Could NOT find PY_em (missing: PY_EM) CMake Error at /opt/ros/noetic/share/catkin/cmake/empy.cmake:30 (message): Unable to find either executable 'empy' or Python module 'em'... try installing the package 'python3-empy' Call Stack (most recent call first): /opt/ros/noetic/share/catkin/cmake/all.cmake:164 (include) /opt/ros/noetic/share/catkin/cmake/catkinConfig.cmake:20 (include) CMakeLists.txt:58 (find_package)
需要指定python版本，这里为python3:
gland@gland-desktop:~/Downloads/Mpu6050$ cd mpu6050_ws/ gland@gland-desktop:~/Downloads/Mpu6050/mpu6050_ws$ catkin_make -DPYTHON_EXECUTABLE=/usr/bin/python3
（2）、下载mpu6050的ROS node代码，并解压到catkin工作空间的src目录下：
GitHub - chrisspen/ros_mpu6050_node: ROS driver for the MPU6050 IMU and DMP.https://github.com/chrisspen/ros_mpu6050_node
GitHub - chrisspen/ros_mpu6050_node: ROS driver for the MPU6050 IMU and DMP.https://github.com/chrisspen/ros_mpu6050_node
（3）、下载ros_mpu6050_node的i2c 库，并解压复制到/usr/share/arduino/libraries目录下（该路径由ros_mpu6050_node/CMakelist.txt指定），更名为i2cdevlib:
https://github.com/jrowberg/i2cdevlib/archive/refs/heads/master.ziphttps://github.com/jrowberg/i2cdevlib/archive/refs/heads/master.zip
gland@gland-desktop:~/Downloads/Mpu6050$ ls bcm2835-1.50.tar.gz i2cdevlib-master.zip ros_mpu6050_node-master.zip bcm2835-1.71 imu_ws bcm2835-1.71.tar.gz mpu6050_ws gland@gland-desktop:~/Downloads/Mpu6050$ unzip i2cdevlib-master.zip gland@gland-desktop:~/Downloads/Mpu6050$ sudo mkdir /usr/share/arduino/libraries gland@gland-desktop:~/Downloads/Mpu6050$ sudo mv i2cdevlib-master /usr/share/arduino/libraries/ gland@gland-desktop:~/Downloads/Mpu6050$ sudo mv /usr/share/arduino/libraries/i2cdevlib-master /usr/share/arduino/libraries/i2cdevlib
（4）、在工程目录下再次编译ros_mpu6050_node:
gland@gland-desktop:~/Downloads/Mpu6050$ cd mpu6050_ws/ gland@gland-desktop:~/Downloads/Mpu6050/mpu6050_ws$ catkin_make -DPYTHON_EXECUTABLE=/usr/bin/python3
（5）、在运行roscore的条件下，配置并运行：
gland@gland-desktop:~/Downloads/Mpu6050/mpu6050_ws$ source devel/setup.bash gland@gland-desktop:~/Downloads/Mpu6050/mpu6050_ws$ source devel/setup.bash gland@gland-desktop:~/Downloads/Mpu6050/mpu6050_ws$ rosrun ros_mpu6050_node mpu6050_node [ INFO] [1646792684.153427382]: Starting mpu6050_node... Using sample rate: 10 Using frame_id: base_imu ADO: 0 Debug: 0 Initializing I2C... bcm2835_init: Unable to open /dev/gpiomem: Permission denied enter bcm2835_i2c_set_baudrate:400000 enter bcm2835_i2c_setClockDivider:624 bcm2835_i2c_setClockDivider paddr:0x13 0xffffffff,0xffffffff Segmentation fault (core dumped)
权限不够，进入管理员权限下执行：
gland@gland-desktop:~/Downloads/Mpu6050/mpu6050_ws$ su Password: root@gland-desktop:/home/gland/Downloads/Mpu6050/mpu6050_ws# source devel/setup.bash root@gland-desktop:/home/gland/Downloads/Mpu6050/mpu6050_ws# rosrun ros_mpu6050_node mpu6050_node [ INFO] [1646792754.154280268]: Starting mpu6050_node... Using sample rate: 10 Using frame_id: base_imu ADO: 0 Debug: 0 Initializing I2C... enter bcm2835_i2c_set_baudrate:400000 enter bcm2835_i2c_setClockDivider:624 bcm2835_i2c_setClockDivider paddr:0xa3004014 0xa2a05000,0xa3004000 Testing device connections...
（6）与MPU6050模块I2C通信不成功，需在如下文件中修改通信波特率：
gland@gland-desktop:~/Downloads/Mpu6050$ sudo vi /usr/share/arduino/libraries/i2cdevlib/RaspberryPi_bcm2835/I2Cdev/I2Cdev.h
从400000改为100000：
//#define i2c_baudrate 400000 #define i2c_baudrate 100000 //uint32_t i2c_baudrate = 400000 ; //400 kHz,
再次运行：
gland@gland-desktop:~/Downloads/Mpu6050/mpu6050_ws$ su Password: root@gland-desktop:/home/gland/Downloads/Mpu6050/mpu6050_ws# source devel/setup.bash root@gland-desktop:/home/gland/Downloads/Mpu6050/mpu6050_ws# rosrun ros_mpu6050_node mpu6050_node [ INFO] [1646794786.612285843]: Starting mpu6050_node... Using sample rate: 10 Using frame_id: base_imu ADO: 0 Debug: 0 Initializing I2C... enter bcm2835_i2c_set_baudrate:100000 enter bcm2835_i2c_setClockDivider:2500 bcm2835_i2c_setClockDivider paddr:0x7d801014 0x7d202000,0x7d801000 Testing device connections... MPU6050 connection successful Initializing I2C devices... Initializing DMP... Setting X accel offset: 0 Setting Y accel offset: 0 Setting Z accel offset: 0 Setting X gyro offset: 0 Setting Y gyro offset: 0 Setting Z gyro offset: 0 Enabling DMP... DMP ready!
如上界面说明运行ros_mpu6050_node成功！
6、读取和显示IMU数据：
另开一个终端，运行：
gland@gland-desktop:~$ rostopic list /imu/data /imu/euler /imu/mag /rosout /rosout_agg gland@gland-desktop:~$ rostopic echo /imu/data
输出结果如下：
---
header:
seq: 941
stamp:
secs: 1646794928
nsecs: 325980730
frame_id: "base_imu"
orientation:
x: -0.69384765625
y: -0.15570068359375
z: -0.10107421875
w: 0.69580078125
orientation_covariance: [0.00030461741978670857, 0.0, 0.0, 0.0, 0.00030461741978670857, 0.0, 0.0, 0.0, 0.007615435494667714]
angular_velocity:
x: -1.2056297063827515
y: 0.3649747371673584
z: 0.365858256816864
angular_velocity_covariance: [7.615435494667715e-07, 0.0, 0.0, 0.0, 7.615435494667715e-07, 0.0, 0.0, 0.0, 7.615435494667715e-07]
linear_acceleration:
x: 0.2166752502441406
y: -0.5817910034179687
z: -0.008978256225585938
linear_acceleration_covariance: [1.5388359840000003e-05, 0.0, 0.0, 0.0, 1.5388359840000003e-05, 0.0, 0.0, 0.0, 1.5388359840000003e-05]
---
header:
seq: 942
stamp:
secs: 1646794928
nsecs: 526115059
frame_id: "base_imu"
成功读到数据。
