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

0413

Browse Source
This commit is contained in:
lyq 2026-04-13 07:24:32 +08:00
parent 2d4f07d069
commit 9fa8c5a0fe
20 changed files with 7099 additions and 1426 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1804
gps_load_now.txt
View File

File diff suppressed because it is too large Load Diff

12
radio_ctrl.sh
View File

@ -5,21 +5,19 @@ set -e
source install/setup.bash source install/setup.bash
# 启动节点 # 启动节点
ros2 run vehicle_params vehicle_params_node & taskset -c 0 ros2 run vehicle_params vehicle_params_node &
sleep 0.2 sleep 0.2
# ros2 run radio_ctrl radio_ctrl_node & taskset -c 1 ros2 run can_radio_ctrl can_radio_ctrl_node &
# sleep 0.2
ros2 run can_radio_ctrl can_radio_ctrl_node &
sleep 0.2 sleep 0.2
ros2 run mc mc_node & taskset -c 3 ros2 run mc mc_node &
sleep 0.2 sleep 0.2
ros2 run ctrl_arbiter ctrl_arbiter_node & taskset -c 1 ros2 run ctrl_arbiter ctrl_arbiter_node &
sleep 0.2 sleep 0.2
ros2 run mqtt_report mqtt_report_node & taskset -c 4 ros2 run mqtt_report mqtt_report_node &
sleep 0.2 sleep 0.2
wait wait

520
routes/gps_load_1773011163644.txt Normal file
View File

@ -0,0 +1,520 @@
31.9884034760
120.88200001800
176.641006
0.000000
31.9883942460
120.88200104900
172.520004
0.000000
31.9883849500
120.88200216000
171.311996
0.000000
31.9883760670
120.88200404500
169.792999
0.000000
31.9883668790
120.88200624200
168.354996
0.000000
31.9883577150
120.88200856600
168.194000
0.000000
31.9883484470
120.88201059200
170.074997
0.000000
31.9883392620
120.88201226200
171.255997
0.000000
31.9883302090
120.88201437800
169.882004
0.000000
31.9883209720
120.88201647900
168.184006
0.000000
31.9883117520
120.88201898600
170.459000
0.000000
31.9883027780
120.88202046600
171.332001
0.000000
31.9882939150
120.88202240200
170.451996
0.000000
31.9882847580
120.88202386100
170.557999
0.000000
31.9882751820
120.88202605600
171.988998
0.000000
31.9882662580
120.88202759800
172.807007
0.000000
31.9882571840
120.88202916000
171.001999
0.000000
31.9882482900
120.88203092200
167.863998
0.000000
31.9882392780
120.88203363800
169.636002
0.000000
31.9882302790
120.88203572500
165.399994
0.000000
31.9882213780
120.88203827900
168.826004
0.000000
31.9882124550
120.88204036400
163.873001
0.000000
31.9882039270
120.88204448500
137.276001
0.000000
31.9881993630
120.88205404600
85.723000
0.000000
31.9882039270
120.88206374100
32.123001
0.000000
31.9882111120
120.88207014000
353.614014
0.000000
31.9882199260
120.88206795800
342.657013
0.000000
31.9882287950
120.88206479400
344.631989
0.000000
31.9882380280
120.88206193800
345.026001
0.000000
31.9882468080
120.88205940800
347.235992
0.000000
31.9882560620
120.88205680300
350.230988
0.000000
31.9882650810
120.88205486200
348.923004
0.000000
31.9882741300
120.88205260500
348.062988
0.000000
31.9882831610
120.88205029800
346.727997
0.000000
31.9882922510
120.88204780800
348.606995
0.000000
31.9883014390
120.88204535800
348.131989
0.000000
31.9883104680
120.88204291700
347.670013
0.000000
31.9883194270
120.88204072900
348.221008
0.000000
31.9883284100
120.88203836800
348.404999
0.000000
31.9883375290
120.88203590900
346.778015
0.000000
31.9883467210
120.88203324700
346.243988
0.000000
31.9883556700
120.88203059800
347.023010
0.000000
31.9883651920
120.88202814300
349.946014
0.000000
31.9883745910
120.88202639500
352.183014
0.000000
31.9883839080
120.88202602900
21.478001
0.000000
31.9883917110
120.88203240400
60.980000
0.000000
31.9883941200
120.88204277500
98.636002
0.000000
31.9883904110
120.88205326200
139.037003
0.000000
31.9883820460
120.88205877300
159.283005
0.000000
31.9883727510
120.88206167600
167.960999
0.000000
31.9883638540
120.88206394700
168.774994
0.000000
31.9883546760
120.88206595700
169.940994
0.000000
31.9883452970
120.88206814000
168.195007
0.000000
31.9883360770
120.88207061100
166.425003
0.000000
31.9883270920
120.88207309200
168.042007
0.000000
31.9883176710
120.88207542500
167.800995
0.000000
31.9883083650
120.88207792200
167.065994
0.000000
31.9882993370
120.88208035500
164.860001
0.000000
31.9882900600
120.88208335200
165.464996
0.000000
31.9882808430
120.88208583400
169.487000
0.000000
31.9882718450
120.88208784500
168.591995
0.000000
31.9882624390
120.88209032100
166.889008
0.000000
31.9882535160
120.88209313400
147.332001
0.000000
31.9882472220
120.88210126000
102.389000
0.000000
31.9882481310
120.88211221600
56.062000
0.000000
31.9882553380
120.88211934700
11.082000
0.000000
31.9882650310
120.88211920800
351.862000
0.000000
31.9882739900
120.88211746100
348.963989
0.000000
31.9882828710
120.88211534800
347.144989
0.000000
31.9882924460
120.88211307800
348.632996
0.000000
31.9883018100
120.88211063800
347.984009
0.000000
31.9883113860
120.88210841500
350.194000
0.000000
31.9883208760
120.88210639100
349.001007
0.000000
31.9883297000
120.88210422300
349.225006
0.000000
31.9883389470
120.88210216700
348.782013
0.000000
31.9883477700
120.88210015600
350.987000
0.000000
31.9883570760
120.88209840900
351.088013
0.000000
31.9883664920
120.88209634900
349.582001
0.000000
31.9883759340
120.88209431000
348.487000
0.000000
31.9883853270
120.88209197400
349.381012
0.000000
31.9883944330
120.88209126800
13.373000
0.000000
31.9884028680
120.88209613500
50.971001
0.000000
31.9884068270
120.88210610800
89.719002
0.000000
31.9884047110
120.88211682300
130.028000
0.000000
31.9883972220
120.88212337300
165.311996
0.000000
31.9883882770
120.88212616600
165.649002
0.000000
31.9883794760
120.88212837400
174.227005
0.000000
31.9883696540
120.88212947700
173.597000
0.000000
31.9883607120
120.88213071400
172.009003
0.000000
31.9883510880
120.88213226500
170.632004
0.000000
31.9883419890
120.88213424500
169.169006
0.000000
31.9883327410
120.88213628700
167.537003
0.000000
31.9883234660
120.88213903000
168.201004
0.000000
31.9883141480
120.88214122100
170.292007
0.000000
31.9883048780
120.88214302500
168.817993
0.000000
31.9882958610
120.88214526000
167.591995
0.000000
31.9882863250
120.88214784300
166.212006
0.000000
31.9882775510
120.88215052300
166.104004
0.000000
31.9882683920
120.88215326000
164.735001
0.000000
31.9882586130
120.88215577900
171.490997
0.000000
31.9882491770
120.88215754000
170.835999
0.000000
31.9882402610
120.88215944900
169.276993
0.000000
31.9882310640
120.88216165900
167.016006
0.000000
31.9882215010
120.88216447300
164.996994
0.000000
31.9882124260
120.88216760200
161.955002
0.000000
31.9882038160
120.88217203700
141.097000
0.000000
31.9881986320
120.88218072900
97.032997
0.000000
31.9882003660
120.88219211000
49.464001
0.000000
31.9882078940
120.88219815100
5.940000
0.000000
31.9882173080
120.88219798100
352.351990
0.000000
31.9882264170
120.88219638600
349.980011
0.000000
31.9882353110
120.88219419700
345.760986
0.000000
31.9882441290
120.88219177800
347.029999
0.000000
31.9882532020
120.88218953900
348.696991
0.000000
31.9882622350
120.88218742500
348.976990
0.000000
31.9882715410
120.88218520900
348.084015
0.000000
31.9882803660
120.88218299700
346.346008
0.000000
31.9882892580
120.88218058000
349.834991
0.000000
31.9882983530
120.88217841600
348.748993
0.000000
31.9883073330
120.88217658800
351.826996
0.000000
31.9883164720
120.88217486500
350.562988
0.000000
31.9883256410
120.88217277300
348.729004
0.000000
31.9883345950
120.88217067700
347.226990
0.000000
31.9883436970
120.88216828500
349.636993
0.000000
31.9883525600
120.88216647700
349.973999
0.000000
31.9883614550
120.88216460600
349.079987
0.000000
31.9883706900
120.88216267300
350.602997
0.000000
31.9883797700
120.88216097700
349.066010
0.000000
31.9883889650
120.88215872100
347.101013
0.000000
31.9883977610
120.88215630700
348.395996
0.000000

