新采集的路线1204

gps_load_now.txt

@@ -1,96 +1,68 @@
-32.0311468570
+32.0309247030
-120.91525671300
+120.91507181700
-358.096008
+175.167999
 0.000000
-32.0311563730
+32.0309153960
-120.91525624100
+120.91507298400
-354.798004
+169.643997
 0.000000
-32.0311655490
+32.0309064620
-120.91525493600
+120.91507499500
-347.576996
+172.268997
 0.000000
-32.0311743100
+32.0308971460
-120.91525172600
+120.91507587000
-332.431000
+177.580002
 0.000000
-32.0311818630
+32.0308879100
-120.91524556200
+120.91507735600
-313.209991
+152.240005
 0.000000
-32.0311872560
+32.0308810980
-120.91523692500
+120.91508470900
-291.302002
+102.825996
 0.000000
-32.0311895010
+32.0308796330
-120.91522644400
+120.91509556100
-273.058014
+94.540001
 0.000000
-32.0311897810
+32.0308790170
-120.91521522700
+120.91510617400
-268.415009
+84.416000
 0.000000
-32.0311893590
+32.0308824190
-120.91520456100
+120.91511639600
-264.153992
+37.883999
 0.000000
-32.0311880140
+32.0308908460
-120.91519349600
+120.91512016300
-256.733002
+353.354004
 0.000000
-32.0311854800
+32.0308998390
-120.91518299200
+120.91511894700
-247.852005
+353.295013
 0.000000
-32.0311813510
+32.0309090620
-120.91517328200
+120.91511812300
-235.455994
+358.222992
 0.000000
-32.0311754410
+32.0309183710
-120.91516488300
+120.91511749800
-219.582001
+346.201996
 0.000000
-32.0311677300
+32.0309261310
-120.91515878500
+120.91511212000
-201.425995
+297.071991
 0.000000
-32.0311588910
+32.0309276550
-120.91515602500
+120.91510141900
-183.102005
+249.197006
 0.000000
-32.0311498140
+32.0309219690
-120.91515571600
+120.91509306000
-178.761002
+200.531006
 0.000000
-32.0311408050
+32.0309129310
-120.91515620700
+120.91509103000
-172.496002
+176.186005
-0.000000
-32.0311313060
-120.91515857400
-158.169998
-0.000000
-32.0311232530
-120.91516330100
-142.091995
-0.000000
-32.0311168570
-120.91517109100
-123.244003
-0.000000
-32.0311126890
-120.91518064100
-104.664001
-0.000000
-32.0311106850
-120.91519129000
-97.190002
-0.000000
-32.0311099930
-120.91520212400
-89.829002
-0.000000
-32.0311102590
-120.91521279400
-85.438004
 0.000000

routes/gps_load_1764825047297.txt

