lighthouse定位系统入门教程-拼字母ABCD

说明

介绍如何实现7台无人机的拼字母ABCD
本文基于无人机多机套件主机测试
Crazyflie无人机集群套件，采购地址

仿真飞行：

roscd crazyswarm/scripts/

脚本代码

import numpy as np
from pycrazyswarm import Crazyswarm

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

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

def get_letter_C_coordinates():
    coordinates = [
        [1.0, 0, 1], [0, 0, 1], [0, 3, 1],
        [1, 3, 1], [1, 2, 1],
        [0, 2, 1], [0, 1, 1]
    ]
    return np.array(coordinates)

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

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

    coords_A = get_letter_A_coordinates()
    coords_B = get_letter_B_coordinates()
    coords_C = get_letter_C_coordinates()
    coords_D = get_letter_D_coordinates()
    

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

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

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

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

    for cf, coord in zip(allcfs.crazyflies, coords_D):
        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()

运行脚本

python3 letter.py --sim

启动后，无人机起飞0.5米，然后飞行字母A，B，C，D

真机飞行

1 2 3 4 5 6 7号无人机放置在基站测试区域中心
通过choose.py配置无人机，打开choose.py，勾选使用1号无人机

python3 chooser.py

请输入图片描述

终端下启动launch文件

roslaunch crazyswarm hover_swarm.launch

启动后，会弹出rviz，并显示对应的位置
新终端，启动脚本

python3 letter.py

演示视频

标签: none