296
routes/gps_load_1774512120242.txt Normal file
View File

@ -0,0 +1,296 @@
32.0308993820
120.91507149200
154.393997
0.000000
32.0308908880
120.91507609100
156.770996
0.000000
32.0308820900
120.91508022200
161.320999
0.000000
32.0308729300
120.91508327900
164.388000
0.000000
32.0308636720
120.91508622000
168.738998
0.000000
32.0308545390
120.91508891900
163.585007
0.000000
32.0308456420
120.91509205900
162.567001
0.000000
32.0308367720
120.91509570600
159.817001
0.000000
32.0308283240
120.91509959000
157.233994
0.000000
32.0308198230
120.91510421200
154.225998
0.000000
32.0308118540
120.91510928200
150.563004
0.000000
32.0308041460
120.91511500500
145.574997
0.000000
32.0307970100
120.91512149800
137.447998
0.000000
32.0307903680
120.91512910000
133.304001
0.000000
32.0307845570
120.91513728800
124.938004
0.000000
32.0307792510
120.91514701200
116.857002
0.000000
32.0307757210
120.91515676400
108.000000
0.000000
32.0307731880
120.91516795300
100.412003
0.000000
32.0307720580
120.91517866900
93.139999
0.000000
32.0307721750
120.91518977100
85.161003
0.000000
32.0307733140
120.91520035800
79.917999
0.000000
32.0307754450
120.91521117400
72.524002
0.000000
32.0307786300
120.91522117700
64.149002
0.000000
32.0307831060
120.91523091200
55.757000
0.000000
32.0307888470
120.91523972100
45.703999
0.000000
32.0307955900
120.91524727700
36.721001
0.000000
32.0308032030
120.91525292300
25.339001
0.000000
32.0308117160
120.91525663100
14.061000
0.000000
32.0308211890
120.91525878100
6.761000
0.000000
32.0308306630
120.91525953900
359.402008
0.000000
32.0308396840
120.91525899700
356.489014
0.000000
32.0308495400
120.91525841400
358.388000
0.000000
32.0308588370
120.91525784800
355.955994
0.000000
32.0308688820
120.91525701100
356.532990
0.000000
32.0308790840
120.91525644800
358.276001
0.000000
32.0308892710
120.91525624600
0.597000
0.000000
32.0308993740
120.91525614100
1.859000
0.000000
32.0309095400
120.91525644200
359.981995
0.000000
32.0309196920
120.91525647300
1.118000
0.000000
32.0309296580
120.91525669200
359.282013
0.000000
32.0309397660
120.91525625500
355.990997
0.000000
32.0309498180
120.91525519400
354.601990
0.000000
32.0309598730
120.91525410300
355.829987
0.000000
32.0309700120
120.91525293400
355.640991
0.000000
32.0309800340
120.91525205300
354.127014
0.000000
32.0309900730
120.91525053700
353.371002
0.000000
32.0310000890
120.91524899300
352.497009
0.000000
32.0310100240
120.91524694300
346.531006
0.000000
32.0310197190
120.91524384600
342.578003
0.000000
32.0310289560
120.91523935000
330.714996
0.000000
32.0310376520
120.91523333000
327.092010
0.000000
32.0310455090
120.91522587600
319.910004
0.000000
32.0310527800
120.91521782000
310.996002
0.000000
32.0310587730
120.91520873400
300.246002
0.000000
32.0310630270
120.91519814200
289.242004
0.000000
32.0310656240
120.91518679300
278.085999
0.000000
32.0310664430
120.91517468400
273.287994
0.000000
32.0310667060
120.91516284900
269.044006
0.000000
32.0310656920
120.91515189700
255.488998
0.000000
32.0310627990
120.91514186000
245.468002
0.000000
32.0310583500
120.91513213900
235.746994
0.000000
32.0310527920
120.91512366700
227.744003
0.000000
32.0310462870
120.91511606300
220.703003
0.000000
32.0310390300
120.91510930300
215.496994
0.000000
32.0310312710
120.91510360100
213.026001
0.000000
32.0310230660
120.91509779500
206.169998
0.000000
32.0310148970
120.91509331900
199.132996
0.000000
32.0310059190
120.91509022700
194.789001
0.000000
32.0309966040
120.91508766700
191.897003
0.000000
32.0309876160
120.91508590000
185.632996
0.000000
32.0309781640
120.91508545500
177.522995
0.000000
32.0309691240
120.91508620700
176.319000
0.000000
32.0309594190
120.91508727400
176.514999
0.000000
32.0309500690
120.91508793500
176.947998
0.000000

