C++使用serial串口通信 + ROS2示例IMU串口驱动

---

串行接口 （Serial Interface）简称串口（通常指COM接口），是采用串行通信方式的扩展接口，是指数据一位一位地顺序传送，串口通信就要解析这一位一位数据。这里使用的是亚博智能的10轴IMU模块为例介绍C++使用serial串口通信，此IMU模块为UART通信，它是一异步通讯：不需要时钟信号进行数据同步，它们直接在数据信号中穿插一些同步用的信号位，或者把主体数据进行打包，以数据帧（串口：起始位 数据 校验位(可以没有) 停止位）的格式传输数据，某些通讯中还需要双方约定数据的传输速率（波特率），以便更好地同步。例如：

• 波特率是一个衡量通信速度的参数，它表示每秒钟传送的 bit 的个数；
• 数据位是衡量通信中实际数据位的参数，当计算机发送一个信息包，标准的值是 8 位（也可以是其他，比如5，6，7）；
• 停止位用于表示单个包的最后一位，典型的值为 1，1.5和 2 位，停止位不仅表示传输的结束，并且提供计算机校正时钟同步的机会；
• 奇偶校验位是串口通信中一种简单的检错方式，有四种检错方式——偶、奇、高和低，也可以没有校验位。

一、通信协议

通讯协议分为物理层和协议层。物理层规定通讯系统中具有机械、电子功能部分的特性，确保原始数据在物理媒体的传输（通俗一点就是硬件部分）。协议层主要规定通讯逻辑，统一收发双方的数据打包、解包标准（软件）。使用的IMU模块（自己修改波特率406800）其通信协议（部分）如下：

数据是按照16进制方式发送的，不是ASCII码。每个数据帧（不同类型）包含11位（8bit），其中0x55为帧头，第二位TYPE为数据类型，中间8个为数据位，最后一个为检验位。写代码时注意以下三个方面：

• （1）通过0x55帧头和第二位TYPE数据类型识别数据帧；
• （2）每一个数据分低字节和高字节(千万注意高低顺序)依次传送，二者组合成一个有符号的short类型的数据。例如数据DATA1，其中DATA1L为低字节，DATA1H为高字节。转换方法如下：假设DATA1为实际的数据，DATA1H为其高字节部分，DATA1L为其低字节部分，那么：DATA1=(short)((short)DATA1H<<8|DATA1L)。这里一定要注意DATA1H需要先强制转换为一个有符号的short类型的数据以后再移位，并且DATA1的数据类型也是有符号的short类型，这样才能表示出负数；
• （3）通过检验和来检验数据SUMCRC=0x55+TYPE+DATA1L+DATA1H+DATA2L+DATA2H+DATA3L+DATA3H+DATA4L+DATA4H
  SUMCRC为char型，取校验和的低8位。

二、串口调试工具

sudo apt-get install cutecom

选择串口，设置波特率
打开串口，可以查看每一位数据的十六进制和字符格式的数据输出

三、serial库的使用

3.1 安装serial

对于ROS1来说，可以直接安装ROS版

sudo apt-get install ros-<ros版本>-serial

Cmake配置：

find_package(catkin REQUIRED COMPONENTS
  serial
)

catkin_package(
  CATKIN_DEPENDS serial
)

target_link_libraries(myserial  ${
    
    catkin_LIBRARIES})

但是由于ROS2没有再封装串口库serial，因此需要手动安装serial：

git clone https://github.com/ZhaoXiangBox/serial
cd serial && mkdir build
cmake .. && make
sudo make install

Cmake配置：

set(CMAKE_INSTALL_RPATH /usr/local/lib)
find_package(serial REQUIRED)

ament_target_dependencies(myserial "serial")

3.2 serial的使用

给连接的串口打开权限

sudo chmod 777 /dev/ttyUSB*

（1）首先需要添加serial的头文件

#include "serial/serial.h"

（2）实例化一个对象，之后的操作都是通过对此设置而进行的

serial::Serial serialPort;

（3）串口进行初始化

serialPort.setPort("/dev/ttyUSB0");//选择要开启的串口号
serialPort.setBaudrate(9600);//设置波特率
serial::Timeout _time =serial::Timeout::simpleTimeout(2000);//超时等待
serialPort.setTimeout(_time);

（4）开启串口

