lock

src/base/sweeper_interfaces/CMakeLists.txt

@@ -14,13 +14,15 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/McCtrl.msg"
   "msg/CanFrame.msg"
   "msg/Sub.msg"
-  "msg/Rtk.msg" 
+  "msg/Rtk.msg"
-  "msg/Pl.msg" 
+  "msg/Pl.msg"
-  "msg/DetectLine.msg" 
+  "msg/DetectLine.msg"
-  "msg/Route.msg" 
+  "msg/Route.msg"
-  "msg/Fu.msg"  
+  "msg/Fu.msg"
-  "msg/Task.msg"  
+  "msg/Task.msg"
   "msg/VehicleIdentity.msg"
+  "msg/LockControl.msg"
+  "msg/LockStatus.msg"
   DEPENDENCIES std_msgs
 )
 

src/base/sweeper_interfaces/msg/LockControl.msg

@@ -0,0 +1,21 @@
+# 锁孔板控制指令
+# 用于控制车辆的锁孔板开关
+
+# 锁孔板控制指令类型
+uint8 CMD_UNKNOWN = 0
+uint8 CMD_OPEN = 1
+uint8 CMD_CLOSE = 2
+uint8 CMD_STOP = 3
+uint8 CMD_STATUS = 4
+
+# 控制指令
+uint8 command
+
+# 设备ID（预留，用于支持多个锁孔板）
+uint8 device_id
+
+# 超时时间（毫秒）
+uint32 timeout
+
+# 是否需要等待执行完成（阻塞模式）
+bool wait_complete

src/base/sweeper_interfaces/msg/LockStatus.msg

@@ -0,0 +1,24 @@
+# 锁孔板状态反馈
+
+# 设备ID
+uint8 device_id
+
+# 锁孔板状态
+uint8 STATUS_UNKNOWN = 0
+uint8 STATUS_OPENED = 1
+uint8 STATUS_CLOSED = 2
+uint8 STATUS_OPENING = 3
+uint8 STATUS_CLOSING = 4
+uint8 STATUS_FAULT = 5
+uint8 STATUS_TIMEOUT = 6
+
+uint8 status
+
+# 原始反馈数据（用于调试）
+uint8[] raw_data
+
+# 错误码
+uint32 error_code
+
+# 时间戳（秒）
+int64 timestamp

src/control/lock/CMakeLists.txt

@@ -0,0 +1,53 @@
+cmake_minimum_required(VERSION 3.5)
+project(lock)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sweeper_interfaces REQUIRED)
+find_package(logger REQUIRED)
+find_package(Boost REQUIRED COMPONENTS system)
+
+include_directories(include/lock)
+
+add_executable(lock_node src/lock_node.cpp src/serial_port.cpp)
+target_link_libraries(lock_node ${Boost_LIBRARIES})
+ament_target_dependencies(
+  lock_node
+  rclcpp
+  std_msgs
+  sweeper_interfaces
+  logger)
+
+install(TARGETS lock_node DESTINATION lib/${PROJECT_NAME})
+
+# 安装配置文件和启动文件
+install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/control/lock/config/lock.yaml

@@ -0,0 +1,11 @@
+lock:
+  ros__parameters:
+    # 串口设备名称
+    serial_port: "/dev/ttyTHS0"
+    # 波特率
+    baud_rate: 9600
+    # 锁控板地址
+    device_address: 1
+    # 要控制的通道列表（支持一个或多个通道）
+    # 例如：[1] 只控制通道1；[1, 2, 3] 控制通道1、2、3
+    channel_list: [1]

src/control/lock/include/lock/serial_port.hpp

