提高代码健壮性

src/perception/rtk/src/rtk_node.cpp

@@ -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:  ");
+            if (num < 0)
-        // for (int i = 0; i < 300; i++)
+            {
-        // {
+                LOG_WARN("串口读取失败：返回值为负");
-        //     LOG_DEBUG("%c", serial_buf[i]);
+                return;
-        // }
+            }
-        // LOG_DEBUG("\n");
 
             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;
+                    size_t j = 0;  // 改为 size_t 类型，避免与 sizeof 结果比较的警告
-                while (!c_queue.empty())
+                    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,
+                    LOG_INFO("lat:'%.9lf',lon:'%.9lf',head:'%lf',speed:'%lf',p_quality:'%d',h_quality:'%d'",
-                         message.lon, message.head, message.speed, message.p_quality, message.h_quality);
+                             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;
 };
 

src/perception/rtk/src/serial_read.cpp

@@ -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 (sp == nullptr || !sp->is_open())
-    if (num <= 0)
     {
-        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;
+    }
 }