diff --git a/src/autonomy/fu/src/fu_node.cpp b/src/autonomy/fu/src/fu_node.cpp index 9ed7cdb..8ecfb2c 100644 --- a/src/autonomy/fu/src/fu_node.cpp +++ b/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 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) {