lighthouse定位系统入门教程-拼数字1234

最新推荐文章于 2025-04-14 16:34:09 发布

创客智造

最新推荐文章于 2025-04-14 16:34:09 发布

阅读量337

点赞数 3

CC 4.0 BY-SA版权

分类专栏： lighthouse定位系统入门教程文章标签：无人机 crazyflie ros1 无人机编队 crazyswarm

本文链接：https://blog.csdn.net/ncnynl/article/details/139470718

lighthouse定位系统入门教程专栏收录该内容

20 篇文章

订阅专栏

import numpy as np
from pycrazyswarm import Crazyswarm

def get_letter_1_coordinates():
    coordinates = [
        [0.5, 0.5, 1], [1.5, 0.5, 1], [2.5, 0.5, 1],
        [1.5, 1.5, 1], [1.5, 2.5, 1],
        [1.5, 3, 1], [1, 2.5, 1]
    ]
    return np.array(coordinates)

def get_letter_2_coordinates():
    coordinates = [
        [1, 2.5, 1], [2, 2.5, 1], [1.7, 1.5, 1],
        [1.5, 1, 1], [1, 0.5, 1],
        [2, 0.5, 1], [2.5, 0.5, 1]
    ]
    return np.array(coordinates)

def get_letter_3_coordinates():
    coordinates = [
        [0.5, 0.5, 1], [1.5, 1, 1], [1.5, 0.5, 1],
        [0.5, 1.5, 1], [1.5, 2, 1],
        [0.5, 2.5, 1], [1.5, 2.5, 1]
    ]
    return np.array(coordinates)

def get_letter_4_coordinates():
    coordinates = [
        [1, 0, 1], [1, 0.5, 1], [1, 1, 1],
        [2, 1, 1], [0, 1, 1],
        [0.5, 1.5, 1], [1, 2, 1]
    ]
    return np.array(coordinates)

def main():
    swarm = Crazyswarm()
    timeHelper = swarm.timeHelper
    allcfs = swarm.allcfs

    coords_1 = get_letter_1_coordinates()
    coords_2 = get_letter_2_coordinates()
    coords_3 = get_letter_3_coordinates()
    coords_4 = get_letter_4_coordinates()
    

    allcfs.takeoff(targetHeight=1.0, duration=2.0)
    timeHelper.sleep(2.0)

    for cf, coord in zip(allcfs.crazyflies, coords_1):
        cf.goTo(goal=coord, yaw=0, duration=2.0)
    
    timeHelper.sleep(5.0)  

    for cf, coord in zip(allcfs.crazyflies, coords_2):
        cf.goTo(goal=coord, yaw=0, duration=2.0)
    
    timeHelper.sleep(5.0) 

    for cf, coord in zip(allcfs.crazyflies, coords_3):
        cf.goTo(goal=coord, yaw=0, duration=2.0)
    
    timeHelper.sleep(5.0)  

    for cf, coord in zip(allcfs.crazyflies, coords_4):
        cf.goTo(goal=coord, yaw=0, duration=2.0)
    
    timeHelper.sleep(5.0)  

    allcfs.land(targetHeight=0.0, duration=2.0)
    timeHelper.sleep(2.0)

if __name__ == "__main__":
    main()