联合标定大恒MER139和大疆C型开发板

---

一、编译安装好imu_utils和code_utils

看沈航教程

二、将IMU数据发布到ROS的/imu_data节点。

接沈航运行roslaunch imu_utils bmi088.launch

三、使用STM32F407IGH6和BMI088 IMU的基本示例，将IMU数据发布到ROS的/imu_data节点

大致流程：

1. 首先，请查阅大疆C型开发板的用户手册或硬件文档，以确定BMI088 IMU模块连接到的I2C引脚以及USART引脚。一般来说，这些信息会在原理图或者芯片手册中提供。
2. 使用STM32CubeMX根据找到的引脚信息配置I2C和USART接口，如前面所述。如果需要，您也可以手动修改生成的代码以适应实际引脚。
3. 按照之前的说明将rosserial_stm32库和BMI088-Arduino-Library添加到您的项目中，并编写程序以从内置的BMI088 IMU读取数据并通过ROS发布。
4. 接下来，您需要一个USB转TTL串口模块（例如CH340）来将开发板上的USART接口转换为USB接口。请确保将USB转TTL串口模块连接到正确的USART引脚（TX和RX）以及地线（GND）。
5. 将USB转TTL串口模块的USB端口连接到运行ROS的计算机。
6. 在ROS上位机上执行以下命令与STM32建立连接：

该示例假设您已经在STM32上安装了rosserial_stm32库和BMI088-Arduino-Library。

3.1、为STM32F407IGH6配置了正确的时钟和引脚设置，以及安装了相关库。接下来，请使用STM32CubeMX生成I2C1和USART2的初始化代码，并将其添加到SystemClock_Config(), MX_GPIO_Init(), MX_I2C1_Init()以及MX_USART2_UART_Init()函数中。

这些步骤假定您已经安装了STM32CubeMX。

1. 打开STM32CubeMX，点击File > New Project。
2. 在弹出的对话框中，选择或搜索您的MCU型号：STM32F407IGHx，然后单击Start Project。
3. 在左侧的Pinout & Configuration选项卡中，找到并单击System Core下的RCC。在右侧面板中，设置High Speed Clock (HSE)为Crystal/Ceramic Resonator，设置Low Speed Clock (LSE)为Disable。确保System Clock Mux设置为PLLCLK。
4. 返回Pinout & Configuration选项卡，找到并单击System Core下的SYS。在右侧面板中，设置Debug为Serial Wire。
5. 为I2C1配置引脚：
   • 在Pinout视图中，找到PB6引脚并左键单击。从下拉菜单中选择I2C1_SCL。
   • 然后找到PB7引脚并左键单击。从下拉菜单中选择I2C1_SDA。
   • 点击Connectivity下的I2C1。根据需要在右侧面板中设置参数，例如I2C Speed Mode。
6. 为USART2配置引脚：
   • 在Pinout视图中，找到PA2引脚并左键单击。从下拉菜单中选择USART2_TX。
   • 然后找到PA3引脚并左键单击。从下拉菜单中选择USART2_RX。
   • 点击Connectivity下的USART2。在右侧面板中设置波特率和其他相关参数。
7. 保存并生成代码：点击File > Save Project，然后选择您的工程目录。接着点击Project > Generate Code。
8. 在生成的项目文件夹中，找到以下文件：
   • Core/Inc/main.h
   • Core/Src/main.c
   • Core/Inc/gpio.h 和 Core/Src/gpio.c（MX_GPIO_Init()）
   • Core/Inc/i2c.h 和 Core/Src/i2c.c（MX_I2C1_Init()）
   • Core/Inc/usart.h 和 Core/Src/usart.c（MX_USART2_UART_Init()）
   • Core/Src/stm32f4xx_hal_msp.c

将这些文件添加到您的STM32开发环境的工程中，并进行必要的修改以使用已安装的库。

现在，您可以根据前面提供的示例编写和运行程序，以将IMU数据发布到ROS的/imu_data节点。

3.2 如何在 STM32 上安装 rosserial_stm32 库和 BMI088-Arduino-Library 的方法如下：

