测试环境:
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)测试代码如下:
/* gcc -o i2ctest i2ctest.c -l bcm2835 */
/* sudo i2ctest */
#include
#include
#include "bcm2835.h"
int main()
{
uint8_t temp = 0;
printf("Hello BCM2835!\r\n");
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%x\r\n", 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"
成功读到数据。