lighthouse定位系统入门教程-生成航点和轨迹
说明：
- 介绍如何生成先上升后螺旋下降的航点，以及生成轨迹
步骤：
- 使用python脚本test.py绘制所需要的图像，并输出三维坐标点到文件spiral_coordinates.csv
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# 参数设置
height = 1.5 # 螺旋上升的总高度
num_points = 50 # 坐标点数量
radius = 0.3 # 螺旋半径的一半，宽度为0.5米
turns = 3 # 螺旋圈数
# 生成三维坐标点
z = np.linspace(0, height, num_points)
theta = np.linspace(0, 2 * np.pi * turns, num_points)
x = radius * np.cos(theta)
y = radius * np.sin(theta)
# 将坐标点写入文件
with open('spiral_coordinates.csv', 'w') as f:
for xi, yi, zi in zip(x, y, z):
f.write(f'{xi},{yi},{zi}\n')
# 绘制螺旋图像
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(x, y, z)
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_title('3D Spiral')
plt.show()
- 运行上面的脚本
python test.py
- 得到给定航点文件spiral_coordinates.csv
0.3,0.0,0.0
0.2780750272038065,0.11258010146381223,0.030612244897959183
0.21550480502931826,0.20870476518104591,0.061224489795918366
0.12143500293671819,0.2743237869047437,0.09183673469387754
0.009615473271496599,0.29984586486020637,0.12244897959183673
-0.10360951632639225,0.2815405266149281,0.15306122448979592
-0.20169026707839496,0.2220833991225947,0.18367346938775508
-0.2702906603707257,0.13016512173526745,0.21428571428571427
-0.29938361782510087,0.019221065994213967,0.24489795918367346
-0.28471672410320065,-0.09453246540708606,0.2755102040816326
-0.2284337875107404,-0.19446851859233646,0.30612244897959184
-0.13876148707225058,-0.26597979191189997,0.336734693877551
-0.02880690777230473,-0.2986137338847594,0.36734693877551017
0.0853582759893096,-0.28760035591099825,0.3979591836734694
0.18704694055762,-0.23454944474040895,0.42857142857142855
0.26139561123701666,-0.1472152656011816,0.45918367346938777
0.2975370041469738,-0.03836314850535199,0.4897959183673469
0.29018845891170886,0.0760963751728521,0.5204081632653061
0.24042408656038716,0.17943315914736452,0.5510204081632653
0.1555177704931577,0.25654282890160374,0.5816326530612245
0.04787996851001393,0.29615453502433503,0.6122448979591837
-0.06675628018689422,0.2924783736545471,0.6428571428571428
-0.17163499803665086,0.2460516763790868,0.673469387755102
-0.25142643146755195,0.163660470363165,0.7040816326530612
-0.29446774709731954,0.057347588610412094,0.7346938775510203
-0.29446774709731965,-0.05734758861041135,0.7653061224489796
-0.25142643146755234,-0.16366047036316436,0.7959183673469388
-0.17163499803665105,-0.24605167637908665,0.826530612244898
-0.06675628018689445,-0.29247837365454704,0.8571428571428571
0.047879968510013186,-0.2961545350243351,0.8877551020408163
0.15551777049315707,-0.2565428289016041,0.9183673469387755
0.24042408656038672,-0.17943315914736513,0.9489795918367346
0.29018845891170875,-0.07609637517285257,0.9795918367346939
0.29753700414697387,0.0383631485053515,1.010204081632653
0.26139561123701693,0.14721526560118117,1.0408163265306123
0.18704694055762017,0.23454944474040884,1.0714285714285714
0.08535827598931033,0.287600355910998,1.1020408163265305
-0.02880690777230398,0.2986137338847595,1.1326530612244898
-0.13876148707225017,0.26597979191190024,1.163265306122449
-0.22843378751074006,0.19446851859233683,1.193877551020408
-0.2847167241032005,0.09453246540708653,1.2244897959183674
-0.2993836178251009,-0.019221065994213613,1.2551020408163265
-0.2702906603707258,-0.13016512173526726,1.2857142857142856
-0.2016902670783955,-0.2220833991225942,1.316326530612245
-0.10360951632639233,-0.28154052661492807,1.346938775510204
0.009615473271496046,-0.29984586486020637,1.3775510204081631
0.12143500293671726,-0.2743237869047441,1.4081632653061225
0.21550480502931801,-0.20870476518104616,1.4387755102040816
0.2780750272038062,-0.112580101463813,1.4693877551020407
0.3,-2.2043642384652356e-16,1.5
- 进入生成轨迹目录
cd uav_trajectories/build
- 生成给定航点的轨迹traj1.csv
./genTrajectory -i ~/spiral_coordinates.csv --v_max 2.0 --a_max 2.0 -o traj1.csv
- 轨迹文件traj1.csv
Duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7
0.786342,0.3,0,0,0,-1.11135,3.1923,-3.26651,1.15904,0,0,0,0,6.23953,-17.9855,18.3848,-6.52026,0,0,0,0,1.684,-4.85099,4.95741,-1.75792,0,0,0,0,0,0,0,0
0.684072,0.278075,-0.0330961,-0.0172929,-0.0114288,-5.07938,17.8977,-21.8174,9.11204,0.11258,0.126464,-0.0291117,-0.0554396,6.89893,-24.7159,30.6683,-12.9819,0.0306122,0.0350985,-0.00667775,-0.0145256,2.27008,-8.06658,9.95141,-4.19572,0,0,0,0,0,0,0,0
0.694974,0.215505,-0.0535261,-0.00854438,-0.000208951,-7.51326,25.8198,-30.8629,12.6609,0.208705,0.0334023,0.00688339,0.0124935,5.59801,-19.3081,23.077,-9.46119,0.0612245,0.014781,0.00376678,0.0034913,2.53859,-8.71481,10.3975,-4.25885,0,0,0,0,0,0,0,0
0.737515,0.121435,-0.073279,-0.00382069,0.00132063,-6.56399,21.3352,-24.0893,9.32751,0.274324,0.0423107,-0.0156173,-0.00687492,1.2273,-4.19817,4.90322,-1.94254,0.0918367,0.0237535,-0.00159932,-0.00172655,1.73797,-5.67022,6.42244,-2.49257,0,0,0,0,0,0,0,0
0.686753,0.00961547,-0.0789896,-0.000522736,0.000732562,-9.43905,33.0252,-40.0981,16.6884,0.299846,-0.00184763,-0.00787457,0.00267592,-1.54844,5.23565,-6.23596,2.56292,0.122449,0.0198805,0.000693815,0.000718448,2.56968,-8.97038,10.8747,-4.52115,0,0,0,0,0,0,0,0
0.689558,-0.10361,-0.0745961,0.00370387,0.000951814,-7.95924,27.8189,-33.7017,13.9866,0.281541,-0.0255118,-0.0109705,-0.000854565,-5.01302,17.2538,-20.7039,8.53709,0.153061,0.0215486,-0.000298341,-0.000311764,2.48078,-8.63846,10.4439,-4.32869,0,0,0,0,0,0,0,0
0.686753,-0.20169,-0.0586781,0.0070197,0.000648726,-5.62832,19.8465,-24.2097,10.1071,0.222083,-0.0541567,-0.00728356,0.00112638,-7.72831,26.8907,-32.5441,13.5155,0.183673,0.0208303,0.000128504,0.000133989,2.54757,-8.90117,10.7991,-4.49225,0,0,0,0,0,0,0,0
0.689558,-0.270291,-0.0344508,0.00946839,0.000416801,-2.2618,8.07955,-9.91412,4.14917,0.130165,-0.0711354,-0.00476093,0.000615969,-9.11728,31.6821,-38.2473,15.8358,0.214286,0.0211396,-5.53262e-05,-5.77174e-05,2.49014,-8.66768,10.4757,-4.34079,0,0,0,0,0,0,0,0
0.686753,-0.299384,-0.00507279,0.0104647,5.25875e-05,1.34724,-4.49418,5.29687,-2.16022,0.0192211,-0.0793313,-0.000582557,0.00101658,-9.47631,33.1525,-40.2517,16.7522,0.244898,0.0210064,2.38226e-05,2.48492e-05,2.54347,-8.88834,10.7851,-4.48689,0,0,0,0,0,0,0,0
0.689558,-0.284717,0.0249971,0.00996085,-0.000288574,4.71208,-16.2184,19.4662,-8.02851,-0.0945325,-0.0752363,0.00326769,0.000837499,-8.11901,28.3675,-34.3588,14.2571,0.27551,0.0210638,-1.02574e-05,-1.06997e-05,2.49188,-8.67309,10.4816,-4.34304,0,0,0,0,0,0,0,0
0.686753,-0.228434,0.0514346,0.00798839,-0.00060079,7.56287,-26.3001,31.8162,-13.2094,-0.194469,-0.0604437,0.00681826,0.000721617,-5.87174,20.6938,-25.2356,10.5333,0.306122,0.0210391,4.41657e-06,4.607e-06,2.54271,-8.88596,10.7825,-4.4859,0,0,0,0,0,0,0,0
0.689558,-0.138761,0.0703449,0.00485376,-0.000819492,9.03208,-31.3818,37.8826,-15.6843,-0.26598,-0.0366873,0.0092956,0.000420336,-2.55345,9.0927,-11.1369,4.65537,0.336735,0.0210497,-1.90167e-06,-1.98366e-06,2.4922,-8.6741,10.4827,-4.34345,0,0,0,0,0,0,0,0
0.689558,-0.0288069,0.0789771,0.00100731,-0.000920865,9.33008,-32.5001,39.2929,-16.2849,-0.298614,-0.00762398,0.0104472,9.2029e-05,1.02297,-3.34953,3.89435,-1.57114,0.367347,0.0210451,8.1881e-07,8.54116e-07,2.4923,-8.67443,10.4831,-4.34359,0,0,0,0,0,0,0,0
0.686753,0.0853583,0.0760638,-0.00298535,-0.000886583,8.43114,-29.5705,35.9566,-14.9795,-0.2876,0.0225776,0.0100576,-0.000264547,4.53853,-15.6717,18.8772,-7.81474,0.397959,0.0210471,-3.5256e-07,-3.67761e-07,2.54252,-8.88537,10.7819,-4.48565,0,0,0,0,0,0,0,0
0.673508,0.187047,0.0620332,-0.00654209,-0.000723166,6.72237,-24.1408,30.0057,-12.7671,-0.234549,0.0494688,0.00820404,-0.000576076,8.10309,-28.7257,35.4288,-14.9971,0.428571,0.0210462,1.51803e-07,1.58349e-07,2.79595,-9.96317,12.3275,-5.22954,0,0,0,0,0,0,0,0
0.689736,0.261396,0.0389352,-0.00914241,-0.000453851,2.83775,-10.0776,12.3225,-5.14485,-0.147215,0.0691338,0.00514867,-0.000806148,8.93431,-31.0271,37.4396,-15.4955,0.459184,0.0210466,-6.53628e-08,-6.81811e-08,2.48913,-8.66115,10.4643,-4.33472,0,0,0,0,0,0,0,0
0.672864,0.297537,0.0101462,-0.0104065,-0.000118283,-0.811784,2.65698,-3.11282,1.27141,-0.0383631,0.0786918,0.00134187,-0.000917206,10.5463,-37.6396,46.6294,-19.8032,0.489796,0.0210465,2.81436e-08,2.93571e-08,2.80897,-10.0191,12.4086,-5.26898,0,0,0,0,0,0,0,0
0.696221,0.290188,-0.0201258,-0.0101495,0.000234611,-3.99052,13.5762,-16.1191,6.57897,0.0760964,0.0767485,-0.00266155,-0.000894708,8.01509,-27.7231,33.2474,-13.6614,0.520408,0.0210465,-1.2118e-08,-1.26404e-08,2.37734,-8.19513,9.80907,-4.02543,0,0,0,0,0,0,0,0
0.672864,0.240424,-0.0474561,-0.00840894,0.000553197,-7.92005,28.0939,-34.6753,14.69,0.179433,0.0635868,-0.00627574,-0.000741215,7.01121,-25.19,31.3306,-13.3411,0.55102,0.0210465,5.21778e-09,5.44257e-09,2.80897,-10.0191,12.4086,-5.26898,0,0,0,0,0,0,0,0
0.696221,0.155518,-0.0678499,-0.0054393,0.000790931,-8.44255,29.0389,-34.7088,14.23,0.256543,0.041131,-0.00897271,-0.000479475,2.9815,-10.4701,12.6695,-5.23676,0.581633,0.0210465,-2.24682e-09,-2.34326e-09,2.37734,-8.19513,9.80907,-4.02543,0,0,0,0,0,0,0,0
0.696221,0.04788,-0.0783264,-0.00167462,0.000913054,-8.9444,30.8457,-36.9266,15.1552,0.296155,0.0126632,-0.0103581,-0.000147612,-0.404607,1.19238,-1.28146,0.485997,0.612245,0.0210465,9.67826e-10,1.00853e-09,2.37734,-8.19513,9.80907,-4.02543,0,0,0,0,0,0,0,0
0.672864,-0.0667563,-0.0773541,0.00233483,0.00090172,-9.61704,34.406,-42.6851,18.1456,0.292478,-0.0176556,-0.0102296,0.00020581,-4.40417,15.4929,-19.0254,8.0325,0.642857,0.0210465,-4.17653e-10,-4.33279e-10,2.80897,-10.0191,12.4086,-5.26898,0,0,0,0,0,0,0,0
0.731762,-0.171635,-0.0650753,0.00600301,0.000758585,-4.79602,15.847,-18.1254,7.09727,0.246052,-0.0453937,-0.00860577,0.000529157,-5.0912,16.5915,-18.8198,7.32851,0.673469,0.0210465,1.81993e-10,1.84303e-10,1.85674,-6.08966,6.93492,-2.70772,0,0,0,0,0,0,0,0
0.687374,-0.251426,-0.0432846,0.00879375,0.000504571,-3.46236,12.2915,-15.0479,6.29523,0.16366,-0.0664968,-0.0057241,0.000775156,-8.88185,30.9365,-37.4481,15.5493,0.704082,0.0210465,-8.3383e-11,-7.41207e-11,2.5313,-8.83816,10.7149,-4.45375,0,0,0,0,0,0,0,0
0.731762,-0.294468,-0.0151672,0.0102991,0.000176804,0.0966443,-0.158683,0.0722835,3.55271e-14,0.0573476,-0.0778802,-0.00200576,0.000907854,-6.99377,22.943,-26.1287,10.2019,0.734694,0.0210465,4.75654e-11,1.97501e-11,1.85674,-6.08966,6.93492,-2.70772,0,0,0,0,0,0,0,0
0.687374,-0.294468,0.0151672,0.0102991,-0.000176804,3.69176,-12.6924,15.2423,-6.29523,-0.0573476,-0.0778802,0.00200576,0.000907854,-8.78897,30.7742,-37.3694,15.5493,0.765306,0.0210465,-4.75646e-11,1.97484e-11,2.5313,-8.83816,10.7149,-4.45375,0,0,0,0,0,0,0,0
0.731762,-0.251426,0.0432846,0.00879375,-0.000504571,4.93487,-16.075,18.2292,-7.09727,-0.16366,-0.0664968,0.0057241,0.000775156,-4.95674,16.3707,-18.7193,7.32851,0.795918,0.0210465,8.3383e-11,-7.41169e-11,1.85674,-6.08966,6.93492,-2.70772,0,0,0,0,0,0,0,0
0.687374,-0.171635,0.0650753,0.00600301,-0.000758585,8.76631,-30.5267,36.9466,-15.3396,-0.246052,-0.0453937,0.00860577,0.000529157,-3.74526,13.2768,-16.2405,6.79038,0.826531,0.0210465,-1.81993e-10,1.84302e-10,2.5313,-8.83816,10.7149,-4.45375,0,0,0,0,0,0,0,0
0.673508,-0.0667563,0.0773541,0.00233483,-0.00090172,10.5266,-37.5102,46.4076,-19.6853,-0.292478,-0.0176556,0.0102296,0.00020581,-0.203582,0.963923,-1.36994,0.63127,0.857143,0.0210465,4.17653e-10,-4.33278e-10,2.79594,-9.96315,12.3274,-5.22953,0,0,0,0,0,0,0,0
0.689736,0.04788,0.0783264,-0.00167462,-0.000913054,8.75444,-30.5413,36.9545,-15.3226,-0.296155,0.0126632,0.0103581,-0.000147612,3.35182,-11.4661,13.7091,-5.63886,0.887755,0.0210465,-9.67827e-10,1.00853e-09,2.48913,-8.66116,10.4643,-4.33472,0,0,0,0,0,0,0,0
0.673508,0.155518,0.0678499,-0.0054393,-0.000790931,7.70337,-27.616,34.2899,-14.5801,-0.256543,0.041131,0.00897271,-0.000479475,7.1769,-25.4027,31.3008,-13.2413,0.918367,0.0210465,2.24682e-09,-2.34326e-09,2.79594,-9.96315,12.3274,-5.22953,0,0,0,0,0,0,0,0
0.689736,0.240424,0.0474561,-0.00840894,-0.000553197,3.95694,-13.9626,17.009,-7.08413,-0.179433,0.0635868,0.00627574,-0.000741215,8.49809,-29.4837,35.5565,-14.7103,0.94898,0.0210465,-5.21778e-09,5.44257e-09,2.48913,-8.66116,10.4643,-4.33472,0,0,0,0,0,0,0,0
0.675452,0.290188,0.0201258,-0.0101495,-0.000234611,0.532966,-2.12874,2.8004,-1.23362,-0.0760964,0.0767485,0.00266155,-0.000894708,10.3684,-36.8326,45.4325,-19.2147,0.979592,0.0210465,1.2118e-08,-1.26404e-08,2.75701,-9.79614,12.0859,-5.11231,0,0,0,0,0,0,0,0
0.671776,0.297537,-0.0101462,-0.0104065,0.000118283,-3.48744,12.2325,-15.004,6.33295,0.0383631,0.0786918,-0.00134187,-0.000917206,10.0744,-36.0748,44.8089,-19.0738,1.0102,0.0210465,-2.81436e-08,2.93571e-08,2.83113,-10.1145,12.547,-5.33639,0,0,0,0,0,0,0,0
0.673508,0.261396,-0.0389352,-0.00914241,0.000453851,-6.9263,24.5045,-30.1857,12.7671,0.147215,0.0691338,-0.00514867,-0.000806148,7.92946,-28.4161,35.2756,-14.9971,1.04082,0.0210466,6.53628e-08,-6.81811e-08,2.79594,-9.96316,12.3275,-5.22954,0,0,0,0,0,0,0,0
0.689736,0.187047,-0.0620332,-0.00654209,0.000723166,-8.3669,29.021,-34.9931,14.4757,0.234549,0.0494688,-0.00820404,-0.000576076,4.22727,-14.9003,18.1398,-7.55192,1.07143,0.0210462,-1.51803e-07,1.58349e-07,2.48912,-8.66113,10.4643,-4.3347,0,0,0,0,0,0,0,0
0.671776,0.0853583,-0.0760638,-0.00298535,0.000886583,-10.6238,37.9387,-47.0472,20.0048,0.2876,0.0225776,-0.0100576,-0.000264547,0.889035,-3.41722,4.41843,-1.93003,1.10204,0.0210471,3.5256e-07,-3.67761e-07,2.83115,-10.1146,12.5471,-5.33644,0,0,0,0,0,0,0,0
0.675452,-0.0288069,-0.0789771,0.00100731,0.000920865,-9.91445,35.3015,-43.6042,18.4584,0.298614,-0.00762398,-0.0104472,9.2029e-05,-3.08081,10.7235,-13.0636,5.47877,1.13265,0.0210451,-8.1881e-07,8.54116e-07,2.75698,-9.79598,12.0857,-5.1122,0,0,0,0,0,0,0,0
0.671776,-0.138761,-0.0703449,0.00485376,0.000819492,-8.25003,29.6303,-36.8697,15.7129,0.26598,-0.0366873,-0.0092956,0.000420336,-6.75192,23.9373,-29.5541,12.5297,1.16327,0.0210497,1.90167e-06,-1.98366e-06,2.8312,-10.1149,12.5476,-5.33666,0,0,0,0,0,0,0,0
0.675452,-0.228434,-0.0514346,0.00798839,0.00060079,-4.97889,17.8992,-22.2358,9.44855,0.194469,-0.0604437,-0.00681826,0.000721617,-9.11093,32.2647,-39.723,16.7788,1.19388,0.0210391,-4.41657e-06,4.607e-06,2.75685,-9.79527,12.0846,-5.11172,0,0,0,0,0,0,0,0
0.671776,-0.284717,-0.0249971,0.00996085,0.000288574,-1.22894,4.63064,-5.92289,2.56966,0.0945325,-0.0752363,-0.00326769,0.000837499,-10.5884,37.802,-46.87,19.9274,1.22449,0.0210638,1.02574e-05,-1.06997e-05,2.83151,-10.1166,12.5501,-5.33781,0,0,0,0,0,0,0,0
0.675452,-0.299384,0.00507279,0.0104647,-5.25875e-05,2.76111,-9.58471,11.6566,-4.88307,-0.0192211,-0.0793313,0.000582557,0.00101658,-10.0113,35.6445,-44.0261,18.6365,1.2551,0.0210064,-2.38226e-05,2.48492e-05,2.75614,-9.79147,12.0791,-5.10914,0,0,0,0,0,0,0,0
0.671776,-0.270291,0.0344508,0.00946839,-0.000416801,6.48484,-22.9798,28.3637,-12.0227,-0.130165,-0.0711354,0.00476093,0.000615969,-8.45451,30.3406,-37.7366,16.0778,1.28571,0.0211396,5.53262e-05,-5.77174e-05,2.83318,-10.1257,12.5634,-5.34404,0,0,0,0,0,0,0,0
0.675452,-0.20169,0.0586781,0.0070197,-0.000648726,8.94481,-31.6644,38.9752,-16.4605,-0.222083,-0.0541567,0.00728356,0.00112638,-5.28581,19.0185,-23.6359,10.0457,1.31633,0.0208303,-0.000128504,0.000133989,2.75235,-9.77099,12.049,-5.0952,0,0,0,0,0,0,0,0
0.687374,-0.10361,0.0745961,0.00370387,-0.000951814,9.43231,-32.9104,39.8784,-16.5698,-0.281541,-0.0255118,0.0109705,-0.000854565,-1.36132,4.89743,-6.0465,2.54456,1.34694,0.0215486,0.000298341,-0.000311764,2.54135,-8.89177,10.7922,-4.48901,0,0,0,0,0,0,0,0
0.731762,0.00961547,0.0789896,-0.000522736,-0.000732562,6.79215,-22.308,25.4243,-9.93185,-0.299846,-0.00184763,0.00787457,0.00267592,1.58348,-4.91721,5.41805,-2.07003,1.37755,0.0198805,-0.000693815,0.000718448,1.83895,-5.99863,6.81101,-2.65458,0,0,0,0,0,0,0,0
0.689825,0.121435,0.073279,-0.00382069,-0.00132063,7.62722,-26.689,32.3482,-13.4269,-0.274324,0.0423107,0.0156173,-0.00687492,5.65946,-19.8887,24.1463,-10.0284,1.40816,0.0237535,0.00159932,-0.00172655,2.54069,-8.93739,10.8619,-4.51535,0,0,0,0,0,0,0,0
0.680308,0.215505,0.0535261,-0.00854438,0.000208951,5.33559,-18.9517,23.3123,-9.81916,-0.208705,0.0334023,-0.00688339,0.0124935,8.03132,-27.2961,32.7335,-13.568,1.43878,0.014781,-0.00376678,0.0034913,2.54533,-8.73544,10.5376,-4.38474,0,0,0,0,0,0,0,0
0.763615,0.278075,0.0333043,-0.00864645,-0.00571439,0.939185,-3.1327,3.55591,-1.36656,-0.11258,0.126464,0.0291117,-0.0554396,5.69945,-18.9481,21.4269,-8.20807,1.46939,0.0350985,0.00667775,-0.0145256,1.53619,-5.10814,5.7774,-2.2135,0,0,0,0,0,0,0,0
- 新建脚本文件spiral_coordinates.py
#!/usr/bin/env python
import numpy as np
from pycrazyswarm import *
import uav_trajectory
if __name__ == "__main__":
swarm = Crazyswarm()
timeHelper = swarm.timeHelper
allcfs = swarm.allcfs
traj1 = uav_trajectory.Trajectory()
traj1.loadcsv("traj1.csv")
TRIALS = 1
TIMESCALE = 1.0
for i in range(TRIALS):
for cf in allcfs.crazyflies:
cf.uploadTrajectory(0, 0, traj1)
allcfs.takeoff(targetHeight=0.2, duration=2.0)
timeHelper.sleep(2.5)
for cf in allcfs.crazyflies:
pos = np.array(cf.initialPosition) + np.array([0, 0, 0.2])
cf.goTo(pos, 0, 2.0)
timeHelper.sleep(2.5)
allcfs.startTrajectory(0, timescale=TIMESCALE)
timeHelper.sleep(traj1.duration * TIMESCALE + 2.0)
# allcfs.startTrajectory(0, timescale=TIMESCALE, reverse=True)
# timeHelper.sleep(traj1.duration * TIMESCALE + 2.0)
allcfs.land(targetHeight=0.06, duration=5.0)
timeHelper.sleep(3.0)
- 运行脚本，查看模拟效果
python3 spiral_coordinates.py --sim
- 通过choose.py配置无人机，打开choose.py，勾选使用5号无人机
python3 chooser.py
- 终端下启动launch文件
roslaunch crazyswarm hover_swarm.launch
启动后，会弹出rviz，并显示对应的位置
新终端，启动脚本
python3 spiral_coordinates.py
获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号