首先，确保您已在计算机上安装了 STM32CubeMX 和一个支持 STM32 的开发环境（例如 Keil uVision、IAR Embedded Workbench 或 System Workbench for STM32）。

1. 下载 rosserial_stm32 和 BMI088-Arduino-Library 库：

通过以下链接下载这两个库的 ZIP 文件：

• rosserial_stm32: https://github.com/yoneken/rosserial_stm32/archive/refs/heads/master.zip
• BMI088-Arduino-Library: https://github.com/Seeed-Studio/Seeed_BMI088/archive/refs/heads/master.zip

解压这两个 ZIP 文件到一个临时文件夹。

1. 创建一个新的 STM32 项目：

使用 STM32CubeMX 创建一个新的 STM32 项目，根据您的开发板配置硬件。配置 I2C 接口以用于与 BMI088 IMU 通信。

2. 将库文件添加到您的项目中：

将从 rosserial_stm32 和 BMI088-Arduino-Library ZIP 文件中解压出来的文件夹复制到您的项目目录中。将它们重命名为简短的名称，例如将 rosserial_stm32-master 重命名为 rosserial_stm32，将 Seeed_BMI088-master 重命名为 Seeed_BMI088。

3. 在您的项目设置中包含库：

打开您的 STM32 项目，并在项目设置中包含这两个库的文件夹。对于不同的开发环境，操作可能略有不同：

• Keil uVision：右键单击项目名称 -> Options for Target… -> C/C++ -> Include Paths
• IAR Embedded Workbench：右键单击项目名称 -> Options -> C/C++ Compiler -> Preprocessor -> Additional include directories
• System Workbench for STM32 (SW4STM32)：右键单击项目名称 -> Properties -> C/C++ General -> Paths and Symbols -> Includes

4. 在步骤 3 中提到的文件夹路径添加到 Include 路径列表中。

现在您已经在 STM32 项目中安装了 rosserial_stm32 库和 BMI088-Arduino-Library。您可以按照前面给出的指南编写程序以从内置的 BMI088 IMU 读取数据并通过 ROS 发布。

（这个示例仅用于指导，您可能需要根据具体硬件配置进行调整。）

main.c

#include "main.h"
#include "stm32f4xx_hal.h"
#include "BMI088.h"
#include <stdint.h>
#include "ros.h"
#include "sensor_msgs/Imu.h"

I2C_HandleTypeDef hi2c1;
UART_HandleTypeDef huart2;

void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_I2C1_Init(void);
static void MX_USART2_UART_Init(void);

int _write(int file, char *ptr, int len) {
  HAL_UART_Transmit(&huart2, (uint8_t *)ptr, len, 50);
  return len;
}

int main(void) {
  HAL_Init();
  SystemClock_Config();
  MX_GPIO_Init();
  MX_I2C1_Init();
  MX_USART2_UART_Init();

  printf("Initializing BMI088...\r\n");
  BMI088 bmi088(&hi2c1);
  while (bmi088.begin() != BMI088_OK) {
    printf("Failed to initialize BMI088. Retrying in 3 seconds...\r\n");
    HAL_Delay(3000);
  }

  printf("BMI088 initialized.\r\n");

  ros::NodeHandle nh;
  sensor_msgs::Imu imu_msg;
  ros::Publisher imu_pub("imu_data", &imu_msg);
  nh.initNode();
  nh.advertise(imu_pub);

  while (1) {
    float ax, ay, az;
    float gx, gy, gz;
    bmi088.readAcceleration(ax, ay, az);
    bmi088.readGyroscope(gx, gy, gz);

    imu_msg.header.stamp = nh.now();
    imu_msg.linear_acceleration.x = ax;
    imu_msg.linear_acceleration.y = ay;
    imu_msg.linear_acceleration.z = az;
    imu_msg.angular_velocity.x = gx;
    imu_msg.angular_velocity.y = gy;
    imu_msg.angular_velocity.z = gz;

    imu_pub.publish(&imu_msg);
    nh.spinOnce();

    HAL_Delay(10);
  }
}

void SystemClock_Config(void) {
  // Clock configuration code generated by STM32CubeMX
}