@@ -0,0 +1,332 @@
+32.0310062300
+120.91508837100
+192.574005
+0.000000
+32.0309966680
+120.91508582400
+191.985992
+0.000000
+32.0309878290
+120.91508368100
+190.095993
+0.000000
+32.0309782290
+120.91508183300
+182.136002
+0.000000
+32.0309691320
+120.91508170900
+177.313004
+0.000000
+32.0309598680
+120.91508247300
+172.324997
+0.000000
+32.0309505220
+120.91508425100
+168.134995
+0.000000
+32.0309415740
+120.91508691000
+166.386993
+0.000000
+32.0309324890
+120.91508851300
+172.783997
+0.000000
+32.0309229090
+120.91509012000
+168.578995
+0.000000
+32.0309140780
+120.91509229000
+166.811005
+0.000000
+32.0309051460
+120.91509472400
+165.830994
+0.000000
+32.0308963060
+120.91509693900
+169.735001
+0.000000
+32.0308867260
+120.91509924600
+168.291000
+0.000000
+32.0308772720
+120.91510142700
+169.839996
+0.000000
+32.0308683980
+120.91510326300
+170.018005
+0.000000
+32.0308594670
+120.91510533100
+166.720001
+0.000000
+32.0308507300
+120.91510801800
+162.283005
+0.000000
+32.0308414490
+120.91511150600
+160.276001
+0.000000
+32.0308322270
+120.91511521300
+160.251007
+0.000000
+32.0308230440
+120.91511922500
+157.701004
+0.000000
+32.0308141650
+120.91512387100
+154.352005
+0.000000
+32.0308054470
+120.91512915300
+148.410995
+0.000000
+32.0307974410
+120.91513576500
+138.578003
+0.000000
+32.0307907500
+120.91514307500
+132.796997
+0.000000
+32.0307850720
+120.91515141400
+121.928001
+0.000000
+32.0307805000
+120.91516066300
+116.037003
+0.000000
+32.0307766080
+120.91517127500
+107.122002
+0.000000
+32.0307745290
+120.91518162500
+96.809998
+0.000000
+32.0307739520
+120.91519245000
+88.552002
+0.000000
+32.0307747490
+120.91520329000
+77.658997
+0.000000
+32.0307772880
+120.91521349200
+64.290001
+0.000000
+32.0307823290
+120.91522328700
+47.359001
+0.000000
+32.0307894970
+120.91523096000
+36.080002
+0.000000
+32.0307976360
+120.91523738300
+30.979000
+0.000000
+32.0308055610
+120.91524256000
+26.162001
+0.000000
+32.0308139090
+120.91524678200
+15.933000
+0.000000
+32.0308233950
+120.91524910900
+4.803000
+0.000000
+32.0308331110
+120.91524969200
+358.704010
+0.000000
+32.0308422060
+120.91524933500
+356.233002
+0.000000
+32.0308512330
+120.91524834200
+353.705994
+0.000000
+32.0308602030
+120.91524716700
+354.881989
+0.000000
+32.0308699110
+120.91524640300
+354.579987
+0.000000
+32.0308790400
+120.91524550800
+354.855011
+0.000000
+32.0308881120
+120.91524480900
+357.506012
+0.000000
+32.0308971240
+120.91524413800
+355.522003
+0.000000
+32.0309061540
+120.91524343500
+355.557007
+0.000000
+32.0309151310
+120.91524259400
+355.384003
+0.000000
+32.0309248150
+120.91524175100
+355.596985
+0.000000
+32.0309338400
+120.91524100100
+355.588989
+0.000000
+32.0309435560
+120.91524009100
+355.132996
+0.000000
+32.0309532770
+120.91523905600
+353.201996
+0.000000
+32.0309623680
+120.91523783100
+353.092987
+0.000000
+32.0309713470
+120.91523671000
+356.117004
+0.000000
+32.0309803210
+120.91523592700
+353.936005
+0.000000
+32.0309893530
+120.91523464500
+351.126007
+0.000000
+32.0309989390
+120.91523284100
+352.040985
+0.000000
+32.0310085870
+120.91523110700
+348.987000
+0.000000
+32.0310174600
+120.91522884000
+345.084015
+0.000000
+32.0310268560
+120.91522583600
+340.417999
+0.000000
+32.0310353180
+120.91522201400
+335.906006
+0.000000
+32.0310433100
+120.91521715800
+324.356995
+0.000000
+32.0310510080
+120.91521017700
+316.306000
+0.000000
+32.0310578080
+120.91520196700
+308.945007
+0.000000
+32.0310631970
+120.91519342700
+298.385986
+0.000000
+32.0310669850
+120.91518381600
+287.634003
+0.000000
+32.0310690180
+120.91517322400
+275.351990
+0.000000
+32.0310692190
+120.91516232300
+265.686005
+0.000000
+32.0310684190
+120.91515169800
+262.846008
+0.000000
+32.0310667670
+120.91514114000
+252.485992
+0.000000
+32.0310635690
+120.91513059500
+244.078995
+0.000000
+32.0310590500
+120.91512108200
+235.897003
+0.000000
+32.0310532920
+120.91511179600
+229.807999
+0.000000
+32.0310467450
+120.91510366600
+222.167007
+0.000000
+32.0310397350
+120.91509674500
+213.647995
+0.000000
+32.0310320550
+120.91509095600
+206.363998
+0.000000
+32.0310232240
+120.91508615500
+199.858994
+0.000000
+32.0310140310
+120.91508258800
+195.602997
+0.000000
+32.0310051760
+120.91508022200
+187.149994
+0.000000
+32.0309962010
+120.91507896700
+180.582993
+0.000000
+32.0309871640
+120.91507898300
+176.052002
+0.000000
+32.0309775250
+120.91507982600
+175.110992
+0.000000
+32.0309685520
+120.91508080400
+174.738007
+0.000000

routes/gps_load_1764825202500.txt

