ROS读取惯导数据并以话题形式发布

一、建立工作空间

1、创建工作空间

mkdir p~/catkin ws/src

cd ~/catkin ws/

catkin make

2.创建功能包

cd ~/catkin ws/src

#创建名为 IMU_package的功能包

catkin_create_pkg IMU_package rospy std_msgs

3.添加Python脚本创建scripts目录(存放Python程序):

cd IMU_package

mkdir scripts

cd scripts

4、报错 /usr/bin/env: ‘python\r’: No such file or directory

sudo apt update sudo apt install dos2unix

dos2unix IMU_xujun.py

5、设置文件可执行权限：

chmod +x publisher_node.py

chmod +x subscriber_node.py

6、修改package.xml

确认以下依赖已包含：

<exec_depend>rospy</exec_depend>

<exec_depend>std_msgs</exec_depend>

7、编译工作空间

cd ~/catkin_ws

catkin_make

source devel/setup.bash # 激活环境变量

8. 运行程序

终端1：启动ROS核心

roscore

终端2：运行发布程序

rosrun IMU_package IMU_xujun.py

9、订阅节点话题

rostopic list

rostopic echo /imu_data

二、开发读取程序

#!/usr/bin/env python3
import rospy
import serial
from sensor_msgs.msg import Imu  # 根据实际数据类型选择合适的消息类型
def read_imu_data(port, baudrate):
    """
    读取串口中的惯导数据
    :param port: 串口号
    :param baudrate: 波特率
    :return: 解析后的惯导数据
    """
    try:
        ser = serial.Serial(port, baudrate, timeout=1)
        if not ser.is_open:
            rospy.logerr("无法打开串口")
            return None
        while not rospy.is_shutdown():
            if ser.in_waiting > 0:
                data = ser.readline().decode().strip()  # 读取一行数据并解码
                # 在这里解析数据并转换为合适的格式
                # 假设数据格式为 "ax,ay,az,wx,wy,wz"
                print("原始数据",data)
                print(type(data))
                try:
                    imu_data = list(map(str, data.split(',')))
                    print("转换后",imu_data)
                    return imu_data
                except ValueError:
                    rospy.logwarn("解析数据失败")
                    continue
    except serial.SerialException as e:
        rospy.logerr("串口错误: %s", e)
        return None
def imu_publisher():
    # 初始化 ROS 节点
    rospy.init_node('imu_publisher', anonymous=True)
    # 配置串口参数
    port = rospy.get_param('~port', '/dev/ttyUSB0')  # 默认串口号
    baudrate = rospy.get_param('~baudrate', 115200)  # 默认波特率
    # 创建 Imu 消息对象
    imu_msg = Imu()
    imu_msg.header.frame_id = "imu_link"  # 设置参考系
 # 创建发布者，发布 IMU 数据到话题 "imu_data"
    pub = rospy.Publisher('imu_data', Imu, queue_size=10)
    rate = rospy.Rate(100)  # 设置发布频率为 100Hz
    while not rospy.is_shutdown():
        imu_data = read_imu_data(port, baudrate)
        if imu_data is not None:
            # 填充消息数据
            imu_msg.header.stamp = rospy.Time.now()
            imu_msg.angular_velocity.x = float(imu_data[2])
            imu_msg.angular_velocity.y =  float(imu_data[3])
            imu_msg.angular_velocity.z =  float(imu_data[4])
            imu_msg.linear_acceleration.x =  float(imu_data[5])
            imu_msg.linear_acceleration.y =  float(imu_data[6])
            imu_msg.linear_acceleration.z =  float(imu_data[7])
            # 发布消息
            pub.publish(imu_msg)
            rospy.loginfo("发布 IMU 数据成功")
        rate.sleep()
if __name__ == '__main__':
    try:
        imu_publisher()
    except rospy.ROSInterruptException:
        pass

三、测试界面