基于ROS 2的IMU驱动节点，C++版本，imu型号，维特的jy901b_ros2 维特imu

python的代码网上有一堆，c++的没有找到，在此补充一下，实际测试运行好像c++占用cpu要比python低一些

一、头文件包含部分

#include <chrono>
#include <memory>
#include <string>
#include <vector>
#include <thread>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <termios.h>
#include <fcntl.h>
#include <unistd.h>
#include <cmath>
#include <array>

这些头文件包含了所需的标准C++库、ROS 2相关库以及串口通信相关的头文件。

二、类定义：IMUDriverNode

class IMUDriverNode : public rclcpp::Node {
public:
  IMUDriverNode()
  : Node("imu_driver_node") {
    // 创建IMU数据发布者
    imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("imu", 10);
 
    // 声明ROS参数，并从参数服务器获取串口和波特率参数
    this->declare_parameter<std::string>("port", "/dev/ttyUSB0");
    this->declare_parameter<int>("baud", 115200);
    port_ = this->get_parameter("port").as_string();
    baud_rate_ = this->get_parameter("baud").as_int();
 
    // 打开串口
    serial_fd_ = open_serial_port(port_, baud_rate_);
    if (serial_fd_ < 0) {
      RCLCPP_ERROR(this->get_logger(), "Failed to open serial port");
      rclcpp::shutdown();
      return;
    }
 
    // 创建驱动线程
    driver_thread_ = std::thread(&IMUDriverNode::driver_loop, this);
  }
 
  ~IMUDriverNode() {
    // 在节点析构时关闭串口和等待驱动线程结束
    if (serial_fd_ >= 0) {
      close(serial_fd_);
    }
    if (driver_thread_.joinable()) {
      driver_thread_.join();
    }
  }

私有成员变量:

private:
  int serial_fd_;
  std::string port_;
  int baud_rate_;
  std::thread driver_thread_;
 
  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
 
  std::array<double, 3> angular_velocity_ = {0, 0, 0};
  std::array<double, 3> acceleration_ = {0, 0, 0};
  std::array<double, 3> angle_degree_ = {0, 0, 0};

• serial_fd_: 串口文件描述符。
• port_ 和 baud_rate_: 存储串口和波特率参数。
• driver_thread_: 用于串口数据读取和处理的线程。
• imu_pub_: IMU数据发布者。
• angular_velocity_, acceleration_, angle_degree_: 存储IMU测量值的数组

三、打开串口，这里根据官方给出的ros1版本做了修改：

这里附上官方文档的链接：SDK适用TTL/232 · 深圳维特智能科技有限公司

private:
  int open_serial_port(const std::string& port, int baud_rate) {
    // 打开并配置串口
    int fd = open(port.c_str(), O_RDWR | O_NOCTTY);
    if (fd < 0) {
      RCLCPP_ERROR(this->get_logger(), "Error opening serial port");
      return -1;
    }
 
    // 获取当前串口配置
    struct termios tty;
    if (tcgetattr(fd, &tty) != 0) {
      RCLCPP_ERROR(this->get_logger(), "Error getting termios attributes");
      close(fd);
      return -1;
    }
 
    // 设置波特率
    cfsetospeed(&tty, B230400);
    cfsetispeed(&tty, B230400);
 
    // 配置串口属性
    tty.c_cflag &= ~PARENB; // 禁用奇偶校验
    tty.c_cflag &= ~CSTOPB; // 设置为1个停止位
    tty.c_cflag &= ~CSIZE;//清除字符大小掩码
    tty.c_cflag |= CS8;     //设置字符大小为8位
    tty.c_cflag &= ~CRTSCTS; // 禁用硬件流控制
    tty.c_cflag |= CREAD | CLOCAL; //  启用接收 & 忽略控制线状态
 
    tty.c_lflag &= ~ICANON;//禁用规范模式
    tty.c_lflag &= ~ECHO; //禁用回显
    tty.c_lflag &= ~ECHOE; //禁用擦除
    tty.c_lflag &= ~ECHONL; //禁用换行回显
    tty.c_lflag &= ~ISIG; // 禁用特殊字符的信号处理
    tty.c_iflag &= ~(IXON | IXOFF | IXANY); //关闭软件流控制
    tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL); //  禁用特殊字符处理
 
    tty.c_oflag &= ~OPOST; // 禁止对输出字节进行特殊处理
    tty.c_oflag &= ~ONLCR; //禁止将换行符转换为回车换行
 
    tty.c_cc[VTIME] = 1;    // 设置等待时间为0.1秒
    tty.c_cc[VMIN] = 0;
 
    // 应用新的串口配置
    if (tcsetattr(fd, TCSANOW, &tty) != 0) {
      RCLCPP_ERROR(this->get_logger(), "Error setting termios attributes");
      close(fd);
      return -1;
    }
 
    return fd;
  }

四、串口数据读取和处理的主循环。在循环中，通过调用 read 读取串口数据，处理每个字节数据，并根据处理结果发布IMU数据：

