From 759985784d912402c6b4a52b7ba4acc86360fdb7 Mon Sep 17 00:00:00 2001 From: Alvin-lyq Date: Thu, 16 Apr 2026 10:24:45 +0800 Subject: [PATCH] lock fix bug --- src/control/lock/config/lock.yaml | 2 +- src/control/lock/src/lock_node.cpp | 90 ++++++++++++++++++++---------- 2 files changed, 63 insertions(+), 29 deletions(-) diff --git a/src/control/lock/config/lock.yaml b/src/control/lock/config/lock.yaml index e0cbacd..36ebf00 100644 --- a/src/control/lock/config/lock.yaml +++ b/src/control/lock/config/lock.yaml @@ -1,4 +1,4 @@ -lock: +lock_node: ros__parameters: # 串口设备名称 serial_port: "/dev/ttyTHS0" diff --git a/src/control/lock/src/lock_node.cpp b/src/control/lock/src/lock_node.cpp index d4d921d..9385757 100644 --- a/src/control/lock/src/lock_node.cpp +++ b/src/control/lock/src/lock_node.cpp @@ -1,10 +1,11 @@ #include #include +#include -#include -#include -#include #include +#include +#include +#include #include "logger/logger.h" #include "rclcpp/rclcpp.hpp" @@ -14,7 +15,7 @@ class LockNode : public rclcpp::Node { -public: + public: // 构造函数 LockNode(std::string name) : Node(name) { @@ -23,7 +24,7 @@ public: // 声明并获取参数 this->declare_parameter("serial_port", "/dev/ttyUSB0"); this->declare_parameter("baud_rate", 9600); - this->declare_parameter("device_address", 1); // 锁控板地址 + this->declare_parameter("device_address", 1); // 锁控板地址 this->declare_parameter("channel_list", std::vector{1}); // 通道列表 std::string serial_port; @@ -39,6 +40,22 @@ public: device_address_ = static_cast(device_address); packet_seq_ = 0; + RCLCPP_INFO(this->get_logger(), "======================================"); + RCLCPP_INFO(this->get_logger(), "串口设备: %s", serial_port.c_str()); + RCLCPP_INFO(this->get_logger(), "波特率: %d", baud_rate); + RCLCPP_INFO(this->get_logger(), "设备地址: %d", device_address); + + // 打印通道列表（循环输出） + std::string channel_str = "["; + for (size_t i = 0; i < channel_list.size(); ++i) + { + if (i > 0) channel_str += ", "; + channel_str += std::to_string(channel_list[i]); + } + channel_str += "]"; + RCLCPP_INFO(this->get_logger(), "通道列表: %s", channel_str.c_str()); + RCLCPP_INFO(this->get_logger(), "======================================"); + // 初始化通道状态 for (int64_t ch : channel_list) { @@ -57,32 +74,36 @@ public: // 初始化串口 serial_port_ = std::make_unique(); - serial_port_->init(serial_port.c_str(), baud_rate); + bool init_success = serial_port_->init(serial_port.c_str(), baud_rate); + if (!init_success) + { + LOG_ERROR("串口初始化失败，程序退出"); + throw std::runtime_error("串口初始化失败"); + } // 创建订阅者 - 接收控制指令 control_subscriber_ = this->create_subscription( - "lock_control", 10, - std::bind(&LockNode::control_callback, this, std::placeholders::_1)); + "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)); + timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&LockNode::timer_callback, this)); } -private: + private: // 协议相关常数 - enum ProtocolConstants { + enum ProtocolConstants + { FRAME_HEADER1 = 0xA5, FRAME_HEADER2 = 0xA5, HOST_ADDRESS = 0x00 }; // 命令类型 - enum CommandTypes { + enum CommandTypes + { CMD_RESET = 0x01, CMD_QUERY_ADDR = 0x02, CMD_QUERY_CHANNEL_COUNT = 0x03, @@ -95,7 +116,8 @@ private: }; // 错误码 - enum ResultCodes { + enum ResultCodes + { RESULT_SUCCESS = 0x00, RESULT_DATA_LEN_ERROR = 0x01, RESULT_PARAM_ERROR = 0x02, @@ -268,9 +290,8 @@ private: // 更新对应通道的状态 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; + 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 @@ -301,9 +322,8 @@ private: // 更新对应通道的状态 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; + 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 @@ -480,7 +500,7 @@ private: // 提取帧 std::vector frame(rx_buffer_.begin() + header_pos, - rx_buffer_.begin() + header_pos + frame_len); + rx_buffer_.begin() + header_pos + frame_len); // 解析帧 parse_frame(frame); @@ -552,15 +572,29 @@ int main(int argc, char** argv) // 初始化日志系统 logger::Logger::Init("lock", "./nodes_log"); - rclcpp::init(argc, argv); + try + { + rclcpp::init(argc, argv); - // 创建节点 - auto node = std::make_shared("lock_node"); + // 创建节点 + auto node = std::make_shared("lock_node"); - // 运行节点 - rclcpp::spin(node); + // 运行节点 + rclcpp::spin(node); - rclcpp::shutdown(); + rclcpp::shutdown(); + } + catch (const std::exception& e) + { + LOG_ERROR("程序异常: %s", e.what()); + // 确保关闭前确保ros和日志系统 + if (rclcpp::ok()) + { + rclcpp::shutdown(); + } + logger::Logger::Shutdown(); + return 1; + } // 关闭日志系统 logger::Logger::Shutdown();