lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

0414-2

Browse Source
This commit is contained in:
Alvin-lyq 2026-04-14 10:14:12 +08:00
parent fbde7a9430
commit e62d791607
1 changed files with 28 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
src/autonomy/fu/src/fu_node.cpp
View File

@ -317,6 +317,9 @@ class fu_node : public rclcpp::Node
rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;
// ======== 线程安全锁 ========
std::mutex grid_mutex_; // 保护栅格数据相关变量
// ======== 规划层消息缓存 ======== // ======== 规划层消息缓存 ========
double pl_x_ = 0.0, pl_y_ = 0.0; double pl_x_ = 0.0, pl_y_ = 0.0;
int pl_speed_ = 0; int pl_speed_ = 0;
@ -463,8 +466,6 @@ class fu_node : public rclcpp::Node
// ======== 栅格回调 ======== // ======== 栅格回调 ========
void gridCallback(const std_msgs::msg::Int32MultiArray::SharedPtr msg) void gridCallback(const std_msgs::msg::Int32MultiArray::SharedPtr msg)
{ {
last_grid_time_ = node_clock_->now();
if (msg->layout.dim.size() != 2) if (msg->layout.dim.size() != 2)
{ {
LOG_ERROR("栅格地图消息维度无效！"); LOG_ERROR("栅格地图消息维度无效！");
@ -487,7 +488,13 @@ class fu_node : public rclcpp::Node
findVehicleRectangle(grid); findVehicleRectangle(grid);
analyzeObstacles(grid); analyzeObstacles(grid);
if (enable_visualize_grid_) visualizeGridInTerminal(grid); if (enable_visualize_grid_) visualizeGridInTerminal(grid);
cacheGridData(grid);
// 只在更新共享变量时加锁
{
std::lock_guard<std::mutex> lock(grid_mutex_);
last_grid_time_ = node_clock_->now();
cacheGridData(grid);
}
// 立即执行紧急停车检查（高优先级） // 立即执行紧急停车检查（高优先级）
updateObstacleAnalysis(); updateObstacleAnalysis();
@ -947,11 +954,15 @@ class fu_node : public rclcpp::Node
} }
// ======== 计算行驶状态 ======== // ======== 计算行驶状态 ========
int16_t calculateDrivingStatus() const int16_t calculateDrivingStatus()
{ {
// 检查栅格数据新鲜度（超过3秒认为数据过期） // 检查栅格数据新鲜度（超过3秒认为数据过期）
auto now = node_clock_->now(); 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信号 // 检查RTK信号
auto rtk_now = system_clock::now(); auto rtk_now = system_clock::now();
@ -1001,10 +1012,20 @@ class fu_node : public rclcpp::Node
// 检查栅格数据新鲜度（超过3秒认为数据过期） // 检查栅格数据新鲜度（超过3秒认为数据过期）
auto now = node_clock_->now(); 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秒内，继续使用现有数据进行障碍物检测 // 数据在3秒内，继续使用现有数据进行障碍物检测
} }