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

说明

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

仿真飞行：

roscd crazyswarm/scripts/

脚本代码

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()

运行脚本

python3 number.py --sim

启动后，无人机起飞0.5米，然后飞行数字1，2，3，4

真机飞行

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

python3 chooser.py

请输入图片描述

终端下启动launch文件

roslaunch crazyswarm hover_swarm.launch

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

python3 number.py

演示视频

标签: none