1、判断RTK信号质量和超时情况

2、修改档位和速度，适配新200底盘

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 speed = 0;       // 从PL消息获取的速度
-    int reliability = 0;
+    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<int>(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);

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;
         }
 

src/pl/src/pl_node.cpp

@@ -13,19 +13,29 @@
 #include <arpa/inet.h>
 #include <unistd.h>
 #include <cstring>
+#include <chrono> // 新增：用于时间戳处理
 
 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<double> rtk_timeout = duration<double>(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<sweeper_interfaces::msg::Rtk>("rtk_message", 10, std::bind(&pl_node::msg_callback, this, std::placeholders::_1));
         task_subscribe_ = this->create_subscription<sweeper_interfaces::msg::Task>("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<double> 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) // 直行