From dea1c5c797e5c20038ced632ff92c2f19fe6b7e9 Mon Sep 17 00:00:00 2001 From: lyq Date: Sat, 25 Oct 2025 11:13:55 +0800 Subject: [PATCH] =?UTF-8?q?1=E3=80=81=E5=88=A4=E6=96=ADRTK=E4=BF=A1?= =?UTF-8?q?=E5=8F=B7=E8=B4=A8=E9=87=8F=E5=92=8C=E8=B6=85=E6=97=B6=E6=83=85?= =?UTF-8?q?=E5=86=B5=202=E3=80=81=E4=BF=AE=E6=94=B9=E6=A1=A3=E4=BD=8D?= =?UTF-8?q?=E5=92=8C=E9=80=9F=E5=BA=A6=EF=BC=8C=E9=80=82=E9=85=8D=E6=96=B0?= =?UTF-8?q?200=E5=BA=95=E7=9B=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/fu/src/fu_node.cpp | 25 ++++++++++++++++--------- src/pl/src/pl.cpp | 2 +- src/pl/src/pl_node.cpp | 36 ++++++++++++++++++++++++++++++++++-- 3 files changed, 51 insertions(+), 12 deletions(-) diff --git a/src/fu/src/fu_node.cpp b/src/fu/src/fu_node.cpp index b885f77..7a3791d 100644 --- a/src/fu/src/fu_node.cpp +++ b/src/fu/src/fu_node.cpp @@ -216,11 +216,11 @@ private: // 接收PL消息的参数 double x = 0.0; double y = 0.0; - int speed = 0; // 从PL消息获取的速度 - int reliability = 0; + int speed = 0; // 从PL消息获取的速度 + int reliability = 0; // RTK信号质量 1行 0不行 // 发布消息的参数 - int publish_speed = 0; // 转速占空比 0-100 + int publish_speed = 0; // 转速 0-2000 int publish_angle = 0; // [-66.0,66.0]，负数表示左转，正数表示右转 // 车体矩形边界（栅格坐标） @@ -677,15 +677,22 @@ private: // 发布控制指令 sweeper_interfaces::msg::McCtrl message; message.sweep = (avoid_ctx_.state == AVOID_NONE); // 避障状态为无避障 且 任务开始 才开启清扫 - message.gear = 0; + message.gear = 2; // 前进挡 message.brake = (publish_speed == 0) ? 1 : 0; - message.rpm = publish_speed * 25; + + if (reliability == 1) + message.rpm = publish_speed; + else + { + message.rpm = 0; + RCLCPP_ERROR(this->get_logger(), "rtk信号差,停车！！！"); + } message.angle = publish_angle; // 负数表示左转，正数表示右转 message.angle_speed = 800; pub_mc->publish(message); - RCLCPP_INFO_STREAM(this->get_logger(), "控制: 速度=" << publish_speed * 30 << ", 角度=" << publish_angle << ", 状态=" << [&]() + RCLCPP_INFO_STREAM(this->get_logger(), "控制: 速度=" << publish_speed << ", 角度=" << publish_angle << ", 状态=" << [&]() { switch(avoid_state_){ case AVOID_NONE: return "无避障"; @@ -711,7 +718,7 @@ private: float target_angle = calculate_the_angle(x, y); // 初始化控制参数 - publish_speed = std::max(15, std::min(speed, 30)); // 限制速度范围 + publish_speed = std::max(800, std::min(speed, 1500)); // 限制速度范围 publish_angle = static_cast(target_angle); // 安全检查：未检测到车体位置 @@ -885,7 +892,7 @@ private: else { // 距离足够，可以减速接近 - publish_speed = std::max(10, publish_speed / 2); + publish_speed = std::max(800, publish_speed / 2); RCLCPP_INFO(this->get_logger(), "障碍物较宽，减速接近中，距离=%d格", obstacle_analysis_.obstacle_distance); } @@ -1594,7 +1601,7 @@ private: double return_time = (current_time - avoid_ctx_.last_state_change_time).seconds(); // 恢复正常速度 - publish_speed = std::min(25, speed); + publish_speed = std::min(1500, speed); // 计算目标角度 float target_angle = calculate_the_angle(x, y); diff --git a/src/pl/src/pl.cpp b/src/pl/src/pl.cpp index c66f369..16f6bff 100644 --- a/src/pl/src/pl.cpp +++ b/src/pl/src/pl.cpp @@ -267,7 +267,7 @@ void PL_ProcThread() x = x_cm_tmp; y = y_cm_tmp; - pl_speed = 80; + pl_speed = 1500; task_status = TaskStatus::RUNNING; } diff --git a/src/pl/src/pl_node.cpp b/src/pl/src/pl_node.cpp index a4a8ad2..73cb4e0 100644 --- a/src/pl/src/pl_node.cpp +++ b/src/pl/src/pl_node.cpp @@ -13,19 +13,29 @@ #include #include #include +#include // 新增：用于时间戳处理 using namespace std; +using namespace std::chrono; // 新增：时间戳命名空间 pthread_t pl_thread_t; int is_start = 0; static int sock = -1; +// 新增：全局变量记录最后接收RTK数据的时间戳 +system_clock::time_point last_rtk_time; +// 新增：超时阈值（0.5秒） +const duration rtk_timeout = duration(0.2); + class pl_node : public rclcpp::Node { public: pl_node(std::string name) : Node(name) { RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str()); + // 初始化最后接收时间为当前时间（避免初始状态直接超时） + last_rtk_time = system_clock::now(); + // 创建订阅者订阅话题 msg_subscribe_ = this->create_subscription("rtk_message", 10, std::bind(&pl_node::msg_callback, this, std::placeholders::_1)); task_subscribe_ = this->create_subscription("task_message/download", 10, std::bind(&pl_node::task_callback, this, std::placeholders::_1)); @@ -43,6 +53,9 @@ private: // 收到话题数据的回调函数 void msg_callback(const sweeper_interfaces::msg::Rtk::SharedPtr msg) { + // 新增：更新最后接收时间戳 + last_rtk_time = system_clock::now(); + // RCLCPP_INFO(this->get_logger(), "lat:'%.9lf',lon:'%.9lf',head:'%lf',speed:'%lf',p_quality:'%d',h_quality:'%d'", // msg->lat, msg->lon, msg->head, msg->speed, msg->p_quality, msg->h_quality); @@ -50,7 +63,6 @@ private: if (msg->p_quality != 4 || msg->h_quality != 4) { g_rtk.reliability = 0; - RCLCPP_INFO(this->get_logger(), "rtk信号差!"); } g_rtk.lat = msg->lat; @@ -80,6 +92,10 @@ private: void timer_callback_pl() { + // 新增：计算当前时间与最后接收时间的差值 + auto now = system_clock::now(); + duration time_since_last = now - last_rtk_time; + if (is_start == 1) { if (sock < 0) // 首次连接 @@ -128,7 +144,23 @@ private: sweeper_interfaces::msg::Pl message; message.x = x; message.y = y; - message.speed = (float)pl_speed; + + // 修改：同时判断RTK信号质量和超时情况 + if (g_rtk.reliability == 0 || time_since_last > rtk_timeout) + { + message.speed = 0; + if (time_since_last > rtk_timeout) + { + RCLCPP_ERROR(this->get_logger(), "RTK数据超时(%.2fs)，停车！！！", time_since_last.count()); + } + else + { + RCLCPP_ERROR(this->get_logger(), "RTK信号差，停车！！！"); + } + } + else + message.speed = (float)pl_speed; + message.reliability = g_rtk.reliability; // message.drive_mode = drive_mode; if (drive_mode == Direction::STRAIGHT_D) // 直行