static void MX_GPIO_Init(void) {
  // GPIO initialization code generated by STM32CubeMX
}

static void MX_I2C1_Init(void) {
  // I2C1 initialization code generated by STM32CubeMX
}

static void MX_USART2_UART_Init(void) {
  // USART2 initialization code generated by STM32CubeMX
}

main.h

#ifndef __MAIN_H
#define __MAIN_H

#ifdef __cplusplus
extern "C" {
#endif

#include "stm32f4xx_hal.h"

void Error_Handler(void);

#ifdef __cplusplus
}
#endif

#endif /* __MAIN_H */

3.2、为STM32F407IGH6配置了正确的时钟和引脚设置，以及安装了相关库。接下来，请使用STM32CubeMX生成I2C1和USART2的初始化代码，并将其添加到SystemClock_Config(), MX_GPIO_Init(), MX_I2C1_Init()以及MX_USART2_UART_Init()函数中。

这些步骤假定您已经安装了STM32CubeMX。

1. 打开STM32CubeMX，点击File > New Project。
2. 在弹出的对话框中，选择或搜索您的MCU型号：STM32F407IGHx，然后单击Start Project。
3. 在左侧的Pinout & Configuration选项卡中，找到并单击System Core下的RCC。在右侧面板中，设置High Speed Clock (HSE)为Crystal/Ceramic Resonator，设置Low Speed Clock (LSE)为Disable。确保System Clock Mux设置为PLLCLK。
4. 返回Pinout & Configuration选项卡，找到并单击System Core下的SYS。在右侧面板中，设置Debug为Serial Wire。
5. 为I2C1配置引脚：
   • 在Pinout视图中，找到PB6引脚并左键单击。从下拉菜单中选择I2C1_SCL。
   • 然后找到PB7引脚并左键单击。从下拉菜单中选择I2C1_SDA。
   • 点击Connectivity下的I2C1。根据需要在右侧面板中设置参数，例如I2C Speed Mode。
6. 为USART2配置引脚：
   • 在Pinout视图中，找到PA2引脚并左键单击。从下拉菜单中选择USART2_TX。
   • 然后找到PA3引脚并左键单击。从下拉菜单中选择USART2_RX。
   • 点击Connectivity下的USART2。在右侧面板中设置波特率和其他相关参数。
7. 保存并生成代码：点击File > Save Project，然后选择您的工程目录。接着点击Project > Generate Code。
8. 在生成的项目文件夹中，找到以下文件：
   • Core/Inc/main.h
   • Core/Src/main.c
   • Core/Inc/gpio.h 和 Core/Src/gpio.c（MX_GPIO_Init()）
   • Core/Inc/i2c.h 和 Core/Src/i2c.c（MX_I2C1_Init()）
   • Core/Inc/usart.h 和 Core/Src/usart.c（MX_USART2_UART_Init()）
   • Core/Src/stm32f4xx_hal_msp.c

将这些文件添加到您的STM32开发环境的工程中，并进行必要的修改以使用已安装的库。

现在，您可以根据前面提供的示例编写和运行程序，以将IMU数据发布到ROS的/imu_data节点。

3.3、编译并烧写程序到您的STM32F407IGH6开发板。在ROS上位机上，运行以下命令与STM32建立连接：

rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=115200

注意：请根据实际情况更改端口/dev/ttyACM0和波特率115200。

现在，您应该能够在/imu_data节点上看到来自BMI088的数据。如需验证，请使用以下命令查看数据：

rostopic echo /imu_data

4、use

ymj@ymj:~/kalibr_ws/src/imu_utils/launch$ roslaunch imu_utils bmi088.launch

5、/imu_data节点

5.1、下位机发送了角速度+加速度

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Imu
import serial
import struct