serialPort.open();//开启串口

（5）判断一下是否打开了

if(serialPort.isOpen())
{
    
    
	ROS_INFO("serial port is open");
}
else 
{
    
    
	ROS_ERROR("serial port error");
}

（6）发送数据

先设置一个数组，注意数组类型一定是uint8_t类型的

uint8_t  senddata[要发送数据的长度]={
    
    1};

定义好数组之后我们就可以给单片机发送数据了

serialPort.write(senddata,sizeof(senddata));//两个参数，第一个参数是要发送的数据地址，第二个数据是要发送数据的长度

（7）接收下位机的数据

先创建一个数组，数组类型也是uint8_t类型的

uint_8 receivedata[要接受数据的长度]={
    
    1};

接收数据的函数

serialPort.read(receivedata,sizeof(receivedata));

3.3 绑定端口

一旦出现断电或者重新插拔设备情况，代码就会出现找不到设备或者找错设备的错误，因为端口序号分配是随机的，因此我们需要绑定端口，确保每次插拔端端口一样，不需要修改代码
首先通过插拔两个端口，我们可以lsusb查看端口信息：

可以看到IMU端口为Bus 001 Device 007: ID 10c4:ea60 Silicon Labs CP210x UART Bridge
然后创建imu_usb.rules（随便命名）文件，填写以下内容：

KERNEL=="ttyUSB*", ATTRS{
    
    idVendor}=="10c4", ATTRS{
    
    idProduct}=="ea60", MODE:="0777", SYMLINK+="imu_usb

然后：

sudo cp imu_usb.rules /etc/udev/rules.d
service udev reload
service udev restart

重新插拔设备，输入以下指令检测绑定端口是否成功

ll /dev/imu_usb

四、编写IMU ROS2串口驱动

ROS2节点为例，首先通过串口调试工具可以看到一帧数据：

通过上面的通信协议以及串口使用方法，我编写了以下代码来实现IMU的ROS2串口驱动：

#include <iostream>
#include <chrono>
#include <cmath>
#include <serial/serial.h>
#include <sensor_msgs/msg/imu.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include "rclcpp/rclcpp.hpp"

#define BAUDRATE 460800

// std::atomic_bool 是 C++ 标准库中的原子布尔类型。它是一种特殊的数据类型，用于在多线程环境中进行原子操作。
// 原子操作是指不可中断的操作，要么完全执行，要么完全不执行。std::atomic_bool 类型提供了原子性的读取和写入操作，以确保多个线程对该变量的访问不会导致竞争条件或数据不一致的问题。
std::atomic_bool imu_thread_running;
std::atomic_bool imu_data_ready;
std::vector<uint8_t> buff;
std::vector<int16_t> acceleration(4, 0);
std::vector<int16_t> angularVelocity(4, 0);
std::vector<int16_t> magnetometer(4, 0);
std::vector<int16_t> angle_degree(4, 0);

class IMUDriverNode : public rclcpp::Node
{
    
    
public:
    IMUDriverNode(const char *nodeName) : Node(nodeName)
    {
    
    
        // 获取串口
        _port_name = this->declare_parameter("~port_name", "/dev/ttyUSB0");
        // 发布IMU数据
        publisher_ = this->create_publisher<sensor_msgs::msg::Imu>("/imu_raw", 10);
        // IMU驱动线程
        imu_thread_ = std::thread(&IMUDriverNode::imuThread, this, _port_name);
    }

