From 9c5beaa5a016471761d6bfc6b7b983616808a154 Mon Sep 17 00:00:00 2001 From: qiuwb <389791945@qq.com> Date: Wed, 19 Apr 2023 19:40:48 +0800 Subject: [PATCH] =?UTF-8?q?tello=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/TelloControl/DroneController.py | 69 +++++++++++++++++---- src/TelloControl/Pass.py | 93 +++++++++++++++++++++++++++++ 2 files changed, 149 insertions(+), 13 deletions(-) create mode 100644 src/TelloControl/Pass.py diff --git a/src/TelloControl/DroneController.py b/src/TelloControl/DroneController.py index 03cbcf8..03d7544 100644 --- a/src/TelloControl/DroneController.py +++ b/src/TelloControl/DroneController.py @@ -22,14 +22,10 @@ class DroneController: self.drone.connect() self.drone.streamon() self.drone.LOGGER.setLevel(logging.ERROR) + self.up = False sleep(5) kp.init() - self.socketInit() - def socketInit(self): - self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - self.socket.bind(('', 9000)) - self.server_address=('192.168.10.3',9999) def find_face(self, img): face_cascade = cv2.CascadeClassifier("Resources/haarcascade_frontalface_default.xml") face_cascade.load("Resources/haarcascade_frontalface_default.xml") @@ -99,6 +95,8 @@ class DroneController: cv2.imwrite('D:/snap-{}.jpg'.format(time.strftime("%H%M%S", time.localtime())), image) if kp.getKey("UP"): self.drone.takeoff() + time.sleep(2) + self.autoGo() elif kp.getKey("DOWN"): self.drone.land() @@ -129,6 +127,7 @@ class DroneController: elif kp.getKey("d"): key_pressed = 1 yv = self.speed + info_text = "battery : {0}% height: {1}cm time: {2}".format(self.drone.get_battery(), self.drone.get_height(), time.strftime("%H:%M:%S",time.localtime())) cv2.putText(image, info_text, (10, 20), self.font, self.font_scale, (0, 0, 255), self.line_thickness) if key_pressed == 1: @@ -137,6 +136,47 @@ class DroneController: self.drone.send_rc_control(lr, fb, ud, yv) + + def Rotation(self): + self.Toward = '北' + # self.steps = Map.Map('116.481028,39.989643', '116.434446,39.90816') + self.steps=[{'instruction': '向南步行16米左转', 'orientation': '南', 'road': [], 'distance': '16', 'duration': '13', 'polyline': '116.480885,39.989371;116.480907,39.989353;116.48089,39.989227', 'action': '左转', 'assistant_action': [], 'walk_type': '0'}, {'instruction': '向东南步行292米右转', 'orientation': '东南', 'road': [], 'distance': '292', 'duration': '234', 'polyline': '116.480885,39.989223;116.480933,39.989201;116.480933,39.989201;116.480955,39.989184;116.481155,39.989058;116.481155,39.989058;116.481428,39.98888;116.481428,39.98888;116.481467,39.988859;116.481467,39.988859;116.482574,39.988125;116.48322,39.987665;116.48322,39.987665;116.48332,39.987565;116.48332,39.987565;116.48342,39.987435', 'action': '右转', 'assistant_action': [], 'walk_type': '0'}, {'instruction': '沿望京东路辅路向西南步行193米直行', 'orientation': '西南', 'road': '望京东路辅路', 'distance': '193', 'duration': '154', 'polyline': '116.48342,39.987431;116.483368,39.987387;116.483368,39.987387;116.48263,39.986753;116.48263,39.986753;116.482591,39.986719;116.482591,39.986719;116.482148,39.986289;116.482014,39.986068', 'action': '直行', 'assistant_action': [], 'walk_type': '0'}, {'instruction': '沿望京东路向西南步行93米左转', 'orientation': '西南', 'road': '望京东路', 'distance': '93', 'duration': '74', 'polyline': '116.48201,39.986063;116.481289,39.985434', 'action': '左转', 'assistant_action': [], 'walk_type': '0'}, {'instruction': '沿广顺南大街向东南步行161米右转', 'orientation': '东南', 'road': '广顺南大街', 'distance': '161', 'duration': '129', 'polyline': '116.481285,39.98543;116.481411,39.985339;116.481411,39.985339;116.481758,39.98513;116.481758,39.98513;116.482053,39.984931;116.482053,39.984931;116.482374,39.984714;116.482374,39.984714;116.482426,39.984674;116.482426,39.984674;116.482726,39.98447', 'action': '右转', 'assistant_action': [], 'walk_type': '0'}, {'instruction': '沿京密路向西南步行3665米直行', 'orientation': '西南', 'road': '京密路', 'distance': '3665', 'duration': '2932', 'polyline': '116.482726,39.984466;116.482591,39.984336;116.482591,39.984336;116.481606,39.983429;116.481606,39.983429;116.481233,39.983086;116.480729,39.982643;116.479983,39.981957;116.479653,39.981667;116.479653,39.981667;116.47809,39.980269;116.47809,39.980269;116.478021,39.980204;116.478021,39.980204;116.477982,39.980165;116.477982,39.980165;116.477205,39.979436;116.477205,39.979436;116.476892,39.979149;116.476892,39.979149;116.476662,39.97895;116.476662,39.97895;116.476055,39.97839;116.476055,39.97839;116.475039,39.977478;116.475039,39.977478;116.475,39.977448;116.475,39.977448;116.47388,39.976428;116.47388,39.976428;116.473286,39.975885;116.473286,39.975885;116.472765,39.97543;116.472765,39.97543;116.4724,39.975104;116.4724,39.975104;116.471315,39.974149;116.471315,39.974149;116.46924,39.972261;116.46924,39.972261;116.468281,39.971402;116.468281,39.971402;116.466997,39.97023;116.466523,39.969805;116.466523,39.969805;116.465473,39.968902;116.465473,39.968902;116.465365,39.968754;116.465365,39.968754;116.464961,39.968381;116.464961,39.968381;116.464939,39.968359;116.464939,39.968359;116.464779,39.968216;116.464779,39.968216;116.464466,39.967934;116.464466,39.967934;116.462817,39.966432;116.462817,39.966432;116.462747,39.96638;116.462747,39.96638;116.460803,39.964601;116.460803,39.964601;116.460182,39.964054;116.460182,39.964054;116.460117,39.963984;116.460117,39.963984;116.460017,39.963898;116.460017,39.963898;116.459405,39.963342;116.459405,39.963342;116.458086,39.962135;116.458086,39.962135;116.457917,39.961988;116.457917,39.961988;116.457773,39.961853;116.457773,39.961853;116.45737,39.961476;116.457339,39.961454;116.457339,39.961454;116.457201,39.961341;116.457201,39.961341;116.45691,39.961085;116.45691,39.961085;116.456766,39.960972;116.456766,39.960972;116.456658,39.960881;116.456658,39.960881;116.456415,39.960716;116.456415,39.960716;116.45605,39.960473;116.45553,39.960217;116.45553,39.960217;116.454766,39.959839;116.454583,39.959744;116.45444,39.959644;116.454301,39.959575', 'action': '直行', 'assistant_action': [], 'walk_type': '0'}, {'instruction': '沿香河园路向西南步行561米向右前方行走', 'orientation': '西南', 'road': '香河园路', 'distance': '561', 'duration': '449', 'polyline': '116.454297,39.95957;116.453516,39.95885;116.453516,39.95885;116.453316,39.958663;116.453316,39.958663;116.452821,39.958212;116.452821,39.958212;116.452669,39.958069;116.452669,39.958069;116.452296,39.957726;116.452296,39.957726;116.450929,39.956506;116.450929,39.956506;116.450916,39.956489;116.450916,39.956489;116.450668,39.956285;116.450668,39.956285;116.450456,39.956098;116.450456,39.956098;116.449991,39.955694', 'action': '向右前方行走', 'assistant_action': [], 'walk_type': '0'}, {'instruction': '向西南步行633米左转', 'orientation': '西南', 'road': [], 'distance': '633', 'duration': '506', 'polyline': '116.449987,39.95569;116.449631,39.955538;116.449536,39.95546;116.449536,39.95546;116.449275,39.955234;116.449275,39.955234;116.449023,39.955004;116.449023,39.955004;116.448581,39.954531;116.448581,39.954531;116.448442,39.954401;116.448108,39.954132;116.448108,39.954132;116.447643,39.953737;116.447643,39.953737;116.447526,39.95365;116.447526,39.95365;116.446636,39.952999;116.446636,39.952999;116.446237,39.952717;116.446237,39.952717;116.446107,39.952622;116.446107,39.952622;116.445412,39.952118;116.445412,39.952118;116.445126,39.951931;116.445126,39.951931;116.444714,39.951667', 'action': '左转', 'assistant_action': [], 'walk_type': '0'}, {'instruction': '沿左家庄南斜街向南步行14米右转', 'orientation': '南', 'road': '左家庄南斜街', 'distance': '14', 'duration': '11', 'polyline': '116.444709,39.951662;116.444714,39.951532', 'action': '右转', 'assistant_action': [], 'walk_type': '0'}, {'instruction': '沿香河园路向西南步行971米右转', 'orientation': '西南', 'road': '香河园路', 'distance': '971', 'duration': '777', 'polyline': '116.444714,39.951528;116.444353,39.951285;116.444353,39.951285;116.444141,39.951124;116.444141,39.951124;116.442604,39.949987;116.442604,39.949987;116.442569,39.949965;116.442569,39.949965;116.442361,39.949818;116.442361,39.949818;116.442201,39.949701;116.442201,39.949701;116.441341,39.949067;116.441341,39.949067;116.44125,39.948997;116.441202,39.948967;116.441202,39.948967;116.44112,39.948898;116.44112,39.948898;116.440495,39.948372;116.440495,39.948372;116.439891,39.947873;116.439891,39.947873;116.439553,39.947587;116.439201,39.947266;116.438945,39.947001;116.438945,39.947001;116.438898,39.946953;116.438898,39.946953;116.438576,39.946584;116.438576,39.946584;116.438464,39.946441;116.438464,39.946441;116.438294,39.946254;116.438294,39.946254;116.438225,39.946185;116.438225,39.946185;116.437921,39.945781;116.437921,39.945781;116.43783,39.945647;116.43783,39.945647;116.437678,39.945434;116.437439,39.945052;116.437439,39.945052;116.437344,39.944891', 'action': '右转', 'assistant_action': [], 'walk_type': '0'}, {'instruction': '沿香河园路向西步行279米左转', 'orientation': '西', 'road': '香河园路', 'distance': '279', 'duration': '223', 'polyline': '116.437339,39.944887;116.436549,39.944878;116.436549,39.944878;116.436016,39.944878;116.436016,39.944878;116.435451,39.944874;116.435304,39.944848;116.435304,39.944848;116.435182,39.944844;116.435182,39.944844;116.434961,39.944839;116.434961,39.944839;116.434831,39.944839;116.434831,39.944839;116.434557,39.944839;116.434557,39.944839;116.434444,39.944839;116.434444,39.944839;116.434258,39.944839;116.434258,39.944839;116.434236,39.944839;116.434236,39.944839;116.434006,39.944835', 'action': '左转', 'assistant_action': [], 'walk_type': '0'}, {'instruction': '沿东直门北大街向南步行408米直行', 'orientation': '南', 'road': '东直门北大街', 'distance': '408', 'duration': '326', 'polyline': '116.434002,39.944831;116.434049,39.944683;116.434036,39.944271;116.434036,39.944271;116.434023,39.943898;116.434023,39.943898;116.434023,39.943832;116.434023,39.943832;116.434002,39.943464;116.434002,39.943464;116.434002,39.94339;116.433932,39.943225;116.433932,39.943225;116.433902,39.942912;116.433911,39.942617;116.433911,39.942617;116.433958,39.94171;116.433958,39.94171;116.43398,39.941124', 'action': '直行', 'assistant_action': [], 'walk_type': '0'}, {'instruction': '沿东直门南大街向南步行1055米直行', 'orientation': '南', 'road': '东直门南大街', 'distance': '1055', 'duration': '844', 'polyline': '116.43398,39.94112;116.43401,39.940495;116.43401,39.940495;116.434049,39.939566;116.434049,39.939566;116.434102,39.938802;116.434102,39.938802;116.434188,39.938607;116.43424,39.937661;116.43424,39.937661;116.434253,39.93747;116.434253,39.93747;116.434271,39.9373;116.434271,39.937196;116.434271,39.937196;116.434271,39.937166;116.434271,39.937166;116.434284,39.936936;116.434284,39.936936;116.43431,39.936194;116.434223,39.93602;116.434223,39.93602;116.434223,39.935959;116.434223,39.935959;116.434223,39.935716;116.434223,39.935716;116.434262,39.934397;116.434262,39.934397;116.434262,39.934379;116.434262,39.934379;116.434314,39.933125;116.434314,39.933125;116.434314,39.93306;116.434314,39.93306;116.434362,39.932214;116.434362,39.932214;116.434379,39.931797;116.43444,39.931584', 'action': '直行', 'assistant_action': [], 'walk_type': '0'}, {'instruction': '沿朝阳门北大街向南步行848米直行', 'orientation': '南', 'road': '朝阳门北大街', 'distance': '848', 'duration': '678', 'polyline': '116.43444,39.93158;116.434462,39.931215;116.434462,39.931215;116.43447,39.93105;116.43447,39.93105;116.43447,39.930872;116.43447,39.930872;116.434475,39.930794;116.434475,39.930794;116.434475,39.93076;116.434475,39.93076;116.434501,39.930234;116.434501,39.930234;116.434523,39.929531;116.434523,39.929531;116.434562,39.928681;116.434562,39.928681;116.434562,39.928633;116.434562,39.928633;116.43457,39.928585;116.434575,39.928398;116.434575,39.928398;116.434614,39.92753;116.434614,39.92753;116.434631,39.927053;116.434631,39.927053;116.434631,39.926949;116.434631,39.926949;116.434635,39.926758;116.434635,39.926758;116.434635,39.926497;116.434635,39.926445;116.434644,39.926211;116.434696,39.925035;116.434696,39.925035;116.434705,39.925009;116.434705,39.925009;116.434744,39.923976;116.434753,39.923885', 'action': '直行', 'assistant_action': [], 'walk_type': '0'}, {'instruction': '沿朝阳门南大街向南步行904米直行', 'orientation': '南', 'road': '朝阳门南大街', 'distance': '904', 'duration': '723', 'polyline': '116.434753,39.92388;116.434753,39.923867;116.434753,39.923867;116.434805,39.922565;116.434826,39.922283;116.434826,39.922283;116.434852,39.92201;116.434852,39.92201;116.434852,39.921931;116.434852,39.921931;116.434874,39.921515;116.434874,39.921515;116.434896,39.921089;116.434896,39.921089;116.434913,39.920703;116.434913,39.920703;116.434935,39.920304;116.434935,39.920304;116.434957,39.919839;116.434957,39.919839;116.434974,39.919475;116.434974,39.919475;116.434987,39.91921;116.434987,39.91921;116.435009,39.918668;116.435009,39.918668;116.435026,39.917982;116.435026,39.917982;116.435048,39.917465;116.435048,39.917465;116.435074,39.916962;116.435074,39.916962;116.435078,39.916645;116.435078,39.916645;116.435078,39.916597;116.435078,39.916597;116.435104,39.916367;116.435104,39.916367;116.435104,39.916237;116.435104,39.916237;116.435104,39.916137;116.435104,39.916137;116.435109,39.916094;116.435109,39.916094;116.435117,39.915794;116.435117,39.915794;116.435126,39.915668', 'action': '直行', 'assistant_action': [], 'walk_type': '0'}, {'instruction': '沿建国门北大街向南步行27米向左前方行走', 'orientation': '南', 'road': '建国门北大街', 'distance': '27', 'duration': '22', 'polyline': '116.435126,39.915664;116.435135,39.915603;116.435135,39.915421', 'action': '向左前方行走', 'assistant_action': [], 'walk_type': '0'}, {'instruction': '步行102米右转', 'orientation': [], 'road': [], 'distance': '102', 'duration': '82', 'polyline': '116.435135,39.915417;116.435208,39.915417;116.435208,39.915417;116.435217,39.91526;116.435217,39.91526;116.435082,39.91526;116.435082,39.91526;116.434727,39.915247;116.434727,39.915247;116.434579,39.915247;116.434579,39.915247;116.434592,39.915065;116.434592,39.915065;116.434661,39.915065', 'action': '右转', 'assistant_action': [], 'walk_type': '4'}, {'instruction': '沿建国门北大街向南步行746米直行', 'orientation': '南', 'road': '建国门北大街', 'distance': '746', 'duration': '597', 'polyline': '116.434661,39.915061;116.434661,39.914952;116.434661,39.914952;116.434683,39.914523;116.434683,39.914523;116.434705,39.914319;116.434705,39.914319;116.434705,39.914258;116.434705,39.914258;116.434727,39.913967;116.434727,39.913967;116.434757,39.913555;116.434757,39.913555;116.434757,39.91349;116.434757,39.91349;116.434783,39.913173;116.434783,39.913173;116.434787,39.913025;116.434787,39.913025;116.434796,39.912839;116.434796,39.912839;116.434826,39.912244;116.434826,39.912244;116.434852,39.911784;116.434852,39.911784;116.434852,39.911684;116.434883,39.911202;116.434883,39.911202;116.434918,39.910109;116.434918,39.910109;116.434965,39.909045;116.434965,39.909045;116.434965,39.909028;116.434931,39.908932;116.434887,39.908867;116.434887,39.908867;116.434831,39.908724;116.434818,39.908655;116.434818,39.908542;116.434826,39.908424;116.434883,39.908303', 'action': '直行', 'assistant_action': [], 'walk_type': '0'}, {'instruction': '沿环二环城市绿道向南步行25米右转', 'orientation': '南', 'road': '环二环城市绿道', 'distance': '25', 'duration': '20', 'polyline': '116.434883,39.908299;116.434887,39.908077', 'action': '右转', 'assistant_action': [], 'walk_type': '0'}, {'instruction': '步行37米到达目的地', 'orientation': [], 'road': [], 'distance': '37', 'duration': '30', 'polyline': '116.434887,39.908073;116.434449,39.908073', 'action': [], 'assistant_action': '到达目的地', 'walk_type': '0'}] + + if self.steps[0]['orientation'] == '南': + self.drone.rotate_clockwise(180) + self.Toward = '南' + elif self.steps[0]['orientation'] == '西': + self.drone.rotate_counter_clockwise(90) + self.Toward = '西' + elif self.steps[0]['orientation'] == '东': + self.drone.rotate_clockwise(90) + self.Toward = '东' + + def move(self, distance): + if distance>500: + self.drone.move_forward(500) + self.move(distance-500) + elif distance<20: + self.drone.move_forward(20+distance) + self.drone.move_back(20) + + def autoGo(self): + # while self.up!=True: + # time.sleep(5) + self.Rotation() + for step in self.steps: + # self.move(int(step['distance'])*10) + if step['action']=='左转': + self.drone.rotate_counter_clockwise(90) + elif step['action']=='右转': + self.drone.rotate_clockwise(90) + elif step['action']=='向左前方行走': + self.drone.rotate_counter_clockwise(45) + elif step['action']=='向右前方行走': + self.drone.rotate_clockwise(45) + print(step['action']) + time.sleep(1) + def run(self): p_error_rotate, p_error_up = 0, 0 while True: @@ -147,14 +187,17 @@ class DroneController: p_error_rotate, p_error_up = self.track_face(img, info, p_error_rotate, p_error_up) cv2.imshow("Drone Control Centre", image) cv2.waitKey(1) - # if self.drone.get_frame_read().grabbed: - # # 转换帧格式 - # encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 90] - # result, frame_encode = cv2.imencode('.jpg', img, encode_param) - # # 发送数据包 - # self.socket.sendto(frame_encode.tobytes(), self.server_address) + def stop(self): + self.drone.land() + self.drone.streamoff() + cv2.destroyAllWindows() + + def __del__(self): + self.drone.land() if __name__ == '__main__': - tello=DroneController() - tello.run() \ No newline at end of file + + tello_control = DroneController() + tello_control.run() + tello_control.stop() \ No newline at end of file diff --git a/src/TelloControl/Pass.py b/src/TelloControl/Pass.py new file mode 100644 index 0000000..7b32dda --- /dev/null +++ b/src/TelloControl/Pass.py @@ -0,0 +1,93 @@ +import cv2 +import socket +import threading +import matplotlib.pyplot as plt +import matplotlib.animation as animation +import numpy as np + +class VideoCaptureTransfer: + def __init__(self, source=0, dest_ip='127.0.0.1', dest_port=8000): + self.capture = cv2.VideoCapture(source) + + self.dest_ip = dest_ip + self.dest_port = dest_port + + self.frame = None + self.save_video = False + + self.lock = threading.Lock() + + self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + + self.thread = threading.Thread(target=self._capture_send) + self.thread.start() + + def _capture_send(self): + while self.capture.isOpened(): + ret, frame = self.capture.read() + if not ret: + break + + frame = cv2.resize(frame, (640, 480)) + + if self.save_video: + self.output.write(frame) + + with self.lock: + self.frame = frame.copy() + + _, data = cv2.imencode('.jpg', frame, [int(cv2.IMWRITE_JPEG_QUALITY), 50]) + + self.sock.sendto(data.tobytes(), (self.dest_ip, self.dest_port)) + + self.capture.release() + cv2.destroyAllWindows() + self.output.release() + + def start_recording(self, filename="output.avi", fmt="XVID"): + fourcc = cv2.VideoWriter_fourcc(*fmt) + fps = int(self.capture.get(cv2.CAP_PROP_FPS)) + frame_size = (int(self.capture.get(cv2.CAP_PROP_FRAME_WIDTH)), int(self.capture.get(cv2.CAP_PROP_FRAME_HEIGHT))) + self.output = cv2.VideoWriter(filename, fourcc, fps, frame_size) + self.save_video = True + + def stop_recording(self): + self.save_video = False + + def read(self): + with self.lock: + return self.frame + + def release(self): + self.thread.join() + self.sock.close() + +if __name__ == '__main__': + vct = VideoCaptureTransfer(0,"192.168.43.18",9999) + + def on_key_press(event): + if event.key == ' ': + if not vct.save_video: + vct.start_recording() + + def on_key_release(event): + if event.key == ' ': + if vct.save_video: + vct.stop_recording() + + fig, ax = plt.subplots() + im = ax.imshow(np.zeros((480, 640, 3), dtype=np.uint8)) + + def update_frame(_): + frame = vct.read() + im.set_data(frame) + return [im] + + ani = animation.FuncAnimation(fig, update_frame, interval=50, blit=True) + + fig.canvas.mpl_connect('key_press_event', on_key_press) + fig.canvas.mpl_connect('key_release_event', on_key_release) + + plt.show() + + vct.release()