From e62d791607147cb02c12846383e7ee1f1d81601c Mon Sep 17 00:00:00 2001 From: Alvin-lyq Date: Tue, 14 Apr 2026 10:14:12 +0800 Subject: [PATCH] 0414-2 --- src/autonomy/fu/src/fu_node.cpp | 35 ++++++++++++++++++++++++++------- 1 file changed, 28 insertions(+), 7 deletions(-) diff --git a/src/autonomy/fu/src/fu_node.cpp b/src/autonomy/fu/src/fu_node.cpp index fb466f8..0c27966 100644 --- a/src/autonomy/fu/src/fu_node.cpp +++ b/src/autonomy/fu/src/fu_node.cpp @@ -317,6 +317,9 @@ class fu_node : public rclcpp::Node rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; + // ======== 线程安全锁 ======== + std::mutex grid_mutex_; // 保护栅格数据相关变量 + // ======== 规划层消息缓存 ======== double pl_x_ = 0.0, pl_y_ = 0.0; int pl_speed_ = 0; @@ -463,8 +466,6 @@ class fu_node : public rclcpp::Node // ======== 栅格回调 ======== void gridCallback(const std_msgs::msg::Int32MultiArray::SharedPtr msg) { - last_grid_time_ = node_clock_->now(); - if (msg->layout.dim.size() != 2) { LOG_ERROR("栅格地图消息维度无效！"); @@ -487,7 +488,13 @@ class fu_node : public rclcpp::Node findVehicleRectangle(grid); analyzeObstacles(grid); if (enable_visualize_grid_) visualizeGridInTerminal(grid); - cacheGridData(grid); + + // 只在更新共享变量时加锁 + { + std::lock_guard lock(grid_mutex_); + last_grid_time_ = node_clock_->now(); + cacheGridData(grid); + } // 立即执行紧急停车检查（高优先级） updateObstacleAnalysis(); @@ -947,11 +954,15 @@ class fu_node : public rclcpp::Node } // ======== 计算行驶状态 ======== - int16_t calculateDrivingStatus() const + int16_t calculateDrivingStatus() { // 检查栅格数据新鲜度（超过3秒认为数据过期） auto now = node_clock_->now(); - auto grid_age = (now - last_grid_time_).seconds(); + double grid_age = 0.0; + { + std::lock_guard lock(grid_mutex_); + grid_age = (now - last_grid_time_).seconds(); + } // 检查RTK信号 auto rtk_now = system_clock::now(); @@ -1001,10 +1012,20 @@ class fu_node : public rclcpp::Node // 检查栅格数据新鲜度（超过3秒认为数据过期） auto now = node_clock_->now(); - auto grid_age = (now - last_grid_time_).seconds(); + double grid_age = 0.0; + { + std::lock_guard lock(grid_mutex_); + grid_age = (now - last_grid_time_).seconds(); + } // 如果有有效栅格数据，即使稍微过期也继续使用（只要数据不是太旧） - if (grid_data_valid_ && grid_age <= 3.0) + bool is_grid_valid = false; + { + std::lock_guard lock(grid_mutex_); + is_grid_valid = grid_data_valid_; + } + + if (is_grid_valid && grid_age <= 3.0) { // 数据在3秒内，继续使用现有数据进行障碍物检测 }