@@ -0,0 +1,100 @@
+32.0309685520
+120.91508080400
+174.738007
+0.000000
+32.0309594760
+120.91508193400
+170.802002
+0.000000
+32.0309504680
+120.91508341600
+174.061005
+0.000000
+32.0309413970
+120.91508461300
+175.490997
+0.000000
+32.0309322910
+120.91508506600
+178.044998
+0.000000
+32.0309227710
+120.91508524700
+178.255997
+0.000000
+32.0309134700
+120.91508573500
+175.113998
+0.000000
+32.0309043040
+120.91508663600
+172.272003
+0.000000
+32.0308953330
+120.91508869600
+165.223007
+0.000000
+32.0308868920
+120.91509233900
+145.375000
+0.000000
+32.0308803380
+120.91509975500
+119.100998
+0.000000
+32.0308771860
+120.91511044800
+88.364998
+0.000000
+32.0308801960
+120.91512096200
+38.959000
+0.000000
+32.0308889470
+120.91512500000
+358.279999
+0.000000
+32.0308980650
+120.91512459800
+354.290009
+0.000000
+32.0309072300
+120.91512317000
+348.907990
+0.000000
+32.0309161820
+120.91512084500
+348.226990
+0.000000
+32.0309250920
+120.91511863300
+348.507996
+0.000000
+32.0309340350
+120.91511645900
+347.182007
+0.000000
+32.0309428210
+120.91511396100
+341.677002
+0.000000
+32.0309514190
+120.91510961900
+327.466003
+0.000000
+32.0309580580
+120.91510225800
+290.079987
+0.000000
+32.0309583760
+120.91509116000
+240.046997
+0.000000
+32.0309517360
+120.91508382200
+191.212997
+0.000000
+32.0309609090
+120.91508219100
+175.347000
+0.000000

routes/gps_load_1764828281583.txt

@@ -0,0 +1,72 @@
+32.0309150690
+120.91506536800
+177.654007
+0.000000
+32.0309060550
+120.91506541700
+178.863998
+0.000000
+32.0308966140
+120.91506616000
+161.466995
+0.000000
+32.0308891770
+120.91507222500
+118.348999
+0.000000
+32.0308869110
+120.91508264700
+88.599998
+0.000000
+32.0308874270
+120.91509340300
+85.597000
+0.000000
+32.0308877380
+120.91510406400
+92.059998
+0.000000
+32.0308873680
+120.91511488600
+78.463997
+0.000000
+32.0308919650
+120.91512440800
+27.441999
+0.000000
+32.0309008020
+120.91512683500
+359.238007
+0.000000
+32.0309098920
+120.91512631500
+355.177002
+0.000000
+32.0309188450
+120.91512515300
+351.442993
+0.000000
+32.0309272450
+120.91512120700
+309.044006
+0.000000
+32.0309305690
+120.91511117100
+264.268005
+0.000000
+32.0309294050
+120.91510059200
+259.207001
+0.000000
+32.0309264150
+120.91509008900
+223.201996
+0.000000
+32.0309182090
+120.91508508500
+176.380997
+0.000000
+32.0309275230
+120.91508596900
+206.153000
+0.000000

routes/gps_load_1764829550392.txt

@@ -0,0 +1,68 @@
+32.0309247030
+120.91507181700
+175.167999
+0.000000
+32.0309153960
+120.91507298400
+169.643997
+0.000000
+32.0309064620
+120.91507499500
+172.268997
+0.000000
+32.0308971460
+120.91507587000
+177.580002
+0.000000
+32.0308879100
+120.91507735600
+152.240005
+0.000000
+32.0308810980
+120.91508470900
+102.825996
+0.000000
+32.0308796330
+120.91509556100
+94.540001
+0.000000
+32.0308790170
+120.91510617400
+84.416000
+0.000000
+32.0308824190
+120.91511639600
+37.883999
+0.000000
+32.0308908460
+120.91512016300
+353.354004
+0.000000
+32.0308998390
+120.91511894700
+353.295013
+0.000000
+32.0309090620
+120.91511812300
+358.222992
+0.000000
+32.0309183710
+120.91511749800
+346.201996
+0.000000
+32.0309261310
+120.91511212000
+297.071991
+0.000000
+32.0309276550
+120.91510141900
+249.197006
+0.000000
+32.0309219690
+120.91509306000
+200.531006
+0.000000
+32.0309129310
+120.91509103000
+176.186005
+0.000000

src/fu/src/fu_node.cpp