4
routes/gps_load_1775863113036.txt Normal file
View File

@ -0,0 +1,4 @@
31.9884051240
120.88199379700
166.173004
0.000000

4
routes/gps_load_1775863454941.txt Normal file
View File

@ -0,0 +1,4 @@
31.9884074160
120.88210960000
351.987000
0.000000

4
routes/gps_load_1775864103449.txt Normal file
View File

@ -0,0 +1,4 @@
31.9884089500
120.88200292800
169.895004
0.000000

0
routes/gps_load_1775864390068.txt Normal file
View File

4
routes/gps_load_1775864661670.txt Normal file
View File

@ -0,0 +1,4 @@
31.9883943820
120.88203535100
350.290009
0.000000

4
routes/gps_load_1775865323278.txt Normal file
View File

@ -0,0 +1,4 @@
31.9884007200
120.88201090400
172.895996
0.000000

16
routes/gps_load_1775866821231.txt Normal file
View File

@ -0,0 +1,16 @@
31.9883951330
120.88200944400
167.565002
0.000000
31.9883861530
120.88201161600
168.610001
0.000000
31.9883772220
120.88201386800
170.662003
0.000000
31.9883680740
120.88201547700
171.835007
0.000000

28
routes/gps_load_1775867256930.txt Normal file
View File

@ -0,0 +1,28 @@
31.9883986530
120.88201005600
170.298996
0.000000
31.9883896310
120.88201227500
171.167007
0.000000
31.9883803310
120.88201421200
171.770996
0.000000
31.9883707050
120.88201595300
172.005005
0.000000
31.9883613970
120.88201781500
172.697998
0.000000
31.9883517010
120.88201907900
172.865997
0.000000
31.9883424810
120.88202076200
171.052002
0.000000

1296
routes/gps_load_1775867440359.txt Normal file
View File

File diff suppressed because it is too large Load Diff

180
routes/gps_load_1776034709327.txt Normal file
View File

@ -0,0 +1,180 @@
31.9888879580
120.88156724000
200.296997
0.000000
31.9888790760
120.88156503900
203.539993
0.000000
31.9888877450
120.88156189900
203.727005
0.000000
31.9888981430
120.88156355200
188.300995
0.000000
31.9889034970
120.88157396700
179.001999
0.000000
31.9889006800
120.88158404500
178.875000
0.000000
31.9889003970
120.88159477000
178.658005
0.000000
31.9889008230
120.88160542300
178.065994
0.000000
31.9889017020
120.88161626100
178.406998
0.000000
31.9889020800
120.88162711400
178.070007
0.000000
31.9889025550
120.88163772200
177.162003
0.000000
31.9889024570
120.88164891400
176.940002
0.000000
31.9889021590
120.88165959200
177.087006
0.000000
31.9889025520
120.88167026800
177.007996
0.000000
31.9889039220
120.88168081500
177.209000
0.000000
31.9889050260
120.88169194600
176.563004
0.000000
31.9889052120
120.88170256200
176.337997
0.000000
31.9889054960
120.88171331200
175.901993
0.000000
31.9889058590
120.88172396100
175.283997
0.000000
31.9889056210
120.88173474000
175.050003
0.000000
31.9889060450
120.88174566600
175.432007
0.000000
31.9889066370
120.88175653100
176.069000
0.000000
31.9889070050
120.88176818000
175.787003
0.000000
31.9889079860
120.88177933800
175.220993
0.000000
31.9889079700
120.88179017100
175.240997
0.000000
31.9889073860
120.88180108100
175.160004
0.000000
31.9889084340
120.88181179100
175.490997
0.000000
31.9889089330
120.88182256400
175.315002
0.000000
31.9889092650
120.88183321900
175.528000
0.000000
31.9889085810
120.88184441500
175.296997
0.000000
31.9889090300
120.88185534700
175.024002
0.000000
31.9889099030
120.88186627200
174.320999
0.000000
31.9889112840
120.88187725100
174.156998
0.000000
31.9889129790
120.88188807900
174.117996
0.000000
31.9889124180
120.88189868100
173.921005
0.000000
31.9889083650
120.88190980000
173.104004
0.000000
31.9889054180
120.88191984200
174.453995
0.000000
31.9888983800
120.88192845400
179.750000
0.000000
31.9888894780
120.88193290800
181.578003
0.000000
31.9888805660
120.88193581100
181.313004
0.000000
31.9888715030
120.88193784100
182.753998
0.000000
31.9888621880
120.88193951300
182.287994
0.000000
31.9888529000
120.88194128200
181.498993
0.000000
31.9888441490
120.88194397800
181.550995
0.000000
31.9888344020
120.88194815400
182.026993
0.000000

992
routes/gps_load_1776034966319.txt Normal file
View File

