fu_node 260306
This commit is contained in:
parent
0dd6687c09
commit
96dde92d9e
@ -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<sweeper_interfaces::msg::McCtrl>("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<std::vector<int>> cached_grid_;
|
||||
bool grid_data_valid_ = false;
|
||||
rclcpp::Time last_grid_time_;
|
||||
|
||||
// ======== 发布者和订阅者 ========
|
||||
rclcpp::Publisher<sweeper_interfaces::msg::McCtrl>::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<double> 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,9 +1038,17 @@ class fu_node : public rclcpp::Node
|
||||
|
||||
pub_mc->publish(message);
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// ======== 主函数 ========
|
||||
|
||||
Loading…
Reference in New Issue
Block a user