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

fu <- uss

Browse Source
This commit is contained in:
Alvin-lyq 2026-05-06 13:52:12 +08:00
parent 65d9734daf
commit d56fd743e6
2 changed files with 66 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/autonomy/fu/config/fu_params.yaml
View File

@ -45,3 +45,5 @@ FU:
# 绕过所需最小空间(栅格数)
min_free_space: 6
# 超声波停车距离阈值(cm)
uss_stop_distance: 200

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

@ -8,6 +8,7 @@
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include <sstream>
#include <std_msgs/msg/int32_multi_array.hpp>
#include <std_msgs/msg/u_int16_multi_array.hpp>
#include <string>
#include <vector>
@ -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<sweeper_interfaces::msg::Task>(
"task_message/download", 10, std::bind(&fu_node::task_callback, this, std::placeholders::_1));
uss_subscriber_ = this->create_subscription<std_msgs::msg::UInt16MultiArray>(
"USS", 10, std::bind(&fu_node::uss_callback, this, std::placeholders::_1));
// ======== 发布者初始化 ========
pub_mc = this->create_publisher<sweeper_interfaces::msg::McCtrl>("auto_mc_ctrl", 10);
@ -294,6 +299,7 @@ class fu_node : public rclcpp::Node
std::lock_guard<std::mutex> 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<std::mutex> 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<sweeper_interfaces::msg::McCtrl>::SharedPtr pub_mc;
rclcpp::Publisher<sweeper_interfaces::msg::Fu>::SharedPtr pub_fu;
@ -373,6 +400,7 @@ class fu_node : public rclcpp::Node
rclcpp::Subscription<sweeper_interfaces::msg::Pl>::SharedPtr msg_subscribe_pl;
rclcpp::Subscription<sweeper_interfaces::msg::Rtk>::SharedPtr msg_subscribe_rtk;
rclcpp::Subscription<sweeper_interfaces::msg::Task>::SharedPtr task_subscribe_;
rclcpp::Subscription<std_msgs::msg::UInt16MultiArray>::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<std::mutex> 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())
{