0414-2

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);
+
+        // 只在更新共享变量时加锁
+        {
+            std::lock_guard<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> lock(grid_mutex_);
+            is_grid_valid = grid_data_valid_;
+        }
+
+        if (is_grid_valid && grid_age <= 3.0)
         {
             // 数据在3秒内，继续使用现有数据进行障碍物检测
         }