From d56fd743e6b772352da3c39cfbcbaf6f70275e1f Mon Sep 17 00:00:00 2001 From: Alvin-lyq Date: Wed, 6 May 2026 13:52:12 +0800 Subject: [PATCH] fu <- uss --- src/autonomy/fu/config/fu_params.yaml | 4 +- src/autonomy/fu/src/fu_node.cpp | 65 ++++++++++++++++++++++++++- 2 files changed, 66 insertions(+), 3 deletions(-) diff --git a/src/autonomy/fu/config/fu_params.yaml b/src/autonomy/fu/config/fu_params.yaml index 8f2c476..d06d530 100644 --- a/src/autonomy/fu/config/fu_params.yaml +++ b/src/autonomy/fu/config/fu_params.yaml @@ -44,4 +44,6 @@ FU: # 绕过所需最小空间(栅格数) min_free_space: 6 - \ No newline at end of file + + # 超声波停车距离阈值(cm) + uss_stop_distance: 200 diff --git a/src/autonomy/fu/src/fu_node.cpp b/src/autonomy/fu/src/fu_node.cpp index da672b9..0b05a3e 100644 --- a/src/autonomy/fu/src/fu_node.cpp +++ b/src/autonomy/fu/src/fu_node.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -242,6 +243,7 @@ class fu_node : public rclcpp::Node this->declare_parameter("parallel_distance", 3); this->declare_parameter("avoid_able_width", 6); this->declare_parameter("min_free_space", 6); + this->declare_parameter("uss_stop_distance", 200); // ======== 参数读取 ======== this->get_parameter("enable_obstacle_stop", enable_obstacle_stop_); @@ -259,6 +261,7 @@ class fu_node : public rclcpp::Node this->get_parameter("parallel_distance", parallel_distance_); this->get_parameter("avoid_able_width", avoid_able_width_); this->get_parameter("min_free_space", min_free_space_); + this->get_parameter("uss_stop_distance", uss_stop_distance_); parameter_callback_handle_ = this->add_on_set_parameters_callback(std::bind(&fu_node::parameter_callback, this, std::placeholders::_1)); @@ -272,6 +275,8 @@ class fu_node : public rclcpp::Node "rtk_message", 10, std::bind(&fu_node::msg_callback_rtk, this, std::placeholders::_1)); task_subscribe_ = this->create_subscription( "task_message/download", 10, std::bind(&fu_node::task_callback, this, std::placeholders::_1)); + uss_subscriber_ = this->create_subscription( + "USS", 10, std::bind(&fu_node::uss_callback, this, std::placeholders::_1)); // ======== 发布者初始化 ======== pub_mc = this->create_publisher("auto_mc_ctrl", 10); @@ -294,6 +299,7 @@ class fu_node : public rclcpp::Node std::lock_guard lock(grid_mutex_); last_grid_time_ = node_clock_->now(); } + last_uss_time_ = node_clock_->now(); front_stop_zone_ = {0, 0, 0, 0}; rear_stop_zone_ = {0, 0, 0, 0}; @@ -303,10 +309,10 @@ class fu_node : public rclcpp::Node LOG_INFO( "\n---------- FU节点参数配置 ----------\n 遇障停车: %s\n 绕障: %s\n 栅格可视化: %s\n" " 路点容差: %.2f米\n 横向偏移: %d栅格\n 平行距离: %d栅格\n" - " 可绕宽度: %d栅格\n 最小空闲: %d栅格\n--------------------------------------------\n", + " 可绕宽度: %d栅格\n 最小空闲: %d栅格\n 超声波停车距离: %dcm\n--------------------------------------------\n", (enable_obstacle_stop_ ? "开启" : "关闭"), (enable_obstacle_avoid_ ? "开启" : "关闭"), (enable_visualize_grid_ ? "是" : "否"), waypoint_tolerance_, lateral_offset_, parallel_distance_, - avoid_able_width_, min_free_space_); + avoid_able_width_, min_free_space_, uss_stop_distance_); } private: @@ -356,6 +362,13 @@ class fu_node : public rclcpp::Node bool grid_data_valid_ = false; rclcpp::Time last_grid_time_; + // ======== USS超声波传感器数据 ======== + uint16_t uss_data_[8] = {0}; + bool uss_data_valid_ = false; + int uss_stop_distance_ = 200; + rclcpp::Time last_uss_time_; + std::mutex uss_mutex_; + // ======== 任务消息回调 ======== void task_callback(const sweeper_interfaces::msg::Task::SharedPtr msg) { @@ -366,6 +379,20 @@ class fu_node : public rclcpp::Node } } + // ======== USS超声波传感器回调 ======== + void uss_callback(const std_msgs::msg::UInt16MultiArray::SharedPtr msg) + { + if (msg->data.size() < 2) return; + + std::lock_guard lock(uss_mutex_); + for (size_t i = 0; i < msg->data.size() && i < 8; i++) + { + uss_data_[i] = msg->data[i]; + } + uss_data_valid_ = true; + last_uss_time_ = node_clock_->now(); + } + // ======== 发布者和订阅者 ======== rclcpp::Publisher::SharedPtr pub_mc; rclcpp::Publisher::SharedPtr pub_fu; @@ -373,6 +400,7 @@ class fu_node : public rclcpp::Node rclcpp::Subscription::SharedPtr msg_subscribe_pl; rclcpp::Subscription::SharedPtr msg_subscribe_rtk; rclcpp::Subscription::SharedPtr task_subscribe_; + rclcpp::Subscription::SharedPtr uss_subscriber_; rclcpp::TimerBase::SharedPtr timer_; // ======== 参数回调 ======== @@ -408,6 +436,8 @@ class fu_node : public rclcpp::Node avoid_able_width_ = param.as_int(); else if (param.get_name() == "min_free_space") min_free_space_ = param.as_int(); + else if (param.get_name() == "uss_stop_distance") + uss_stop_distance_ = param.as_int(); } return result; } @@ -1081,6 +1111,37 @@ class fu_node : public rclcpp::Node // 开始清扫任务 if (is_start) { + // 超声波传感器近距离障碍物检测 + if (enable_obstacle_stop_) + { + auto uss_now = node_clock_->now(); + bool uss_valid = false; + uint16_t front_left = 0, front_right = 0; + double uss_age = 0.0; + { + std::lock_guard lock(uss_mutex_); + uss_age = (uss_now - last_uss_time_).seconds(); + uss_valid = uss_data_valid_; + front_left = uss_data_[0]; + front_right = uss_data_[1]; + } + + if (uss_valid && uss_age < 1.0) + { + if ((front_left > 0 && front_left < (uint16_t)uss_stop_distance_) || + (front_right > 0 && front_right < (uint16_t)uss_stop_distance_)) + { + state_machine_->publish_speed = 0; + state_machine_->publish_angle = 0.0f; + state_machine_->setState(StateMachine::WAITING); + LOG_WARN("[超声波停车] 前方检测到障碍物！左:%ucm 右:%ucm 阈值:%dcm，立即停车！", + front_left, front_right, uss_stop_distance_); + publishControlCommand(true); + return; + } + } + } + // 紧急停车优先级最高 if (executeEmergencyStop()) {