@@ -210,7 +210,7 @@ private:
         int calculateSafeDistance(int speed) const
         {
             // 基于速度计算安全距离：速度越快，需要的距离越远
-            return std::max(3, speed / 10 + 2); // 最少3格，速度30时为5格
+            return std::max(3, speed / 400 + 2); // 最少3格，速度1500时为5格
         }
 
         // 评估绕障可行性
@@ -790,8 +790,8 @@ private:
 
             // 发布控制指令
             sweeper_interfaces::msg::McCtrl message;
-            message.sweep = false;
+            message.sweep = true;
-            message.brake = 1; // 0开;1关
+            message.brake = 0; // 0开;1关
             message.gear = 2;  // 0空挡;1后退;2前进
 
             if (reliability == 1)

src/pl/src/pl.cpp

@@ -214,10 +214,10 @@ void PL_ProcThread()
         des_pos = Road_Planning_Find_Aim_Point(road_pos, 3);       // 找到当前点后第 3 个点，作为目标点
         direction_pos = Road_Planning_Find_Aim_Point(road_pos, 6); // 找到当前点后第 6 个点，判断直行/转弯
 
-        printf("当前位置: %d\n", road_pos);
+        // printf("当前位置: %d\n", road_pos);
-        printf("目标位置: %d\n", des_pos);
+        // printf("目标位置: %d\n", des_pos);
         // printf("direction_pos: %d\n", direction_pos);
-        printf("\n");
+        // printf("\n");
 
         drive_mode = straight_or_turn(g_rtk.direction, GPSPointQueue[direction_pos].Heading, 12);
 

src/pl/src/pl_node.cpp

@@ -25,7 +25,7 @@ static int sock = -1;
 // 新增：全局变量记录最后接收RTK数据的时间戳
 system_clock::time_point last_rtk_time;
 // 新增：超时阈值（0.5秒）
-const duration<double> rtk_timeout = duration<double>(0.2);
+const duration<double> rtk_timeout = duration<double>(0.5);
 
 class pl_node : public rclcpp::Node
 {

src/radio_ctrl/src/radio_ctrl.cpp

@@ -142,32 +142,32 @@ private:
             pub_->publish(msg);
 
             // 降低打印频率：每 PRINT_INTERVAL 次回调打印一次
-            if (++print_counter >= PRINT_INTERVAL)
+            // if (++print_counter >= PRINT_INTERVAL)
-            {
+            // {
-                std::cout << "\n====================================="
+            //     std::cout << "\n====================================="
-                          << "\n  刹车: " << (msg.brake ? "已刹车" : "未刹车")
+            //               << "\n  刹车: " << (msg.brake ? "已刹车" : "未刹车")
-                          << "\n  挡位: ";
+            //               << "\n  挡位: ";
-                switch (msg.gear)
+            //     switch (msg.gear)
-                {
+            //     {
-                case 0:
+            //     case 0:
-                    std::cout << "空挡";
+            //         std::cout << "空挡";
-                    break;
+            //         break;
-                case 2:
+            //     case 2:
-                    std::cout << "前进挡";
+            //         std::cout << "前进挡";
-                    break;
+            //         break;
-                case 1:
+            //     case 1:
-                    std::cout << "后退挡";
+            //         std::cout << "后退挡";
-                    break;
+            //         break;
-                default:
+            //     default:
-                    std::cout << "未知挡位(" << static_cast<int>(msg.gear) << ")";
+            //         std::cout << "未知挡位(" << static_cast<int>(msg.gear) << ")";
-                    break;
+            //         break;
-                }
+            //     }
-                std::cout << "\n  行走电机转速: " << static_cast<int>(msg.rpm) << " RPM"
+            //     std::cout << "\n  行走电机转速: " << static_cast<int>(msg.rpm) << " RPM"
-                          << "\n  轮端转向角度: " << msg.angle << "°"
+            //               << "\n  轮端转向角度: " << msg.angle << "°"
-                          << "\n  清扫状态: " << (msg.sweep ? "正在清扫" : "未清扫")
+            //               << "\n  清扫状态: " << (msg.sweep ? "正在清扫" : "未清扫")
-                          << "\n=====================================" << std::endl;
+            //               << "\n=====================================" << std::endl;
-                print_counter = 0; // 重置计数器
+            //     print_counter = 0; // 重置计数器
-            }
+            // }
         }
         else
         {