    void joinIMUThread()
    {
    
    
        imu_thread_.join();
    }

private:
    // IMU驱动线程
    void imuThread(const std::string &port_name)
    {
    
    
        // 1 创建串口对象
        serial::Serial imu_serial;
        // 串口初始化
        try
        {
    
    
            imu_serial.setPort(port_name);
            imu_serial.setBaudrate(BAUDRATE);
            serial::Timeout timeout = serial::Timeout::simpleTimeout(500);
            imu_serial.setTimeout(timeout);
            imu_serial.open();
            RCLCPP_INFO(this->get_logger(), "\033[32mSerial port opened successfully...\033[0m");
        }
        catch (const serial::IOException &e)
        {
    
    
            RCLCPP_ERROR(this->get_logger(), "Failed to open the IMU serial port.");
            return;
        }

        // Clear the buffer
        imu_serial.flush();

        // 2 循环读取串口数据
        while (rclcpp::ok() && imu_thread_running.load())
        {
    
    
            if (imu_serial.available())
            {
    
    
                uint8_t data;
                imu_serial.read(&data, 1);
                // 一位一位地添加到队列
                buff.push_back(data);

                // 当大于11个数据位时解析数据，四种数据的第一位均为0x55
                if (buff.size() >= 11 && buff[0] == 0x55)
                {
    
    
                    // 获取11位数据
                    std::vector<uint8_t> data_buff(buff.begin(), buff.begin() + 11);
                    // 获取中间数字位
                    std::vector<uint8_t> data(buff.begin() + 2, buff.begin() + 10);
                    // 获取完一帧的标志位
                    bool angle_flag = false;

                    // 加速度数据
                    if (data_buff[1] == 0x51)
                    {
    
    
                        // 检查检验和
                        if (checkSum(data_buff))
                            // 获取16位数据
                            acceleration = hexToShort(data);
                        else
                            RCLCPP_WARN(this->get_logger(), "0x51 Check failure.");
                    }
                    // 下面的都类似
                    else if (data_buff[1] == 0x52)
                    {
    
    
                        if (checkSum(data_buff))
                            angularVelocity = hexToShort(data);
                        else
                            RCLCPP_WARN(this->get_logger(), "0x52 Check failure.");
                    }
                    else if (data_buff[1] == 0x53)
                    {
    
    
                        if (checkSum(data_buff))
                        {
    
    
                            angle_degree = hexToShort(data);
                            angle_flag = true;
                        }
                        else
                            RCLCPP_WARN(this->get_logger(), "0x53 Check failure.");
                    }
                    // 实际上没用到
                    else if (data_buff[1] == 0x54)
                    {
    
    
                        if (checkSum(data_buff))
                            magnetometer = hexToShort(data);
                        else
                            RCLCPP_WARN(this->get_logger(), "0x54 Check failure.");
                    }

                    buff.clear();

                    // 已经获取角度数据（顺序第三个），可以发布了
                    if (angle_flag)
                    {
    
    
                        // 可以输出数据的标志位
                        imu_data_ready.store(true);
                    }
                }
                else if (buff[0] != 0x55)
                {
    
    
                    buff.clear();
                }
            }

            if (imu_data_ready.load())
            {
    
    
                sensor_msgs::msg::Imu msg;
                msg.header.stamp = this->now();
                msg.header.frame_id = "base_link";

                // 将16数据转化位double
                // Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec
                // 加速度X=((AxH<<8)|AxL)/32768*16g(g为重力加速度，可取9.8m/s2)
                msg.linear_acceleration.x = static_cast<double>(acceleration[0]) / 32768.0 * 16 * 9.8;
                msg.linear_acceleration.y = static_cast<double>(acceleration[1]) / 32768.0 * 16 * 9.8;
                msg.linear_acceleration.z = static_cast<double>(acceleration[2]) / 32768.0 * 16 * 9.8;

                // 角速度X=((WxH<<8)|WxL)/32768*2000 °/s
                // 转化为弧度X=((WxH<<8)|WxL)/32768*2000*PI/180 rad/s
                msg.angular_velocity.x = static_cast<double>(angularVelocity[0]) / 32768.0 * 2000 * M_PI / 180;
                msg.angular_velocity.y = static_cast<double>(angularVelocity[1]) / 32768.0 * 2000 * M_PI / 180;
                msg.angular_velocity.z = static_cast<double>(angularVelocity[2]) / 32768.0 * 2000 * M_PI / 180;

                // 滚转角X=((RollH<<8)|RollL)/32768*180 (°)
                // 转化为弧度X=((RollH<<8)|RollL)/32768*180*PI/180=((RollH<<8)|RollL)/32768*PI (rad)
                double roll = static_cast<double>(angle_degree[0]) / 32768.0 * M_PI;
                double pitch = static_cast<double>(angle_degree[1]) / 32768.0 * M_PI;
                double yaw = static_cast<double>(angle_degree[2]) / 32768.0 * M_PI;
                tf2::Quaternion quat;
                quat.setRPY(roll, pitch, yaw);
                // Assign quaternion to msg.orientation
                msg.orientation.x = quat.getX();
                msg.orientation.y = quat.getY();
                msg.orientation.z = quat.getZ();
                msg.orientation.w = quat.getW();

                publisher_->publish(msg);
                imu_data_ready.store(false);
            }
        }
        imu_serial.close();
    }