@@ -0,0 +1,131 @@
+#ifndef LOCK__SERIAL_PORT_HPP_
+#define LOCK__SERIAL_PORT_HPP_
+
+#include <boost/asio.hpp>
+#include <cstdint>
+#include <string>
+#include <vector>
+
+/**
+ * @brief 串口通信类，使用Boost ASIO实现
+ *
+ * 提供串口的打开、关闭、读取、写入等功能
+ * 禁止拷贝，禁止移动
+ */
+class SerialPort
+{
+public:
+    /**
+     * @brief 默认构造函数
+     */
+    SerialPort();
+
+    /**
+     * @brief 析构函数，自动关闭串口
+     */
+    ~SerialPort();
+
+    /**
+     * @brief 禁止拷贝构造
+     */
+    SerialPort(const SerialPort&) = delete;
+
+    /**
+     * @brief 禁止拷贝赋值
+     */
+    SerialPort& operator=(const SerialPort&) = delete;
+
+    /**
+     * @brief 禁止移动构造
+     */
+    SerialPort(SerialPort&&) = delete;
+
+    /**
+     * @brief 禁止移动赋值
+     */
+    SerialPort& operator=(SerialPort&&) = delete;
+
+    /**
+     * @brief 初始化并打开串口
+     * @param port_name 串口设备名，如 "/dev/ttyUSB0"
+     * @param baud_rate 波特率，默认 115200
+     * @param parity 校验位，默认无校验
+     * @param stop_bits 停止位，默认1位
+     * @param character_size 数据位，默认8位
+     * @return true 打开成功，false 打开失败
+     */
+    bool init(const std::string& port_name,
+              unsigned int baud_rate = 115200,
+              boost::asio::serial_port_base::parity::type parity =
+                  boost::asio::serial_port_base::parity::none,
+              boost::asio::serial_port_base::stop_bits::type stop_bits =
+                  boost::asio::serial_port_base::stop_bits::one,
+              unsigned int character_size = 8);
+
+    /**
+     * @brief 读取串口数据
+     * @param buf 数据缓冲区
+     * @param size 要读取的字节数
+     * @return 实际读取的字节数，失败返回-1
+     */
+    int read(char buf[], int size);
+
+    /**
+     * @brief 读取串口数据到vector
+     * @param size 要读取的字节数
+     * @return 读取到的数据，失败返回空vector
+     */
+    std::vector<uint8_t> read(int size);
+
+    /**
+     * @brief 写入串口数据
+     * @param buf 数据缓冲区
+     * @param size 要写入的字节数
+     * @return 实际写入的字节数，失败返回-1
+     */
+    int write(const char buf[], int size);
+
+    /**
+     * @brief 写入串口数据(vector版本)
+     * @param data 要写入的数据
+     * @return 实际写入的字节数，失败返回-1
+     */
+    int write(const std::vector<uint8_t>& data);
+
+    /**
+     * @brief 写入串口数据(string版本)
+     * @param data 要写入的数据
+     * @return 实际写入的字节数，失败返回-1
+     */
+    int write(const std::string& data);
+
+    /**
+     * @brief 检查串口是否打开
+     * @return true 已打开，false 未打开
+     */
+    bool is_open() const;
+
+    /**
+     * @brief 关闭串口
+     */
+    void close();
+
+    /**
+     * @brief 获取最后一次错误信息
+     * @return 错误描述字符串
+     */
+    std::string get_last_error() const;
+
+private:
+    /**
+     * @brief 设置错误信息
+     * @param error 错误描述
+     */
+    void set_error(const std::string& error);
+
+    boost::asio::io_service service_;
+    boost::asio::serial_port* port_;
+    std::string last_error_;
+};
+
+#endif  // LOCK__SERIAL_PORT_HPP_

src/control/lock/launch/lock.launch.py

@@ -0,0 +1,23 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    # 获取配置文件路径
+    config_dir = os.path.join(
+        get_package_share_directory("lock"), "config", "lock.yaml"
+    )
+
+    return LaunchDescription(
+        [
+            Node(
+                package="lock",
+                executable="lock_node",
+                name="lock_node",
+                parameters=[config_dir],  # 从YAML文件加载参数
+                output="screen",
+            ),
+        ]
+    )

src/control/lock/package.xml

@@ -0,0 +1,23 @@
+<?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>lock</name>
+  <version>0.0.0</version>
+  <description>Lock control package for sweeper</description>
+  <maintainer email="zxwl@todo.todo">zxwl</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>sweeper_interfaces</depend>
+  <depend>logger</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/control/lock/src/lock_node.cpp

