From 96dde92d9e98f38839b290d076a4e8906b5a7685 Mon Sep 17 00:00:00 2001 From: lyq Date: Fri, 6 Mar 2026 14:58:55 +0800 Subject: [PATCH] fu_node 260306 --- src/autonomy/fu/src/fu_node.cpp | 82 +++++++++++++++++++++++---------- 1 file changed, 58 insertions(+), 24 deletions(-) diff --git a/src/autonomy/fu/src/fu_node.cpp b/src/autonomy/fu/src/fu_node.cpp index fba0bc0..550e134 100644 --- a/src/autonomy/fu/src/fu_node.cpp +++ b/src/autonomy/fu/src/fu_node.cpp @@ -122,18 +122,6 @@ int ntzx_GPS_posit(double Cur_navAngle, double Cur_lonti, double Cur_lati, doubl return 0; } -// ================== 状态接口定义 ================== -class StateMachine; - -class AvoidanceState -{ - public: - virtual ~AvoidanceState() = default; - virtual void enter(StateMachine& sm) {} - virtual void execute(StateMachine& sm) = 0; - virtual void exit(StateMachine& sm) {} -}; - // ================== 状态机核心类 ================== class StateMachine { @@ -279,7 +267,7 @@ class fu_node : public rclcpp::Node pub_mc = this->create_publisher("auto_mc_ctrl", 10); // ======== 定时器初始化 ======== - timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&fu_node::timer_callback, this)); + timer_ = this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&fu_node::timer_callback, this)); // ======== 初始化 ======== avoiding_obstacle_ = false; @@ -346,6 +334,7 @@ class fu_node : public rclcpp::Node int current_waypoint_idx_; std::vector> cached_grid_; bool grid_data_valid_ = false; + rclcpp::Time last_grid_time_; // ======== 发布者和订阅者 ======== rclcpp::Publisher::SharedPtr pub_mc; @@ -451,6 +440,8 @@ 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("栅格地图消息维度无效！"); @@ -474,6 +465,20 @@ class fu_node : public rclcpp::Node analyzeObstacles(grid); if (enable_visualize_grid_) visualizeGridInTerminal(grid); cacheGridData(grid); + + // 立即执行紧急停车检查（高优先级） + updateObstacleAnalysis(); + if (enable_obstacle_stop_ && is_start) + { + if (state_machine_->obstacle_analysis.has_front_critical) + { + state_machine_->publish_speed = 0; + state_machine_->publish_angle = 0.0f; + state_machine_->setState(StateMachine::WAITING); + publishControlCommand(true); // 立即发布带刹车的停车指令 + LOG_WARN("[紧急停车] 栅格回调中检测到前方障碍物，立即停车！"); + } + } } // ======== 栅格缓存 ======== @@ -852,7 +857,16 @@ class fu_node : public rclcpp::Node if (state_machine_->obstacle_analysis.has_front_critical) { state_machine_->publish_speed = 0; + state_machine_->publish_angle = 0.0f; + state_machine_->setState(StateMachine::WAITING); LOG_WARN("[绕障安全] 路点跟踪中检测到前方必停区障碍物，立即停车"); + + // 立即发布停车指令（带刹车） + publishControlCommand(true); + avoiding_obstacle_ = false; + current_waypoint_idx_ = 0; + avoidance_waypoints_.clear(); + return; } } @@ -913,6 +927,18 @@ class fu_node : public rclcpp::Node { if (!enable_obstacle_stop_) return false; + // 检查栅格数据新鲜度（超过500ms认为数据过期） + auto now = node_clock_->now(); + auto grid_age = (now - last_grid_time_).seconds(); + if (grid_age > 0.5) + { + LOG_WARN("[紧急停车] 栅格数据过期(%.3fs)，执行安全停车！", grid_age); + state_machine_->publish_speed = 0; + state_machine_->publish_angle = 0.0f; + state_machine_->setState(StateMachine::WAITING); + return true; + } + if (state_machine_->obstacle_analysis.has_front_critical) { state_machine_->publish_speed = 0; @@ -933,6 +959,13 @@ class fu_node : public rclcpp::Node // 开始清扫任务 if (is_start) { + // 紧急停车优先级最高 + if (executeEmergencyStop()) + { + publishControlCommand(true); // 紧急停车使用刹车 + return; + } + // PL速度为0，停车 if (pl_speed_ == 0) { @@ -943,13 +976,6 @@ class fu_node : public rclcpp::Node return; } - // 紧急停车优先级最高 - if (executeEmergencyStop()) - { - publishControlCommand(); - return; - } - // 若之前处于WAITING状态，且现在没有障碍物且RTK信号良好，恢复到NORMAL状态 auto now = system_clock::now(); duration time_since_last_rtk = now - last_rtk_time; @@ -998,11 +1024,11 @@ class fu_node : public rclcpp::Node } // ======== 发布控制指令 ======== - void publishControlCommand() + void publishControlCommand(bool emergency_brake = false) { auto message = sweeper_interfaces::msg::McCtrl(); message.sweep = true; - message.brake = 0; + message.brake = emergency_brake ? 1 : 0; // 紧急停车时启用刹车 message.gear = 2; message.angle_speed = 800; @@ -1012,8 +1038,16 @@ class fu_node : public rclcpp::Node pub_mc->publish(message); - LOG_INFO("[控制发布] 状态：%s | 转速：%d | 转向角度：%.1f°", state_machine_->getStateString().c_str(), - message.rpm, message.angle); + if (emergency_brake) + { + LOG_WARN("[控制发布] 紧急刹车！状态：%s | 转速：%d | 转向角度：%.1f° | 刹车：%d", + state_machine_->getStateString().c_str(), message.rpm, message.angle, message.brake); + } + else + { + LOG_INFO("[控制发布] 状态：%s | 转速：%d | 转向角度：%.1f°", state_machine_->getStateString().c_str(), + message.rpm, message.angle); + } } };