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

提高代码健壮性

Browse Source
This commit is contained in:
lyq 2026-02-02 13:59:53 +08:00
parent 26d7ce5bf6
commit 93afcdebbb
2 changed files with 302 additions and 118 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

168
src/perception/rtk/src/rtk_node.cpp
View File

@ -47,100 +47,216 @@ class rtk_node : public rclcpp::Node
line_string = p_gpybm;
// 解析过程添加完整的错误检查
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;
}
}
catch (const exception& e)
{
LOG_ERROR("解析GPYBM数据时发生异常：%s", e.what());
return;
}
catch (...)
{
LOG_ERROR("解析GPYBM数据时发生未知异常");
return;
}
}
void timer_callback()
{
try
{
// 读取串口传来的定位信息
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 (num < 0)
{
LOG_WARN("串口读取失败：返回值为负");
return;
}
if (c_queue.size() >= 400)
{
std::queue<char> empty;
swap(empty, c_queue);
LOG_WARN("队列已满，已清空队列");
}
for (int i = 0; i < num; i++)
{
if (serial_buf[i] == '*')
{
// c_queue.push(serial_buf[i]);
memset(gps_buf, 0, sizeof(gps_buf));
int j = 0;
while (!c_queue.empty())
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<char> empty;
swap(empty, c_queue);
continue;
}
// 解析定位信息
// 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);
// 创建消息
@ -154,8 +270,9 @@ class rtk_node : public rclcpp::Node
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);
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);
}
@ -165,6 +282,15 @@ class rtk_node : public rclcpp::Node
}
}
}
catch (const exception& e)
{
LOG_ERROR("定时器回调函数异常：%s", e.what());
}
catch (...)
{
LOG_ERROR("定时器回调函数未知异常");
}
}
// 声名定时器指针
rclcpp::TimerBase::SharedPtr timer_;
@ -188,7 +314,7 @@ class rtk_node : public rclcpp::Node
std::queue<char> 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;
};

68
src/perception/rtk/src/serial_read.cpp
View File

@ -2,20 +2,32 @@
#include "logger/logger.h"
Boost_serial::Boost_serial() { ; }
Boost_serial::Boost_serial() : sp(nullptr) { ; }
Boost_serial::~Boost_serial()
{
if (sp)
{
try
{
if (sp->is_open())
{
sp->close();
}
delete sp;
}
catch (...)
{
LOG_ERROR("关闭串口时发生异常");
}
}
}
void Boost_serial::init(const string port_name)
{
try
{
sp = new serial_port(service);
sp->open(port_name);
if (sp->is_open())
@ -30,15 +42,61 @@ void Boost_serial::init(const string port_name)
else
{
LOG_ERROR("打开串口失败！");
delete sp;
sp = nullptr;
}
}
catch (const exception& e)
{
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;
}
}