树莓派4B ubuntu-20.04 MPU6050的ROS node移植

测试环境：

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 <stdio.h>
#include <stdlib.h>
#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"

成功读到数据。