Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Prototype Position High Level Commander
# -*- coding: utf-8 -*-
#
# || ____ _ __
# +------+ / __ )(_) /_______________ _____ ___
# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
#
# Copyright (C) 2018 Bitcraze AB
#
# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# as published by the Free Software Foundation; either version 2
# of the License, or (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
# MA 02110-1301, USA.
"""
The PositionHlCommander is used to make it easy to write scripts that moves the
Crazyflie around. Some sort of absolute positioning support is required, for
instance the Loco Positioning System. This implementation uses the High Level
Commander.
The API contains a set of primitives that are easy to understand and use, such
as "go forward" or "turn around".
The PositionHlCommander can be used as context manager using the with keyword.
In this mode of operation takeoff and landing is executed when the context is
created/closed.
"""
import math
import sys
import time
from threading import Thread
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
if sys.version_info < (3,):
from Queue import Queue, Empty
else:
from queue import Queue, Empty
class PositionHlCommander:
"""The position High Level Commander"""
VELOCITY = 0.2
RATE = 360.0 / 5
def __init__(self, crazyflie, default_height=0.3):
"""
Construct an instance of a PositionHlCommander
:param crazyflie: a Crazyflie or SyncCrazyflie instance
:param default_height: the default height to fly at
"""
if isinstance(crazyflie, SyncCrazyflie):
self._cf = crazyflie.cf
else:
self._cf = crazyflie
self.default_height = default_height
self._x = 0.0
self._y = 0.0
self._z = 0.0
self._is_flying = False
def take_off(self, height=None, velocity=VELOCITY):
"""
Takes off, that is starts the motors, goes straight up and hovers.
Do not call this function if you use the with keyword. Take off is
done automatically when the context is created.
:param height: the height (meters) to hover at. None uses the default
height set when constructed.
:param velocity: the velocity (meters/second) when taking off
:return:
"""
if self._is_flying:
raise Exception('Already flying')
if not self._cf.is_connected():
raise Exception('Crazyflie is not connected')
self._is_flying = True
self._reset_position_estimator()
self._activate_high_level_commander()
self._commander = self._cf.high_level_commander
if height is None:
height = self.default_height
duration_s = height / velocity
self._commander.takeoff(height, duration_s)
time.sleep(duration_s)
self._z = height
def land(self, velocity=VELOCITY):
"""
Go straight down and turn off the motors.
Do not call this function if you use the with keyword. Landing is
done automatically when the context goes out of scope.
:param velocity: The velocity (meters/second) when going down
:return:
"""
if self._is_flying:
duration_s = self._z / velocity
self._commander.takeoff(0, duration_s)
time.sleep(duration_s)
self._z = 0.0
self._commander.stop()
self._is_flying = False
def __enter__(self):
self.take_off()
return self
def __exit__(self, exc_type, exc_val, exc_tb):
self.land()
def go_to(self, x, y, z, velocity=VELOCITY):
dx = x - self._x
dy = y - self._y
dz = z - self._z
distance = math.sqrt(dx * dx + dy * dy + dz * dz)
duration_s = distance / velocity
self._cf.commander.go_to(x, y, z, 0, duration_s)
time.sleep(duration_s)
self._x = x
self._y = y
self._z = z
def left(self, distance_m, velocity=VELOCITY):
"""
Go left
:param distance_m: the distance to travel (meters)
:param velocity: the velocity of the motion (meters/second)
:return:
"""
self.move_distance(0.0, distance_m, 0.0, velocity)
def right(self, distance_m, velocity=VELOCITY):
"""
Go right
:param distance_m: the distance to travel (meters)
:param velocity: the velocity of the motion (meters/second)
:return:
"""
self.move_distance(0.0, -distance_m, 0.0, velocity)
def forward(self, distance_m, velocity=VELOCITY):
"""
Go forward
:param distance_m: the distance to travel (meters)
:param velocity: the velocity of the motion (meters/second)
:return:
"""
self.move_distance(distance_m, 0.0, 0.0, velocity)
def back(self, distance_m, velocity=VELOCITY):
"""
Go backwards
:param distance_m: the distance to travel (meters)
:param velocity: the velocity of the motion (meters/second)
:return:
"""
self.move_distance(-distance_m, 0.0, 0.0, velocity)
def up(self, distance_m, velocity=VELOCITY):
"""
Go up
:param distance_m: the distance to travel (meters)
:param velocity: the velocity of the motion (meters/second)
:return:
"""
self.move_distance(0.0, 0.0, distance_m, velocity)
def down(self, distance_m, velocity=VELOCITY):
"""
Go down
:param distance_m: the distance to travel (meters)
:param velocity: the velocity of the motion (meters/second)
:return:
"""
self.move_distance(0.0, 0.0, -distance_m, velocity)
def move_distance(self, distance_x_m, distance_y_m, distance_z_m,
velocity=VELOCITY):
"""
Move in a straight line.
positive X is forward
positive Y is left
positive Z is up
:param distance_x_m: The distance to travel along the X-axis (meters)
:param distance_y_m: The distance to travel along the Y-axis (meters)
:param distance_z_m: The distance to travel along the Z-axis (meters)
:param velocity: the velocity of the motion (meters/second)
:return:
"""
x = self._x + distance_x_m
y = self._y + distance_y_m
z = self._z + distance_z_m
self.go_to(x, y, z, velocity)
def _reset_position_estimator(self):
self._cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
self._cf.param.set_value('kalman.resetEstimation', '0')
time.sleep(2)
def _activate_high_level_commander(self):
self._cf.param.set_value('commander.enHighLevel', '1')
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment