简易通讯完成

CMakeLists.txt

@@ -0,0 +1,47 @@
+cmake_minimum_required(VERSION 3.5)
+project(rm_serial_driver)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(serial REQUIRED)
+find_package(rm_msgs REQUIRED)
+
+# 构建共享库作为组件节点
+add_library(rm_serial_driver SHARED
+  src/rm_serial_driver.cpp
+  src/crc.cpp
+)
+
+target_include_directories(rm_serial_driver PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(rm_serial_driver
+  rclcpp
+  rclcpp_components
+  std_msgs
+  serial
+  rm_msgs
+)
+
+# 注册节点
+rclcpp_components_register_nodes(rm_serial_driver "rm_serial_driver::RMSerialDriver")
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+install(DIRECTORY config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+install(TARGETS rm_serial_driver
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+ament_package()

README.md

@@ -1,2 +1,71 @@
+```markdown
 # rm_serial_driver
-ROS2 : The upper computer serial communication program suitable for XR. 
+
+ROS2: 适用于RM的简易上位机串口通信程序。
+
+## 使用方式
+
+### 1. 安装依赖
+
+首先，安装所需的依赖包：
+
+```bash
+sudo apt install ros-humble-serial-driver
+```
+
+### 2. 编译并安装串口库
+
+克隆串口库源码并编译安装：
+
+```bash
+git clone https://github.com/ZhaoXiangBox/serial.git
+cd serial
+mkdir build
+cd build
+cmake ..
+make
+sudo make install
+```
+
+**注意**：安装完成后可能需要重启系统。
+
+### 3. 下载并编译项目
+
+将项目克隆到你的ROS2工作空间中：
+
+```bash
+cd ~/ros2_ws/src
+git clone https://github.com/goldenfishs/rm_serial_driver.git
+git clone https://github.com/goldenfishs/rm_msg.git  # 包含所需的自定义话题消息，必须一起使用
+```
+
+回到工作空间并编译：
+
+```bash
+cd ~/ros2_ws
+colcon build
+source install/setup.bash
+```
+
+### 4. 运行程序
+
+使用以下命令启动串口驱动：
+
+```bash
+ros2 launch rm_serial_driver rm_serial_driver.launch.py
+```
+使用时请将上位机控制命令发送到data_ai话题，即可将数据传给下位机，下位机发送上来的数据会发布到data_mcu和data_ref话题，其他程序可直接哪去
+
+### 5. 自定义通信协议
+
+如果需要自定义通信协议，可以参考 `protocol.h` 文件，修改对应的协议。同时，修改 `rm_msgs` 包中的自定义消息，注意数据顺序和数据类型。修改完成后，重新编译即可。
+
+```bash
+colcon build
+source install/setup.bash
+```
+
+## 注意事项
+
+- 确保在修改协议和消息后重新编译项目。
+- 如果遇到问题，请检查串口连接和权限设置。

config/serial_config.yaml

@@ -0,0 +1,4 @@
+/rm_serial_driver:
+  ros__parameters:
+    device_name: /dev/ttyACM0
+    baud_rate: 115200

include/rm_serial_driver/crc.hpp

@@ -0,0 +1,14 @@
+#pragma once
+
+#include <cstddef>
+#include <cstdint>
+
+#define CRC16_INIT 0xFFFF
+
+namespace crc16 {
+
+uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc);
+
+bool CRC16_Verify(const uint8_t *buf, size_t len);
+
+} // namespace crc16

include/rm_serial_driver/protocol.hpp

@@ -0,0 +1,69 @@
+#pragma once
+
+#define ID_MCU (0xC4)
+#define ID_REF (0xC5)
+
+#include <cstdint>
+
+typedef struct __attribute__((packed))
+{
+    struct __attribute__((packed))
+    {
+        float q0;
+        float q1;
+        float q2;
+        float q3;
+    } quat; /* 四元数 */
+    struct __attribute__((packed))
+    {
+        float yaw;
+        float pit;
+        float rol;
+    } gimbal; /* 欧拉角 */
+    uint8_t notice; /* 控制命令 */
+} DataMCU_t;
+
+typedef struct __attribute__((packed))
+{
+    uint8_t id;
+    DataMCU_t data;
+    uint16_t crc16;
+} PackageMCU_t;
+
+typedef struct __attribute__((packed))
+{
+    uint16_t team;         /* 本身队伍 */
+    uint16_t time;         /* 比赛开始时间 */
+    uint8_t sentry_hp;     /* 哨兵血量 */
+    uint8_t ballet_remain; /* 剩余弹量 */
+} DataReferee_t;
+
+typedef struct __attribute__((packed))
+{
+    uint8_t id;
+    DataReferee_t data;
+    uint16_t crc16;
+} PackageReferee_t;
+
+typedef struct __attribute__((packed))
+{
+    struct __attribute__((packed))
+    {
+        float yaw;
+        float pit;
+        float rol;
+    } delta_eulr;
+    struct __attribute__((packed))
+    {
+        float vx;
+        float vy;
+        float wz;
+    } delta_pos;
+    uint8_t notice; /* 控制命令 */
+} DataAI_t;
+
+typedef struct __attribute__((packed))
+{
+    DataAI_t data;
+    uint16_t crc16;
+} PackageAI_t;

include/rm_serial_driver/rm_serial_driver.hpp

@@ -0,0 +1,42 @@
+#pragma once
+
+#include <serial/serial.h>
+
+#include <atomic>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <string>
+#include <thread>
+
+#include "rm_msgs/msg/data_mcu.hpp"
+#include "rm_msgs/msg/data_ref.hpp"
+#include "rm_msgs/msg/data_ai.hpp" 
+#include "rm_serial_driver/crc.hpp"
+#include "rm_serial_driver/protocol.hpp"
+
+namespace rm_serial_driver
+{
+
+  class RMSerialDriver : public rclcpp::Node
+  {
+  public:
+    explicit RMSerialDriver(const rclcpp::NodeOptions &options);
+    ~RMSerialDriver() override;
+
+  private:
+    size_t get_expected_length(uint8_t id);
+    void process_packet(uint8_t id, const char *data, size_t packet_size);
+    void receive_data();
+    void reopen_port();
+    void data_ai_callback(const rm_msgs::msg::DataAI::SharedPtr msg);
+    
+    std::unique_ptr<serial::Serial> serial_port_;
+    rclcpp::Publisher<rm_msgs::msg::DataMCU>::SharedPtr data_mcu_pub_;
+    rclcpp::Publisher<rm_msgs::msg::DataRef>::SharedPtr data_ref_pub_;
+    rclcpp::Subscription<rm_msgs::msg::DataAI>::SharedPtr data_ai_sub_; 
+    std::thread read_thread_;   // 串口数据读取线程
+    std::atomic<bool> running_; // 原子操作: 保证多线程操作时的数据一致性
+    std::string serial_buffer_; // 串口数据缓冲区
+  };
+
+} // namespace rm_serial_driver

launch/rm_serial_driver.launch.py

@@ -0,0 +1,26 @@
+from launch import LaunchDescription
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+import os
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+    pkg_share = get_package_share_directory('rm_serial_driver')
+    param_file = os.path.join(pkg_share, 'config', 'serial_config.yaml')
+
+    container = ComposableNodeContainer(
+        name='rm_serial_container',
+        namespace='',
+        package='rclcpp_components',
+        executable='component_container_mt',  # 多线程容器
+        composable_node_descriptions=[
+            ComposableNode(
+                package='rm_serial_driver',
+                plugin='rm_serial_driver::RMSerialDriver',
+                name='rm_serial_driver',
+                parameters=[param_file]
+            )
+        ],
+        output='screen'
+    )
+    return LaunchDescription([container])

package.xml

@@ -0,0 +1,18 @@
+<package format="2">
+  <name>rm_serial_driver</name>
+  <version>0.0.0</version>
+  <description>ROS2 Humble Serial Driver</description>
+  <maintainer email="your_email@example.com">Your Name</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>serial</depend>
+  <depend>rm_msgs</depend>  
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/crc.cpp

@@ -0,0 +1,72 @@
+#include "rm_serial_driver/crc.hpp"
+
+namespace crc16 {
+
+static const uint16_t crc16_tab[256] = {
+    0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, 0x9dc1, 0xaf5a, 0xbed3,
+    0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e,
+    0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399,
+    0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5,
+    0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50,
+    0xfbef, 0xea66, 0xd8fd, 0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
+    0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285, 0x430c, 0x7197, 0x601e,
+    0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
+    0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5,
+    0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
+    0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693,
+    0xc22c, 0xd3a5, 0xe13e, 0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
+    0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1, 0x0948, 0x3bd3, 0x2a5a,
+    0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
+    0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710,
+    0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
+    0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df,
+    0x0c60, 0x1de9, 0x2f72, 0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
+    0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e, 0xf687, 0xc41c, 0xd595,
+    0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
+    0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c,
+    0x3de3, 0x2c6a, 0x1ef1, 0x0f78};
+
+/**
+ * @brief 处理一个字节的数据，更新 CRC：CRC16-CCITT
+ * @param crc 初始值
+ * @param data 当前字节数据
+ * @return uint16_t 更新后的 CRC 值
+ */
+static inline uint16_t CRC16_Byte(uint16_t crc, const uint8_t data)
+{
+  return (crc >> 8) ^ crc16_tab[(crc ^ data) & 0xff];
+}
+
+/**
+ * @brief 计算 CRC：CRC16-CCITT
+ * @param buf 数据缓冲区
+ * @param len 数据长度
+ * @param crc 初始值
+ * @return
+ */
+uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc)
+{
+  while (len--)
+  {
+    crc = CRC16_Byte(crc, *buf++);
+  }
+  return crc;
+}
+
+/**
+ * @brief 验证 CRC：CRC16-CCITT
+ * @param buf 数据缓冲区
+ * @param len 数据长度
+ * @return
+ */
+bool CRC16_Verify(const uint8_t *buf, size_t len)
+{
+  if (len < sizeof(uint16_t))
+    return false;
+
+  uint16_t expected = CRC16_Calc(buf, len - sizeof(uint16_t), CRC16_INIT);
+  auto ptr = reinterpret_cast<const uint16_t *>(buf + (len % 2));
+  return expected == ptr[len / sizeof(uint16_t) - 1];
+}
+
+} // namespace crc16

src/rm_serial_driver.cpp

@@ -0,0 +1,204 @@
+#include "rm_serial_driver/rm_serial_driver.hpp"
+
+#include <chrono>
+#include <cstring>
+#include <iomanip>
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <sstream>
+#include <std_msgs/msg/string.hpp>
+#include <thread>
+
+namespace rm_serial_driver
+{
+
+  /**
+   * @brief 构造函数：初始化节点，打开串口，创建发布者和订阅者，启动数据接收线程，打印启动信息
+   * @param options
+   */
+  RMSerialDriver::RMSerialDriver(const rclcpp::NodeOptions &options)
+      : Node("rm_serial_driver", options), running_(true)
+  {
+    std::string device = this->declare_parameter("device_name", std::string("/dev/ttyACM0"));
+    int baud = this->declare_parameter("baud_rate", 115200);
+    serial_port_ =
+        std::make_unique<serial::Serial>(device, baud, serial::Timeout::simpleTimeout(1000));
+    data_mcu_pub_ = this->create_publisher<rm_msgs::msg::DataMCU>("data_mcu", 10);
+    data_ref_pub_ = this->create_publisher<rm_msgs::msg::DataRef>("data_ref", 10);
+    data_ai_sub_ = this->create_subscription<rm_msgs::msg::DataAI>(
+        "data_ai", 10, std::bind(&RMSerialDriver::data_ai_callback, this, std::placeholders::_1)); // 新增订阅者
+    read_thread_ = std::thread(&RMSerialDriver::receive_data, this);
+    RCLCPP_INFO(get_logger(), "RM 启动！！！");
+  }
+
+  /**
+   * @brief 析构函数：关闭串口，等待数据接收线程结束
+   */
+  RMSerialDriver::~RMSerialDriver()
+  {
+    running_.store(false);
+    if (read_thread_.joinable())
+    {
+      read_thread_.join();
+    }
+    if (serial_port_->isOpen())
+    {
+      serial_port_->close();
+    }
+  }
+
+  /**
+   * @brief AI 数据回调函数
+   * @param msg
+   */
+  void RMSerialDriver::data_ai_callback(const rm_msgs::msg::DataAI::SharedPtr msg)
+  {
+    PackageAI_t package;
+    std::memcpy(&package.data, msg.get(), sizeof(DataAI_t));
+    package.crc16 = crc16::CRC16_Calc(reinterpret_cast<const uint8_t *>(&package.data), sizeof(DataAI_t), CRC16_INIT);
+    serial_port_->write(reinterpret_cast<const uint8_t *>(&package), sizeof(PackageAI_t));
+  }
+
+  /**
+   * @brief 获取期望的数据包长度
+   * @param id
+   * @return
+   */
+  size_t RMSerialDriver::get_expected_length(uint8_t id)
+  {
+    if (id == ID_MCU)
+      return sizeof(PackageMCU_t);
+    else if (id == ID_REF)
+      return sizeof(PackageReferee_t);
+    return 0;
+  }
+
+  /**
+   * @brief 处理接收到的数据包
+   * @param id
+   * @param data
+   * @param packet_size
+   */
+  void RMSerialDriver::process_packet(uint8_t id, const char *data, size_t packet_size)
+  {
+    if (id == ID_MCU)
+    {
+      PackageMCU_t mcu;
+      std::memcpy(&mcu, data, packet_size); // 将 data 拷贝到 mcu 中
+      if (crc16::CRC16_Verify(reinterpret_cast<uint8_t *>(&mcu) + 1, packet_size - 1))
+      {
+        rm_msgs::msg::DataMCU msg;
+        std::memcpy(&msg, &mcu.data, sizeof(mcu.data)); // 将数据拷贝到 msg 中
+        data_mcu_pub_->publish(msg);
+      }
+      else
+      {
+        RCLCPP_ERROR(get_logger(), "CRC 校验失败(MCU)");
+      }
+    }
+    else if (id == ID_REF)
+    {
+      PackageReferee_t ref;
+      std::memcpy(&ref, data, packet_size);
+      if (crc16::CRC16_Verify(reinterpret_cast<uint8_t *>(&ref) + 1, packet_size - 1))
+      {
+        rm_msgs::msg::DataRef msg;
+        std::memcpy(&msg, &ref.data, sizeof(ref.data));
+        data_ref_pub_->publish(msg);
+      }
+      else
+      {
+        RCLCPP_WARN(get_logger(), "CRC 校验失败(MCU)");
+      }
+    }
+    else
+    {
+      // 未知 ID，丢弃该字节
+    }
+  }
+
+  /**
+   * @brief 串口数据接收线程
+   * @param args 线程参数
+   */
+  void RMSerialDriver::receive_data()
+  {
+    while (rclcpp::ok() && running_.load())
+    {
+      try
+      {
+        if (serial_port_->available())
+        {
+          std::string buffer = serial_port_->read(serial_port_->available());
+          serial_buffer_ += buffer;
+
+          while (true)
+          {
+            size_t header_pos = serial_buffer_.find_first_of(
+                std::string() + static_cast<char>(ID_MCU) + static_cast<char>(ID_REF)); // 查找第一个出现的字符
+            if (header_pos == std::string::npos)
+            {
+              serial_buffer_.clear();
+              break;
+            }
+            if (header_pos > 0)
+            {
+              serial_buffer_.erase(0, header_pos);
+            }
+            if (serial_buffer_.empty())
+              break;
+
+            uint8_t id = static_cast<uint8_t>(serial_buffer_[0]);
+            size_t expected_length = get_expected_length(id);
+            if (expected_length == 0)
+            {
+              serial_buffer_.erase(0, 1);
+              continue;
+            }
+            if (serial_buffer_.size() < expected_length)
+              break;
+
+            process_packet(id, serial_buffer_.data(), expected_length);
+            serial_buffer_.erase(0, expected_length);
+          }
+        }
+      }
+      catch (const std::exception &ex)
+      {
+        reopen_port();
+      }
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+  }
+
+  /**
+   * @brief 重启串口
+   * @note 当串口意外关闭时，尝试重新打开串口
+   */
+  void RMSerialDriver::reopen_port()
+  {
+    if (serial_port_->isOpen())
+    {
+      serial_port_->close();
+    }
+
+    bool port_opened = false;
+    while (!port_opened && rclcpp::ok())
+    {
+      try
+      {
+        RCLCPP_WARN(get_logger(), "串口意外关闭，正在尝试重新打开...");
+        serial_port_->open();
+        port_opened = true;
+      }
+      catch (const std::exception &ex)
+      {
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+      }
+    }
+  }
+
+} // namespace rm_serial_driver
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(rm_serial_driver::RMSerialDriver)