diff --git a/bin/cfkinect b/bin/cfkinect new file mode 100755 index 0000000000000000000000000000000000000000..673645857dd3f639825f2f78cb84a829f5caef49 --- /dev/null +++ b/bin/cfkinect @@ -0,0 +1,85 @@ +#!/usr/bin/env python2 + +from __future__ import absolute_import + +APP_NAME = "cfkinect" + +def init_config_path(isInSource): + import sys + import os + import os.path as _path + from os.path import expanduser + + if isInSource: + configPath = _path.join(sys.path[0], "..", "conf") + else: + prefix = expanduser("~") + + if sys.platform == "linux2": + if _path.exists(_path.join(prefix, ".local")): + configPath = _path.join(prefix, ".local", APP_NAME) + else: + configPath = _path.join(prefix, "."+APP_NAME) + elif sys.platform == "win32": + configPath = _path.join(os.environ['APPDATA'], APP_NAME) + elif sys.platform == "darwin": + from AppKit import NSSearchPathForDirectoriesInDomains + # FIXME: Copy-pasted from StackOverflow, not tested! + # http://developer.apple.com/DOCUMENTATION/Cocoa/Reference/Foundation/Miscellaneous/Foundation_Functions/Reference/reference.html#//apple_ref/c/func/NSSearchPathForDirectoriesInDomains + # NSApplicationSupportDirectory = 14 + # NSUserDomainMask = 1 + # True for expanding the tilde into a fully qualified path + configPath = path.join(NSSearchPathForDirectoriesInDomains(14, 1, True)[0], APPNAME) + else: + #Unknown OS, I hope this is good enough + configPath = _path.join(prefix, "."+APP_NAME) + + if not _path.exists(configPath): os.makedirs(configPath) + + return configPath + +def init_paths(): + """Make the app work in the source tree. + This puts the root module folder in path[0] and the config folder in + path[1]""" + import sys + import os.path as _path + + sys.path = ["",""] + sys.path + inSource = False + + if hasattr(sys, "frozen"): + if sys.frozen in ('dll', 'console_exe', 'windows_exe'): + sys.path[0] = _path.normpath(_path.dirname(_path.realpath(sys.executable))) + elif frozen in ('macosx_app',): + # FIXME: Copy-pasted from StackOverflow, not tested! + # py2app: + # Notes on how to find stuff on MAC, by an expert (Bob Ippolito): + # http://mail.python.org/pipermail/pythonmac-sig/2004-November/012121.html + approot = os.environ['RESOURCEPATH'] + else: + prefix = _path.normpath(_path.join(_path.dirname(_path.realpath(__file__)), '..')) + + src_lib = _path.join(prefix, 'lib') + share_lib = prefix + inSource = False + for location in [src_lib, share_lib] + sys.path: + main_ui = _path.join(location, 'cfclient', 'ui','main.ui') + if _path.exists(main_ui): + sys.path[0] = location + if location == src_lib: + inSource = True + break + + if sys.path[0] == "": + raise Exception("Cannot find cfclient install folder!") + + sys.path[1] = init_config_path(inSource) + print "Info: Using config path: ", sys.path[1] + print "Info: sys.path=", sys.path + + +if __name__ == '__main__': + init_paths() + import cfkinect + cfkinect.main() diff --git a/lib/cfkinect.py b/lib/cfkinect.py new file mode 100644 index 0000000000000000000000000000000000000000..4c401fe251cb72f808acf63bbdc3db6d64863e37 --- /dev/null +++ b/lib/cfkinect.py @@ -0,0 +1,156 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2013 Bitcraze AB +# +# Crazyflie Nano Quadcopter Client +# +# 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. + +""" +Kinect controller +""" + +import sys +import os +import logging +import signal + +import cflib.crtp +from cflib.crazyflie import Crazyflie +import cfclient.utils +from kinect.kinect import Kinect +from kinect.pid import PID, PID_RP + +from cfclient.utils.config import Config + +# Roll/pitch limit +CAP = 15.0 +# Thrust limit +TH_CAP = 55000 + +class KinectPilot(): + """Crazyflie Kinect Pilot""" + + def __init__(self): + """Initialize the headless client and libraries""" + cflib.crtp.init_drivers() + + self._cf = Crazyflie(ro_cache=sys.path[0]+"/cflib/cache", + rw_cache=sys.path[1]+"/cache") + + self.kinect = Kinect() + + # Create PID controllers for piloting the Crazyflie + # The Kinect camera is in the back of the Crazyflie so + # pitch = depth, roll = x, thrust = y + self.r_pid = PID_RP(P=0.04, D=0.25, set_point=0.0) + self.p_pid = PID_RP(P=0.20, D=0.25, set_point=0.0) + self.t_pid = PID(P=15.0, D=1000.0, I=40.0, set_point=0.0) + + signal.signal(signal.SIGINT, signal.SIG_DFL) + + def connect_crazyflie(self, link_uri): + """Connect to a Crazyflie on the given link uri""" + self._cf.connectionFailed.add_callback(self._connection_failed) + self._cf.open_link(link_uri) + + def _connection_failed(self, link, message): + """Callback for a failed Crazyflie connection""" + print "Connection failed on {}: {}".format(link, message) + sys.exit(-1) + + def _p2t(self, percentage): + """Convert a percentage to raw thrust""" + return int(65000 * (percentage / 100.0)) + + def control(self, dry=False): + """Control loop for Kinect control""" + safety = 10 + while True: + (x,y,depth) = self.kinect.find_position() + if x and y and depth: + safety = 10 + roll = self.r_pid.update(320-x) + pitch = self.p_pid.update(170-depth) + thrust = self.t_pid.update(120-y) + roll_sp = -roll + pitch_sp = -pitch + thrust_sp = thrust+40000 + if (roll_sp > CAP): + roll_sp = CAP + if (roll_sp < -CAP): + roll_sp = -CAP + + if (pitch_sp > CAP): + pitch_sp = CAP + if (pitch_sp < -CAP): + pitch_sp = -CAP + + if (thrust_sp > TH_CAP): + thrust_sp = TH_CAP + if (thrust_sp < 0): + thrust_sp = 0 + + print self.t_pid.error + + texts = ["R=%.2f,P=%.2f,T=%.2f" % (roll_sp, pitch_sp, thrust_sp), + "TH: P=%.2f" % self.t_pid.P_value, + "TH: D=%.2f" % self.t_pid.D_value, + "TH: I=%.2f" % self.t_pid.I_value, + "TH: e=%.2f" % self.t_pid.error] + self.kinect.show(texts) + if not dry: + self._cf.commander.send_setpoint(roll_sp, pitch_sp, 0, thrust_sp) + else: + safety = safety - 1 + if safety < 0 and not dry: + self._cf.commander.send_setpoint(0, 0, 0, 0) + break + +def main(): + """Main Crazyflie Kinect application""" + import argparse + + parser = argparse.ArgumentParser(prog="cfkinect") + parser.add_argument("-u", "--uri", action="store", dest="uri", type=str, + default="radio://0/10/250K", + help="URI to use for connection to the Crazyradio" + " dongle, defaults to radio://0/10/250K") + parser.add_argument("-y", "--dry", dest="dry", action="store_true", + help="Do not send commands to Crazyflie") + parser.add_argument("-d", "--debug", action="store_true", dest="debug", + help="Enable debug output") + (args, unused) = parser.parse_known_args() + + if args.debug: + logging.basicConfig(level=logging.DEBUG) + else: + logging.basicConfig(level=logging.INFO) + + kinect_pilot = KinectPilot() + + if not args.dry: + kinect_pilot.connect_crazyflie(link_uri=args.uri) + + kinect_pilot.control(args.dry) + diff --git a/lib/kinect/__init__.py b/lib/kinect/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e30fbc0704efd2a82e75b06d09ea81bf0752a73f --- /dev/null +++ b/lib/kinect/__init__.py @@ -0,0 +1,27 @@ +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2013 Bitcraze AB +# +# Crazyflie Nano Quadcopter Client +# +# 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. + + diff --git a/lib/kinect/frame_convert.py b/lib/kinect/frame_convert.py new file mode 100644 index 0000000000000000000000000000000000000000..9a674130c0261ea436f82d1798fbedc9955f1db8 --- /dev/null +++ b/lib/kinect/frame_convert.py @@ -0,0 +1,60 @@ +import numpy as np + + +def pretty_depth(depth): + """Converts depth into a 'nicer' format for display + + This is abstracted to allow for experimentation with normalization + + Args: + depth: A numpy array with 2 bytes per pixel + + Returns: + A numpy array that has been processed whos datatype is unspecified + """ + np.clip(depth, 0, 2**10 - 1, depth) + depth >>= 2 + depth = depth.astype(np.uint8) + return depth + + +def pretty_depth_cv(depth): + """Converts depth into a 'nicer' format for display + + This is abstracted to allow for experimentation with normalization + + Args: + depth: A numpy array with 2 bytes per pixel + + Returns: + An opencv image who's datatype is unspecified + """ + import cv + depth = pretty_depth(depth) + image = cv.CreateImageHeader((depth.shape[1], depth.shape[0]), + cv.IPL_DEPTH_8U, + 1) + cv.SetData(image, depth.tostring(), + depth.dtype.itemsize * depth.shape[1]) + return image + + +def video_cv(video): + """Converts video into a BGR format for opencv + + This is abstracted out to allow for experimentation + + Args: + video: A numpy array with 1 byte per pixel, 3 channels RGB + + Returns: + An opencv image who's datatype is 1 byte, 3 channel BGR + """ + import cv + video = video[:, :, ::-1] # RGB -> BGR + image = cv.CreateImageHeader((video.shape[1], video.shape[0]), + cv.IPL_DEPTH_8U, + 3) + cv.SetData(image, video.tostring(), + video.dtype.itemsize * 3 * video.shape[1]) + return image diff --git a/lib/kinect/kinect.py b/lib/kinect/kinect.py new file mode 100644 index 0000000000000000000000000000000000000000..4a9a268f9c55adeacfd0543b9c778446456b213f --- /dev/null +++ b/lib/kinect/kinect.py @@ -0,0 +1,188 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2013 Bitcraze AB +# +# Crazyflie Nano Quadcopter Client +# +# 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. + +""" +Kinect/OpenCV interface +""" + +from freenect import sync_get_depth as get_depth, sync_get_video as get_video +from frame_convert import video_cv, pretty_depth_cv +import cv +import numpy as np +import time + +IMG_WIDTH = 640 +IMG_HEIGHT = 480 + +class Kinect: + + def __init__(self): + self.text = [] + self.writer = cv.CreateVideoWriter("out.avi", cv.CV_FOURCC('F', 'L', 'V', '1'), 25, (IMG_WIDTH, IMG_HEIGHT), True) + self.lasttime = time.time() + return + + def _get_pos_spatial(self, th_img): + moments = cv.Moments(cv.GetMat(th_img)) + mom10 = cv.GetSpatialMoment(moments, 1, 0) + mom01 = cv.GetSpatialMoment(moments, 0, 1) + area = cv.GetCentralMoment(moments, 0, 0) + + if area > 10: + pos = [int(mom10/area), int(mom01/area)] + else: + pos = None + + return pos + + def _get_pos_countour(self, th_img): + storage = cv.CreateMemStorage(0) + contour = None + ci = cv.CreateImage(cv.GetSize(th_img), 8, 1) + ci = cv.CloneImage(th_img) + + contour = cv.FindContours(th_img, storage, cv.CV_RETR_CCOMP, cv.CV_CHAIN_APPROX_SIMPLE) + points = [] + + while contour: + bound_rect = cv.BoundingRect(list(contour)) + contour = contour.h_next() + pt1 = (bound_rect[0], bound_rect[1]) + pt2 = (bound_rect[0] + bound_rect[2], bound_rect[1] + bound_rect[3]) + points.append(pt1) + points.append(pt2) + #cv.Rectangle(img, pt1, pt2, cv.CV_RGB(255,0,0), 1) + + center_point = None + + if len(points): + center_point = reduce(lambda a, b: ((a[0] + b[0]) / 2, (a[1] + b[1]) / 2), points) + #cv.Circle(depth, center_point, 10, cv.CV_RGB(255, 255, 255), 1) + + return center_point + + def _get_pos(self, img, debug=False): + th_img = self._get_threashold_image_hsv(img) + #sp = self._get_pos_spatial(th_img) + cp = self._get_pos_countour(th_img) + + #if sp: + # cv.Circle(img, (sp[0], sp[1]) , 10, cv.CV_RGB(255, 255, 255), 1) + if cp: + cv.Circle(img, (cp[0], cp[1]) , 10, cv.CV_RGB(255, 0, 0), 1) + + return cp + + def _get_threashold_image_hsv(self, img, debug=False): + """Get the binary threashold image using a HSV method""" + imgHSV = cv.CreateImage(cv.GetSize(img), 8, 3); + cv.CvtColor(img, imgHSV, cv.CV_BGR2HSV); + cv.Smooth(imgHSV, imgHSV, cv.CV_GAUSSIAN, 3, 0) + img_th = cv.CreateImage(cv.GetSize(img), 8, 1); + + # Filter out red colors + cv.InRangeS(imgHSV, cv.Scalar(160, 190, 30), cv.Scalar(180, 255, 200), img_th); + # Fix the image a bit by eroding and dilating + eroded = cv.CreateImage(cv.GetSize(img), 8, 1); + cv.Erode( img_th, eroded, None, 1) + dilated = cv.CreateImage(cv.GetSize(img), 8, 1); + cv.Dilate( eroded, dilated, None, 4) + + if debug: + cv.ShowImage('HSV', imgHSV) + cv.ShowImage('threas', img_th) + cv.SaveImage('threas-%d.png' % (time.time()), img_th) + cv.ShowImage('eroded', eroded) + cv.ShowImage('dialated', dilated) + + return dilated + + def _get_depth(self, depth_image, debug=False): + """Get the depth reading from the Kinect""" + depth = None + + # Only use part of the span to avoid anything else than the Crazyflie + img_th = cv.CreateImage(cv.GetSize(depth_image), 8, 1); + cv.InRangeS(depth_image, 10, 210, img_th); + + # Calculate the mean depth + depth = cv.Avg(depth_image, img_th)[0] + + if debug: + font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1, 1, 0, 1, 1) + s2 = "%d" % depth + cv.PutText(img_th, s2, (0,60),font, 200) + cv.ShowImage('depth th', img_th) + + return depth + + + def find_position(self): + (kinect_depth,_), (rgb,_) = get_depth(), get_video() + self.img = video_cv(rgb) + depth_img = pretty_depth_cv(kinect_depth) + + position = self._get_pos(self.img) + + depth = self._get_depth(depth_img, debug=True) + + font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1, 1, 0, 1, 1) + + fps = 1/(time.time() - self.lasttime) + s1 = "FPS:%.1f" % fps + self.lasttime = time.time() + cv.PutText(self.img,s1, (0,30),font, cv.CV_RGB(255, 0, 0)) + + dt = "Depth: %d" % depth + if position: + pt = "Pos: X=%d Y=%d" % (position[0], position[1]) + else: + pt = "Pos: N/A" + cv.PutText(self.img, dt, (0,60),font, cv.CV_RGB(255, 0, 0)) + cv.PutText(self.img, pt, (0,90),font, cv.CV_RGB(255, 0, 0)) + + offset = 120 + for t in self.text: + cv.PutText(self.img, t, (0,offset),font, cv.CV_RGB(255, 0, 0)) + offset += 30 + + cv.ShowImage('RGB', self.img) + #cv.SaveImage('RGB-%d.png' % (time.time()), self.img) + #cv.ShowImage('DEPTH', depth_img) + cv.WriteFrame(self.writer, self.img) + cv.WaitKey(5) + + #cv.ShowImage('depth_mask', depth_mask) + try: + return (position[0], position[1], depth) + except: + return (None, None, None) + + def show(self, texts): + self.text = texts + diff --git a/lib/kinect/pid.py b/lib/kinect/pid.py new file mode 100644 index 0000000000000000000000000000000000000000..2c49748ec1d50c88e8d0979050808a1b26ad4fa5 --- /dev/null +++ b/lib/kinect/pid.py @@ -0,0 +1,155 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2013 Bitcraze AB +# +# Crazyflie Nano Quadcopter Client +# +# 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. + +import math + +class PID: + + def __init__(self, P=1.0, I=0.0, D=10.0, Derivator=0, Integrator=0, + Integrator_max=200, Integrator_min=-200, set_point=0.0, + power=1.0): + + self.Kp=P + self.Ki=I + self.Kd=D + self.Derivator=Derivator + self.power = power + self.Integrator=Integrator + self.Integrator_max=Integrator_max + self.Integrator_min=Integrator_min + self.last_error = 0.0 + self.last_value = 0.0 + + self.set_point=set_point + self.error=0.0 + + def update(self,current_value): + """ + Calculate PID output value for given reference input and feedback + """ + + self.error = self.set_point - current_value + + self.P_value = self.Kp * self.error + if (self.last_value >= current_value): + change = self.error - self.last_error + else: + change = 0.0 + + if self.error > 0.0: + self.I_value = self.Integrator * self.Ki + else: + self.I_value = (self.Integrator * self.Ki)*0.5 + + + #self.D_value = self.Kd * ( self.error - self.Derivator) + self.D_value = self.Kd * change + self.Derivator = self.error + + self.Integrator = self.Integrator + self.error/200.0 + + if self.Integrator > self.Integrator_max: + self.Integrator = self.Integrator_max + elif self.Integrator < self.Integrator_min: + self.Integrator = self.Integrator_min + + self.last_error = self.error + self.last_value = current_value + + PID = self.P_value + self.I_value + self.D_value + + return PID + + def set_point(self,set_point): + """Initilize the setpoint of PID""" + self.set_point = set_point + self.Integrator=0 + self.Derivator=0 + +class PID_RP: + + def __init__(self, P=1.0, I=0.0, D=10.0, Derivator=0, Integrator=0, + Integrator_max=250, Integrator_min=-100, set_point=0.0, + power=1.0): + + self.Kp=P + self.Ki=I + self.Kd=D + self.Derivator=Derivator + self.power = power + self.Integrator=Integrator + self.Integrator_max=Integrator_max + self.Integrator_min=Integrator_min + self.last_error = 0.0 + self.last_value = 0.0 + + self.set_point=set_point + self.error=0.0 + + def update(self,current_value): + """ + Calculate PID output value for given reference input and feedback + """ + + self.error = self.set_point - current_value + + self.P_value = self.Kp * self.error + change = self.error - self.last_error + + if self.error > 0.0: + self.I_value = self.Integrator * self.Ki + else: + self.I_value = (self.Integrator * self.Ki)*0.5 + + + #self.D_value = self.Kd * ( self.error - self.Derivator) + self.D_value = self.Kd * change + self.Derivator = self.error + + self.Integrator = self.Integrator + self.error/200.0 + + if self.Integrator > self.Integrator_max: + self.Integrator = self.Integrator_max + elif self.Integrator < self.Integrator_min: + self.Integrator = self.Integrator_min + + self.last_error = self.error + self.last_value = current_value + + PID = self.P_value + self.I_value + self.D_value + + return PID + + def set_point(self,set_point): + """ + Initilize the setpoint of PID + """ + self.set_point = set_point + self.Integrator=0 + self.Derivator=0 +