0414-5

src/autonomy/fu/src/fu_node.cpp

@@ -974,7 +974,6 @@ class fu_node : public rclcpp::Node
         // 状态3: 雷达数据超时，停车
         if (grid_age > 3.0)
         {
-            LOG_WARN("[栅格超时检测] 栅格数据已过期(%.3fs)！", grid_age);
             return 3;
         }
 
@@ -1053,6 +1052,21 @@ class fu_node : public rclcpp::Node
         // 更新障碍物分析
         updateObstacleAnalysis();
 
+        // 检查栅格数据超时（不依赖任务是否开始）
+        auto now = node_clock_->now();
+        double grid_age = 0.0;
+        bool is_grid_valid = false;
+        {
+            std::lock_guard<std::mutex> lock(grid_mutex_);
+            grid_age = (now - last_grid_time_).seconds();
+            is_grid_valid = grid_data_valid_;
+        }
+
+        if (grid_age > 3.0)
+        {
+            LOG_WARN("[栅格超时检测] 栅格数据已过期(%.3fs)！即使任务未开始也会提示。", grid_age);
+        }
+
         // 开始清扫任务
         if (is_start)
         {