From d4d226d999d0572cd41731d8e13ebc9147f5aaef Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Wed, 19 Feb 2025 21:49:10 +0800 Subject: [PATCH] =?UTF-8?q?=E7=AE=80=E6=98=93=E9=80=9A=E8=AE=AF=E5=AE=8C?= =?UTF-8?q?=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 47 ++++ README.md | 71 +++++- config/serial_config.yaml | 4 + include/rm_serial_driver/crc.hpp | 14 ++ include/rm_serial_driver/protocol.hpp | 69 ++++++ include/rm_serial_driver/rm_serial_driver.hpp | 42 ++++ launch/rm_serial_driver.launch.py | 26 +++ package.xml | 18 ++ src/crc.cpp | 72 +++++++ src/rm_serial_driver.cpp | 204 ++++++++++++++++++ 10 files changed, 566 insertions(+), 1 deletion(-) create mode 100644 CMakeLists.txt create mode 100644 config/serial_config.yaml create mode 100644 include/rm_serial_driver/crc.hpp create mode 100644 include/rm_serial_driver/protocol.hpp create mode 100644 include/rm_serial_driver/rm_serial_driver.hpp create mode 100644 launch/rm_serial_driver.launch.py create mode 100644 package.xml create mode 100644 src/crc.cpp create mode 100644 src/rm_serial_driver.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..bf56ffc --- /dev/null +++ b/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 + $ + $ +) + +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() \ No newline at end of file diff --git a/README.md b/README.md index dab3ce6..3f08a20 100644 --- a/README.md +++ b/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 +``` + +## 注意事项 + +- 确保在修改协议和消息后重新编译项目。 +- 如果遇到问题，请检查串口连接和权限设置。 diff --git a/config/serial_config.yaml b/config/serial_config.yaml new file mode 100644 index 0000000..5be6f22 --- /dev/null +++ b/config/serial_config.yaml @@ -0,0 +1,4 @@ +/rm_serial_driver: + ros__parameters: + device_name: /dev/ttyACM0 + baud_rate: 115200 \ No newline at end of file diff --git a/include/rm_serial_driver/crc.hpp b/include/rm_serial_driver/crc.hpp new file mode 100644 index 0000000..7e0b040 --- /dev/null +++ b/include/rm_serial_driver/crc.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include +#include + +#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 \ No newline at end of file diff --git a/include/rm_serial_driver/protocol.hpp b/include/rm_serial_driver/protocol.hpp new file mode 100644 index 0000000..47ca384 --- /dev/null +++ b/include/rm_serial_driver/protocol.hpp @@ -0,0 +1,69 @@ +#pragma once + +#define ID_MCU (0xC4) +#define ID_REF (0xC5) + +#include + +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; \ No newline at end of file diff --git a/include/rm_serial_driver/rm_serial_driver.hpp b/include/rm_serial_driver/rm_serial_driver.hpp new file mode 100644 index 0000000..de15c28 --- /dev/null +++ b/include/rm_serial_driver/rm_serial_driver.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#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_port_; + rclcpp::Publisher::SharedPtr data_mcu_pub_; + rclcpp::Publisher::SharedPtr data_ref_pub_; + rclcpp::Subscription::SharedPtr data_ai_sub_; + std::thread read_thread_; // 串口数据读取线程 + std::atomic running_; // 原子操作: 保证多线程操作时的数据一致性 + std::string serial_buffer_; // 串口数据缓冲区 + }; + +} // namespace rm_serial_driver diff --git a/launch/rm_serial_driver.launch.py b/launch/rm_serial_driver.launch.py new file mode 100644 index 0000000..edff082 --- /dev/null +++ b/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]) \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..2696010 --- /dev/null +++ b/package.xml @@ -0,0 +1,18 @@ + + rm_serial_driver + 0.0.0 + ROS2 Humble Serial Driver + Your Name + Apache-2.0 + + ament_cmake + geometry_msgs + rclcpp + std_msgs + serial + rm_msgs + + + ament_cmake + + \ No newline at end of file diff --git a/src/crc.cpp b/src/crc.cpp new file mode 100644 index 0000000..582c9d2 --- /dev/null +++ b/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(buf + (len % 2)); + return expected == ptr[len / sizeof(uint16_t) - 1]; +} + +} // namespace crc16 \ No newline at end of file diff --git a/src/rm_serial_driver.cpp b/src/rm_serial_driver.cpp new file mode 100644 index 0000000..ad4a18c --- /dev/null +++ b/src/rm_serial_driver.cpp @@ -0,0 +1,204 @@ +#include "rm_serial_driver/rm_serial_driver.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +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(device, baud, serial::Timeout::simpleTimeout(1000)); + data_mcu_pub_ = this->create_publisher("data_mcu", 10); + data_ref_pub_ = this->create_publisher("data_ref", 10); + data_ai_sub_ = this->create_subscription( + "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(&package.data), sizeof(DataAI_t), CRC16_INIT); + serial_port_->write(reinterpret_cast(&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(&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(&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(ID_MCU) + static_cast(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(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) \ No newline at end of file