def main():
    rospy.init_node('imu_publisher')
    imu_pub = rospy.Publisher('/imu', Imu, queue_size=10)

    # 配置串口参数
    port = '/dev/ttyUSB0'  # 根据实际情况修改你的串口号
    baudrate = 115200
    ser = serial.Serial(port, baudrate)

    rate = rospy.Rate(100)  # 发布频率为100Hz
    imu_msg = Imu()

    while not rospy.is_shutdown():
        # 从串口读取一个字节
        byte = ser.read(1)

        if byte == b'\xD4':
            buffer = b''
            while len(buffer) < 28:
                byte = ser.read(1)
                if byte == b'\xD4':  # 如果遇到新的帧头，重置buffer
                    buffer = b''
                else:
                    buffer += byte

            # 添加四个0字节到数据末尾
            data = b'\xD4' + buffer + b'\x00\x00\x00\x00'

            # 解析数据包中的陀螺仪和加速度计数据
            float_values = struct.unpack('<6f', data[1:1 + 6 * 4])

            imu_msg.angular_velocity.x = float_values[0]
            imu_msg.angular_velocity.y = float_values[1]
            imu_msg.angular_velocity.z = float_values[2]
            imu_msg.linear_acceleration.x = float_values[3]
            imu_msg.linear_acceleration.y = float_values[4]
            imu_msg.linear_acceleration.z = float_values[5]

            # 打印角速度和加速度信息
            rospy.loginfo("Angular Velocity: x=%f, y=%f, z=%f",
                          imu_msg.angular_velocity.x,
                          imu_msg.angular_velocity.y,
                          imu_msg.angular_velocity.z)
            rospy.loginfo("Linear Acceleration: x=%f, y=%f, z=%f",
                          imu_msg.linear_acceleration.x,
                          imu_msg.linear_acceleration.y,
                          imu_msg.linear_acceleration.z)

            imu_pub.publish(imu_msg)
        rate.sleep()


if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass

5.2、下位机发送了角速度+加速度+四元数

介绍：

读取电控数据，在rviz工具可视化

• 启动 rosrun imu_publisher display_imu_rviz.py 发布数据姿态数据，以及TF坐标

• 在rviz中Add imu（需要下载）

  sudo apt-get install ros-noetic-robot-state-publisher
  sudo apt-get install ros-noetic-tf
  sudo apt-get install ros-noetic-image-transport
  sudo apt-get install ros-noetic-compressed-image-transport

#!/usr/bin/env python
import time

import rospy
from sensor_msgs.msg import Imu, NavSatFix
import serial
import struct
import tf2_ros
from geometry_msgs.msg import TransformStamped
from numpy import linalg
import numpy as np

class MovingAverageFilter:
    def __init__(self, window_size):
        self.window_size = window_size
        self.data = []

    def add_value(self, value):
        self.data.append(value)
        if len(self.data) > self.window_size:
            self.data.pop(0)

    def get_average(self):
        return sum(self.data) / len(self.data) if self.data else 0


class MovingMedianFilter:
    def __init__(self, window_size):
        self.window_size = window_size
        self.data = []

    def add_value(self, value):
        self.data.append(value)
        if len(self.data) > self.window_size:
            self.data.pop(0)

    def get_median(self):
        return np.median(self.data) if self.data else 0
