LOCO定位系统入门教程-使用python脚本移动到指定位置

说明：

• 介绍如何使用python脚本移动到指定位置

步骤：

• 函数go_to的参数

def go_to(self, x, y, z, yaw, duration_s, relative=False, group_mask=0)

x：X (m)
y：Y (m)
z：Z (m)
yaw：Yaw (radians)
duration_s：到达该位置所需的时间（s）
relative：如果 x、y、z 相对于当前位置，则为 True
group_mask：Mask for which CFs this should apply to

• 点击查看相关API

• 新建一个python脚本sbs_swarm.py

• 代码如下

import time

import cflib.crtp
from cflib.crazyflie.swarm import CachedCfFactory
from cflib.crazyflie.swarm import Swarm
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie


def activate_led_bit_mask(scf: SyncCrazyflie):
    scf.cf.param.set_value('led.bitmask', 255)


def deactivate_led_bit_mask(scf: SyncCrazyflie):
    scf.cf.param.set_value('led.bitmask', 0)


def light_check(scf: SyncCrazyflie):
    activate_led_bit_mask(scf)
    time.sleep(2)
    deactivate_led_bit_mask(scf)
    time.sleep(2)


def take_off(scf: SyncCrazyflie):
    commander = scf.cf.high_level_commander

    commander.takeoff(0.5, 5.0)
    time.sleep(10)


def land(scf: SyncCrazyflie):
    commander = scf.cf.high_level_commander

    commander.land(0.0, 2.0)
    time.sleep(2)

    commander.stop()


def run_square_sequence(scf: SyncCrazyflie):
    box_size = 1.0
    flight_time = 20.0

    commander = scf.cf.high_level_commander

    #commander.go_to(1.56, 1.56, 0.5, 0, flight_time, relative=False)
    commander.go_to(1.0, 0, 0.5, 0, flight_time, relative=False)
    time.sleep(flight_time)

    #commander.go_to(1.5, 0.5, 0, 0, flight_time, relative=True)
    #time.sleep(flight_time)

    #commander.go_to(1.5, 1.5, 0, 0, flight_time, relative=True)
    #time.sleep(flight_time)

    #commander.go_to(0.5, 1.5, 0, 0, flight_time, relative=True)
    #time.sleep(flight_time)


uris = [
    'radio://0/80/2M/E7E7E7E7E7',
    # Add more URIs if you want more copters in the swarm
]

# The layout of the positions (1m apart from each other):
#   <------ 1 m ----->
#   0                1
#          ^              ^
#          |Y             |
#          |              |
#          +------> X    1 m
#                         |
#                         |
#   3               2     .


h = 0.0  # remain constant height similar to take off height
x0, y0 = +0.5, +0.5
x1, y1 = -0.5, -0.5

#    x   y   z  time
sequence0 = [
    (x1, y0, h, 3.0),
    (x0, y1, h, 3.0),
    (x0,  0, h, 3.0),
]

sequence1 = [
    (x0, y0, h, 3.0),
    (x1, y1, h, 3.0),
    (.0, y1, h, 3.0),
]

sequence2 = [
    (x0, y1, h, 3.0),
    (x1, y0, h, 3.0),
    (x1,  0, h, 3.0),
]

sequence3 = [
    (x1, y1, h, 3.0),
    (x0, y0, h, 3.0),
    (.0, y0, h, 3.0),
]

seq_args = {
    uris[0]: [sequence0],
}


def run_sequence(scf: SyncCrazyflie, sequence):
    cf = scf.cf

    for arguments in sequence:
        commander = scf.cf.high_level_commander

        x, y, z = arguments[0], arguments[1], arguments[2]
        duration = arguments[3]

        print('Setting position {} to cf {}'.format((x, y, z), cf.link_uri))
        commander.go_to(x, y, z, 0, duration, relative=True)
        time.sleep(duration)


if __name__ == '__main__':
    cflib.crtp.init_drivers()
    factory = CachedCfFactory(rw_cache='./cache')
    with Swarm(uris, factory=factory) as swarm:
        print('Connected to  Crazyflies')
        #swarm.parallel_safe(light_check)
        #print('Light check done')

        swarm.reset_estimators()
        print('Estimators have been reset')

        swarm.parallel_safe(take_off)
        swarm.parallel_safe(run_square_sequence)
        # swarm.parallel_safe(run_sequence, args_dict=seq_args)
        swarm.parallel_safe(land)

• 运行

python3 sbs_swarm.py

• 结果如下

  先检查灯光，起飞到0.5米，然后飞行到（1.56, 1.56, 0.5）的位置，最后垂直着陆