@@ -0,0 +1,569 @@
+#include <stdlib.h>
+#include <string.h>
+
+#include <queue>
+#include <mutex>
+#include <vector>
+#include <map>
+
+#include "logger/logger.h"
+#include "rclcpp/rclcpp.hpp"
+#include "serial_port.hpp"
+#include "sweeper_interfaces/msg/lock_control.hpp"
+#include "sweeper_interfaces/msg/lock_status.hpp"
+
+class LockNode : public rclcpp::Node
+{
+public:
+    // 构造函数
+    LockNode(std::string name) : Node(name)
+    {
+        LOG_INFO("%s节点已经启动.", name.c_str());
+
+        // 声明并获取参数
+        this->declare_parameter("serial_port", "/dev/ttyUSB0");
+        this->declare_parameter("baud_rate", 9600);
+        this->declare_parameter("device_address", 1);  // 锁控板地址
+        this->declare_parameter("channel_list", std::vector<int64_t>{1});  // 通道列表
+
+        std::string serial_port;
+        int baud_rate;
+        int device_address;
+        std::vector<int64_t> channel_list;
+        this->get_parameter("serial_port", serial_port);
+        this->get_parameter("baud_rate", baud_rate);
+        this->get_parameter("device_address", device_address);
+        this->get_parameter("channel_list", channel_list);
+
+        // 存储参数
+        device_address_ = static_cast<uint8_t>(device_address);
+        packet_seq_ = 0;
+
+        // 初始化通道状态
+        for (int64_t ch : channel_list)
+        {
+            uint8_t channel = static_cast<uint8_t>(ch);
+            channel_list_.push_back(channel);
+            channel_status_[channel] = sweeper_interfaces::msg::LockStatus::STATUS_UNKNOWN;
+            LOG_INFO("初始化通道: %d", channel);
+        }
+
+        if (channel_list_.empty())
+        {
+            LOG_WARN("通道列表为空，使用默认通道1");
+            channel_list_.push_back(1);
+            channel_status_[1] = sweeper_interfaces::msg::LockStatus::STATUS_UNKNOWN;
+        }
+
+        // 初始化串口
+        serial_port_ = std::make_unique<SerialPort>();
+        serial_port_->init(serial_port.c_str(), baud_rate);
+
+        // 创建订阅者 - 接收控制指令
+        control_subscriber_ = this->create_subscription<sweeper_interfaces::msg::LockControl>(
+            "lock_control", 10,
+            std::bind(&LockNode::control_callback, this, std::placeholders::_1));
+
+        // 创建发布者 - 发布状态反馈
+        status_publisher_ = this->create_publisher<sweeper_interfaces::msg::LockStatus>("lock_status", 10);
+
+        // 创建定时器 - 定时读取串口状态
+        timer_ = this->create_wall_timer(
+            std::chrono::milliseconds(100),
+            std::bind(&LockNode::timer_callback, this));
+    }
+
+private:
+    // 协议相关常数
+    enum ProtocolConstants {
+        FRAME_HEADER1 = 0xA5,
+        FRAME_HEADER2 = 0xA5,
+        HOST_ADDRESS = 0x00
+    };
+
+    // 命令类型
+    enum CommandTypes {
+        CMD_RESET = 0x01,
+        CMD_QUERY_ADDR = 0x02,
+        CMD_QUERY_CHANNEL_COUNT = 0x03,
+        CMD_OPEN_LOCK = 0x04,
+        CMD_OPEN_LOCK_SPEED = 0x09,
+        CMD_OPEN_ALL = 0x05,
+        CMD_READ_STATUS = 0x06,
+        CMD_READ_ALL_STATUS = 0x07,
+        CMD_READ_VERSION = 0x08
+    };
+
+    // 错误码
+    enum ResultCodes {
+        RESULT_SUCCESS = 0x00,
+        RESULT_DATA_LEN_ERROR = 0x01,
+        RESULT_PARAM_ERROR = 0x02,
+        RESULT_PROGRAM_ERROR = 0x03,
+        RESULT_BUSY = 0x04
+    };
+
+    /**
+     * @brief 计算异或校验和
+     * @param data 数据指针
+     * @param len 数据长度
+     * @return 校验和
+     */
+    uint8_t calculate_checksum(const uint8_t* data, int len)
+    {
+        uint8_t checksum = 0;
+        for (int i = 0; i < len; ++i)
+        {
+            checksum ^= data[i];
+        }
+        return checksum;
+    }
+
+    /**
+     * @brief 构造协议帧
+     * @param cmd 命令
+     * @param data 数据
+     * @param data_len 数据长度
+     * @return 完整的协议帧
+     */
+    std::vector<uint8_t> build_frame(uint8_t cmd, const uint8_t* data, int data_len)
+    {
+        std::vector<uint8_t> frame;
+
+        // 帧头
+        frame.push_back(FRAME_HEADER1);
+        frame.push_back(FRAME_HEADER2);
+
+        // 源地址（主机）
+        frame.push_back(HOST_ADDRESS);
+
+        // 目的地址（锁控板）
+        frame.push_back(device_address_);
+
+        // 包序号
+        frame.push_back(packet_seq_++);
+        // packet_seq_是uint8_t，自然溢出
+
+        // 命令
+        frame.push_back(cmd);
+
+        // 结果（主机发送时固定为0）
+        frame.push_back(0x00);
+
+        // 数据长度
+        frame.push_back(static_cast<uint8_t>(data_len));
+
+        // 数据
+        if (data_len > 0 && data != nullptr)
+        {
+            frame.insert(frame.end(), data, data + data_len);
+        }
+
+        // 校验和（从源地址到数据的异或）
+        uint8_t checksum = calculate_checksum(frame.data() + 2, frame.size() - 2);
+        frame.push_back(checksum);
+
+        return frame;
+    }
+
+    /**
+     * @brief 解析收到的协议帧
+     * @param frame 协议帧
+     * @return 是否解析成功
+     */
+    bool parse_frame(const std::vector<uint8_t>& frame)
+    {
+        // 检查最小长度
+        if (frame.size() < 9)
+        {
+            LOG_WARN("协议帧长度不足");
+            return false;
+        }
+
+        // 检查帧头
+        if (frame[0] != FRAME_HEADER1 || frame[1] != FRAME_HEADER2)
+        {
+            LOG_WARN("协议帧头错误");
+            return false;
+        }
+
+        // 校验和验证
+        uint8_t checksum = calculate_checksum(frame.data() + 2, frame.size() - 3);
+        if (checksum != frame.back())
+        {
+            LOG_WARN("校验和错误");
+            return false;
+        }
+
+        // 提取字段
+        uint8_t cmd = frame[5];
+        uint8_t result = frame[6];
+        uint8_t data_len = frame[7];
+
+        // 检查数据长度
+        if (static_cast<size_t>(data_len) + 9 > frame.size())
+        {
+            LOG_WARN("数据长度不匹配");
+            return false;
+        }
+
+        // 处理命令
+        return process_command(cmd, result, frame.data() + 8, data_len);
+    }
+
+    /**
+     * @brief 处理解析后的命令
+     * @param cmd 命令
+     * @param result 执行结果
+     * @param data 数据指针
+     * @param data_len 数据长度
+     * @return 是否成功处理
+     */
+    bool process_command(uint8_t cmd, uint8_t result, const uint8_t* data, int data_len)
+    {
+        if (result != RESULT_SUCCESS)
+        {
+            LOG_ERROR("命令执行失败，错误码: 0x%02X", result);
+            global_error_code_ = static_cast<uint32_t>(result);
+            return false;
+        }
+        global_error_code_ = 0;
+
+        switch (cmd)
+        {
+            case CMD_OPEN_LOCK:
+                return handle_open_lock_response(data, data_len);
+            case CMD_READ_STATUS:
+                return handle_read_status_response(data, data_len);
+            case CMD_QUERY_ADDR:
+            case CMD_QUERY_CHANNEL_COUNT:
+            case CMD_OPEN_ALL:
+            case CMD_READ_ALL_STATUS:
+            case CMD_READ_VERSION:
+            case CMD_RESET:
+                return true;
+            default:
+                LOG_WARN("未知命令: 0x%02X", cmd);
+                return false;
+        }
+    }
+
+    /**
+     * @brief 处理开锁命令响应
+     * @param data 数据指针
+     * @param data_len 数据长度
+     * @return 是否成功
+     */
+    bool handle_open_lock_response(const uint8_t* data, int data_len)
+    {
+        if (data_len < 2)
+        {
+            LOG_WARN("开锁命令响应数据长度不足");
+            return false;
+        }
+
+        uint8_t channel = data[0];
+        uint8_t lock_status = data[1];
+
+        // 更新对应通道的状态
+        if (channel_status_.find(channel) != channel_status_.end())
+        {
+            channel_status_[channel] = (lock_status == 1) ?
+                sweeper_interfaces::msg::LockStatus::STATUS_OPENED :
+                sweeper_interfaces::msg::LockStatus::STATUS_CLOSED;
+            LOG_INFO("通道%d锁状态: %s", channel, (lock_status == 1) ? "打开" : "关闭");
+        }
+        else
+        {
+            LOG_WARN("收到未配置的通道%d响应", channel);
+        }
+
+        return true;
+    }
+
+    /**
+     * @brief 处理读取状态命令响应
+     * @param data 数据指针
+     * @param data_len 数据长度
+     * @return 是否成功
+     */
+    bool handle_read_status_response(const uint8_t* data, int data_len)
+    {
+        if (data_len < 2)
+        {
+            LOG_WARN("读取状态命令响应数据长度不足");
+            return false;
+        }
+
+        uint8_t channel = data[0];
+        uint8_t lock_status = data[1];
+
+        // 更新对应通道的状态
+        if (channel_status_.find(channel) != channel_status_.end())
+        {
+            channel_status_[channel] = (lock_status == 1) ?
+                sweeper_interfaces::msg::LockStatus::STATUS_OPENED :
+                sweeper_interfaces::msg::LockStatus::STATUS_CLOSED;
+            LOG_INFO("通道%d锁状态: %s", channel, (lock_status == 1) ? "打开" : "关闭");
+        }
+        else
+        {
+            LOG_WARN("收到未配置的通道%d响应", channel);
+        }
+
+        return true;
+    }
+
+    /**
+     * @brief 锁孔板控制指令回调函数
+     * @param msg 控制指令消息
+     */
+    void control_callback(const sweeper_interfaces::msg::LockControl::SharedPtr msg)
+    {
+        std::lock_guard<std::mutex> lock(lock_mutex_);
+
+        LOG_INFO("收到锁孔板控制指令: command=%d, device_id=%d", msg->command, msg->device_id);
+
+        switch (msg->command)
+        {
+            case sweeper_interfaces::msg::LockControl::CMD_OPEN:
+                // 打开所有配置的通道
+                for (uint8_t channel : channel_list_)
+                {
+                    channel_status_[channel] = sweeper_interfaces::msg::LockStatus::STATUS_OPENING;
+                    send_open_lock_command(channel);
+                }
+                break;
+            case sweeper_interfaces::msg::LockControl::CMD_CLOSE:
+                // 协议中没有专门的关锁命令，关闭是通过开锁命令的逆操作
+                // 某些锁是断电闭合，需要根据实际情况实现
+                for (uint8_t channel : channel_list_)
+                {
+                    channel_status_[channel] = sweeper_interfaces::msg::LockStatus::STATUS_CLOSING;
+                }
+                break;
+            case sweeper_interfaces::msg::LockControl::CMD_STOP:
+                // 协议中没有停止命令，可能需要忽略或发送其他指令
+                break;
+            case sweeper_interfaces::msg::LockControl::CMD_STATUS:
+                // 查询所有配置的通道状态
+                for (uint8_t channel : channel_list_)
+                {
+                    send_read_status_command(channel);
+                }
+                break;
+            default:
+                LOG_WARN("未知的控制指令: %d", msg->command);
+                break;
+        }
+    }
+
+    /**
+     * @brief 发送开锁命令到指定通道
+     * @param channel 通道号
+     */
+    void send_open_lock_command(uint8_t channel)
+    {
+        std::vector<uint8_t> data;
+        data.push_back(channel);
+
+        std::vector<uint8_t> frame = build_frame(CMD_OPEN_LOCK, data.data(), static_cast<uint8_t>(data.size()));
+        send_frame(frame);
+    }
+
+    /**
+     * @brief 发送读取状态命令到指定通道
+     * @param channel 通道号
+     */
+    void send_read_status_command(uint8_t channel)
+    {
+        std::vector<uint8_t> data;
+        data.push_back(channel);
+
+        std::vector<uint8_t> frame = build_frame(CMD_READ_STATUS, data.data(), static_cast<uint8_t>(data.size()));
+        send_frame(frame);
+    }
+
+    /**
+     * @brief 发送协议帧
+     * @param frame 协议帧
+     */
+    void send_frame(const std::vector<uint8_t>& frame)
+    {
+        if (!serial_port_ || !serial_port_->is_open())
+        {
+            LOG_ERROR("串口未打开，无法发送数据");
+            return;
+        }
+
+        // 打印发送的帧（十六进制）
+        char log_buf[256];
+        int len = 0;
+        for (size_t i = 0; i < frame.size(); ++i)
+        {
+            len += snprintf(log_buf + len, sizeof(log_buf) - len, "%02X ", static_cast<uint8_t>(frame[i]));
+        }
+        LOG_INFO("发送协议帧: %.*s", len - 1, log_buf);
+
+        // 发送数据
+        int sent = serial_port_->write(reinterpret_cast<const char*>(frame.data()), static_cast<int>(frame.size()));
+        if (sent == static_cast<int>(frame.size()))
+        {
+            LOG_INFO("协议帧发送成功");
+        }
+        else
+        {
+            LOG_ERROR("协议帧发送失败，预期发送%d字节，实际发送%d字节", static_cast<int>(frame.size()), sent);
+        }
+    }
+
+    /**
+     * @brief 定时器回调函数 - 用于读取串口状态
+     */
+    void timer_callback()
+    {
+        std::lock_guard<std::mutex> lock(lock_mutex_);
+
+        // 读取串口数据
+        if (serial_port_ && serial_port_->is_open())
+        {
+            char read_buf[256];
+            int num = serial_port_->read(read_buf, sizeof(read_buf) - 1);
+
+            if (num > 0)
+            {
+                read_buf[num] = '\0';
+                LOG_INFO("收到串口数据: %d 字节", num);
+
+                // 处理收到的数据，添加到缓冲区
+                for (int i = 0; i < num; ++i)
+                {
+                    rx_buffer_.push_back(static_cast<uint8_t>(read_buf[i]));
+                }
+
+                // 解析缓冲区中的协议帧
+                parse_received_data();
+            }
+        }
+
+        // 发布状态
+        publish_status();
+    }
+
+    /**
+     * @brief 解析接收缓冲区中的数据
+     */
+    void parse_received_data()
+    {
+        // 查找帧头
+        size_t header_pos = 0;
+        while (header_pos < rx_buffer_.size() - 1)
+        {
+            if (rx_buffer_[header_pos] == FRAME_HEADER1 && rx_buffer_[header_pos + 1] == FRAME_HEADER2)
+            {
+                // 找到了帧头，检查是否有足够的数据
+                if (rx_buffer_.size() - header_pos < 9)
+                {
+                    // 数据不足，等待更多数据
+                    break;
+                }
+
+                // 解析数据长度
+                uint8_t data_len = rx_buffer_[header_pos + 7];
+                size_t frame_len = 9 + data_len;
+
+                if (rx_buffer_.size() - header_pos < frame_len)
+                {
+                    // 数据不足，等待更多数据
+                    break;
+                }
+
+                // 提取帧
+                std::vector<uint8_t> frame(rx_buffer_.begin() + header_pos,
+                                          rx_buffer_.begin() + header_pos + frame_len);
+
+                // 解析帧
+                parse_frame(frame);
+
+                // 移除已解析的数据
+                rx_buffer_.erase(rx_buffer_.begin(), rx_buffer_.begin() + header_pos + frame_len);
+                header_pos = 0;
+            }
+            else
+            {
+                header_pos++;
+            }
+        }
+
+        // 防止缓冲区无限增长
+        if (rx_buffer_.size() > 512)
+        {
+            LOG_WARN("接收缓冲区过大，清空缓冲区");
+            rx_buffer_.clear();
+        }
+    }
+
+    /**
+     * @brief 发布当前状态
+     */
+    void publish_status()
+    {
+        // 为每个通道发布状态消息
+        for (uint8_t channel : channel_list_)
+        {
+            auto msg = sweeper_interfaces::msg::LockStatus();
+
+            msg.device_id = channel;  // 使用device_id字段表示通道号
+            msg.status = channel_status_[channel];
+            msg.error_code = global_error_code_;
+            msg.timestamp = this->now().seconds();
+
+            status_publisher_->publish(msg);
+
+            LOG_INFO("发布锁孔板状态: channel=%d, status=%d", channel, msg.status);
+        }
+    }
+
+    // ROS2 通信对象
+    rclcpp::Subscription<sweeper_interfaces::msg::LockControl>::SharedPtr control_subscriber_;
+    rclcpp::Publisher<sweeper_interfaces::msg::LockStatus>::SharedPtr status_publisher_;
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    // 串口操作对象
+    std::unique_ptr<SerialPort> serial_port_;
+
+    // 状态变量
+    std::map<uint8_t, uint8_t> channel_status_;  // 通道号 -> 状态
+    uint32_t global_error_code_;
+
+    // 锁控制参数
+    uint8_t device_address_;
+    std::vector<uint8_t> channel_list_;  // 要控制的通道列表
+    uint8_t packet_seq_;
+
+    // 接收缓冲区
+    std::vector<uint8_t> rx_buffer_;
+
+    std::mutex lock_mutex_;
+};
+
+int main(int argc, char** argv)
+{
+    // 初始化日志系统
+    logger::Logger::Init("lock", "./nodes_log");
+
+    rclcpp::init(argc, argv);
+
+    // 创建节点
+    auto node = std::make_shared<LockNode>("lock_node");
+
+    // 运行节点
+    rclcpp::spin(node);
+
+    rclcpp::shutdown();
+
+    // 关闭日志系统
+    logger::Logger::Shutdown();
+
+    return 0;
+}