@ -0,0 +1,992 @@
31.9884288270
120.88199411800
170.425995
0.000000
31.9884195560
120.88199595700
170.707001
0.000000
31.9884094890
120.88199769900
168.067993
0.000000
31.9883999460
120.88200005800
169.434006
0.000000
31.9883902240
120.88200221200
169.988998
0.000000
31.9883805260
120.88200405800
171.410004
0.000000
31.9883707670
120.88200550200
170.886993
0.000000
31.9883609870
120.88200672100
171.718002
0.000000
31.9883511580
120.88200828300
169.800995
0.000000
31.9883413380
120.88201020400
167.917999
0.000000
31.9883316010
120.88201259800
167.401993
0.000000
31.9883220080
120.88201467100
166.701996
0.000000
31.9883122290
120.88201727500
165.945007
0.000000
31.9883025210
120.88201964500
168.641006
0.000000
31.9882927610
120.88202148400
169.973007
0.000000
31.9882828310
120.88202346700
169.751007
0.000000
31.9882731670
120.88202453400
171.490005
0.000000
31.9882630830
120.88202608300
169.511002
0.000000
31.9882531540
120.88202817800
166.884003
0.000000
31.9882434740
120.88203047400
169.259995
0.000000
31.9882337350
120.88203261700
166.007004
0.000000
31.9882239680
120.88203518400
169.445999
0.000000
31.9882141760
120.88203672700
170.550995
0.000000
31.9882043150
120.88203791100
169.537003
0.000000
31.9881943540
120.88203931700
171.057999
0.000000
31.9881844590
120.88204050900
170.369995
0.000000
31.9881745500
120.88204215000
169.869995
0.000000
31.9881656940
120.88204409700
168.809006
0.000000
31.9881560110
120.88204588500
174.044006
0.000000
31.9881462130
120.88204769600
175.203003
0.000000
31.9881364390
120.88204927900
170.460007
0.000000
31.9881266150
120.88205084200
167.727997
0.000000
31.9881166550
120.88205319800
170.470993
0.000000
31.9881068560
120.88205520800
169.261002
0.000000
31.9880969370
120.88205696800
172.263000
0.000000
31.9880872250
120.88205878000
171.520004
0.000000
31.9880772850
120.88206045300
172.399994
0.000000
31.9880674380
120.88206166500
170.328003
0.000000
31.9880578240
120.88206379500
167.949005
0.000000
31.9880481160
120.88206600500
167.018997
0.000000
31.9880384190
120.88206878800
166.660995
0.000000
31.9880290080
120.88207158200
164.009003
0.000000
31.9880194140
120.88207401700
167.233002
0.000000
31.9880096690
120.88207667800
169.731995
0.000000
31.9879999030
120.88207869800
170.529007
0.000000
31.9879901320
120.88208005600
170.182007
0.000000
31.9879802750
120.88208139200
172.244995
0.000000
31.9879705660
120.88208310500
171.035004
0.000000
31.9879607330
120.88208440900
172.820999
0.000000
31.9879510260
120.88208515800
171.725006
0.000000
31.9879413400
120.88208651400
168.906998
0.000000
31.9879316280
120.88208839700
166.108994
0.000000
31.9879218700
120.88209050600
166.285004
0.000000
31.9879125360
120.88209279400
155.539001
0.000000
31.9879042770
120.88209882000
113.705002
0.000000
31.9879034150
120.88210948900
64.216003
0.000000
31.9879097670
120.88211773400
19.921000
0.000000
31.9879018800
120.88214155100
353.087006
0.000000
31.9879118920
120.88213964600
354.920990
0.000000
31.9879215670
120.88213832500
350.755005
0.000000
31.9879313070
120.88213620300
349.742004
0.000000
31.9879408040
120.88213409900
350.381012
0.000000
31.9879504440
120.88213231000
350.252991
0.000000
31.9879601710
120.88213007400
349.345001
0.000000
31.9879697330
120.88212779700
349.131989
0.000000
31.9879793550
120.88212556100
351.013000
0.000000
31.9879890450
120.88212374700
351.182007
0.000000
31.9879987480
120.88212225700
351.503998
0.000000
31.9880083320
120.88212042100
350.207001
0.000000
31.9880178080
120.88211806800
348.587006
0.000000
31.9880275440
120.88211552200
347.221008
0.000000
31.9880370210
120.88211282300
346.015015
0.000000
31.9880463960
120.88210988300
346.825012
0.000000
31.9880558180
120.88210719800
350.364990
0.000000
31.9880655130
120.88210526400
352.058990
0.000000
31.9880752090
120.88210353400
352.394989
0.000000
31.9880848000
120.88210192800
353.756989
0.000000
31.9880944970
120.88210070100
355.178009
0.000000
31.9881041880
120.88209920600
356.074005
0.000000
31.9881137920
120.88209828200
356.894012
0.000000
31.9881237680
120.88209777200
354.562988
0.000000
31.9881335080
120.88209643200
352.463989
0.000000
31.9881432480
120.88209520100
352.722992
0.000000
31.9881528840
120.88209330800
351.984009
0.000000
31.9881626740
120.88209142100
350.701996
0.000000
31.9881721460
120.88208939900
350.447998
0.000000
31.9881817170
120.88208715000
346.852997
0.000000
31.9881912790
120.88208433300
346.622986
0.000000
31.9882005950
120.88208153200
350.286987
0.000000
31.9882103180
120.88207989600
349.316010
0.000000
31.9882201690
120.88207773300
350.046997
0.000000
31.9882295740
120.88207578800
350.520996
0.000000
31.9882391170
120.88207399100
350.457001
0.000000
31.9882488730
120.88207176400
350.532990
0.000000
31.9882583430
120.88206993000
349.195007
0.000000
31.9882678760
120.88206714300
345.888000
0.000000
31.9882772500
120.88206455700
345.789001
0.000000
31.9882864870
120.88206163700
347.921997
0.000000
31.9882962520
120.88205940000
349.795990
0.000000
31.9883071850
120.88205675600
350.846985
0.000000
31.9883166500
120.88205490500
352.106995
0.000000
31.9883260940
120.88205330600
351.002014
0.000000
31.9883353000
120.88205149100
350.002014
0.000000
31.9883449910
120.88205001800
348.420013
0.000000
31.9883548130
120.88204720500
347.463989
0.000000
31.9883644680
120.88204457500
349.498993
0.000000
31.9883741550
120.88204250100
350.243011
0.000000
31.9883836730
120.88204108500
4.043000
0.000000
31.9883930210
120.88204380900
40.562000
0.000000
31.9883981230
120.88205396100
82.735001
0.000000
31.9883970740
120.88206489200
118.607002
0.000000
31.9883914010
120.88207325400
154.522995
0.000000
31.9883830050
120.88207741600
162.341995
0.000000
31.9883734900
120.88208174100
160.742996
0.000000
31.9883650790
120.88208596100
159.386002
0.000000
31.9883567500
120.88209027700
158.546997
0.000000
31.9883487090
120.88209501600
157.802994
0.000000
31.9883392960
120.88209847400
171.878998
0.000000
31.9883294280
120.88210070200
171.917007
0.000000
31.9883196580
120.88210280900
172.436005
0.000000
31.9883102590
120.88210446400
172.317993
0.000000
31.9883007180
120.88210611600
172.748993
0.000000
31.9882928670
120.88210017300
172.397003
0.000000
31.9882831910
120.88210172100
173.335007
0.000000
31.9882734280
120.88210337600
169.848007
0.000000
31.9882639020
120.88210557600
169.621002
0.000000
31.9882543540
120.88210790500
167.460007
0.000000
31.9882448110
120.88211065900
164.837997
0.000000
31.9882354900
120.88211392700
165.138000
0.000000
31.9882259450
120.88211659800
172.164001
0.000000
31.9882161630
120.88211821600
172.003998
0.000000
31.9882065760
120.88211973100
171.600006
0.000000
31.9881969180
120.88212178300
170.479996
0.000000
31.9881871950
120.88212397600
168.738998
0.000000
31.9881765580
120.88212671700
167.746994
0.000000
31.9881669830
120.88212893300
169.770004
0.000000
31.9881573540
120.88213105700
171.264008
0.000000
31.9881477280
120.88213282100
171.378998
0.000000
31.9881380980
120.88213472700
171.595993
0.000000
31.9881283770
120.88213630000
171.065002
0.000000
31.9881188150
120.88213829400
170.561996
0.000000
31.9881092070
120.88214024100
169.942993
0.000000
31.9880995730
120.88214261900
168.884995
0.000000
31.9880900110
120.88214488100
171.479996
0.000000
31.9880803630
120.88214679200
171.880997
0.000000
31.9880707650
120.88214861300
171.044006
0.000000
31.9880610140
120.88215030500
170.285004
0.000000
31.9880515290
120.88215248800
168.050995
0.000000
31.9880419770
120.88215496300
167.481003
0.000000
31.9880323460
120.88215740400
169.166000
0.000000
31.9880227560
120.88215966600
169.917007
0.000000
31.9880130960
120.88216144900
171.436005
0.000000
31.9880034160
120.88216310300
172.705002
0.000000
31.9879937470
120.88216478900
170.022003
0.000000
31.9879841140
120.88216696500
169.882996
0.000000
31.9879745770
120.88216928600
168.602005
0.000000
31.9879650420
120.88217171800
168.503006
0.000000
31.9879554810
120.88217388000
161.863998
0.000000
31.9879470130
120.88217976800
123.551003
0.000000
31.9879438320
120.88219016500
82.726997
0.000000
31.9879473800
120.88220070500
41.180000
0.000000
31.9879560100
120.88220607300
6.214000
0.000000
31.9879658200
120.88220647900
359.704987
0.000000
31.9879756630
120.88220554500
353.226013
0.000000
31.9879852980
120.88220410900
353.653992
0.000000
31.9879949830
120.88220227400
345.876007
0.000000
31.9880044320
120.88219922000
344.588013
0.000000
31.9880138650
120.88219612600
344.282990
0.000000
31.9880231890
120.88219298800
344.035004
0.000000
31.9880326450
120.88218974300
345.737000
0.000000
31.9880420060
120.88218714100
349.368011
0.000000
31.9880516450
120.88218484700
351.467010
0.000000
31.9880613210
120.88218297500
353.571014
0.000000
31.9880709480
120.88218145400
353.084015
0.000000
31.9880806620
120.88217999200
351.255005
0.000000
31.9880902120
120.88217803900
349.152008
0.000000
31.9880998580
120.88217577800
348.131989
0.000000
31.9881093870
120.88217321900
349.188995
0.000000
31.9881191110
120.88217095500
350.966003
0.000000
31.9881286160
120.88216888500
351.066010
0.000000
31.9881382200
120.88216715200
350.924988
0.000000
31.9881479280
120.88216542200
352.434998
0.000000
31.9881575040
120.88216374500
350.768005
0.000000
31.9881673360
120.88216150700
349.588989
0.000000
31.9881768640
120.88215932800
348.208008
0.000000
31.9881863110
120.88215654500
345.572998
0.000000
31.9881957480
120.88215351400
344.291992
0.000000
31.9882052170
120.88215036800
345.446991
0.000000
31.9882146060
120.88214748100
349.652008
0.000000
31.9882243120
120.88214539600
350.656006
0.000000
31.9882338760
120.88214350300
351.721008
0.000000
31.9882435670
120.88214168400
350.165009
0.000000
31.9882531000
120.88213914500
346.134003
0.000000
31.9882626280
120.88213654900
348.639008
0.000000
31.9882721350
120.88213424000
349.625000
0.000000
31.9882818070
120.88213216500
352.407013
0.000000
31.9882914790
120.88213071100
355.337006
0.000000
31.9883013140
120.88212979800
355.875000
0.000000
31.9883111540
120.88212892000
356.704987
0.000000
31.9883209290
120.88212770800
353.306000
0.000000
31.9883306260
120.88212631400
352.618011
0.000000
31.9883402150
120.88212463300
351.596985
0.000000
31.9883497810
120.88212293200
351.377991
0.000000
31.9883596780
120.88212104100
351.316010
0.000000
31.9883692300
120.88211908600
351.355011
0.000000
31.9883790050
120.88211742300
352.256012
0.000000
31.9883887150
120.88211585500
355.808990
0.000000
31.9883982530
120.88211685700
31.158001
0.000000
31.9884051090
120.88212498800
71.333000
0.000000
31.9884059610
120.88213648200
114.296997
0.000000
31.9883999470
120.88214557200
156.212006
0.000000
31.9883906740
120.88214833700
172.772003
0.000000
31.9883810250
120.88215021500
169.848007
0.000000
31.9883714990
120.88215247800
168.113007
0.000000
31.9883618870
120.88215498300
167.264008
0.000000
31.9883523490
120.88215771300
165.796005
0.000000
31.9883427950
120.88216086500
164.755997
0.000000
31.9883334650
120.88216420800
164.119995
0.000000
31.9883240410
120.88216733300
165.322998
0.000000
31.9883143990
120.88217063300
166.615997
0.000000
31.9883049760
120.88217341200
167.934998
0.000000
31.9882952900
120.88217605500
168.811005
0.000000
31.9882856680
120.88217851600
168.339996
0.000000
31.9882761020
120.88218114100
166.494995
0.000000
31.9882667370
120.88218407500
164.511002
0.000000
31.9882571930
120.88218708900
169.563995
0.000000
31.9882475750
120.88218922300
170.731995
0.000000
31.9882379920
120.88219114800
171.710007
0.000000
31.9882283010
120.88219291200
172.697998
0.000000
31.9882186950
120.88219456800
170.154007
0.000000
31.9882091750
120.88219678700
169.270996
0.000000
31.9881996680
120.88219911100
169.399002
0.000000
31.9881900310
120.88220087400
175.860992
0.000000
31.9881801400
120.88220208500
176.024002
0.000000
31.9881703760
120.88220302800
171.934006
0.000000
31.9881608010
120.88220495000
170.425995
0.000000
31.9881513270
120.88220718700
169.738007
0.000000
31.9881418710
120.88220955700
169.253006
0.000000
31.9881323390
120.88221193400
168.343002
0.000000
31.9881228200
120.88221443800
167.669006
0.000000
31.9881132920
120.88221692900
170.447998
0.000000
31.9881036200
120.88221886200
170.994003
0.000000
31.9880940790
120.88222141300
161.871002
0.000000
31.9880846190
120.88222468400
166.559006
0.000000
31.9880751580
120.88222720600
170.229004
0.000000
31.9880653400
120.88222928400
171.119003
0.000000
31.9880556590
120.88223111800
171.759995
0.000000
31.9880462150
120.88223291100
172.399002
0.000000

