src/communication/task_manager/src/task_manager_main.cpp

@@ -359,35 +359,13 @@ int handleMqttMessage(char* topicName, int topicLen, MQTTClient_message* message
 
 void try_subscribe_task_topic()
 {
-    if (!identity_ready.load()) {
+    if (!identity_ready.load()) return;
-        LOG_DEBUG("[TASK] Identity not ready, skipping subscribe");
+    if (mqtt_topic_sub_task.empty()) return;
-        return;
-    }
-    if (mqtt_topic_sub_task.empty()) {
-        LOG_DEBUG("[TASK] MQTT topic not configured, skipping subscribe");
-        return;
-    }
 
-    if (!mqtt_manager.isConnected()) {
+    if (subscribed.exchange(true)) return;  // 已经订阅过了
-        LOG_DEBUG("[TASK] MQTT not connected, skipping subscribe");
-        subscribed.store(false);  // 确保下次可以重试
-        return;
-    }
-
-    if (subscribed.load()) {
-        LOG_DEBUG("[TASK] Already subscribed, skipping");
-        return;
-    }
 
     LOG_INFO("[TASK] subscribe MQTT topic: %s", mqtt_topic_sub_task.c_str());
-    bool success = mqtt_manager.subscribe(mqtt_topic_sub_task, 0);
+    mqtt_manager.subscribe(mqtt_topic_sub_task, 0);
-    if (success) {
-        subscribed.store(true);
-        LOG_INFO("[TASK] Subscribe successful");
-    } else {
-        LOG_ERROR("[TASK] Subscribe failed, will retry later");
-        subscribed.store(false);  // 订阅失败，允许重试
-    }
 }
 
 // 周期性上报任务状态到MQTT（200ms间隔持续上报）
@@ -442,10 +420,7 @@ class TaskManagerNode : public rclcpp::Node
                 vid = msg->vid;
                 identity_ready.store(!vid.empty());
 
-                if (!identity_ready.load()) {
+                if (!identity_ready.load()) return;
-                    LOG_WARN("[TASK] Invalid VID received: %s", msg->vid.c_str());
-                    return;
-                }
 
                 // 拼 topic
                 mqtt_topic_sub_task = config.mqtt_topic_sub_task_tmpl;
@@ -460,8 +435,6 @@ class TaskManagerNode : public rclcpp::Node
                 LOG_INFO("[TASK] identity ready VID=%s => sub=%s pub=%s", vid.c_str(), mqtt_topic_sub_task.c_str(),
                          mqtt_topic_push_status.c_str());
 
-                // 重置订阅状态，确保可以重新尝试订阅
-                subscribed.store(false);
                 try_subscribe_task_topic();
             });
     }
@@ -548,7 +521,6 @@ int main(int argc, char** argv)
         {
             LOG_INFO("[TASK] MQTT reconnected: %s", cause);
             subscribed.store(false);
-            // 尝试重新订阅任务话题
             try_subscribe_task_topic();
         });
 
@@ -567,28 +539,9 @@ int main(int argc, char** argv)
     rclcpp::executors::SingleThreadedExecutor executor;
     executor.add_node(node);
 
-    // 用于定期检查订阅状态的定时器
-    auto last_check_time = std::chrono::steady_clock::now();
-    const std::chrono::seconds SUBSCRIBE_CHECK_INTERVAL = std::chrono::seconds(5);
-
     while (rclcpp::ok() && !signal_received)
     {
         executor.spin_some();
-
-        // 定期检查并尝试订阅任务话题
-        auto now = std::chrono::steady_clock::now();
-        if (now - last_check_time >= SUBSCRIBE_CHECK_INTERVAL)
-        {
-            last_check_time = now;
-
-            // 如果已经准备好身份但尚未成功订阅，则尝试订阅
-            if (identity_ready.load() && !subscribed.load())
-            {
-                LOG_INFO("[TASK] Periodic check: identity ready but not subscribed, attempting subscribe");
-                try_subscribe_task_topic();
-            }
-        }
-
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
     }
 

src/perception/rtk/src/rtk_node.cpp

@@ -47,216 +47,100 @@ 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);
-            if (num < 0)
+        // LOG_DEBUG("serial_buf:  ");
-            {
+        // for (int i = 0; i < 300; i++)
-                LOG_WARN("串口读取失败：返回值为负");
+        // {
-                return;
+        //     LOG_DEBUG("%c", serial_buf[i]);
-            }
+        // }
+        // 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));
-                    size_t j = 0;  // 改为 size_t 类型，避免与 sizeof 结果比较的警告
+                int j = 0;
-                    while (!c_queue.empty() && j < sizeof(gps_buf) - 1)
+                while (!c_queue.empty())
                 {
                     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);
 
                 // 创建消息
@@ -270,9 +154,8 @@ 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'",
+                LOG_INFO("lat:'%.9lf',lon:'%.9lf',head:'%lf',speed:'%lf',p_quality:'%d',h_quality:'%d'", message.lat,
-                             message.lat, message.lon, message.head, message.speed, message.p_quality,
+                         message.lon, message.head, message.speed, message.p_quality, message.h_quality);
-                             message.h_quality);
                 // 发布消息
                 command_publisher_->publish(message);
             }
@@ -282,15 +165,6 @@ class rtk_node : public rclcpp::Node
             }
         }
     }
-        catch (const exception& e)
-        {
-            LOG_ERROR("定时器回调函数异常：%s", e.what());
-        }
-        catch (...)
-        {
-            LOG_ERROR("定时器回调函数未知异常");
-        }
-    }
 
     // 声名定时器指针
     rclcpp::TimerBase::SharedPtr timer_;
@@ -314,7 +188,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;
-    size_t position;  // 改为 size_t 类型，匹配 string::find() 的返回类型
+    int position;
     string line_string;
 };
 

src/perception/rtk/src/serial_read.cpp

@@ -2,32 +2,20 @@
 
 #include "logger/logger.h"
 
-Boost_serial::Boost_serial() : sp(nullptr) { ; }
+Boost_serial::Boost_serial() { ; }
 
 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())
@@ -42,61 +30,15 @@ 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)
-{
-    if (sp == nullptr || !sp->is_open())
-    {
-        LOG_WARN("串口未初始化或未打开");
-        return -1;
-    }
-
-    try
 {
     int num = read(*sp, buffer(buf, size));
-        if (num < 0)
+    if (num <= 0)
     {
-            LOG_WARN("串口读取错误");
+        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;
-    }
-}