提高代码健壮性
This commit is contained in:
parent
26d7ce5bf6
commit
93afcdebbb
@ -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<char> 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<char> 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<char> 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<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;
|
||||
};
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user