    // 检验和：四个数据的检验和计算方式是一样的，所以统一写了
    bool checkSum(const std::vector<uint8_t> &data_buff)
    {
    
    
        uint8_t sum = 0;
        for (size_t i = 0; i < data_buff.size() - 1; i++)
        {
    
    
            sum += data_buff[i];
        }

        // 计算结果是否与输出一致
        return sum == data_buff[data_buff.size() - 1];
    }

    // 解析数据
    std::vector<int16_t> hexToShort(const std::vector<uint8_t> &hex_data)
    {
    
    
        std::vector<int16_t> short_data(4, 0);
        for (size_t i = 0; i < hex_data.size(); i += 2)
        {
    
    
            // 将两个连续的字节（低位hex_data[i] 和 高位hex_data[i+1]）组合为一个 int16_t 类型的数值：千万注意高低顺序，仔细看通讯协议
            // 高位hex_data[i+1]需要先强制转换为一个有符号的short类型的数据以后再移位
            short high = static_cast<int16_t>(hex_data[i + 1]);
            // 左移运算符 << 将 high 的二进制表示向左移动 8 位。这样做是因为 int16_t 类型占用 2 个字节，而我们希望将 high 的数据放置在最高的 8 位上。
            // |: 按位或运算符 | 将经过左移的 high 数据和 hex_data[i] 数据进行按位或操作，将它们组合为一个 16 位的数值
            short_data[i / 2] = static_cast<int16_t>((high << 8) | hex_data[i]);
        }

        return short_data;
    }

    // 与上面那个函数是等价的，直接采用C++函数解析
    std::vector<int16_t> hexToShort1(const std::vector<uint8_t> &hex_data)
    {
    
    
        char rawData[] = {
    
    hex_data[0], hex_data[1], hex_data[2], hex_data[3], hex_data[4], hex_data[5], hex_data[6], hex_data[7]};
        // 创建一个存储解析后整数的数组
        short result[4];
        // 使用memcpy将字节数组解析为整数数组
        memcpy(result, rawData, sizeof(result));

        std::vector<int16_t> short_data = {
    
    result[0], result[1], result[2], result[3]};
        return short_data;
    }

    std::string _port_name;
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher_;
    std::thread imu_thread_;
};

int main(int argc, char **argv)
{
    
    
    rclcpp::init(argc, argv);
    auto imu_node = std::make_shared<IMUDriverNode>("imu_driver_node");
    imu_thread_running.store(true);

    rclcpp::spin(imu_node);
    imu_thread_running.store(false);

    imu_node->joinIMUThread();

    rclcpp::shutdown();
    return 0;
}

cmake配置如下:

cmake_minimum_required(VERSION 3.8)
project(imu_driver)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(sensor_msgs REQUIRED)
set(CMAKE_INSTALL_RPATH /usr/local/lib)
find_package(serial REQUIRED)

add_executable(imu_driver_exe src/imu.cpp)
target_include_directories(imu_driver_exe PUBLIC
  $<BUILD_INTERFACE:${
    
    CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_compile_features(imu_driver_exe PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
ament_target_dependencies(
  imu_driver_exe
  "rclcpp"
  "std_msgs"
  "sensor_msgs"
  "tf2"
  "serial"
)

install(TARGETS imu_driver_exe DESTINATION lib/${
    
    PROJECT_NAME})

install(DIRECTORY launch DESTINATION share/${
    
    PROJECT_NAME})

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

package.xml配置如下：

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>gnss_imu_sim</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="[email protected]">zard</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>std_msgs</depend>
  <depend>tf2</depend>
  <depend>sensor_msgs</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

启动文件：

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='gnss_imu_sim',
            executable='imu_driver_exe',
            name='imu_driver_exe',
            parameters=[{
    
    "~port_name":"/dev/imu_usb"}],
            output='both'
        )
    ])

订阅话题查看：

安装IMU可视化工具，ROS2有直接对IMU姿态的可视化工具：

sudo apt install ros-galactic-imu-tools

添加话题，并输入正确的坐标系，即可看到IMU坐标系姿态变化