private:
  bool handle_serial_data(uint8_t raw_data) {
    // 静态缓冲区和索引
    static std::vector<uint8_t> buff(11, 0);
    static int key = 0;
 
    // 将接收到的字节存入缓冲区
    buff[key] = raw_data;
    key++;
 
    // 检查帧头
    if (buff[0] != 0x55) {
      // 如果帧头不对，重置缓冲区和索引
      key = 0;
      return false;
    }
 
    // 等待接收完整帧
    if (key < 11) {
      return false;
    }
 
    // 检查校验和
    bool angle_flag = false;
    if (check_sum(buff.data(), 10, buff[10])) {
      // 解析数据
      switch (buff[1]) {
        case 0x51:
          acceleration_ = parse_data(buff, 16, 9.8);
          break;
        case 0x52:
          angular_velocity_ = parse_data(buff, 2000, M_PI / 180);
          break;
        case 0x53:
          angle_degree_ = parse_data(buff, 180, 1);
          angle_flag = true;
          break;
        default:
          break;
      }
    }
 
    // 重置缓冲区和索引
    buff.assign(11, 0);
    key = 0;
 
    return angle_flag;
  }

五、处理接收到的串口数据。检查帧头是否正确，等待接收完整帧，验证校验和，解析数据，并根据数据类型进行相应的处理。

private:
  std::array<double, 3> parse_data(const std::vector<uint8_t>& data, double scale, double unit) {
    std::array<double, 3> result;
    for (int i = 0; i < 3; ++i) {
      // 解析数据并转换为物理量
      result[i] = static_cast<int16_t>((data[3 + 2 * i] << 8) | data[2 + 2 * i]) / 32768.0 * scale * unit;
    }
    return result;
  }

六、计算并验证数据的校验和，确保数据完整性。

private:
  bool check_sum(const uint8_t* data, size_t len, uint8_t checksum) {
    // 计算校验和
    uint8_t sum = 0;
    for (size_t i = 0; i < len; ++i) {
      sum += data[i];
    }
    // 比较校验和
    return sum == checksum;
  }

七、创建IMU消息并填充加速度、角速度和姿态数据，然后发布至ROS主题。

private:
  void publish_imu_data() {
    // 创建IMU消息并填充数据
    auto imu_msg = sensor_msgs::msg::Imu();
    imu_msg.header.stamp = this->now();
    imu_msg.header.frame_id = "imu_link";
 
    imu_msg.linear_acceleration.x = acceleration_[0];
    imu_msg.linear_acceleration.y = acceleration_[1];
    imu_msg.linear_acceleration.z = acceleration_[2];
 
    imu_msg.angular_velocity.x = angular_velocity_[0];
    imu_msg.angular_velocity.y = angular_velocity_[1];
    imu_msg.angular_velocity.z = angular_velocity_[2];
 
    // 将角度转换为四元数并填充到消息中
    auto quat = get_quaternion_from_euler(angle_degree_[0] * M_PI / 180, angle_degree_[1] * M_PI / 180, angle_degree_[2] * M_PI / 180);
 
    imu_msg.orientation.x = quat[0];
    imu_msg.orientation.y = quat[1];
    imu_msg.orientation.z = quat[2];
    imu_msg.orientation.w = quat[3];
 
    // 发布IMU消息
    imu_pub_->publish(imu_msg);
  }

八、根据给定的欧拉角（roll、pitch、yaw）计算对应的四元数表示。

private:
  std::array<double, 4> get_quaternion_from_euler(double roll, double pitch, double yaw) {
    // 根据欧拉角计算四元数
    std::array<double, 4> quat;
    double cy = cos(yaw * 0.5);
    double sy = sin(yaw * 0.5);
    double cp = cos(pitch * 0.5);
    double sp = sin(pitch * 0.5);
    double cr = cos(roll * 0.5);
    double sr = sin(roll * 0.5);
 
    quat[0] = sr * cp * cy - cr * sp * sy;
    quat[1] = cr * sp * cy + sr * cp * sy;
    quat[2] = cr * cp * sy - sr * sp * cy;
    quat[3] = cr * cp * cy + sr * sp * sy;
    return quat;
  }

九、最后主函数收尾：

int main(int argc, char* argv[]) {
  // 初始化ROS节点
  rclcpp::init(argc, argv);
 
  // 创建IMU驱动节点实例，并进入ROS事件循环
  auto node = std::make_shared<IMUDriverNode>();
  rclcpp::spin(node);
 
  // 关闭ROS节点
  rclcpp::shutdown();
  return 0;
}

十，最后附上CMakeList.txt,如果有遗漏的包可以补充，自己测的时候没有遗漏。

cmake_minimum_required(VERSION 3.5)
project(imu_driver)
 
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(serial REQUIRED)
 
add_executable(imu_driver src/imu_driver.cpp)
 
ament_target_dependencies(imu_driver
  rclcpp
  sensor_msgs
  serial
)
 
install(TARGETS
  imu_driver
  DESTINATION lib/${PROJECT_NAME})
 
ament_package()

运行：colcon build
          source install/setup.bash
          ros2 run imu_driver imu_driver