diff --git a/doc/壹拓鑫锁板协议-锁控板协议_V2.5.pdf b/doc/壹拓鑫锁板协议-锁控板协议_V2.5.pdf new file mode 100644 index 0000000..e7f20a2 Binary files /dev/null and b/doc/壹拓鑫锁板协议-锁控板协议_V2.5.pdf differ diff --git a/doc/清扫车协议-260414.pdf b/doc/清扫车协议-260414.pdf new file mode 100644 index 0000000..301feb5 Binary files /dev/null and b/doc/清扫车协议-260414.pdf differ diff --git a/src/base/sweeper_interfaces/CMakeLists.txt b/src/base/sweeper_interfaces/CMakeLists.txt index c966805..a26e46d 100644 --- a/src/base/sweeper_interfaces/CMakeLists.txt +++ b/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/Pl.msg" - "msg/DetectLine.msg" - "msg/Route.msg" - "msg/Fu.msg" - "msg/Task.msg" + "msg/Rtk.msg" + "msg/Pl.msg" + "msg/DetectLine.msg" + "msg/Route.msg" + "msg/Fu.msg" + "msg/Task.msg" "msg/VehicleIdentity.msg" + "msg/LockControl.msg" + "msg/LockStatus.msg" DEPENDENCIES std_msgs ) diff --git a/src/base/sweeper_interfaces/msg/LockControl.msg b/src/base/sweeper_interfaces/msg/LockControl.msg new file mode 100644 index 0000000..ee3b2dc --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/base/sweeper_interfaces/msg/LockStatus.msg b/src/base/sweeper_interfaces/msg/LockStatus.msg new file mode 100644 index 0000000..e55fed7 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/control/lock/CMakeLists.txt b/src/control/lock/CMakeLists.txt new file mode 100644 index 0000000..7cfe056 --- /dev/null +++ b/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() diff --git a/src/control/lock/config/lock.yaml b/src/control/lock/config/lock.yaml new file mode 100644 index 0000000..e0cbacd --- /dev/null +++ b/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] diff --git a/src/control/lock/include/lock/serial_port.hpp b/src/control/lock/include/lock/serial_port.hpp new file mode 100644 index 0000000..62e93ff --- /dev/null +++ b/src/control/lock/include/lock/serial_port.hpp @@ -0,0 +1,131 @@ +#ifndef LOCK__SERIAL_PORT_HPP_ +#define LOCK__SERIAL_PORT_HPP_ + +#include +#include +#include +#include + +/** + * @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 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& 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_ diff --git a/src/control/lock/launch/lock.launch.py b/src/control/lock/launch/lock.launch.py new file mode 100644 index 0000000..27c53c5 --- /dev/null +++ b/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", + ), + ] + ) diff --git a/src/control/lock/package.xml b/src/control/lock/package.xml new file mode 100644 index 0000000..63297eb --- /dev/null +++ b/src/control/lock/package.xml @@ -0,0 +1,23 @@ + + + + lock + 0.0.0 + Lock control package for sweeper + zxwl + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + sweeper_interfaces + logger + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/src/control/lock/src/lock_node.cpp b/src/control/lock/src/lock_node.cpp new file mode 100644 index 0000000..d4d921d --- /dev/null +++ b/src/control/lock/src/lock_node.cpp @@ -0,0 +1,569 @@ +#include +#include + +#include +#include +#include +#include + +#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{1}); // 通道列表 + + std::string serial_port; + int baud_rate; + int device_address; + std::vector 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(device_address); + packet_seq_ = 0; + + // 初始化通道状态 + for (int64_t ch : channel_list) + { + uint8_t channel = static_cast(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(); + serial_port_->init(serial_port.c_str(), baud_rate); + + // 创建订阅者 - 接收控制指令 + control_subscriber_ = this->create_subscription( + "lock_control", 10, + std::bind(&LockNode::control_callback, this, std::placeholders::_1)); + + // 创建发布者 - 发布状态反馈 + status_publisher_ = this->create_publisher("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 build_frame(uint8_t cmd, const uint8_t* data, int data_len) + { + std::vector 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(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& 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(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(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 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 data; + data.push_back(channel); + + std::vector frame = build_frame(CMD_OPEN_LOCK, data.data(), static_cast(data.size())); + send_frame(frame); + } + + /** + * @brief 发送读取状态命令到指定通道 + * @param channel 通道号 + */ + void send_read_status_command(uint8_t channel) + { + std::vector data; + data.push_back(channel); + + std::vector frame = build_frame(CMD_READ_STATUS, data.data(), static_cast(data.size())); + send_frame(frame); + } + + /** + * @brief 发送协议帧 + * @param frame 协议帧 + */ + void send_frame(const std::vector& 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(frame[i])); + } + LOG_INFO("发送协议帧: %.*s", len - 1, log_buf); + + // 发送数据 + int sent = serial_port_->write(reinterpret_cast(frame.data()), static_cast(frame.size())); + if (sent == static_cast(frame.size())) + { + LOG_INFO("协议帧发送成功"); + } + else + { + LOG_ERROR("协议帧发送失败，预期发送%d字节，实际发送%d字节", static_cast(frame.size()), sent); + } + } + + /** + * @brief 定时器回调函数 - 用于读取串口状态 + */ + void timer_callback() + { + std::lock_guard 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(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 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::SharedPtr control_subscriber_; + rclcpp::Publisher::SharedPtr status_publisher_; + rclcpp::TimerBase::SharedPtr timer_; + + // 串口操作对象 + std::unique_ptr serial_port_; + + // 状态变量 + std::map channel_status_; // 通道号 -> 状态 + uint32_t global_error_code_; + + // 锁控制参数 + uint8_t device_address_; + std::vector channel_list_; // 要控制的通道列表 + uint8_t packet_seq_; + + // 接收缓冲区 + std::vector 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("lock_node"); + + // 运行节点 + rclcpp::spin(node); + + rclcpp::shutdown(); + + // 关闭日志系统 + logger::Logger::Shutdown(); + + return 0; +} diff --git a/src/control/lock/src/serial_port.cpp b/src/control/lock/src/serial_port.cpp new file mode 100644 index 0000000..160fe9e --- /dev/null +++ b/src/control/lock/src/serial_port.cpp @@ -0,0 +1,219 @@ +#include "serial_port.hpp" + +#include "logger/logger.h" +#include + +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 SerialPort::read(int size) +{ + std::vector data(size); + int num = read(reinterpret_cast(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& data) +{ + if (data.empty()) + { + return 0; + } + return write(reinterpret_cast(data.data()), data.size()); +} + +int SerialPort::write(const std::string& data) +{ + if (data.empty()) + { + return 0; + } + return write(reinterpret_cast(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()); +}