996
routes/gps_load_1776035219621.txt Normal file
View File

@ -0,0 +1,996 @@
31.9880371660
120.88223372400
194.660004
0.000000
31.9880291580
120.88222886100
228.110992
0.000000
31.9880248510
120.88221928100
269.445007
0.000000
31.9880270300
120.88220881300
310.962006
0.000000
31.9880345310
120.88220261700
353.014008
0.000000
31.9880441700
120.88220244000
1.493000
0.000000
31.9880535250
120.88220266300
1.903000
0.000000
31.9880628030
120.88220283700
358.403015
0.000000
31.9880717990
120.88220189600
353.795990
0.000000
31.9880816230
120.88220052000
352.881012
0.000000
31.9880912160
120.88219869100
350.682007
0.000000
31.9881008850
120.88219675600
349.848999
0.000000
31.9881105260
120.88219458600
349.087006
0.000000
31.9881202080
120.88219245900
351.635010
0.000000
31.9881299310
120.88219070100
351.960999
0.000000
31.9881394240
120.88218870500
349.122986
0.000000
31.9881491290
120.88218680800
351.157013
0.000000
31.9881587850
120.88218463500
348.523010
0.000000
31.9881684060
120.88218228500
348.373993
0.000000
31.9881780960
120.88218008400
350.006012
0.000000
31.9881876540
120.88217799000
350.856995
0.000000
31.9881973350
120.88217618700
351.162994
0.000000
31.9882070260
120.88217421800
349.714996
0.000000
31.9882166020
120.88217180500
346.192993
0.000000
31.9882261000
120.88216905200
347.070007
0.000000
31.9882355520
120.88216642500
351.324005
0.000000
31.9882453350
120.88216464200
351.757996
0.000000
31.9882549700
120.88216301400
352.566010
0.000000
31.9882648070
120.88216152600
352.635010
0.000000
31.9882743920
120.88215973300
349.920990
0.000000
31.9882840000
120.88215772100
350.675995
0.000000
31.9882936590
120.88215610900
351.578003
0.000000
31.9883033570
120.88215426300
350.829010
0.000000
31.9883130570
120.88215221700
349.118988
0.000000
31.9883226360
120.88215013200
352.636993
0.000000
31.9883323120
120.88214843100
351.582001
0.000000
31.9883419550
120.88214635400
350.446991
0.000000
31.9883515040
120.88214460800
353.234985
0.000000
31.9883612170
120.88214314400
353.684998
0.000000
31.9883710350
120.88214190900
354.329010
0.000000
31.9883807830
120.88214075300
355.074005
0.000000
31.9883904950
120.88213973500
352.118988
0.000000
31.9883999670
120.88213706300
333.291992
0.000000
31.9884073450
120.88213006800
295.446014
0.000000
31.9884092710
120.88211903400
253.494995
0.000000
31.9884043030
120.88210947300
212.669998
0.000000
31.9883951510
120.88210622700
175.559998
0.000000
31.9883853690
120.88210763800
171.011993
0.000000
31.9883757970
120.88210959200
165.261993
0.000000
31.9883663330
120.88211245700
168.957001
0.000000
31.9883566640
120.88211480400
169.300995
0.000000
31.9883470400
120.88211705200
170.526993
0.000000
31.9883372380
120.88211907500
171.501007
0.000000
31.9883275950
120.88212089600
171.641006
0.000000
31.9883179630
120.88212253700
172.972000
0.000000
31.9883082000
120.88212412800
173.037003
0.000000
31.9882984060
120.88212569900
172.578003
0.000000
31.9882886220
120.88212749400
170.544998
0.000000
31.9882789290
120.88212948700
169.968002
0.000000
31.9882693870
120.88213171600
169.229004
0.000000
31.9882597560
120.88213409600
167.932999
0.000000
31.9882501950
120.88213656500
170.858002
0.000000
31.9882403950
120.88213847400
171.917007
0.000000
31.9882307330
120.88214019900
172.184006
0.000000
31.9882209270
120.88214204600
168.772003
0.000000
31.9882113740
120.88214432600
170.865997
0.000000
31.9882017740
120.88214619400
171.772995
0.000000
31.9881920380
120.88214805500
172.080994
0.000000
31.9881824000
120.88214971600
172.852997
0.000000
31.9881726690
120.88215135000
173.417999
0.000000
31.9881628140
120.88215273200
173.528000
0.000000
31.9881532790
120.88215416300
173.772995
0.000000
31.9881435810
120.88215577200
171.001007
0.000000
31.9881338800
120.88215772900
169.990997
0.000000
31.9881243470
120.88216001000
168.022003
0.000000
31.9881147810
120.88216256700
166.690002
0.000000
31.9881052270
120.88216525100
169.632996
0.000000
31.9880955780
120.88216744300
169.248001
0.000000
31.9880859030
120.88216967300
170.932007
0.000000
31.9880762380
120.88217152400
171.565994
0.000000
31.9880665830
120.88217337300
172.384003
0.000000
31.9880568530
120.88217486900
172.552994
0.000000
31.9880471970
120.88217638500
173.565002
0.000000
31.9880373770
120.88217778300
171.923004
0.000000
31.9880276890
120.88217990600
168.037994
0.000000
31.9880181490
120.88218248000
167.908997
0.000000
31.9880084910
120.88218454600
172.285004
0.000000
31.9879988410
120.88218626300
170.503998
0.000000
31.9879891510
120.88218842700
168.919998
0.000000
31.9879796150
120.88219081000
167.873001
0.000000
31.9879700500
120.88219325900
169.529999
0.000000
31.9879603310
120.88219545400
170.378006
0.000000
31.9879507370
120.88219756700
171.475006
0.000000
31.9879411410
120.88219862800
187.455002
0.000000
31.9879319230
120.88219547900
219.893005
0.000000
31.9879262970
120.88218662800
260.657990
0.000000
31.9879270460
120.88217538100
301.605988
0.000000
31.9879339290
120.88216759700
339.027008
0.000000
31.9879432510
120.88216370600
341.308014
0.000000
31.9879525740
120.88216011100
347.223999
0.000000
31.9879621530
120.88215731800
346.131989
0.000000
31.9879715330
120.88215432700
344.334991
0.000000
31.9879810120
120.88215106000
343.712006
0.000000
31.9879904470
120.88214768300
342.459015
0.000000
31.9879996950
120.88214397500
341.450012
0.000000
31.9880090700
120.88214064300
345.088989
0.000000
31.9880182980
120.88213779400
349.161987
0.000000
31.9880281360
120.88213555000
349.852997
0.000000
31.9880378250
120.88213375200
352.760010
0.000000
31.9880475390
120.88213188700
350.787994
0.000000
31.9880572130
120.88212994500
349.352997
0.000000
31.9880667950
120.88212768400
348.791992
0.000000
31.9880764180
120.88212537000
348.196991
0.000000
31.9880859860
120.88212281600
346.270996
0.000000
31.9880956090
120.88211992300
345.720001
0.000000
31.9881050830
120.88211735900
349.204010
0.000000
31.9881146740
120.88211501700
349.029999
0.000000
31.9881242960
120.88211280500
350.003998
0.000000
31.9881340140
120.88211095100
353.240997
0.000000
31.9881437640
120.88210981900
357.322998
0.000000
31.9881534870
120.88210931800
358.148987
0.000000
31.9881633920
120.88210911800
0.152000
0.000000
31.9881732700
120.88210887700
0.063000
0.000000
31.9881830360
120.88210844500
355.510010
0.000000
31.9881927980
120.88210691200
350.351013
0.000000
31.9882023020
120.88210477800
348.263000
0.000000
31.9882119240
120.88210224200
347.200989
0.000000
31.9882214190
120.88209921900
346.522003
0.000000
31.9882308970
120.88209646800
348.588013
0.000000
31.9882404990
120.88209433600
350.572998
0.000000
31.9882502240
120.88209228900
349.985992
0.000000
31.9882597990
120.88209050700
352.312012
0.000000
31.9882695400
120.88208887300
350.029999
0.000000
31.9882792540
120.88208668000
348.798004
0.000000
31.9882887720
120.88208441000
348.540985
0.000000
31.9882984170
120.88208188100
350.110992
0.000000
31.9883081300
120.88207984700
351.752991
0.000000
31.9883177650
120.88207795800
350.993011
0.000000
31.9883273900
120.88207594500
349.037994
0.000000
31.9883367970
120.88207375700
348.467010
0.000000
31.9883460890
120.88207118600
347.402008
0.000000
31.9883553180
120.88206934600
350.721985
0.000000
31.9883647210
120.88206767100
349.246002
0.000000
31.9883746440
120.88206539400
347.968994
0.000000
31.9883841870
120.88206269400
340.122009
0.000000
31.9883921840
120.88205689800
298.937988
0.000000
31.9883952490
120.88204578900
258.208008
0.000000
31.9883910650
120.88203567000
216.893997
0.000000
31.9883821860
120.88203123100
175.807999
0.000000
31.9883724170
120.88203309700
167.957001
0.000000
31.9883628910
120.88203525600
169.238998
0.000000
31.9883532670
120.88203774100
170.182999
0.000000
31.9883435020
120.88203957400
170.679001
0.000000
31.9883338060
120.88204146500
170.710007
0.000000
31.9883241930
120.88204346300
169.500000
0.000000
31.9883146420
120.88204580800
167.207001
0.000000
31.9883051910
120.88204833500
166.988007
0.000000
31.9882955130
120.88205124100
168.501007
0.000000
31.9882858540
120.88205345100
169.529007
0.000000
31.9882761930
120.88205569800
167.968002
0.000000
31.9882666120
120.88205825000
167.612000
0.000000
31.9882569450
120.88206045100
171.028000
0.000000
31.9882472790
120.88206257900
172.533997
0.000000
31.9882375730
120.88206439500
173.695007
0.000000
31.9882278280
120.88206594700
170.684006
0.000000
31.9882182390
120.88206787500
169.761002
0.000000
31.9882085150
120.88207011400
168.636993
0.000000
31.9881989580
120.88207240000
170.201004
0.000000
31.9881891160
120.88207402400
168.300995
0.000000
31.9881794050
120.88207622500
170.130997
0.000000
31.9881699450
120.88207853900
167.714005
0.000000
31.9881602910
120.88208073200
167.529007
0.000000
31.9881506510
120.88208307900
170.263000
0.000000
31.9881410470
120.88208508100
171.789001
0.000000
31.9881313270
120.88208698100
168.533997
0.000000
31.9881216550
120.88208929600
169.335999
0.000000
31.9881121080
120.88209156300
170.804993
0.000000
31.9881024870
120.88209345300
173.037003
0.000000
31.9880926520
120.88209483100
172.324997
0.000000
31.9880831300
120.88209684500
170.819000
0.000000
31.9880732620
120.88209882100
169.332993
0.000000
31.9880637200
120.88210108000
169.376999
0.000000
31.9880540270
120.88210300100
171.067993
0.000000
31.9880442310
120.88210495300
169.697006
0.000000
31.9880345920
120.88210690500
168.873001
0.000000
31.9880249600
120.88210955400
169.779999
0.000000
31.9880153110
120.88211180000
170.268997
0.000000
31.9880055020
120.88211348900
169.738007
0.000000
31.9879959960
120.88211580700
169.112000
0.000000
31.9879864840
120.88211812600
170.082993
0.000000
31.9879767210
120.88212015600
171.987000
0.000000
31.9879670680
120.88212189500
168.899994
0.000000
31.9879573560
120.88212420600
168.216995
0.000000
31.9879477520
120.88212683000
167.970993
0.000000
31.9879382640
120.88212984200
166.725006
0.000000
31.9879286950
120.88213173000
188.231995
0.000000
31.9879196550
120.88212763800
228.044006
0.000000
31.9879149710
120.88211785700
267.828003
0.000000
31.9879170470
120.88210708700
308.385010
0.000000
31.9879245580
120.88210031200
339.127991
0.000000
31.9879337260
120.88209646700
337.468994
0.000000
31.9879427730
120.88209155900
341.226013
0.000000
31.9879522040
120.88208809700
345.983002
0.000000
31.9879615850
120.88208482600
345.602997
0.000000
31.9879712110
120.88208217400
345.144012
0.000000
31.9879808570
120.88207947000
345.937988
0.000000
31.9879902450
120.88207648000
346.337006
0.000000
31.9879999310
120.88207384300
351.003998
0.000000
31.9880095240
120.88207176000
350.786987
0.000000
31.9880189820
120.88206913300
348.781006
0.000000
31.9880285660
120.88206661800
346.785004
0.000000
31.9880380240
120.88206401100
346.201996
0.000000
31.9880475470
120.88206040700
350.578003
0.000000
31.9880571750
120.88205778000
350.246002
0.000000
31.9880669010
120.88205548100
350.858002
0.000000
31.9880765100
120.88205294300
350.916992
0.000000
31.9880860540
120.88205073200
355.056000
0.000000
31.9880956490
120.88204881400
356.011993
0.000000
31.9881053170
120.88204716900
356.520996
0.000000
31.9881147300
120.88204490400
352.306000
0.000000
31.9881243840
120.88204282900
350.395996
0.000000
31.9881338210
120.88204132200
351.488007
0.000000
31.9881436550
120.88203971000
351.765991
0.000000
31.9881529420
120.88203756800
351.317993
0.000000
31.9881624090
120.88203530600
350.472992
0.000000
31.9881717490
120.88203263900
353.013000
0.000000
31.9881812950
120.88202994100
352.364014
0.000000
31.9881909930
120.88202757300
354.285004
0.000000
31.9882004920
120.88202572100
354.549988
0.000000
31.9882101590
120.88202426200
356.778015
0.000000
31.9882196460
120.88202284000
351.897003
0.000000
31.9882291650
120.88202099300
353.246002
0.000000
31.9882385610
120.88201855200
350.131012
0.000000
31.9882481360
120.88201665000
349.195007
0.000000
31.9882570020
120.88201440800
348.510010
0.000000
31.9882674120
120.88201230500
347.377014
0.000000
31.9882761340
120.88200853300
346.194000
0.000000
31.9882853660
120.88200533600
347.186005
0.000000
31.9882951560
120.88200193000
349.088989
0.000000
31.9883043820
120.88200455900
349.721985
0.000000
31.9883117810
120.88199700700
349.675995
0.000000
31.9883216020
120.88199384800
349.923004
0.000000
31.9883310970
120.88199139800
351.084991
0.000000
31.9883408990
120.88198852300
352.026001
0.000000
31.9883503510
120.88198616900
352.084015
0.000000
31.9883600230
120.88198350800
352.751007
0.000000
31.9883698850
120.88198085700
357.583008
0.000000
31.9883795170
120.88198340300
22.377001
0.000000
31.9883869500
120.88199167900
63.713001
0.000000

