lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

lock fix bug

Browse Source
This commit is contained in:
Alvin-lyq 2026-04-16 10:24:45 +08:00
parent 195410b625
commit 759985784d
2 changed files with 63 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/control/lock/config/lock.yaml
View File

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

90
src/control/lock/src/lock_node.cpp
View File

@ -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,
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<sweeper_interfaces::msg::LockStatus>("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<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();