Crash when overlapping commands in high level commander

[ItsVipra]

Hello Bitcraze community,

We've extensively been using a modified version of the synchronisedSequence.py script to fly with our swarm.
During our testing, we've discovered an issue using the high level commander, when overlapping commands are sent to the drone.

When the drone is sent a sufficiently long goto command (>~3s), while another one is currently being executed, the drone will fly along the inverse vector of the first command indefinitely (or at least long enough to crash in all our testing).

For example:

If the first command moves towards +x and -z the drone will move towards -x and +z once interrupted.
From logging data we know the drone's positioning data is unaffected, but the commander returns a setpoint ~20 m away from the current position, instead of the coordinates specified in the second command.

Of course an easy fix is to just not send overlapping commands, however this behavior seems unintended.
Below is a slightly modified synchronisedSequence.py you can use to recreate this behavior. Test at your own risk!

'''
this synchronized sequence file is unmodified except for the STEP_TIME constant, uris and sequence

the sequence consists of a takeoff, 
a short goto command, 
a long goto command, which is triggered before the first command finishes
and a land command for safety
'''
import threading
import time
from collections import namedtuple
from queue import Queue

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

# Time for one step in second
STEP_TIME = 0.1

# Possible commands, all times are in seconds
Takeoff = namedtuple('Takeoff', ['height', 'time'])
Land = namedtuple('Land', ['time'])
Goto = namedtuple('Goto', ['x', 'y', 'z', 'time'])
# RGB [0-255], Intensity [0.0-1.0]
Ring = namedtuple('Ring', ['r', 'g', 'b', 'intensity', 'time'])
# Reserved for the control loop, do not use in sequence
Quit = namedtuple('Quit', [])

uris = [
    'radio://0/80/2M/E7E7E7E700',  # cf_id 0
    # Customize URI according to your setup
]

sequence = [
    # Step, CF_id,  action
    (0,    0,      Takeoff(1, 2)),

    (20,    0,      Goto(0.1,  0,   1, 0.75)), #any goto command

    (25,    0,      Goto(0,  0.5,   1, 10)), #second long and overlapping goto
    #drone should drift away towards -x indefinitely

    (40,    0,      Land(2)), #land quickly for safety
]

def activate_high_level_commander(scf):
    scf.cf.param.set_value('commander.enHighLevel', '1')

def crazyflie_control(scf):
    cf = scf.cf
    control = controlQueues[uris.index(cf.link_uri)]

    commander = scf.cf.high_level_commander

    while True:
        command = control.get()
        if type(command) is Quit:
            return
        elif type(command) is Takeoff:
            commander.takeoff(command.height, command.time)
        elif type(command) is Land:
            commander.land(0.0, command.time)
        elif type(command) is Goto:
            commander.go_to(command.x, command.y, command.z, 0, command.time)
            pass
        else:
            print('Warning! unknown command {} for uri {}'.format(command,
                                                                  cf.uri))


def control_thread():
    pointer = 0
    step = 0
    stop = False

    while not stop:
        print('Step {}:'.format(step))
        while sequence[pointer][0] <= step:
            cf_id = sequence[pointer][1]
            command = sequence[pointer][2]

            print(' - Running: {} on {}'.format(command, cf_id))
            controlQueues[cf_id].put(command)
            pointer += 1

            if pointer >= len(sequence):
                print('Reaching the end of the sequence, stopping!')
                stop = True
                break

        step += 1
        time.sleep(STEP_TIME)

    for ctrl in controlQueues:
        ctrl.put(Quit())


if __name__ == '__main__':
    controlQueues = [Queue() for _ in range(len(uris))]

    cflib.crtp.init_drivers()
    factory = CachedCfFactory(rw_cache='./cache')
    with Swarm(uris, factory=factory) as swarm:
        swarm.parallel_safe(activate_high_level_commander)
        swarm.reset_estimators()

        print('Starting sequence!')

        threading.Thread(target=control_thread).start()

        swarm.parallel_safe(crazyflie_control)

        time.sleep(1)

[krichardsson]

This sounds like a bug. The intended behavior is to re-plan the trajectory from the current state whenever a new goto command is received.
We are using overlapping goto commands from time to time and have not seen this problem, but I will add this as an issue for further investigation.

Great that you supply a script for easy reproduction, thanks!

[krichardsson]

Issue bitcraze/crazyflie-firmware#1111