lock fix bug

src/control/lock/config/lock.yaml

@@ -1,4 +1,4 @@
-lock:
+lock_node:
   ros__parameters:
     # 串口设备名称
     serial_port: "/dev/ttyTHS0"

src/control/lock/src/lock_node.cpp

@@ -1,10 +1,11 @@
 #include <stdlib.h>
 #include <string.h>
+#include <stdexcept>
 
-#include <queue>
-#include <mutex>
-#include <vector>
 #include <map>
+#include <mutex>
+#include <queue>
+#include <vector>
 
 #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<int64_t>{1});  // 通道列表
 
         std::string serial_port;
@@ -39,6 +40,22 @@ public:
         device_address_ = static_cast<uint8_t>(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<SerialPort>();
-        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<sweeper_interfaces::msg::LockControl>(
-            "lock_control", 10,
+            "lock_control", 10, std::bind(&LockNode::control_callback, this, std::placeholders::_1));
-            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(
+        timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&LockNode::timer_callback, this));
-            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) ?
+            channel_status_[channel] = (lock_status == 1) ? sweeper_interfaces::msg::LockStatus::STATUS_OPENED
-                sweeper_interfaces::msg::LockStatus::STATUS_OPENED :
+                                                          : sweeper_interfaces::msg::LockStatus::STATUS_CLOSED;
-                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) ?
+            channel_status_[channel] = (lock_status == 1) ? sweeper_interfaces::msg::LockStatus::STATUS_OPENED
-                sweeper_interfaces::msg::LockStatus::STATUS_OPENED :
+                                                          : sweeper_interfaces::msg::LockStatus::STATUS_CLOSED;
-                sweeper_interfaces::msg::LockStatus::STATUS_CLOSED;
             LOG_INFO("通道%d锁状态: %s", channel, (lock_status == 1) ? "打开" : "关闭");
         }
         else
@@ -480,7 +500,7 @@ private:
 
                 // 提取帧
                 std::vector<uint8_t> 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<LockNode>("lock_node");
+        auto node = std::make_shared<LockNode>("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();