src/control/lock/src/serial_port.cpp

@@ -0,0 +1,219 @@
+#include "serial_port.hpp"
+
+#include "logger/logger.h"
+#include <stdexcept>
+
+SerialPort::SerialPort() : port_(nullptr) { ; }
+
+SerialPort::~SerialPort()
+{
+    close();
+}
+
+bool SerialPort::init(const std::string& port_name,
+                      unsigned int baud_rate,
+                      boost::asio::serial_port_base::parity::type parity,
+                      boost::asio::serial_port_base::stop_bits::type stop_bits,
+                      unsigned int character_size)
+{
+    try
+    {
+        port_ = new boost::asio::serial_port(service_);
+        port_->open(port_name);
+
+        if (port_->is_open())
+        {
+            port_->set_option(boost::asio::serial_port::baud_rate(baud_rate));
+            port_->set_option(boost::asio::serial_port::flow_control(
+                boost::asio::serial_port::flow_control::none));
+            port_->set_option(boost::asio::serial_port::parity(parity));
+            port_->set_option(boost::asio::serial_port::stop_bits(stop_bits));
+            port_->set_option(boost::asio::serial_port::character_size(character_size));
+            LOG_INFO("打开串口成功！设备: %s, 波特率: %d", port_name.c_str(), baud_rate);
+            last_error_.clear();
+            return true;
+        }
+        else
+        {
+            LOG_ERROR("打开串口失败！设备: %s", port_name.c_str());
+            delete port_;
+            port_ = nullptr;
+            set_error("串口打开失败");
+            return false;
+        }
+    }
+    catch (const std::exception& e)
+    {
+        LOG_ERROR("初始化串口时发生异常：%s", e.what());
+        if (port_)
+        {
+            delete port_;
+            port_ = nullptr;
+        }
+        set_error(e.what());
+        return false;
+    }
+    catch (...)
+    {
+        LOG_ERROR("初始化串口时发生未知异常");
+        if (port_)
+        {
+            delete port_;
+            port_ = nullptr;
+        }
+        set_error("未知异常");
+        return false;
+    }
+}
+
+int SerialPort::read(char buf[], int size)
+{
+    if (port_ == nullptr || !port_->is_open())
+    {
+        LOG_WARN("串口未初始化或未打开");
+        set_error("串口未初始化或未打开");
+        return -1;
+    }
+
+    try
+    {
+        int num = boost::asio::read(*port_, boost::asio::buffer(buf, size));
+        if (num < 0)
+        {
+            LOG_WARN("串口读取错误");
+            set_error("读取错误");
+            return -1;
+        }
+        else if (num == 0)
+        {
+            // LOG_INFO("串口无数据可读");
+            return 0;
+        }
+        last_error_.clear();
+        return num;
+    }
+    catch (const std::exception& e)
+    {
+        LOG_ERROR("串口读取异常：%s", e.what());
+        set_error(e.what());
+        return -1;
+    }
+    catch (...)
+    {
+        LOG_ERROR("串口读取未知异常");
+        set_error("未知异常");
+        return -1;
+    }
+}
+
+std::vector<uint8_t> SerialPort::read(int size)
+{
+    std::vector<uint8_t> data(size);
+    int num = read(reinterpret_cast<char*>(data.data()), size);
+
+    if (num > 0)
+    {
+        if (num < size)
+        {
+            data.resize(num);
+        }
+        return data;
+    }
+    return {};
+}
+
+int SerialPort::write(const char buf[], int size)
+{
+    if (port_ == nullptr || !port_->is_open())
+    {
+        LOG_WARN("串口未初始化或未打开");
+        set_error("串口未初始化或未打开");
+        return -1;
+    }
+
+    try
+    {
+        int num = boost::asio::write(*port_, boost::asio::buffer(buf, size));
+        if (num < 0)
+        {
+            LOG_WARN("串口写入错误");
+            set_error("写入错误");
+            return -1;
+        }
+        else if (num == 0)
+        {
+            LOG_INFO("串口无数据可写");
+            return 0;
+        }
+        LOG_INFO("成功发送 %d 字节数据", num);
+        last_error_.clear();
+        return num;
+    }
+    catch (const std::exception& e)
+    {
+        LOG_ERROR("串口写入异常：%s", e.what());
+        set_error(e.what());
+        return -1;
+    }
+    catch (...)
+    {
+        LOG_ERROR("串口写入未知异常");
+        set_error("未知异常");
+        return -1;
+    }
+}
+
+int SerialPort::write(const std::vector<uint8_t>& data)
+{
+    if (data.empty())
+    {
+        return 0;
+    }
+    return write(reinterpret_cast<const char*>(data.data()), data.size());
+}
+
+int SerialPort::write(const std::string& data)
+{
+    if (data.empty())
+    {
+        return 0;
+    }
+    return write(reinterpret_cast<const char*>(data.data()), data.size());
+}
+
+bool SerialPort::is_open() const
+{
+    return (port_ != nullptr && port_->is_open());
+}
+
+void SerialPort::close()
+{
+    if (port_)
+    {
+        try
+        {
+            if (port_->is_open())
+            {
+                port_->close();
+            }
+            delete port_;
+        }
+        catch (...)
+        {
+            LOG_ERROR("关闭串口时发生异常");
+        }
+        port_ = nullptr;
+        last_error_.clear();
+    }
+}
+
+std::string SerialPort::get_last_error() const
+{
+    return last_error_;
+}
+
+void SerialPort::set_error(const std::string& error)
+{
+    last_error_ = error;
+    LOG_ERROR("串口操作错误：%s", error.c_str());
+}