diff --git a/src/autonomy/fu/src/fu_node.cpp b/src/autonomy/fu/src/fu_node.cpp index 550e134..524e016 100644 --- a/src/autonomy/fu/src/fu_node.cpp +++ b/src/autonomy/fu/src/fu_node.cpp @@ -927,10 +927,16 @@ class fu_node : public rclcpp::Node { if (!enable_obstacle_stop_) return false; - // 检查栅格数据新鲜度(超过500ms认为数据过期) + // 检查栅格数据新鲜度(超过3秒认为数据过期) auto now = node_clock_->now(); auto grid_age = (now - last_grid_time_).seconds(); - if (grid_age > 0.5) + + // 如果有有效栅格数据,即使稍微过期也继续使用(只要数据不是太旧) + if (grid_data_valid_ && grid_age <= 3.0) + { + // 数据在3秒内,继续使用现有数据进行障碍物检测 + } + else if (grid_age > 3.0) { LOG_WARN("[紧急停车] 栅格数据过期(%.3fs),执行安全停车!", grid_age); state_machine_->publish_speed = 0;