From 93afcdebbb1f72c8a95047d0b7ce573dfbb79081 Mon Sep 17 00:00:00 2001 From: lyq Date: Mon, 2 Feb 2026 13:59:53 +0800 Subject: [PATCH] =?UTF-8?q?=E6=8F=90=E9=AB=98=E4=BB=A3=E7=A0=81=E5=81=A5?= =?UTF-8?q?=E5=A3=AE=E6=80=A7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/perception/rtk/src/rtk_node.cpp | 324 +++++++++++++++++-------- src/perception/rtk/src/serial_read.cpp | 96 ++++++-- 2 files changed, 302 insertions(+), 118 deletions(-) diff --git a/src/perception/rtk/src/rtk_node.cpp b/src/perception/rtk/src/rtk_node.cpp index 185a4dc..02f1b78 100644 --- a/src/perception/rtk/src/rtk_node.cpp +++ b/src/perception/rtk/src/rtk_node.cpp @@ -47,123 +47,249 @@ class rtk_node : public rclcpp::Node line_string = p_gpybm; - for (int i = 0; i < 3; i++) + // 解析过程添加完整的错误检查 + try { + // 跳过前3个字段 + for (int i = 0; i < 3; i++) + { + position = line_string.find(","); + if (position == string::npos) + { + LOG_WARN("GPYBM数据格式错误：字段数量不足"); + return; + } + line_string.erase(0, position + 1); + } + + // 解析纬度 position = line_string.find(","); + if (position == string::npos) + { + LOG_WARN("GPYBM数据格式错误：缺少纬度字段"); + return; + } + lat_s = line_string.substr(0, position); line_string.erase(0, position + 1); + + // 解析经度 + position = line_string.find(","); + if (position == string::npos) + { + LOG_WARN("GPYBM数据格式错误：缺少经度字段"); + return; + } + lon_s = line_string.substr(0, position); + line_string.erase(0, position + 1); + + // 跳过1个字段 + position = line_string.find(","); + if (position == string::npos) + { + LOG_WARN("GPYBM数据格式错误：字段数量不足"); + return; + } + line_string.erase(0, position + 1); + + // 解析航向 + position = line_string.find(","); + if (position == string::npos) + { + LOG_WARN("GPYBM数据格式错误：缺少航向字段"); + return; + } + head_s = line_string.substr(0, position); + line_string.erase(0, position + 1); + + // 跳过4个字段 + for (int i = 0; i < 4; i++) + { + position = line_string.find(","); + if (position == string::npos) + { + LOG_WARN("GPYBM数据格式错误：字段数量不足"); + return; + } + line_string.erase(0, position + 1); + } + + // 解析速度 + position = line_string.find(","); + if (position == string::npos) + { + LOG_WARN("GPYBM数据格式错误：缺少速度字段"); + return; + } + sp_s = line_string.substr(0, position); + line_string.erase(0, position + 1); + + // 跳过4个字段 + for (int i = 0; i < 4; i++) + { + position = line_string.find(","); + if (position == string::npos) + { + LOG_WARN("GPYBM数据格式错误：字段数量不足"); + return; + } + line_string.erase(0, position + 1); + } + + // 解析定位质量 + position = line_string.find(","); + if (position == string::npos) + { + LOG_WARN("GPYBM数据格式错误：缺少定位质量字段"); + return; + } + p_q_s = line_string.substr(0, position); + line_string.erase(0, position + 1); + + // 解析定向质量 + position = line_string.find(","); + if (position == string::npos) + { + LOG_WARN("GPYBM数据格式错误：缺少定向质量字段"); + return; + } + h_q_s = line_string.substr(0, position); + line_string.erase(0, position + 1); + + // 验证字段内容是否有效 + if (lat_s.empty() || lon_s.empty() || head_s.empty() || sp_s.empty() || p_q_s.empty() || h_q_s.empty()) + { + LOG_WARN("GPYBM数据格式错误：字段内容为空"); + return; + } + + // 检查字段是否包含合法的数字字符 + auto is_valid_number = [](const string& s) -> bool + { + if (s.empty()) return false; + for (char c : s) + { + if (!isdigit(c) && c != '.' && c != '-' && c != '+') + { + return false; + } + } + return true; + }; + + if (!is_valid_number(lat_s) || !is_valid_number(lon_s) || !is_valid_number(head_s) || + !is_valid_number(sp_s) || !is_valid_number(p_q_s) || !is_valid_number(h_q_s)) + { + LOG_WARN("GPYBM数据格式错误：字段包含非法字符"); + return; + } + + // 转换为数值 + lat = atof(lat_s.c_str()); + lon = atof(lon_s.c_str()); + head = atof(head_s.c_str()); + speed = atof(sp_s.c_str()); + p_quality = atof(p_q_s.c_str()); + h_quality = atof(h_q_s.c_str()); + + // 验证地理坐标范围（简单验证） + if (lat < -90.0 || lat > 90.0 || lon < -180.0 || lon > 180.0) + { + LOG_WARN("GPYBM数据错误：坐标值超出范围"); + lat = 0.0; + lon = 0.0; + return; + } } - - position = line_string.find(","); - lat_s = line_string.substr(0, position); - line_string.erase(0, position + 1); - - position = line_string.find(","); - lon_s = line_string.substr(0, position); - line_string.erase(0, position + 1); - - position = line_string.find(","); - line_string.erase(0, position + 1); - - position = line_string.find(","); - head_s = line_string.substr(0, position); - line_string.erase(0, position + 1); - - for (int i = 0; i < 4; i++) + catch (const exception& e) { - position = line_string.find(","); - line_string.erase(0, position + 1); + LOG_ERROR("解析GPYBM数据时发生异常：%s", e.what()); + return; } - - position = line_string.find(","); - sp_s = line_string.substr(0, position); - line_string.erase(0, position + 1); - - for (int i = 0; i < 4; i++) + catch (...) { - position = line_string.find(","); - line_string.erase(0, position + 1); + LOG_ERROR("解析GPYBM数据时发生未知异常"); + return; } - - position = line_string.find(","); - p_q_s = line_string.substr(0, position); - line_string.erase(0, position + 1); - - position = line_string.find(","); - h_q_s = line_string.substr(0, position); - line_string.erase(0, position + 1); - - lat = atof(lat_s.c_str()); - lon = atof(lon_s.c_str()); - head = atof(head_s.c_str()); - speed = atof(sp_s.c_str()); - p_quality = atof(p_q_s.c_str()); - h_quality = atof(h_q_s.c_str()); } void timer_callback() { - // 读取串口传来的定位信息 - memset(serial_buf, 0, sizeof(serial_buf)); - int num = boost_serial->serial_read(serial_buf, 200); - // LOG_DEBUG("num:%d \n",num); - // LOG_DEBUG("serial_buf: "); - // for (int i = 0; i < 300; i++) - // { - // LOG_DEBUG("%c", serial_buf[i]); - // } - // LOG_DEBUG("\n"); - - if (c_queue.size() >= 400) + try { - std::queue empty; - swap(empty, c_queue); - } + // 读取串口传来的定位信息 + memset(serial_buf, 0, sizeof(serial_buf)); + int num = boost_serial->serial_read(serial_buf, 200); - for (int i = 0; i < num; i++) - { - if (serial_buf[i] == '*') + if (num < 0) { - // c_queue.push(serial_buf[i]); - memset(gps_buf, 0, sizeof(gps_buf)); - int j = 0; - while (!c_queue.empty()) + LOG_WARN("串口读取失败：返回值为负"); + return; + } + + if (c_queue.size() >= 400) + { + std::queue empty; + swap(empty, c_queue); + LOG_WARN("队列已满，已清空队列"); + } + + for (int i = 0; i < num; i++) + { + if (serial_buf[i] == '*') { - gps_buf[j] = c_queue.front(); - c_queue.pop(); - j++; + memset(gps_buf, 0, sizeof(gps_buf)); + size_t j = 0; // 改为 size_t 类型，避免与 sizeof 结果比较的警告 + while (!c_queue.empty() && j < sizeof(gps_buf) - 1) + { + gps_buf[j] = c_queue.front(); + c_queue.pop(); + j++; + } + gps_buf[j] = '\0'; // 确保字符串结束符 + + // 检查缓冲区是否足够大 + if (j >= sizeof(gps_buf) - 1) + { + LOG_WARN("GPS数据过长，可能导致缓冲区溢出"); + std::queue empty; + swap(empty, c_queue); + continue; + } + + // 解析定位信息 + GPYBM_to_gps(gps_buf); + + // 创建消息 + auto message = sweeper_interfaces::msg::Rtk(); + + message.lat = lat; + message.lon = lon; + message.head = head; + message.speed = speed; + message.p_quality = p_quality; + message.h_quality = h_quality; + + // 日志打印 + LOG_INFO("lat:'%.9lf',lon:'%.9lf',head:'%lf',speed:'%lf',p_quality:'%d',h_quality:'%d'", + message.lat, message.lon, message.head, message.speed, message.p_quality, + message.h_quality); + // 发布消息 + command_publisher_->publish(message); + } + else + { + c_queue.push(serial_buf[i]); } - // 解析定位信息 - - // LOG_DEBUG("gps_buf: "); - // for (int i = 0; i < 300; i++) - // { - // LOG_DEBUG("%c", gps_buf[i]); - // } - // LOG_DEBUG("\n"); - - GPYBM_to_gps(gps_buf); - - // 创建消息 - auto message = sweeper_interfaces::msg::Rtk(); - - message.lat = lat; - message.lon = lon; - message.head = head; - message.speed = speed; - message.p_quality = p_quality; - message.h_quality = h_quality; - - // 日志打印 - LOG_INFO("lat:'%.9lf',lon:'%.9lf',head:'%lf',speed:'%lf',p_quality:'%d',h_quality:'%d'", message.lat, - message.lon, message.head, message.speed, message.p_quality, message.h_quality); - // 发布消息 - command_publisher_->publish(message); - } - else - { - c_queue.push(serial_buf[i]); } } + catch (const exception& e) + { + LOG_ERROR("定时器回调函数异常：%s", e.what()); + } + catch (...) + { + LOG_ERROR("定时器回调函数未知异常"); + } } // 声名定时器指针 @@ -188,7 +314,7 @@ class rtk_node : public rclcpp::Node std::queue c_queue; // 解析定位信息用到的中间变量 string lat_s, lon_s, head_s, sp_s, p_q_s, h_q_s; - int position; + size_t position; // 改为 size_t 类型，匹配 string::find() 的返回类型 string line_string; }; diff --git a/src/perception/rtk/src/serial_read.cpp b/src/perception/rtk/src/serial_read.cpp index 3a90853..0d0b303 100644 --- a/src/perception/rtk/src/serial_read.cpp +++ b/src/perception/rtk/src/serial_read.cpp @@ -2,43 +2,101 @@ #include "logger/logger.h" -Boost_serial::Boost_serial() { ; } +Boost_serial::Boost_serial() : sp(nullptr) { ; } Boost_serial::~Boost_serial() { if (sp) { - delete sp; + try + { + if (sp->is_open()) + { + sp->close(); + } + delete sp; + } + catch (...) + { + LOG_ERROR("关闭串口时发生异常"); + } } } void Boost_serial::init(const string port_name) { - sp = new serial_port(service); - - sp->open(port_name); - - if (sp->is_open()) + try { - sp->set_option(serial_port::baud_rate(115200)); - sp->set_option(serial_port::flow_control(serial_port::flow_control::none)); - sp->set_option(serial_port::parity(serial_port::parity::none)); - sp->set_option(serial_port::stop_bits(serial_port::stop_bits::one)); - sp->set_option(serial_port::character_size(8)); - LOG_INFO("打开串口成功！"); + sp = new serial_port(service); + sp->open(port_name); + + if (sp->is_open()) + { + sp->set_option(serial_port::baud_rate(115200)); + sp->set_option(serial_port::flow_control(serial_port::flow_control::none)); + sp->set_option(serial_port::parity(serial_port::parity::none)); + sp->set_option(serial_port::stop_bits(serial_port::stop_bits::one)); + sp->set_option(serial_port::character_size(8)); + LOG_INFO("打开串口成功！"); + } + else + { + LOG_ERROR("打开串口失败！"); + delete sp; + sp = nullptr; + } } - else + catch (const exception& e) { - LOG_ERROR("打开串口失败！"); + LOG_ERROR("初始化串口时发生异常：%s", e.what()); + if (sp) + { + delete sp; + sp = nullptr; + } + } + catch (...) + { + LOG_ERROR("初始化串口时发生未知异常"); + if (sp) + { + delete sp; + sp = nullptr; + } } } int Boost_serial::serial_read(char buf[], int size) { - int num = read(*sp, buffer(buf, size)); - if (num <= 0) + if (sp == nullptr || !sp->is_open()) { - LOG_WARN("没有读到数据！"); + LOG_WARN("串口未初始化或未打开"); + return -1; + } + + try + { + int num = read(*sp, buffer(buf, size)); + if (num < 0) + { + LOG_WARN("串口读取错误"); + return -1; + } + else if (num == 0) + { + LOG_INFO("串口无数据可读"); + return 0; + } + return num; + } + catch (const exception& e) + { + LOG_ERROR("串口读取异常：%s", e.what()); + return -1; + } + catch (...) + { + LOG_ERROR("串口读取未知异常"); + return -1; } - return num; }