2348
routes/gps_load_1776035482630.txt Normal file
View File

File diff suppressed because it is too large Load Diff

3
src/autonomy/route/src/route_node.cpp
View File

@ -221,6 +221,7 @@ void* route_thread(void* args)
while (1) while (1)
{ {
double distance = ntzx_GPS_length(last_g_rtk.lon, last_g_rtk.lat, g_rtk.lon, g_rtk.lat); double distance = ntzx_GPS_length(last_g_rtk.lon, last_g_rtk.lat, g_rtk.lon, g_rtk.lat);
LOG_INFO_THROTTLE(1000, "distance : %f",distance);
if (distance >= 1.0) // 每隔1米采样一次 if (distance >= 1.0) // 每隔1米采样一次
{ {
fprintf(fp_routes, "%.10lf\n%.11lf\n%lf\n%lf\n", g_rtk.lat, g_rtk.lon, g_rtk.head, 0.0); fprintf(fp_routes, "%.10lf\n%.11lf\n%lf\n%lf\n", g_rtk.lat, g_rtk.lon, g_rtk.head, 0.0);
@ -229,7 +230,7 @@ void* route_thread(void* args)
last_g_rtk.lat = g_rtk.lat; last_g_rtk.lat = g_rtk.lat;
last_g_rtk.lon = g_rtk.lon; last_g_rtk.lon = g_rtk.lon;
last_g_rtk.head = g_rtk.head; last_g_rtk.head = g_rtk.head;
LOG_DEBUG("hit!"); LOG_INFO("hit!");
} }
} }
} }

