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

Compare commits

base: lyq:e5bd3f9c903abb429903739f03583d857b5f6708
Branches Tags
lyq:master
lyq:humble
..
compare: lyq:fdde0c6e245b227209a2f83ee353a773e087f1a0
Branches Tags
lyq:humble
lyq:master

No commits in common. "e5bd3f9c903abb429903739f03583d857b5f6708" and "fdde0c6e245b227209a2f83ee353a773e087f1a0" have entirely different histories.
e5bd3f9c90 ... fdde0c6e24

10 changed files with 112 additions and 2160 deletions
Show Stats Expand all files Collapse all files

130
gps_load_now.txt
View File

@ -1,68 +1,96 @@
32.0309247030 32.0311468570
120.91507181700 120.91525671300
175.167999 358.096008
0.000000 0.000000
32.0309153960 32.0311563730
120.91507298400 120.91525624100
169.643997 354.798004
0.000000 0.000000
32.0309064620 32.0311655490
120.91507499500 120.91525493600
172.268997 347.576996
0.000000 0.000000
32.0308971460 32.0311743100
120.91507587000 120.91525172600
177.580002 332.431000
0.000000 0.000000
32.0308879100 32.0311818630
120.91507735600 120.91524556200
152.240005 313.209991
0.000000 0.000000
32.0308810980 32.0311872560
120.91508470900 120.91523692500
102.825996 291.302002
0.000000 0.000000
32.0308796330 32.0311895010
120.91509556100 120.91522644400
94.540001 273.058014
0.000000 0.000000
32.0308790170 32.0311897810
120.91510617400 120.91521522700
84.416000 268.415009
0.000000 0.000000
32.0308824190 32.0311893590
120.91511639600 120.91520456100
37.883999 264.153992
0.000000 0.000000
32.0308908460 32.0311880140
120.91512016300 120.91519349600
353.354004 256.733002
0.000000 0.000000
32.0308998390 32.0311854800
120.91511894700 120.91518299200
353.295013 247.852005
0.000000 0.000000
32.0309090620 32.0311813510
120.91511812300 120.91517328200
358.222992 235.455994
0.000000 0.000000
32.0309183710 32.0311754410
120.91511749800 120.91516488300
346.201996 219.582001
0.000000 0.000000
32.0309261310 32.0311677300
120.91511212000 120.91515878500
297.071991 201.425995
0.000000 0.000000
32.0309276550 32.0311588910
120.91510141900 120.91515602500
249.197006 183.102005
0.000000 0.000000
32.0309219690 32.0311498140
120.91509306000 120.91515571600
200.531006 178.761002
0.000000 0.000000
32.0309129310 32.0311408050
120.91509103000 120.91515620700
176.186005 172.496002
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 0.000000

1504
routes/gps_load_1762240877588.txt
View File

File diff suppressed because it is too large Load Diff

332
routes/gps_load_1764825047297.txt
View File

@ -1,332 +0,0 @@
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

100
routes/gps_load_1764825202500.txt
View File

@ -1,100 +0,0 @@
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

72
routes/gps_load_1764828281583.txt
View File

@ -1,72 +0,0 @@
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

68
routes/gps_load_1764829550392.txt
View File

@ -1,68 +0,0 @@
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

6
src/fu/src/fu_node.cpp
View File

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

6
src/pl/src/pl.cpp
View File

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

2
src/pl/src/pl_node.cpp
View File

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

52
src/radio_ctrl/src/radio_ctrl.cpp
View File

@ -142,32 +142,32 @@ private:
pub_->publish(msg); pub_->publish(msg);
// 降低打印频率：每 PRINT_INTERVAL 次回调打印一次 // 降低打印频率：每 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 else
{ {