class IMUPublisher:
    def __init__(self, port, baudrate):
        self.port = port
        self.baudrate = baudrate
        rospy.init_node('imu_publisher')
        self.imu_pub = rospy.Publisher('/imu_data', Imu, queue_size=10)
        self.navsatfix_pub = rospy.Publisher('navsatfix', NavSatFix, queue_size=10)
        self.tf_broadcaster = tf2_ros.TransformBroadcaster()

        self.window_size = 10
        self.w_filter = MovingMedianFilter(self.window_size)
        self.x_filter = MovingMedianFilter(self.window_size)
        self.y_filter = MovingMedianFilter(self.window_size)
        self.z_filter = MovingMedianFilter(self.window_size)

    def normalize_quaternion(self, q):
        norm = linalg.norm([q.w, q.x, q.y, q.z])
        if norm > 1e-6:
            q.w /= norm
            q.x /= norm
            q.y /= norm
            q.z /= norm
        else:
            rospy.logwarn("Quaternion norm is close to zero, using default quaternion values.")
            q.w = 1.0
            q.x = 0.0
            q.y = 0.0
            q.z = 0.0
        return q

    def publish_imu_transform(self, imu_orientation):
        t = TransformStamped()

        t.header.stamp = rospy.Time.now()
        t.header.frame_id = "world"
        t.child_frame_id = "imu_frame"

        t.transform.translation.x = 0.0
        t.transform.translation.y = 0.0
        t.transform.translation.z = 0.0

        t.transform.rotation.w = imu_orientation.w
        t.transform.rotation.x = imu_orientation.x
        t.transform.rotation.y = imu_orientation.y
        t.transform.rotation.z = imu_orientation.z

        self.tf_broadcaster.sendTransform(t)

    def process_imu_data(self, ser, imu_msg):
        byte = ser.read(1)

        if byte == b'\xD4':
            buffer = b''
            while len(buffer) < 40:
                byte = ser.read(1)
                if byte == b'\xD4':
                    buffer = b''
                else:
                    buffer += byte

            data = b'\xD4' + buffer
            imu_msg.header.frame_id = "imu_frame"
            float_values = struct.unpack('<10f', data[1:1 + 10 * 4])
            imu_msg.header.stamp = rospy.Time.now()
            imu_msg.angular_velocity.x = float_values[0]
            imu_msg.angular_velocity.y = float_values[1]
            imu_msg.angular_velocity.z = float_values[2]

            imu_msg.linear_acceleration.x = float_values[3]
            imu_msg.linear_acceleration.y = float_values[4]
            imu_msg.linear_acceleration.z = float_values[5]

            quat_norm = linalg.norm([float_values[6], float_values[7], float_values[8], float_values[9]])

            if quat_norm > 1e-6:
                w_filtered = self.w_filter.add_value(float_values[6] / quat_norm)
                x_filtered = self.x_filter.add_value(float_values[7] / quat_norm)
                y_filtered = self.y_filter.add_value(float_values[8] / quat_norm)
                z_filtered = self.z_filter.add_value(float_values[9] / quat_norm)

                imu_msg.orientation.w = self.w_filter.get_median()
                imu_msg.orientation.x = self.x_filter.get_median()
                imu_msg.orientation.y = self.y_filter.get_median()
                imu_msg.orientation.z = self.z_filter.get_median()
            else:
                rospy.logwarn("Quaternion norm is close to zero, using default quaternion values")
                imu_msg.orientation.w = 1.0
                imu_msg.orientation.x = 0.0
                imu_msg.orientation.y = 0.0
                imu_msg.orientation.z = 0.0

            rospy.loginfo("Angular velocity: x=%f, y=%f, z=%f",
                          imu_msg.angular_velocity.x,
                          imu_msg.angular_velocity.y,
                          imu_msg.angular_velocity.z)
            rospy.loginfo("Linear acceleration: x=%f, y=%f, z=%f",
                          imu_msg.linear_acceleration.x,
                          imu_msg.linear_acceleration.y,
                          imu_msg.linear_acceleration.z)
            rospy.loginfo("Quaternion: w=%f, x=%f, y=%f, z=%f",
                          imu_msg.orientation.w,
                          imu_msg.orientation.x,
                          imu_msg.orientation.y,
                          imu_msg.orientation.z)

            # Publish IMU message
            self.imu_pub.publish(imu_msg)
            # Normalize quaternion before publishing the transform
            normalized_quaternion = self.normalize_quaternion(imu_msg.orientation)
            self.publish_imu_transform(normalized_quaternion)

    def run(self):
        ser = serial.Serial(self.port, self.baudrate, timeout=1)
        imu_msg = Imu()
        while not rospy.is_shutdown():
            try:
                self.process_imu_data(ser, imu_msg)
            except serial.serialutil.SerialException as e:
                rospy.logerr("Error reading from IMU: %s", str(e))
                rospy.loginfo("Attempting to reconnect...")
                ser.close()
                time.sleep(1)
                try:
                    ser.open()
                except serial.serialutil.SerialException as e:
                    rospy.logerr("Failed to reconnect: %s", str(e))

if __name__ == '__main__':
    try:
        imu_publisher = IMUPublisher('/dev/ttyUSB0', 115200)  # Replace with your desired port and baudrate
        imu_publisher.run()
    except rospy.ROSInterruptException:
        pass