6
src/perception/rslidar_pointcloud_merger/CMakeLists.txt
View File

@ -23,6 +23,8 @@ find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED) find_package(tf2_ros REQUIRED)
find_package(tf2_sensor_msgs REQUIRED) find_package(tf2_sensor_msgs REQUIRED)
find_package(PCL REQUIRED COMPONENTS common io segmentation filters) find_package(PCL REQUIRED COMPONENTS common io segmentation filters)
find_package(mc REQUIRED)
find_package(logger REQUIRED)
# GPU加速相关配置 # GPU加速相关配置
if(USE_GPU) if(USE_GPU)
@ -63,7 +65,9 @@ ament_target_dependencies(
pcl_ros pcl_ros
tf2 tf2
tf2_ros tf2_ros
tf2_sensor_msgs) tf2_sensor_msgs
mc
logger)
target_link_libraries(merge_two_lidars ${PCL_LIBRARIES}) target_link_libraries(merge_two_lidars ${PCL_LIBRARIES})

6
src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp
View File

@ -61,6 +61,7 @@
#include <cstring> /* strerror */ #include <cstring> /* strerror */
#endif #endif
#include "logger/logger.h"
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp> #include <sensor_msgs/msg/point_cloud2.hpp>
#include <std_msgs/msg/int32_multi_array.hpp> #include <std_msgs/msg/int32_multi_array.hpp>
@ -1141,8 +1142,7 @@ class LidarMerger : public rclcpp::Node
last_stats_time_ = t; last_stats_time_ = t;
int f = front_rx_.load(), r = rear_rx_.load(), m = merged_count_.load(); int f = front_rx_.load(), r = rear_rx_.load(), m = merged_count_.load();
double merge_rate = (f > 0) ? m * 100.0 / std::min(f, r) : 0.0; double merge_rate = (f > 0) ? m * 100.0 / std::min(f, r) : 0.0;
RCLCPP_INFO(get_logger(), LOG_INFO("Stats | rx(F=%d,R=%d) merged=%d(%.0f%%) | queue(raw_F=%zu,raw_R=%zu,proc=%zu) | sor_method=%d", f,
"Stats | rx(F=%d,R=%d) merged=%d(%.0f%%) | queue(raw_F=%zu,raw_R=%zu,proc=%zu) | sor_method=%d", f,
r, m, merge_rate, raw_queue_front_.size(), raw_queue_rear_.size(), processed_queue_.size(), r, m, merge_rate, raw_queue_front_.size(), raw_queue_rear_.size(), processed_queue_.size(),
sor_method_); sor_method_);
} }
@ -1211,6 +1211,7 @@ class LidarMerger : public rclcpp::Node
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
logger::Logger::Init("merge", "./nodes_log");
/* MultiThreadedExecutor：让订阅回调并发执行（前/后雷达回调互不阻塞） */ /* MultiThreadedExecutor：让订阅回调并发执行（前/后雷达回调互不阻塞） */
rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 2 /* 线程数=2，匹配两个订阅 */); rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 2 /* 线程数=2，匹配两个订阅 */);
@ -1220,5 +1221,6 @@ int main(int argc, char** argv)
executor.spin(); executor.spin();
rclcpp::shutdown(); rclcpp::shutdown();
logger::Logger::Shutdown();
return 0; return 0;
} }