-
-
-
-
解决方案长沙湾流智能科技有限公司是一家专业致力于工程机械传感、 控制、驱动、互联技术产品研究与开发的公司，也是一家专注为客户提供复 杂运动控制系统成套解决方案的高技术企业。
-
串口通信IMU模块如何实现ROS功能
发布时间：
2025-03-02
现在以VALUER湾流的SP9系列IMU/AHRS模块为例进行讲解，基于 STM32F103芯片，编写相应的代码来读取 IMU 传感器数据并通过串口发送出去。以下分两部分介绍 STM32 端和 ROS 端的相关要点和代码示例。
STM32F103 端代码
1. 开发环境
使用 STM32CubeMX 来配置项目快速配置 GPIO、串口、时钟等外设，然后使用 Keil MDK 或者 GCC for ARM 等工具进行代码编译和烧录。
2. 代码实现
以下是MPU6050作为IMU传感器的示例代码，通过 I2C 接口读取数据，并通过串口发送欧拉角。
#include "stm32f1xx_hal.h"
#include "i2c.h"
#include "usart.h"
#include "gpio.h"
#include <math.h>
#define MPU6050_ADDR 0xD0
// 读取MPU6050寄存器
uint8_t MPU6050_ReadReg(uint8_t reg) {
uint8_t data;
HAL_I2C_Master_Transmit(&hi2c1, MPU6050_ADDR, ®, 1, 100);
HAL_I2C_Master_Receive(&hi2c1, MPU6050_ADDR, &data, 1, 100);
return data;
}
// 初始化MPU6050
void MPU6050_Init() {
uint8_t data = 0x00;
HAL_I2C_Master_Transmit(&hi2c1, MPU6050_ADDR, &0x6B, 1, 100);
HAL_I2C_Master_Transmit(&hi2c1, MPU6050_ADDR, &data, 1, 100);
}
// 读取加速度计和陀螺仪数据
void MPU6050_ReadData(int16_t *ax, int16_t *ay, int16_t *az, int16_t *gx, int16_t *gy, int16_t *gz) {
uint8_t buffer[14];
HAL_I2C_Master_Transmit(&hi2c1, MPU6050_ADDR, &0x3B, 1, 100);
HAL_I2C_Master_Receive(&hi2c1, MPU6050_ADDR, buffer, 14, 100);
*ax = (buffer[0] << 8) | buffer[1];
*ay = (buffer[2] << 8) | buffer[3];
*az = (buffer[4] << 8) | buffer[5];
*gx = (buffer[8] << 8) | buffer[9];
*gy = (buffer[10] << 8) | buffer[11];
*gz = (buffer[12] << 8) | buffer[13];
}
// 计算欧拉角
void CalculateEulerAngles(int16_t ax, int16_t ay, int16_t az, float *roll, float *pitch) {
*roll = atan2(ay, az) * 180 / M_PI;
*pitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180 / M_PI;
}
// 通过串口发送欧拉角
void SendEulerAngles(float roll, float pitch, float yaw) {
char buffer[50];
sprintf(buffer, "%.2f,%.2f,%.2f\n", roll, pitch, yaw);
HAL_UART_Transmit(&huart1, (uint8_t *)buffer, strlen(buffer), 100);
}
int main(void) {
HAL_Init();
SystemClock_Config();
MX_GPIO_Init();
MX_I2C1_Init();
MX_USART1_UART_Init();
MPU6050_Init();
int16_t ax, ay, az, gx, gy, gz;
float roll, pitch, yaw = 0;
while (1) {
MPU6050_ReadData(&ax, &ay, &az, &gx, &gy, &gz);
CalculateEulerAngles(ax, ay, az, &roll, &pitch);
SendEulerAngles(roll, pitch, yaw);
HAL_Delay(100);
}
}
ROS 端代码
要在 ROS（Robot Operating System）中实现串口通信的 IMU（Inertial Measurement Unit）模块的欧拉角功能，一般需要完成以下几个主要步骤：
1. 环境准备
确保你已经安装了 ROS，并且熟悉基本的 ROS 工作空间和包的创建。以下以 ROS2 为例，假设你使用的是 Ubuntu 系统。
2. 创建 ROS2 工作空间和功能包
首先，创建一个 ROS2 工作空间和功能包：
bash
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python imu_serial_node --dependencies rclpy sensor_msgs
cd ~/ros2_ws
colcon build
source install/setup.bash
3. 编写 Python 代码
在~/ros2_ws/src/imu_serial_node/imu_serial_node目录下创建一个 Python 脚本，例如imu_serial_publisher.py。
python
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
import serial
import math
class IMUSerialPublisher(Node):
def __init__(self):
super().__init__('imu_serial_publisher')
# 创建一个发布者，发布Imu消息到/imu/data话题
self.publisher_ = self.create_publisher(Imu, 'imu/data', 10)
# 初始化串口通信
try:
self.ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=1)
self.get_logger().info('Serial port opened successfully.')
except serial.SerialException as e:
self.get_logger().error(f'Failed to open serial port: {e}')
return
# 创建一个定时器，定期调用read_imu_data函数
timer_period = 0.1 # seconds
self.timer = self.create_timer(timer_period, self.read_imu_data)
def read_imu_data(self):
try:
# 从串口读取一行数据
line = self.ser.readline().decode('utf-8').strip()
# 假设数据格式为 "roll,pitch,yaw"
data = line.split(',')
if len(data) == 3:
roll = float(data[0])
pitch = float(data[1])
yaw = float(data[2])
# 创建Imu消息
imu_msg = Imu()
imu_msg.header.stamp = self.get_clock().now().to_msg()
imu_msg.header.frame_id = 'imu_link'
# 将欧拉角转换为四元数
q = self.euler_to_quaternion(roll, pitch, yaw)
imu_msg.orientation.x = q[0]
imu_msg.orientation.y = q[1]
imu_msg.orientation.z = q[2]
imu_msg.orientation.w = q[3]
# 发布Imu消息
self.publisher_.publish(imu_msg)
self.get_logger().info(f'Published IMU data: Roll={roll}, Pitch={pitch}, Yaw={yaw}')
except (ValueError, IndexError) as e:
self.get_logger().warning(f'Failed to parse IMU data: {e}')
except serial.SerialException as e:
self.get_logger().error(f'Serial communication error: {e}')
def euler_to_quaternion(self, roll, pitch, yaw):
cr = math.cos(roll * 0.5)
sr = math.sin(roll * 0.5)
cp = math.cos(pitch * 0.5)
sp = math.sin(pitch * 0.5)
cy = math.cos(yaw * 0.5)
sy = math.sin(yaw * 0.5)
w = cr * cp * cy + sr * sp * sy
x = sr * cp * cy - cr * sp * sy
y = cr * sp * cy + sr * cp * sy
z = cr * cp * sy - sr * sp * cy
return [x, y, z, w]
def main(args=None):
rclpy.init(args=args)
imu_serial_publisher = IMUSerialPublisher()
rclpy.spin(imu_serial_publisher)
imu_serial_publisher.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
4. 配置setup.py
确保~/ros2_ws/src/imu_serial_node/setup.py文件中包含以下内容：
python
from setuptools import setup
import os
from glob import glob
package_name = 'imu_serial_node'
setup(
name=package_name,
version='0.0.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
(os.path.join('share', package_name), glob('launch/*.launch.py')),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='your_name',
maintainer_email='your_email',
description='ROS2 node for reading IMU data from serial port',
license='Apache License 2.0',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'imu_serial_publisher = imu_serial_node.imu_serial_publisher:main'
],
},
)
5. 编译和运行
在工作空间根目录下重新编译：
bash
cd ~/ros2_ws
colcon build
source install/setup.bash
运行节点：
bash
ros2 run imu_serial_node imu_serial_publisher
6. 订阅话题
你可以使用以下命令订阅/imu/data话题，查看发布的 IMU 数据：
bash
ros2 topic echo /imu/data
注意事项
- 确保 IMU 模块通过串口正确连接到计算机，并且串口设备名称（如
/dev/ttyUSB0）正确。 - 确保 IMU 模块发送的数据格式与代码中解析的格式一致（这里假设为 "roll,pitch,yaw"）。
- 如果使用的是 ROS1，代码和命令会有所不同，但基本思路是相似的。
通过以上步骤，你就可以在 ROS 中实现串口通信的 IMU 模块的欧拉角功能。在这里可以获取更多VALUER湾流的模块信息，也可以在这里留言。
上一页
上一页
相关资讯
关注我们